#include <ThermalCam.h>

void ThermalCam::setup(int servoPin, int temperatureBorder){
	myServo.setup(servoPin, 90, true);
	sensor.init(16);
  
	this-> sweepSpeed = 2;
	this-> temperatureBorder = temperatureBorder;
	this-> servoRange = 90;
}

/*
    Function for getting the thermaldata.
*/

void ThermalCam::getThermalData() {
    sensor.readTemperatures();
    pixelTemps = sensor.getPixelTemperatures();
}

/*
    Function sets thermalcam-servo to 90 degrees.
    Servo does a complete sweep and every degree the thermaldata gets checked for a cold source.
    If there is a cold source, "true" is returned back, else "false".
*/

boolean ThermalCam::searchColdSpot() {
    myServo.setPos(90);
    delay(100);
    for (int i = 0; i < servoRange; i++) {
        myServo.sweep((int)sweepSpeed, servoRange, 15);
        getThermalData();
	if (foundColdPixel()) {
	    return true;
	}
    }
    return false;
}

/*
    Function, which gives back the current position (angle) of the thermalcam-servo.
*/

int ThermalCam::getPos() {
    return myServo.getPos();
}

/*
    Function, which checks the two rows in the middle of the thermalcam-pixelarray for a cold source.
*/

boolean ThermalCam::foundColdPixel() {
    for( int i = 28; i < 35; i++ ) {
    if( pixelTemps[i] < temperatureBorder ) return true;
    }
    return false;
}