/**
 * The main Arduino project file
 */

//#define DEBUG
#define LCD

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

#include "Controller.h"
#include "Monitor.h"
#include "Plant.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 Model_Init(void);
}

extern unsigned long plant_step_time;
extern unsigned long controller_step_time;

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

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

  Mcu_Init();
  Port_Init();
  Model_Init();

  sei();  // enable interrupts
}

void loop()
{
  int val;
  val = analogRead(TREF);
  rtPpControllerInput1_Tref = (real_T)map(val, 0, 1023, 10, 40);     // scale between 10 and 40 degrees

#ifdef LCD
  lcd.setCursor(0, 0);
  dtostrf(rtPpControllerInput1_Tref, 2, 0, str);
  sprintf(line, "Tref: %s    %cC", str, 176);
  lcd.print(line);

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

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

  lcd.setCursor(0, 3);
  dtostrf(Monito_PpMonitorOutput_HeatCost, 6, 1, str);
  sprintf(line, "Energy: %s  J", 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(rtPpControllerInput1_Tref, 7);
      Serial.print("\t");
      Serial.print(rtPpControllerOutput_Terr, 7);
      Serial.print("\t");
      Serial.print(rtPpControllerOutput_Power, 7);
      Serial.print("\t");
      Serial.println(Plant_PpPlantOutput_Tcurrent, 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 */
}