#ifndef BeerBotLib_h
#define BeerBotLib_h
 
#include "Arduino.h"
#include "Servo.h"
#include "NewPing.h"
 
/**
* class to add some additional features for the servo.
*/
 
class NewServo {
	public:
		void setup(int servoPin, int pos, boolean newServo);
		int getPos();
		void setPos(int pos);
 		void sweep(int servoSpeed, int servoRange, int servoDelay);
	private:
		Servo myServo;
		int position;
		int direction;
};
 
/**
* class for working with an ultrasonic distance sensor.
* is not used for the roboter in its current state. 
*/
 
class DistanceSensor {
	public:
		void setup(int inputPin, int outputPin, int servoPin);
		int getPos();
		int getDistance();
 
	private:
		NewPing sensor;
		NewServo myServo;	 
};
 
/**
* class to control the stepper motors.
*/
 
class StepperMotor {
	public:
		void setup(int stepPin, int directionPin);
		void makeStep(boolean direction);
	private:
		int stepPin;
		int directionPin;
};
 
/**
* class to control the whole gear of the servo.
*/
 
class Gear {
	public:
		void setup(float wheelDiameter, float axisDistance, int leftMotorStepPin, int leftMotorDirPin, int rightMotorStepPin, int rightMotorDirPin);
		void turnToServoPos(int servPos, int stepSpeed, int tolerance, int partAngle);
		void turnAngle (int angle, boolean turnDirection, int stepSpeed); 
		void drive(float distance, int stepSpeed, boolean driveDirection);
	private:
		float wheelDiameter;
		float axisDistance;
		StepperMotor rightMotor;
		StepperMotor leftMotor;
};
 
/**
* class to trigger the pusher(s).
*/
 
class Pusher {
	public:
		void setup(int pusherPin);
		void push(int duration);
	private:
		int pusherPin;
};
 
/**
* class to control the barriers.
*/
 
class Barrier {
	public:
		void setup(int servoPin, int closedPos, int openPos);
		void openBarrier();
		void closeBarrier();
	private:
		NewServo myServo;
		boolean closed;
		int closedPos;
		int openPos;	
};
 
/**
* class to control the push button contact sensors.
*/
 
class ContactSensor {
	public:
		void setup(int rightButtonPin, int leftButtonPin, int conductorPin);
		boolean rightButtonPushed();
		boolean leftButtonPushed();
		boolean isConducting();
	private:
		int rightButtonPin;
		int leftButtonPin;
		int conductorPin;
};
 
 
#endif

#include "BeerBotLib.h"
 
////////////////////////////////////////
// NewServo class
////////////////////////////////////////
 
/**
* setup method for servo class.
* the setup mehod has to be called, before the implicit constructed objects can be used.
* 
* @param servoPin: pin the servo is attached to,
*		 pos: initial position of the servo,
*		 newServo: specifies if the servo is one of the new models.
*/
 
void NewServo::setup(int servoPin, int pos, boolean newServo) {
	if (newServo) {
		myServo.attach(servoPin, 800, 2100);
	}
	else {
		myServo.attach(servoPin);
	}
	setPos(pos);
	direction = 0;
}
 
/**
* getter method for servo position.
* 
* @return the last position to which the servo was set. 
*/
 
int NewServo::getPos() {
	return position;
}
 
/**
* setter method for the servo method.
* servo position is set to new position and servo is moved to new position.
* 
* @param pos: new servo position. 
*/
 
void NewServo::setPos(int pos) {
	myServo.write(pos);
	this-> position = pos;
}
 
// void NewServo::moveLeft(int servoSpeed, int servoDelay) {
// 	if (position > 0) {
// 		setPos(position - servoSpeed);
// 		delay(servoDelay);
// 	}
// }
//
// void NewServo::moveRight(int servoSpeed, int servoDelay) {
// 	if (position < 180) {
// 		setPos(position + servoSpeed);
// 		delay(servoDelay);
// 	}
// }
 
/**
* method to let the servo sweep.
* sweeps only one step on each method call
* 
* @param servoSpeed: specifies the lenght of each step,
*	  	 servoRange: specifies the range in wich the servo should sweep
*		 (e.g. servoRange = 50 --> servo sweeps from 65 to 115),
*		 servoDelay: delay after each step.
*/
 
void NewServo::sweep(int servoSpeed, int servoRange, int servoDelay) {
	if (direction == 0) {
		direction = 1;
	}
	if (direction == 1) {
		moveRight(servoSpeed, servoDelay);
		if (position >= 90 + servoRange/2) {
			direction = -1;
		}
	}
	if (direction == -1) {
		moveLeft(servoSpeed, servoDelay);
		if (position <= 90 - servoRange/2) {
			direction = 1;
		}
	}
}
 
////////////////////////////////////////
// DistanceSensor class
////////////////////////////////////////
 
/**
* setup method for the distance sensor.
* initializes the distance sensor using third-party library NewPing.
* has to be called before the object can be used.
*
* @param inputPin: the echo pin of the sensor,
	 	 outputPin: the trigger pin of the sensor,
		 servoPin: the pin of the servo on which the sensor is mounted (if that is the case).
*/
 
