// C++ code
//
#include <math.h>
const int TEMP_PIN = 0;
const int RED_PIN = 3;
const int GREEN_PIN = 5;

const float THERMISTOR_T0 = 25.0;
const float THERMISTOR_R0 = 10000.0;
const float THERMISTOR_B = 3960.0;

const unsigned long PERIOD_TEMP = 1000;
const unsigned long PERIOD_DRYER = 5000;
const unsigned long PERIOD_CONTROL_HEAT = 1000;


const float TEMP_MIN = 36.0;
const float TEMP_MAX = 40.0;
const float TEMP_CORE = 38.0;

const float KP = 0.4044;
const float KI = 0.0100;
const float KD = 3.3718;





inline float map_temp(int val, float T0, float R0, float B)
{

  float temp_c = 1 / (log(1 / (1023. / val - 1)) / B + 1.0 / 298.15) - 273.15;
  return temp_c;

}



struct Temp
{
  unsigned long change;
  int pin;
  float value;
};

void setup_temp(struct Temp & temp, int pin)
{
  temp.value = 0;
  temp.pin = pin;
  temp.change = - PERIOD_TEMP;
}

void loop_temp(struct Temp & temp, unsigned long time)
{
  if (time - temp.change >= PERIOD_TEMP)
  {
    int val = analogRead(temp.pin);
    float tep_C = map_temp(val, THERMISTOR_T0,THERMISTOR_R0,THERMISTOR_B);
    temp.change += PERIOD_TEMP;
    temp.value = tep_C;
  
  }
  
}

struct Green_LED
{
  bool state;
  unsigned long change;
  int pin;
};

void setup_led_green(struct Green_LED & led, int pin)
{
  led.state = LOW;
  led.pin = pin;
  pinMode(led.pin, OUTPUT);
  led.change = - PERIOD_TEMP;
}

void loop_led_green(struct Green_LED & led, unsigned long time, struct Temp & temp)
{
  if (time - led.change >= PERIOD_TEMP)
  {
    led.change += PERIOD_TEMP;
    led.state = (temp.value >= TEMP_MIN && temp.value <= TEMP_MAX) ? HIGH : LOW;
    digitalWrite(led.pin, led.state);
  }
}

struct Dryer_LED
{
  bool state;
  unsigned long change;
  int pin;
  float on_time;
  float error_prev;
  float integral;

};

void setup_dryer_led(struct Dryer_LED & led, int pin)
{
  led.state = LOW;
  led.pin = pin;
  pinMode(led.pin, OUTPUT);
  led.change = - PERIOD_DRYER;
  led.on_time = 0;
  led.error_prev = 0;
  led.integral = 0;
  
}

void loop_dryer_led(struct Dryer_LED & led, unsigned long time, struct Temp & temp)
{
  if (time - led.change >= PERIOD_DRYER)
  {
    led.change += PERIOD_DRYER;
    led.state = (temp.value < TEMP_CORE) ? HIGH : LOW;
    digitalWrite(led.pin, led.state);

    float error = TEMP_CORE - temp.value;
    led.integral += (error * PERIOD_CONTROL_HEAT / 1000);
    float derivative = (led.error_prev - error)/ (PERIOD_CONTROL_HEAT / 1000);
    float PID = KP * error + KI * led.integral + KD * derivative;
    led.error_prev = error;
    Serial.println(PID);
    if (0 < PID)
    {
      led.on_time = PID * PERIOD_CONTROL_HEAT;
    }
    

  }
  if (time - led.change >= led.on_time)
    {
      digitalWrite(led.pin, LOW);
      led.state = LOW;
      //led.on_time = 5000;

    }

}

struct Monitor 
{
  unsigned long change;
};

void setup_monitor(struct Monitor & mon)
{
  Serial.begin(9600);
  if (Serial)
  {
    Serial.println ("#> Arduino Sensor de Temperatura ");
	Serial.println ("#$ -y20 :45 -l36 -l40 - tTemperatura -tLED ");
  }
  mon.change = - PERIOD_TEMP;
}

void loop_monitor(struct Monitor & mon, unsigned long time, struct Temp & temp, struct Dryer_LED & led)
{
  if (Serial && time - mon.change >= PERIOD_TEMP)
  {
    mon.change += PERIOD_TEMP;
    Serial.print(temp.value);
    Serial.print("  ");
    Serial.println(led.state ? 1 : 0);

      
  }
}





struct Temp temp;
struct Green_LED led_green;
struct Dryer_LED dryer_led;
struct Monitor mon;


void setup()
{
  setup_temp(temp,TEMP_PIN);
  setup_led_green(led_green,GREEN_PIN);
  setup_dryer_led(dryer_led,RED_PIN);
  setup_monitor(mon);

}

void loop()
{
  unsigned long current_time = millis();
  loop_temp(temp, current_time);
  loop_led_green(led_green, current_time, temp);
  loop_dryer_led(dryer_led, current_time, temp);
  loop_monitor(mon, current_time, temp, dryer_led);

}