#include "thermometer.h"
#include "fatal.h"
#include "AD7190.h"
#include "Streaming.h"
#include "Config.h"



const bool COMPILE_TIME_DEBUG = false;

const float INVALID_TEMPERATURE = -300.0;




String ThermConfig::exec(String params) {
  String res = "done";

  int space = params.indexOf(' ');
  String p1 = "";
  char pb1[128];
  if (space != -1) {
    params.toCharArray(pb1, 128, space+1);
  }

  if (params.startsWith("pm ") && (space != -1)) {
    unsigned long v = atol(pb1);
    m_thermometer->setPeriodMeasure(v);
  } else if (params.startsWith("a ") && (space != -1)) {
    float v = atof(pb1);
    m_thermometer->setAlpha(v);
  } else if (params.startsWith("debug ") && (space != -1)) {
    bool b = (strcmp(pb1, "on") == 0);
    m_thermometer->setDebug(b);
  } else if (params.startsWith("info ") && (space != -1)) {
    bool b = (strcmp(pb1, "on") == 0);
    m_thermometer->setInfo(b);
  } else if (params.equalsIgnoreCase("show")) {
      m_stream->print("PeriodMeasure (ms): "); m_stream->print(m_thermometer->getPeriodMeasure()); m_stream->println();
      m_stream->print("Alpha:              "); m_stream->print(m_thermometer->getAlpha()); m_stream->println();
      for (unsigned int i = 0; i < NUM_OF_CHANNELS; i++) {
        m_stream->print("Calibration:        "); m_stream->print(i); m_stream->print(", ");
        m_stream->print(m_thermometer->getCalibrateFactor(i), 6); m_stream->println();
      }
      m_stream->print("Info:               "); m_stream->print(m_thermometer->getInfo()); m_stream->println();
      m_stream->print("Debug:              "); m_stream->print(m_thermometer->getDebug()); m_stream->println();
      m_stream->print("COMPILE_TIME_DEBUG: "); m_stream->print(COMPILE_TIME_DEBUG); m_stream->println();

  } else {
    res = "subcommand not found";
  }
  return res;
}

String ThermValues::exec(String params) {
  m_stream->print("TimeOuts:    "); m_stream->print(m_thermometer->m_timeOutFailureCnt); m_stream->println();
  m_stream->print("Cycles:      "); m_stream->print(m_thermometer->m_cylceCnt); m_stream->println();
  m_stream->print("Period (ms): "); m_stream->print(m_thermometer->getPeriodMeasure()); m_stream->println();
  m_stream->print("Alpha:       "); m_stream->print(m_thermometer->getAlpha()); m_stream->println();

  for (unsigned int i = 0; i < NUM_OF_CHANNELS; i++) {
    m_stream->print("index: "); m_stream->print(i); m_stream->print(", ");
    m_stream->print("cal: "); m_stream->print(m_thermometer->getCalibrateFactor(i), 6); m_stream->print(", ");
    m_stream->print("t: "); m_stream->print(m_thermometer->m_temperature[i]); m_stream->print(", ");
    m_stream->print("ts: "); m_stream->print(m_thermometer->m_smoothedTemperature[i]);
    m_stream->println();
  }

  return "done";
}

ThermCalibrate::ThermCalibrate(Thermometer *thermometer) : m_thermometer(thermometer), m_enabled(false),
    m_start_calibration(false), m_r_cal(1500.0) {

}

String ThermCalibrate::exec(String params) {
  String res = "done";

  int space = params.indexOf(' ');
  String p1 = "";
  char pb1[128];
  if (space != -1) {
    params.toCharArray(pb1, 128, space+1);
  }

  if (params.startsWith("enable")) {
    m_enabled = true;
  } else if (params.startsWith("disable")) {
    m_enabled = false;
  } else if (params.startsWith("help")) {
    *m_stream << "  enable :           enable the calibration mode" << endl;
    *m_stream << "  disable :          disable the calibration mode" << endl;
    *m_stream << "  show :             show parameters for calibration process" << endl;
    *m_stream << "  r :                set value of calibration resistor" << endl;
    *m_stream << "  start <0..3>:      start calibration on channel" << endl;
    *m_stream << "  stop <10..3>:       stop calibration on channel" << endl;
  } else if (params.startsWith("start ") && (space != -1) && m_enabled) {
    m_channel = atoi(pb1);
    if ((m_channel + 1) > NUM_OF_CHANNELS) {
      res = "Invalid channel number";
    } else {
      m_start_calibration = true;
    }
  } else if (params.startsWith("stop")) {
    m_start_calibration = false;
  } else if (params.startsWith("r ") && (space != -1)) {
    m_r_cal = atof(pb1);
  } else if (params.equalsIgnoreCase("show")) {
    *m_stream << "  enabled :          " << m_enabled << endl;
    *m_stream << "  r_cal :            " << m_r_cal << endl;
  } else {
    res = "subcommand not found";
  }
  return res;
}

