#include <NewPing.h>
#include <Servo.h>
#include "i2c_t3.h"
#include "teensy3_Melexis90620.h"
#include "BeerBotLib.h"
#include "ThermalCam.h"


ThermalCam thermCam;
int thermCamServoPin = 14;
int temperatureBorder = 24;

ContactSensor frontSensor;
int frontLeftPushButtonPin = 8;
int frontRightPushButtonPin = 15;

ContactSensor capSensor;
int capRightPushButtonPin = 7;
int capLeftPushButtonPin = 6;
int capConductorPin = 9;

Pusher pusher;
int pusherPin = 21;

Barrier lowBarrier;
int lowBarrierServoPin = 12;
int lowBarrierClosedPos = 150;
int lowBarrierOpenPos = 60;

Barrier highBarrier;
int highBarrierServoPin = 13;
int highBarrierClosedPos = 130;
int highBarrierOpenPos = 83;

Gear myGear;
float wheelDiameter = 9.0;
float axisDistance = 24.0;
int leftMotorStepPin = 5;
int leftMotorDirPin = 4;
int rightMotorStepPin = 3;
int rightMotorDirPin = 2;

int phase = 0;
boolean bottleFound = false;
int lastFoundPos = 90;

void setup() {
  Serial.begin(9600); // for debugging purposes only
  
  thermCam.setup(thermCamServoPin, temperatureBorder);
  myGear.setup(wheelDiameter, axisDistance, leftMotorStepPin, leftMotorDirPin, rightMotorStepPin, rightMotorDirPin);
  frontSensor.setup(frontRightPushButtonPin, frontLeftPushButtonPin, -1);
  capSensor.setup(capRightPushButtonPin, capLeftPushButtonPin, capConductorPin);
  pusher.setup(pusherPin);
  lowBarrier.setup(lowBarrierServoPin, lowBarrierClosedPos, lowBarrierOpenPos);
  highBarrier.setup(highBarrierServoPin, highBarrierClosedPos, highBarrierOpenPos);    
}

void loop() {
  
	main1();
  
}

/**
* test functions for the different components of the robot.
* for debugging and presentation purposes.
*/

void testOpen() {
  lowBarrier.closeBarrier();
  delay(500);
  highBarrier.closeBarrier();
  delay(1000);
  pusher.push(300);
  delay(2000);
}

void testPusher() {
  pusher.push(300);
  delay(3000);
}

void testBarriers() {
  highBarrier.closeBarrier();
  delay(15);
  lowBarrier.closeBarrier();
  delay(1000);
  lowBarrier.openBarrier();
  delay(15);
  highBarrier.openBarrier();
  delay(1000);
}

void testDrive() {
  myGear.drive(20, 12, true);
  myGear.turnAngle(90, true, 12);
}

/**
* the main function of the robots software.
* defines the different phases of the process
* it is not working pefectly in its current state due to some problems with e.g. the contact sensors.
* it is modified to work as good as possible under the given circumstances.
*/

void main1() {
  
  switch (phase) {
    
    case 0:
      if (bottleLoaded()) {
        phase = 4;
      }
      else if (thermCam.searchColdSpot()) {
        phase = 2;
      }
      else if (bottleClose()) {
        phase = 3;
      }
      else {
        phase = 1;
      }
      
      break;
    
    case 1:
      easySearch(100, 5);
      if (bottleFound) {
        phase = 2;
      }
      
      break;
      
    case 2:
      myGear.turnToServoPos(thermCam.getPos(), 12, 10, 10);
      delay(500);
      myGear.drive(5, 12, true);
      delay(500);
      
      if (bottleClose()) {
        phase = 3;
      }
      if (bottleLoaded()) {
        phase = 4;
      }
      else {
        bottleFound = thermCam.searchColdSpot();
        if (thermCam.getPos() != 90) {
          lastFoundPos = thermCam.getPos();
        }
        Serial.println(lastFoundPos);

        if (!bottleFound) {
          phase = 3;
        }
      }
      
      break;
    
    case 3:
     // adjustPosition();
     myGear.turnToServoPos(lastFoundPos, 12, 0, 5);
     myGear.drive(3, 12, true);
     delay(1000);
      
      if (bottleLoaded()) {
        phase = 4;
      }
      
      break;
   
   case 4:
     lowBarrier.closeBarrier();
     for (int i = 0; i < 4; i++) {
       highBarrier.openBarrier();
       delay(500);
       highBarrier.closeBarrier();
       delay(500);
     }
     
     phase = 5;
     
     break;
   
   case 5:
     if (capSensor.isConducting()) {
       pusher.push(300);
       delay(2000); 
     }
     
     break;
  }
}

/**
* helper function to check if the bottle is loaded by the robot.
*/

boolean bottleLoaded() {
  return (capSensor.leftButtonPushed() || capSensor.rightButtonPushed());
}

/**
* helper function to check if the robot touched the bottle.
*/

boolean bottleClose() {
  return (frontSensor.leftButtonPushed() || frontSensor.rightButtonPushed());
}

/**
* an easy algorithm that makes the robot drive forward and scan for cold bottles in steps.
*/

boolean easySearch(float totalDistance, float partDistance) {
  for (float driven = 0; driven < totalDistance; driven += partDistance) {
    myGear.drive(partDistance, 12, true);    
    
    if (thermCam.searchColdSpot()) {
      bottleFound = true;
      break;
    }
    
  }
}

/**
* a more complex function to scan the room for cold bottles.
* has not been tested.
*/

boolean complexSearch(float totalDistance, float partDistance) {
  for (float driven = 0; driven < totalDistance; driven += partDistance) {
    myGear.drive(partDistance, 12, true);
    if (thermCam.searchColdSpot()) {
      return true;
    }
  }
  
  myGear.turnAngle(90, false, 12);
  myGear.drive(partDistance, 12, true);
  myGear.turnAngle(90, false, 12);
  
  for (float driven = 0; driven < totalDistance; driven += partDistance) {
    myGear.drive(partDistance, 12, true);
    if (thermCam.searchColdSpot()) {
      return true;
    }
  }
  
  myGear.turnAngle(90, true, 12);
  myGear.drive(partDistance, 12, true);
  myGear.turnAngle(90, true, 12);
 
  return false; 
}

/**
* a function to adjust the postion of the robot as it touches the bottle.
*/

void adjustPosition() {
  if (frontSensor.leftButtonPushed()) {
    myGear.turnAngle(2, true, 12);
  }
  else if (frontSensor.rightButtonPushed()) {
    myGear.turnAngle(2, false, 12);
  }
  myGear.drive(1, 12, true);
}