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


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)) {
    m_thermometer->m_debug = (strcmp(pb1, "on") == 0);
  } else if (params.startsWith("info ") && (space != -1)) {
    m_thermometer->m_info = (strcmp(pb1, "on") == 0);
  } else if (params.equalsIgnoreCase("show")) {
      m_stream->print("PeriodMeasure (ms): "); m_stream->print(m_thermometer->m_periodMillis); m_stream->println();
      m_stream->print("Alpha:              "); m_stream->print(m_thermometer->m_alpha); 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->m_periodMillis); m_stream->println();
  m_stream->print("Alpha:       "); m_stream->print(m_thermometer->m_alpha); 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->m_calibrateFactor[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";
}



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

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

void Thermometer::setAlpha(float a) {
  m_alpha = a;
}


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] = m_alpha * t + (1 - m_alpha) * m_lastSmoothedTemperature[index];
  }
  if (m_debug || m_info) {
    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 (m_debug) { 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 (m_debug) { 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 (m_debug) { 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);



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

  m_calibrateFactor[0] = 1.002999;
  m_calibrateFactor[1] = 1.001804;
  m_calibrateFactor[2] = 1.000794;
  m_calibrateFactor[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 (m_debug) { Serial.println(); }
    if (m_debug) { Serial.println("State 0"); }
    // start conversion
    SPI_Enable(AD7190_SLAVE_ID);
    startSingleConv();

    state = 1;
    if (m_debug) { 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 (m_debug) { Serial.print("m_n["); Serial.print(channelIndex); Serial.print("]="); Serial.print(m_n[channelIndex], 16); Serial.println(); }

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

      timeOutCnt = CONV_TIMEOUT;
    }
    timeOutCnt--;
    break;

  case 9:
    if (m_debug) { Serial.println("State 9"); }

    // end cycle
    SPI_Disable(AD7190_SLAVE_ID);

    // increase failure counter
    m_timeOutFailureCnt++;

    state = 0;
    break;

  case 10:
    if (m_debug) { Serial.println("State 10"); }

    SPI_Disable(AD7190_SLAVE_ID);

    for (unsigned int i = 0; i < NUM_OF_CHANNELS; i++) {
      if (m_debug) { Serial.print("i="); Serial.print(i); Serial.print(", "); }
      if (m_debug) { Serial.print("m_n="); Serial.print(m_n[i]); Serial.print(", "); }
      if (m_debug) { Serial.print(""); Serial.print((m_n[i] - ((i == 3) ? 0 : m_n[i + 1]))); Serial.print(", "); }
      if (m_debug) { Serial.print("m_calibrateFactor="); Serial.print(m_calibrateFactor[i], 6); Serial.print(", "); }
      float r = (((float)(m_n[i] - ((i == 3) ? 0 : m_n[i + 1]))) / ((float)N_MAX)) * R_REF * m_calibrateFactor[i];
      if (m_debug) { Serial.print("r="); Serial.print(r); Serial.print(", "); }
      float t = pt1000(r);
      if (m_debug) { Serial.print("t="); Serial.print(t); Serial.print(", "); }
      if (m_debug) { Serial.println(); }
      setTemperature(i, t);
    }

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

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

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

  default:
    fatal(FATAL_ILLEGAL_STATE);
    break;
  }
}