void DistanceSensor::setup(int inputPin, int outputPin, int servoPin) {
	sensor.setup(outputPin, inputPin);
	myServo.setup(servoPin, 90, false);
}
 
/**
* getter method for the servo on which the sensor is mounted.
*
* @return the getPos method of the servo.
*/
 
int DistanceSensor::getPos() {
	return myServo.getPos();
}
 
/**
* method to measure the distance with the sensor.
* uses the third-party library NewPing.
* 
* @return the currently measured distance in cm.
*/
 
int DistanceSensor::getDistance() {
	return sensor.ping_cm();
}
 
////////////////////////////////////////
// Pusher class
////////////////////////////////////////
 
/**
* setup method for the pusher.
* sets the pin to which the relay controller of the pusher is connected
* and sets the pin to HIGH as default because that is needed for the relay.
*
* @param pusherPin: the pin to which the relay controller is connected.
*/
 
void Pusher::setup(int pusherPin) {
	this-> pusherPin = pusherPin;
	pinMode(pusherPin, OUTPUT);
	digitalWrite(pusherPin, HIGH);
}
 
/**
* method to trigger the pusher.
* pusher should not be triggerd for longer than 500ms continously.
*
* @param duration: the duration for which the pusher should be triggerd in ms.
*/
 
void Pusher::push(int duration) {
	digitalWrite(pusherPin, LOW);
	delay(duration);
	digitalWrite(pusherPin, HIGH);
}
 
////////////////////////////////////////
// Barrier class
////////////////////////////////////////
 
/**
* setup method for the barrier class. 
* the setup mehod has to be called, before the implicit constructed objects can be used.
*
* @param servoPin: the pin to which the servo of the barrier is connected,
*		 closedPos: the servo position in which the barrier is closed,
*		 openPos: the servo position in which the barrier is opened.
*/
 
void Barrier::setup(int servoPin, int closedPos, int openPos) {
	this-> closedPos = closedPos;
	this-> openPos = openPos;
	myServo.setup(servoPin, openPos, false);
	closed = false;
}
 
/**
* method that open the barrier.
* sets the servo to the specified open position.
*/
 
void Barrier::openBarrier() {
	if (closed) {
		myServo.setPos(openPos);
		closed = false;
		delay(1000);
	}
}
 
/**
* method that closes the barrier.
* sets the servo to the specified closed position.
*/
 
void Barrier::closeBarrier() {
	if (!closed) {
		myServo.setPos(closedPos);
		closed = true;
		delay(1000);
	}
}
 
 
////////////////////////////////////////
// ContactSensor class
////////////////////////////////////////
 
/**
* setup method for the contact sensor class.
* the setup mehod has to be called, before the implicit constructed objects can be used.
*
* @param leftButtonPin: pin to which the left button is connected,
*		 rightButtonPin: pin to which the right button is connected,
*		 conductorPin: pin to which the conduction signal is connected.
*/
 
void ContactSensor::setup(int leftButtonPin, int rightButtonPin, int conductorPin) {
	this-> leftButtonPin = leftButtonPin;
	this-> rightButtonPin = rightButtonPin;
	this-> conductorPin = conductorPin;
 
	pinMode(leftButtonPin, INPUT);
	pinMode(rightButtonPin, INPUT);
	pinMode(conductorPin, INPUT);
}
 
/**
* method to check if the left button is pushed.
*
* @return true if button is pushed, false if not.
*/
 
boolean ContactSensor::leftButtonPushed() {
	return (digitalRead(leftButtonPin) == HIGH);
}
 
/**
* method to check if the right button is pushed.
*
* @return true if button is pushed, false if not.
*/
 
boolean ContactSensor::rightButtonPushed() {
	return (digitalRead(leftButtonPin) == HIGH);
}
 
/**
* method to check if an electric current is flowing.
*
* @return true if button is pushed, false if not.
*/
 
boolean ContactSensor::isConducting() {
	if (conductorPin > 0) {
		return (digitalRead(leftButtonPin) == HIGH);
	}
	return false;
}
 
////////////////////////////////////////
// StepperMotor class
////////////////////////////////////////
 
/**
* setup method for the stepper motor class.
* the setup mehod has to be called, before the implicit constructed objects can be used.
*
* @param stepPin: the pin which is connected to the stepper pin of the motor driver,
		 directionPin: the pin which is connected to the direction pin of the motor driver.
*/
 
void StepperMotor::setup(int stepPin, int directionPin) {
	this-> stepPin = stepPin;
	this-> directionPin = directionPin;
 
	pinMode(stepPin, OUTPUT);
	pinMode(directionPin, OUTPUT);
 
	Serial.println("motor");
}
 
/**
* method to make one step.
* one step is an 1.8 degree turn of the axis.
*
* @param stepDirection: defines the direction in which the step is made.
*/
 
