#include <max6675.h>

#define DO 4
#define CS 5
#define CLK 6
#define PWM 11

MAX6675 thermo(CLK,CS,DO);

float kp=0.5;
float kd=70;
float ki=0.001;

int milliOld;
int milliNew;
int dt;

float desTemp = 0; //setpoint temperature
float mesTemp = 0; //actual temperature
float tempError = 0;
float tempErrorOld;
float tempErrorChange;
float tempErrorSlope = 0;
float tempErrorArea = 0;

float PID_val = 0;

void setup() {
  TCCR2B = TCCR2B & B11111000 | 0x03; //sets pin 3 and 11 to PWM freq of 928.5Hz
  pinMode(PWM,OUTPUT);
  milliNew = millis();
  Serial.begin(9600);
  delay(100);
  Serial.println("LABEL, Time, Temperature"); //PLX-DAQ stuff
  mesTemp = thermo.readCelsius();

}

void loop() {
  desTemp = desired_temperature(milliNew / 3.6e6); //updating the setpoint based on time passed
  mesTemp = thermo.readCelsius(); //reading the temperature

  milliOld = milliNew;
  milliNew = millis();
  dt = milliNew-milliOld;

  tempErrorOld = tempError;
  tempError = desTemp-mesTemp;
  tempErrorChange = tempError-tempErrorOld;
  tempErrorSlope = tempErrorChange/dt;
  tempErrorArea = tempErrorArea + tempError*dt;

  PID_val = kp*tempError + kd*tempErrorSlope + ki*tempErrorArea;

  if(PID_val < 0){
    PID_val = 0;
  }
  if(PID_val > 255){
    PID_val = 255;
  }
  analogWrite(PWM,255-PID_val);
  Serial.print("DATA,TIME,"); //more PLX-DAQ stuff
  Serial.println(mesTemp);
  delay(1000);


}

float desired_temperature (float time_hours)
{
  if (time_hours < 1.0)
    return 20 + 80 * time_hours ;  // ramp from 20 to 100 over 1 hour
  if (time_hours < 3.0)
    return 100;   // hold at 100 between hours 1 and 3
  if (time_hours < 4.0)
    return 20 + 80 * (4.0 - time_hours) ; // then ramp down from 100 to 20 over 1 hour
  return 0;   // otherwise inactive
}