#include <DallasTemperature.h>
#include <OneWire.h>
#include <PID_v1.h>
#include <analogWrite.h>

/**
   Constants for driver mode. MODE_DIRECT means any received value for the
   TARGET characteristic is interpreted as power level for the Peltier element
   in the range of -128 to +127, with negative values being interpreted as
   cooling, positive values interpreted as heating and 0 being interpreted as
   turning the element off. MODE_PID on the other hand means that the value of
   the TARGET characteristic is interpreted as target temperature that the PID
   algorithm will try to approach.
 **/
#define MODE_DIRECT 0
#define MODE_PID 1

// Pin definitions for temperature probe and the board's builtin LED
#define TEMP_PROBE 19
#define LED_BUILTIN 22

// Pin definitions for the wire connections responsible to heating, cooling and
// enabling the driver
#define DRIVER_COOL 23
#define DRIVER_WARM 18
#define DRIVER_ENABLE 5

// Acceptable maximum and minimum temperature for the Peltier element
//#define MAX_TEMP 45
//#define MIN_TEMP 10

int MAX_TEMP = 40;
int MIN_TEMP = 15;

// PID constants
#define KP 10
#define KI 10
#define KD 0

// Initialise OneWire protocol with pin for temperature probe
OneWire oneWire(TEMP_PROBE);
DallasTemperature temperatureSensors(&oneWire);

// Variables for keeping track of last main loop update and last temperature
// update.
unsigned long lastUpdate = millis();
unsigned long tempLastUpdate = millis();

// Initialise variables holding temperature PID setpoint and current PID output
// level.
double temperature = 0, setpoint = 30, pidOutput = 0;
int target;
char t = '0';

// Initialise PID with given variables and constants
PID pid(&temperature, &pidOutput, &setpoint, KP, KI, KD, DIRECT);


/**
   Requests the current temperature of the Peltier element from the temperature
   probe and stores the retrieved value in the variable `temperature`. The
   value is refreshed at most every 100ms. Calling the function more often than
   that will simply drop any extraneous requests.

   @returns True if the value was refreshed, false otherwise
 **/
bool requestTemperature() {
  // Make sure the value is only read at most every 100ms
  if (millis() > tempLastUpdate + 100) {
    // Request temperature from temperature probe
    temperatureSensors.requestTemperatures();
    // Write temperature to variable `temperature`
    temperature = temperatureSensors.getTempCByIndex(0);

    // Update refresh timestamp
    tempLastUpdate = millis();
    return true;
  }

  // Return false if value was not refreshed
  return false;
}


/**
   Sets the Peltier element to warming mode by writing the given value to the
   DRIVER_WARM pin and pulling the DRIVER_ENABLE pin HIGH.

   @param value Power level at which the Peltier element should be heating
 **/
void warm(uint32_t value) {
  // Write value to pin for warming and set cooling pin to 0
  analogWrite(DRIVER_WARM, value);
  analogWrite(DRIVER_COOL, 0);

  // Enable driver
  digitalWrite(DRIVER_ENABLE, HIGH);
}

/**
   Sets the Peltier element to cooling mode by writing the given value to the
   DRIVER_COOL pin and pulling the DRIVER_ENABLE pin HIGH.

   @param value Power level at which the Peltier element should be cooling
 **/
void cool(uint32_t value) {
  // Write value to pin for cooling and set warming pin to 0
  analogWrite(DRIVER_WARM, 0);
  analogWrite(DRIVER_COOL, value);

  // Enable driver
  digitalWrite(DRIVER_ENABLE, HIGH);
}

/**
   Disables the heating element altogether by writing 0 to both, the heating
   and the warming pin, as well as pulling the DRIVER_ENABLE pin to LOW.
 **/
void shutoff() {
  // Write 0 to pin for cooling and pin for warming
  analogWrite(DRIVER_WARM, 0);
  analogWrite(DRIVER_COOL, 0);

  // Disable driver
  digitalWrite(DRIVER_ENABLE, LOW);
}

/**
   Setup function called by the Arduino runtime on boot. Initialises the serial
   link to 115200 baud for debugging, sets pin modes initialises temperature
   sensor, PID and BLE interface.
 **/
void setup() {
  // Begin serial connection
  Serial.begin(115200);

  // Initialise pins
  pinMode(DRIVER_ENABLE, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  // Turn off builtin LED
  digitalWrite(LED_BUILTIN, LOW);

  // Initialise temperature sensor and request current temperature
  temperatureSensors.begin();
  while (!requestTemperature()) {}

  // Initialise PID
  pid.SetOutputLimits(-255, 255);
  pid.SetMode(AUTOMATIC);
}

void loop() {
  // Update temperature value
  requestTemperature();

  // Ensure code is run at most every 100ms
  if (millis() > lastUpdate + 100) {

    //Send the current temperature of the Peltier to Processing
    String temp = String(temperature, 2);
    Serial.println(temp);

    // Try and retrieve data from writable characteristics
    int driverMode = MODE_DIRECT;
    //int8_t target = Serial.read();

    // Print debugging output to serial link
    //    Serial.print("TEMP: ");
    //    Serial.print(temperature);
    //    Serial.print(" MODE: ");
    //    Serial.print(driverMode);
    //    Serial.print(" VALUE: ");
    //    Serial.print(target);
    //    Serial.print(" OUT: ");

    // If we're in direct driver mode
    if (driverMode == MODE_DIRECT) {

      if (Serial.available() > 0) {
        t = Serial.read();

        if (t == '0') {
          target = 0;
        } else if (t == '1') {
          target = -80;
        } else if (t == '2') {
          target = 0;
        } else if (t == '3') {
          target = 80;
        }
      }

      // Map value of target value from [-128, 127] to [0, 255]
      uint32_t output = abs(map(target, -128, 127, -255, 255));

      // Check target value
      if (target == 0) {
        // Turn element off if target value is 0
        shutoff();
      } else if (target > 0) {
        // Turn heating on if value is bigger than 0
        warm(output);
        if (temperature > MAX_TEMP) {
          target = 20;
        }
      } else if (target < 0) {
        // Turn cooling on if value is smaller than 0
        cool(output);
        if (temperature < MIN_TEMP) {
          target = -40;
        }
      }

      // Print output value
      //      Serial.println(output);
    }


    else if (driverMode == MODE_PID) { //This part could be removed since the functionality is not used

      if (Serial.available() > 0) {
        t = Serial.read();

        if (t == 'a') {
          target = 30;
        } else if (t == 'b') {
          target = 20;
        } else if (t == 'c') {
          target = 28;
        } else if (t == 'd') {
          target = 35;
        }
      }
      //
      //      if (target == 0) {
      //        shutoff();
      //      }
      // If target temperature is outside of acceptable range
      if (target > MAX_TEMP || target < MIN_TEMP) {
        // Set setpoint to current temperature, i.e. maintain current status
        setpoint = temperature;
      }  else {
        // Otherwise set PID setpoint to target temperature
        setpoint = target;
      }

      // Compute new PID value
      pid.Compute();

      // Check computed PID output value
      if (pidOutput > 0) {
        // Warm if it is bigger than 0
        warm(pidOutput);
      } else if (pidOutput < 0) {
        // Cool if it is smaller than 0
        cool(abs(pidOutput));
      } else {
        // Turn element off if it is equal to 0
        shutoff();
      }

      // Print output value
      //      Serial.println(pidOutput);
    } else {
      // If driver mode has any other value, turn element off
      shutoff();
      //      Serial.println("OFF");
    }
    // Update refresh timestamp
    lastUpdate = millis();
  }
}