Thermometer::Thermometer() : m_period(DEFAULT_PERIOD), m_periodMillis(DEFAULT_PERIOD),
                             m_debug(DEBUG), m_info(INFO),
                             thermConfig(this), thermValues(this), thermCalibrate(this),
                             m_timeOutFailureCnt(0), m_cylceCnt(0), m_alpha(1.0),
                             m_calibrationMode(false)
{
  for (unsigned int i = 0; i < NUM_OF_CHANNELS; i++) {
    m_lastSmoothedTemperature[i] = INVALID_TEMPERATURE;
  }
}



void Thermometer::setPeriodMeasure(unsigned long p) {
  Config::setULong(Config::THERMOMETER_PERIOD, p);
  m_periodMillis = p;
  m_period = Metro(p);
}

unsigned long Thermometer::getPeriodMeasure() {
  return m_periodMillis;
}

void Thermometer::setAlpha(float a) {
  Config::setFloat(Config::THERMOMETER_ALPHA, a);
  m_alpha = a;
}

float Thermometer::getAlpha() {
  return m_alpha;
}

void Thermometer::setDebug(bool b) {
  Config::setBool(Config::THERMOMETER_DEBUG, b);
  m_debug = b;
}

bool Thermometer::getDebug() {
  return m_debug;
}

void Thermometer::setInfo(bool b) {
  Config::setBool(Config::THERMOMETER_INFO, b);
  m_info = b;
}

bool Thermometer::getInfo() {
  return m_info;
}


void Thermometer::setCalibrateFactor(unsigned int i, float c) {
  Config::setFloat(Config::THERMOMETER_CAL[i], c);
  m_calibrateFactor[i] = c;
}

float Thermometer::getCalibrateFactor(unsigned int i) {
  return m_calibrateFactor[i];
}


float pt1000(float r) {
  return (r / PT1000_R0 - 1) / PT1000_Coeff;
}

void Thermometer::setTemperature(unsigned int index, float t) {
  m_temperature[index] = t;

  if (m_lastSmoothedTemperature[index] == INVALID_TEMPERATURE) {
    m_smoothedTemperature[index] = t;
  } else {
    m_smoothedTemperature[index] = getAlpha() * t + (1 - m_alpha) * m_lastSmoothedTemperature[index];
  }
  if (getDebug() || getInfo()) {
    Serial.print("setTemperature: i="); Serial.print(index); Serial.print(", t="); Serial.print(t);
    Serial.print(", t_smoothed="); Serial.print(m_smoothedTemperature[index]);
    Serial.println();
  }
  m_lastSmoothedTemperature[index] = m_smoothedTemperature[index];
}

void Thermometer::prepareAdc() {
    unsigned long oldRegValue = AD7190_GetRegisterValue(AD7190_REG_CONF, 3, 1);
    oldRegValue &= ~(AD7190_CONF_CHAN(0xFF));
    unsigned long newRegValue = oldRegValue |
        AD7190_CONF_CHAN(1 << AD7190_CH_AIN1P_AINCOM) |
        AD7190_CONF_CHAN(1 << AD7190_CH_AIN2P_AINCOM) |
        AD7190_CONF_CHAN(1 << AD7190_CH_AIN3P_AINCOM) |
        AD7190_CONF_CHAN(1 << AD7190_CH_AIN4P_AINCOM);
    if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print("CONF: "); Serial.print(newRegValue, 16); Serial.println(); }
    AD7190_SetRegisterValue(AD7190_REG_CONF, newRegValue, 3, 1);

    newRegValue = AD7190_MODE_SEL(AD7190_MODE_IDLE) |
        AD7190_MODE_CLKSRC(AD7190_CLK_INT) |
        AD7190_MODE_REJ60 |
        AD7190_MODE_DAT_STA |
        AD7190_MODE_RATE(0x060);
    if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print("MODE: "); Serial.print(newRegValue, 16); Serial.println(); }

    AD7190_SetRegisterValue(AD7190_REG_MODE, newRegValue, 3, 1);
}

