/**
 * The main Arduino project file
 */

//#define DEBUG
#define LCD

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

#include "Controller.h"
#include "Monitor.h"
#include "Plant.h"

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

typedef struct
{
  int16_T i;  // integer part
  int16_T f;  // fractional part
} fixed_point;

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

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

#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;

int16_T F_DIGITS[5] = {0, 10, 100, 1000, 10000};

/* Convert fixed point number with 4 binary digits to integer and fraction */
fixed_point convert_fixed_point( int16_T number, uint8_T fd, uint8_T fb)
{
  assert(fd <= 4);
  fixed_point fp_number;
  fp_number.i = number >> fb;
  fp_number.f = (number & 0x000F) * F_DIGITS[fd] / (1 << fb);
  return fp_number;
}

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 = (uint16_T)map(val, 0, 1023, 10, 40);     // scale between 10 and 40 degrees
  rtPpControllerInput1_Tref = (uint16_T)map(val, 0, 1023, 10<<4, 40<<4);     // scale between 10 and 40 degrees

#ifdef LCD
  lcd.setCursor(0, 0);
  fp = convert_fixed_point(rtPpControllerInput1_Tref, 0, 4);
  sprintf(line, "Tref: %d    %cC", fp.i, 176);
  lcd.print(line);

  lcd.setCursor(0, 1);
  fp = convert_fixed_point(Plant_PpPlantOutput_Tcurrent, 2, 4);
  sprintf(line, "Tcur: %d.%.2d %cC", fp.i, fp.f, 176);
  lcd.print(line);

  lcd.setCursor(0, 2);
  fp = convert_fixed_point(rtPpControllerOutput_Power, 2, 4);
  sprintf(line, "Power:  %d.%.2d W", fp.i, fp.f);
  lcd.print(line);

  lcd.setCursor(0, 3);
  fp = convert_fixed_point(Monitor_PpControllerOutput_Power, 2, 4);
  sprintf(line, "Energy: %d.%.2d  J", Monitor_PpControllerOutput_Power);
  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");
      fp = convert_fixed_point(rtPpControllerInput1_Tref, 0, 4);
      sprintf(line, "%d", fp.i);
      Serial.print(line);
      Serial.print("\t");
      fp = convert_fixed_point(rtPpControllerOutput_Terr, 4, 4);
      sprintf(line, "%d.%.4d", fp.i, fp.f);
      Serial.print(line);
      Serial.print("\t");
      fp = convert_fixed_point(rtPpControllerOutput_Power, 4, 4);
      sprintf(line, "%d.%.4d", fp.i, fp.f);
      Serial.print(line);
      Serial.print("\t");
      fp = convert_fixed_point(Plant_PpPlantOutput_Tcurrent, 4, 4);
      sprintf(line, "%d.%.4d", fp.i, fp.f);
      Serial.println(line);
      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 */
}