void StepperMotor::makeStep(boolean stepDirection){    
  if (stepDirection){
    digitalWrite(directionPin, HIGH);
  }
  else {
    digitalWrite(directionPin,LOW);
  }
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(1);
  digitalWrite(stepPin, LOW);  
}
 
////////////////////////////////////////
// Gear class
////////////////////////////////////////
 
/**
* setup method for the gear class.
* the setup mehod has to be called, before the implicit constructed objects can be used.
*
* @param wheelDiameter: diameter of the wheel,
* 		 axisDistance: length of the axis (distance between the weels),
*		 leftMotorStepPin: stepper pin of the left stepper motor,
*		 leftMotorDirPin: direction pin of the left stepper motor,
		 rightMotorStepPin: stepper pin of the right stepper motor,
		 rightMotorDirPin: stepper pin of the right stepper motor.
*/
 
void Gear::setup(float wheelDiameter, float axisDistance, int leftMotorStepPin, int leftMotorDirPin, int rightMotorStepPin, int rightMotorDirPin) {
	this-> wheelDiameter = wheelDiameter;
	this-> axisDistance = axisDistance;	
	leftMotor.setup(leftMotorStepPin, leftMotorDirPin);
	rightMotor.setup(rightMotorStepPin, rightMotorDirPin);
}
 
/**
* method to drive in a straight line.
* 
* @param distance: the distance to drive in cm,
*		 stepSpeed: delay between to steps in ms,
*		 driveDirection: true to drive forward, false to drive backwards.
*/
 
void Gear::drive(float distance, int stepSpeed, boolean driveDirection) {
	//stepsValue ist die Anzahl der Schritte die jeder Motor machen muss
	float stepsValue = round(distance/((wheelDiameter*PI)/200));
	//Beide Motoren fahren in die gleiche Richtung. Da der eine aber umgedreht ist, bekommen sie unterschiedliche direction Anweisungen.
	for (int countSteps = 0; countSteps < stepsValue; countSteps++) {
		rightMotor.makeStep(driveDirection);                                                      
		leftMotor.makeStep(!driveDirection);
		delay(stepSpeed);                                                    
	}	
}
 
/**
* method to turn the robot to a servo position.
* if the servo is not in the tolarance range (90 - tolerance/2 < servoPos > 90 + tolerance/2),
* the robot is turned in the direction which the servo, respectivly the mounted sensor, is facing.
*
* @param servoPos: the position of the servo,
*		 stepSpeed: the delay between the stepper motor steps,
*		 tolerance: the tolerance range (90 - tolerance/2 < servoPos > 90 + tolerance/2);
*		 partAngle: the angle for the turning of the robot.
*/
 
void Gear::turnToServoPos(int servoPos, int stepSpeed, int tolerance, int partAngle) {
	boolean turnDirection = true;
 
	if (servoPos > 90 + tolerance/2 || servoPos < 90 - tolerance/2) {
		if (servoPos > 90) {
			turnAngle(partAngle, !turnDirection, stepSpeed);
		}
		else {
			turnAngle(partAngle, turnDirection, stepSpeed);
		}	
	}
}
 
/**
* method to turn the robot by an specific angle.
*
* @param angle: amount of degrees (0 - 360) by which the robot should turn,
*		 turnDirection: true --> anti-clockwise turning, false --> clockwise turning,
*		 stepSpeed: the delay between two steps oft the stepper motors.
*/
 
void Gear::turnAngle (int angle, boolean turnDirection, int stepSpeed) {
	float stepsValue = round(angle/(360.0/((axisDistance*PI)/((wheelDiameter*PI)/200.0))));
 
	for (int countSteps = 0; countSteps < stepsValue; countSteps++) {
		if (turnDirection) {
			rightMotor.makeStep(false);
			leftMotor.makeStep(false);
			delay(stepSpeed);
		}
		else {
			rightMotor.makeStep(true);
			leftMotor.makeStep(true);
			delay(stepSpeed);
		}
	}
}

#ifndef ThermalCam_h
#define ThermalCam_h
 
 
#include "Arduino.h"
#include "i2c_t3.h"
#include "teensy3_Melexis90620.h"
#include "BeerBotLib.h"
 
class ThermalCam {
	private:
		Melexis90620 sensor;
		NewServo myServo;
 
		int sweepSpeed;
		int servoSpeed;
		float tolerance;
 
		double* pixelTemps;
		double pixelCompressedTo16[16];
		double midPixels;
		double leftSide;
		double rightSide;
		double pixelSum;
		double coldestPixel;
		double temperatureBorder;
		double pixelCompressedTo8[8];
		int dire;
		int servoRange;
		float searchTolerance;
 
		boolean foundColdPixel();
		void getThermalData();
		void summarizeVerticalPixels();
		void summarizeMidPixels();
		boolean checkMidPixelsForCold();
		void summarizeSides();
		void moveToColderSide();
		void moveToCold();
		void searchColdestPixel();
		void compressPixelTo8();
	public:
		void setup(int servoPin, int temperatureBorder);
		boolean searchColdSpot();
		void followColdSpot();
		int getPos();
};
 
#endif