void Thermometer::startSingleConv() {
  unsigned long oldRegValue = AD7190_GetRegisterValue(AD7190_REG_MODE, 3, 0);
  oldRegValue &= ~(AD7190_MODE_SEL(0x07));
  unsigned long newRegValue = oldRegValue | AD7190_MODE_SEL(AD7190_MODE_SINGLE);
  if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print("MODE: "); Serial.print(newRegValue, 16); Serial.println(); }
  AD7190_SetRegisterValue(AD7190_REG_MODE, newRegValue, 3, 0);
}

void Thermometer::begin(CmdServer *cmdServer) {
  thermConfig.registerYourself(cmdServer);
  thermValues.registerYourself(cmdServer);
  thermCalibrate.registerYourself(cmdServer);



  unsigned char ad7190Status = AD7190_Init();
  if (ad7190Status != 1)
    fatal(FATAL_ADC_NOTFOUND);

  AD7190_RangeSetup(1, AD7190_CONF_GAIN_1);

  AD7190_Calibrate(AD7190_MODE_CAL_INT_ZERO, AD7190_CH_AIN4P_AINCOM);
  AD7190_Calibrate(AD7190_MODE_CAL_INT_FULL, AD7190_CH_AIN4P_AINCOM);

  AD7190_Calibrate(AD7190_MODE_CAL_INT_ZERO, AD7190_CH_AIN3P_AINCOM);
  AD7190_Calibrate(AD7190_MODE_CAL_INT_FULL, AD7190_CH_AIN3P_AINCOM);

  AD7190_Calibrate(AD7190_MODE_CAL_INT_ZERO, AD7190_CH_AIN2P_AINCOM);
  AD7190_Calibrate(AD7190_MODE_CAL_INT_FULL, AD7190_CH_AIN2P_AINCOM);

  AD7190_Calibrate(AD7190_MODE_CAL_INT_ZERO, AD7190_CH_AIN1P_AINCOM);
  AD7190_Calibrate(AD7190_MODE_CAL_INT_FULL, AD7190_CH_AIN1P_AINCOM);


  if (! Config::isInitialized()) {
    Serial << "Initializing EEPROM" << endl;
    Config::setFloat(Config::THERMOMETER_ALPHA, 1.0);
    Config::setULong(Config::THERMOMETER_PERIOD, 1000);
    for (int i = 0; i < 4; i++) {
      Config::setFloat(Config::THERMOMETER_CAL[i], 1.0);
    }
    Config::setBool(Config::THERMOMETER_DEBUG, true);
    Config::setBool(Config::THERMOMETER_INFO, true);
    Config::setMagic();
  }

  setAlpha(Config::getFloat(Config::THERMOMETER_ALPHA));
  setPeriodMeasure(Config::getULong(Config::THERMOMETER_PERIOD));
  for (int i = 0; i < 4; i++) {
    setCalibrateFactor(i, Config::getFloat(Config::THERMOMETER_CAL[i]));
  }
  setDebug(Config::getBool(Config::THERMOMETER_DEBUG));
  setInfo(Config::getBool(Config::THERMOMETER_INFO));

  //setCalibrateFactor(0, 1.002999);
  //setCalibrateFactor(1, 1.001804);
  //setCalibrateFactor(2, 1.000794);
  //setCalibrateFactor(3, 1.001071);

  // prepare
  prepareAdc();
}


