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




Thermometer::Thermometer() : m_period(DEFAULT_PERIOD),
                             m_timeOutFailureCnt(0)
{

}



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

void Thermometer::setTemperature(unsigned int index, float t) {
  if (DEBUG || INFO) { Serial.print("setTemperature: i="); Serial.print(index); Serial.print(", t="); Serial.print(t); Serial.println(); }
  m_temperature[index] = t;
}

void 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 (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 (DEBUG) { Serial.print("MODE: "); Serial.print(newRegValue, 16); Serial.println(); }

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

void 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 (DEBUG) { Serial.print("MODE: "); Serial.print(newRegValue, 16); Serial.println(); }
  AD7190_SetRegisterValue(AD7190_REG_MODE, newRegValue, 3, 0);
}

void Thermometer::begin(CmdServer *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 (DEBUG) { Serial.println(); }
    if (DEBUG) { Serial.println("State 0"); }
    // start conversion
    SPI_Enable(AD7190_SLAVE_ID);
    startSingleConv();

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

    timeOutCnt = CONV_TIMEOUT;
    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 (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 (DEBUG) { Serial.println("Switching to State 10"); }
      }

      timeOutCnt = CONV_TIMEOUT;
    }
    timeOutCnt--;
    break;

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

    // end cycle
    SPI_Disable(AD7190_SLAVE_ID);

    // increase failure counter
    m_timeOutFailureCnt++;

    state = 0;
    break;

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

    SPI_Disable(AD7190_SLAVE_ID);

    for (unsigned int i = 0; i < NUM_OF_CHANNELS; i++) {
      if (DEBUG) { Serial.print("i="); Serial.print(i); Serial.print(", "); }
      if (DEBUG) { Serial.print("m_n="); Serial.print(m_n[i]); Serial.print(", "); }
      if (DEBUG) { Serial.print(""); Serial.print((m_n[i] - ((i == 3) ? 0 : m_n[i + 1]))); Serial.print(", "); }
      if (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 (DEBUG) { Serial.print("r="); Serial.print(r); Serial.print(", "); }
      float t = pt1000(r);
      if (DEBUG) { Serial.print("t="); Serial.print(t); Serial.print(", "); }
      if (DEBUG) { Serial.println(); }
      setTemperature(i, t);
    }

    if (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;
  }
}