/**
 * The main Arduino project file
 */

//#define DEBUG
#define LCD

#include "rtwtypes.h"
#include <LiquidCrystal_I2C.h>

#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

char line[LCD_COLUMNS+1];
char str[10];

#define TREF  0

extern "C" {
  void Mcu_Init(void);
  void Port_Init(void);

  void Plant_initialize(void);
  void Controller_initialize(void);
  void Monitor_initialize(void);
}

extern real_T reference_temperature;
extern real_T error;
extern real_T regulator_power;
extern real_T current_temperature;
extern real_T total_energy;
extern real_T PID_P;
extern real_T PID_I;
extern unsigned long plant_step_time;
extern unsigned long controller_step_time;
extern boolean_T reset_integrator;

void setup()
{
  //cli();  // disable interrupts

  Serial.begin(9600);
#ifdef LCD
  lcd.init();
  lcd.backlight();
#endif /* LCD */

  Mcu_Init();
  Port_Init();

  Plant_initialize();
  Controller_initialize();
  Monitor_initialize();

  sei();  // enable interrupts
}

void loop()
{
  static real_T old_ref_temp = 0;
  int val;
  val = analogRead(TREF);
  reference_temperature = (real_T)map(val, 0, 1023, 10, 40);     // scale between 10 and 40 degrees
  if (reference_temperature != old_ref_temp)
  {
    reset_integrator = true;
    old_ref_temp = reference_temperature;
  } 
  else
  {
    reset_integrator = false;
  }
#ifdef LCD
  lcd.setCursor(0, 0);
  dtostrf(reference_temperature, 2, 0, str);
  sprintf(line, "Tref: %s", str);
  lcd.print(line);

  lcd.setCursor(0, 1);
  dtostrf(current_temperature, 5, 2, str);
  sprintf(line, "Tcur: %s", str);
  lcd.print(line);

  lcd.setCursor(0, 2);
  dtostrf(regulator_power, 7, 2, str);
  sprintf(line, "Power:  %s", str);
  lcd.print(line);

  lcd.setCursor(0, 3);
  dtostrf(total_energy, 6, 1, str);
  sprintf(line, "Energy: %s", str);
  lcd.print(line);

  lcd.setCursor(12, 0);
  dtostrf(PID_P, 6, 3, str);
  sprintf(line, "P:%s", str);
  lcd.print(line);

  lcd.setCursor(12, 1);
  dtostrf(PID_I, 6, 3, str);
  sprintf(line, "I:%s", str);
  lcd.print(line);

#endif /* LCD */

#ifdef DEBUG
  unsigned long time = millis();
  static long last_time = -10;    // last time the log was printed
  delay(5);
  if (time <= 400050)    // simulate for 400 s
  {
    // Set reference temperature
    if (time % 100 <= 10 && time - last_time > 10)  // 
    {
      Serial.print(time / 1000.);
      Serial.print("\t");
      Serial.print(reference_temperature, 7);
      Serial.print("\t");
      Serial.print(error, 7);
      Serial.print("\t");
      Serial.print(regulator_power, 7);
      Serial.print("\t");
      Serial.println(current_temperature, 7);
      last_time = time;
    }
  }
  else                // print statistic and stop
  {
    unsigned long total_time = micros();
    Serial.print("Plant simulation load: ");
    Serial.print(100. * (real_T)plant_step_time / total_time);
    Serial.print("%\tController simulation load: ");
    Serial.print(100. * (real_T)controller_step_time / total_time);
    Serial.println("%");
    delay(1000000);
  }
#endif /* DEBUG */

  delay(100);
}