void Thermometer::exec() {
  static uint8_t state = 0;

  static unsigned long timeOutCnt;
  unsigned long command;
  float r;
  static unsigned int channelCnt;


  switch (state) {
  case 0:
    if (COMPILE_TIME_DEBUG && getDebug()) { Serial.println(); }
    if (COMPILE_TIME_DEBUG && getDebug()) { Serial.println("State 0"); }
    // start conversion
    SPI_Enable(AD7190_SLAVE_ID);
    startSingleConv();

    state = 1;
    if (COMPILE_TIME_DEBUG && getDebug()) { Serial.println("Switching to State 1"); }

    timeOutCnt = CONV_TIMEOUT;
    m_cylceCnt++;
    channelCnt = 0;
    break;

  case 1:
    if (timeOutCnt == 0) {
      state = 9;
    } else if (digitalRead(MISO) != HIGH) {
      unsigned long n = AD7190_GetRegisterValue(AD7190_REG_DATA, 4, 0);
      unsigned int channelIndex = (n & 0xff) - 4;
      unsigned long adcValue = (n & 0xffffff00) >> 8;

      m_n[channelIndex] = adcValue;

      if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print("m_n["); Serial.print(channelIndex); Serial.print("]="); Serial.print(m_n[channelIndex], 16); Serial.println(); }

      channelCnt++;
      if (channelCnt >= 4) {
        state = 10;
        if (COMPILE_TIME_DEBUG && getDebug()) { Serial.println("Switching to State 10"); }
      }

      timeOutCnt = CONV_TIMEOUT;
    }
    timeOutCnt--;
    break;

  case 9:
    if (COMPILE_TIME_DEBUG && getDebug()) { Serial.println("State 9"); }


    // end cycle
    SPI_Disable(AD7190_SLAVE_ID);

    // increase failure counter
    m_timeOutFailureCnt++;

    if (getDebug() || getInfo()) {
      Serial << "Timeout: " << m_timeOutFailureCnt << endl;
    }

    state = 0;
    break;

  case 10:
    if (COMPILE_TIME_DEBUG && getDebug()) { Serial.println("State 10"); }

    SPI_Disable(AD7190_SLAVE_ID);

    if (m_calibrationMode || thermCalibrate.m_start_calibration) {
      Serial << "No, no, we are in calibration mode, so directly switch to state 20" << endl;
      state = 20;
    } else {
      for (unsigned int i = 0; i < NUM_OF_CHANNELS; i++) {
        if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print("i="); Serial.print(i); Serial.print(", "); }
        if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print("m_n="); Serial.print(m_n[i]); Serial.print(", "); }
        if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print(""); Serial.print((m_n[i] - ((i == 3) ? 0 : m_n[i + 1]))); Serial.print(", "); }
        if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print("calibrateFactor="); Serial.print(getCalibrateFactor(i), 6); Serial.print(", "); }
        float r = (((float)(m_n[i] - ((i == 3) ? 0 : m_n[i + 1]))) / ((float)N_MAX)) * R_REF * getCalibrateFactor(i);
        if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print("r="); Serial.print(r); Serial.print(", "); }
        float t = pt1000(r);
        if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print("t="); Serial.print(t); Serial.print(", "); }
        if (COMPILE_TIME_DEBUG && getDebug()) { Serial.println(); }
        setTemperature(i, t);
      }

      if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print("timeOuts="); Serial.print(m_timeOutFailureCnt); Serial.print(", "); }
      if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print("cycles="); Serial.print(m_cylceCnt); Serial.print(", "); }
      if (COMPILE_TIME_DEBUG && getDebug()) { Serial.println(); }

      if (COMPILE_TIME_DEBUG && getDebug()) { Serial.println("Switching to state 11"); }
      state = 11;
    }
    break;

  case 11:
    if (m_period.check() == 1)
      state = 0;
    break;

  case 20:
    if (thermCalibrate.m_start_calibration) {
      if (m_calibrationMode) {
        // do the calibration stuff
        for (unsigned int i = 0; i < NUM_OF_CHANNELS; i++) {
          float r = (((float)(m_n[i] - ((i == 3) ? 0 : m_n[i + 1]))) / ((float)N_MAX)) * R_REF;
          Serial << "t=" << thermCalibrate.m_turn << ", c=" << i << ", r=" << _FLOAT(r, 6) << ", n=" << m_n[i] << endl;
          if (i == thermCalibrate.m_channel) {
            thermCalibrate.m_r_sum += r;
          }
        }
        (thermCalibrate.m_turn)++;
        float r_avg = thermCalibrate.m_r_sum / thermCalibrate.m_turn;
        Serial << "r_avg on channel " << thermCalibrate.m_channel << ": " << _FLOAT(r_avg, 6) << endl;
        float calFact = thermCalibrate.m_r_cal / r_avg;
        Serial << "calibration factor: " << _FLOAT(calFact, 6) << endl;
      } else {
        thermCalibrate.m_preserved_alpha = m_alpha;
        m_alpha = 1;
        thermCalibrate.m_preserved_period = m_periodMillis;
        m_periodMillis = 500;
        m_period = Metro(m_periodMillis);
        thermCalibrate.m_r_sum = 0.0;
        thermCalibrate.m_turn = 0;
        m_calibrationMode = true;
      }
    } else {
      // end calibrationMode
      m_calibrationMode = false;
      m_alpha = thermCalibrate.m_preserved_alpha;
      m_periodMillis = thermCalibrate.m_preserved_period;
      m_period = Metro(m_periodMillis);
      Serial << "Calibration stopped" << endl;
      float r_avg = thermCalibrate.m_r_sum / thermCalibrate.m_turn;
      float calFact = thermCalibrate.m_r_cal / r_avg;
      Serial << "Save calibration factor for channel " << thermCalibrate.m_channel <<": " << _FLOAT(calFact, 6) << endl;
      setCalibrateFactor(thermCalibrate.m_channel, calFact);
    }

    state = 11;
    break;


  default:
    fatal(FATAL_ILLEGAL_STATE);
    break;
  }
}