//Define Variables we'll be connecting to
double Setpoint, Input, RealOutput;
double Output;

//Define entrada do Potenciometro no Pino 2
const int SETPOINT_PIN = 2;

//Declarações para valores PID
double kp=25;
double ki=0.1;
double kd=0.0;

double error=0;       //e(n)
double errorAnt=0;    //e(n-1)
double errorAntAnt=0; //e(n-2)

double cv    = 0;     //c(n)
double cvAnt = 0;     //c(n-1)
double Ts    = 0.1;

double valorPID = 0; 

double periodo = 100; //Tiempo para las lecturas del encoder
volatile unsigned long tiempoAnterior = 0;

void setup()
{
  //Incia Serial
  Serial.begin(115200);
  //Inicializa SetPoint em zero
  Setpoint = 0;
  valorPID = 0;
  Input = 0;
  //Define input com valores do simulador
  Input = simPlant(0); // simulate heating
  //Escreve na Serial
  Serial.println("Setpoint Input Output Watts");
}

void loop()
{
  // gather Input from INPUT_PIN or simulated block
  float heaterWatts = (int)valorPID * 40.0 / 255; // 20W heater
  float blockTemp = simPlant(heaterWatts); // simulate heating
  Input = blockTemp;   // read input from simulated heater block

  //Leitura do potenciometro e armazena na variavel SetPoint
  Setpoint = analogRead(SETPOINT_PIN) / 4; // Read setpoint from potentiometer

  if(millis() - tiempoAnterior >= periodo) 
  {
    tiempoAnterior = millis();
     valorPID = funcionPID();
  }

  //Escreve os valores na serial
  report();
}

void report(void)
{
  static uint32_t last = 0;
  const int interval = 10;
  if (millis() - last > interval) {
    last += interval;
    //    Serial.print(millis()/1000.0);
    Serial.print(Setpoint);
    Serial.print(' ');
    Serial.print(Input);
    Serial.print(' ');
    Serial.print(valorPID);
    Serial.print(' ');
    Serial.println(40.0 * (int)valorPID/255);
  }
}

double funcionPID()
{
{
  error    = Setpoint - Input;
  cv = cvAnt + (kp+(kd/Ts))*error + (-kp + ki*Ts - (2*kd/Ts))*errorAnt + (kd/Ts)*errorAntAnt; 
  cvAnt = cv;
  errorAntAnt = errorAnt; 
  errorAnt    = error;
  
  if(cv > 255)
  {
    cv = 255;    
  }
  if(cv < 0)
  {
    cv = 0;
  }
   return cv;
}
}

float simPlant(float Q) { // heat input in W (or J/s)
  // simulate a 1x1x2cm aluminum block with a heater and passive ambient cooling
 // float C = 237; // W/mK thermal conduction coefficient for Al
  float h = 5 ; // W/m2K thermal convection coefficient for Al passive
  float Cps = 0.89; // J/g°C
  float area = 1e-4; // m2 area for convection
  float mass = 10 ; // g
  float Tamb = 25; // °C
  static float T = Tamb; // °C
  static uint32_t last = 0;
  uint32_t interval = 100; // ms

  if (millis() - last >= interval) {
    last += interval;
    // 0-dimensional heat transfer
    T = T + Q * interval / 1000 / mass / Cps - (T - Tamb) * area * h;
  }
  return T;
}