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

#include "Resources.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(getResource(PERIOD_MEASURE_KEY)); m_stream->print(m_thermometer->getPeriodMeasure()); m_stream->println();
      m_stream->print(getResource(ALPHA_SPACES_KEY)); m_stream->print(m_thermometer->getAlpha()); m_stream->println();
      for (unsigned int i = 0; i < NUM_OF_CHANNELS; i++) {
        m_stream->print(getResource(CALIBRATION_SPACES_KEY)); m_stream->print(i); m_stream->print(getResource(COMMA_SPACE_KEY));
        m_stream->print(m_thermometer->getCalibrateFactor(i), 6); m_stream->println();
      }
      m_stream->print(getResource(INFO_SPACES_KEY)); m_stream->print(m_thermometer->getInfo()); m_stream->println();
      m_stream->print(getResource(DEBUG_SPACES_KEY)); m_stream->print(m_thermometer->getDebug()); m_stream->println();
      m_stream->print(getResource(COMPILE_TIME_DEBUG_KEY)); m_stream->print(COMPILE_TIME_DEBUG); m_stream->println();

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

String ThermValues::exec(String params) {
  m_stream->print(getResource(TIMEOUTS_SPACES_KEY)); m_stream->print(m_thermometer->m_timeOutFailureCnt); m_stream->println();
  m_stream->print(getResource(CYCLES_SPACES_KEY)); m_stream->print(m_thermometer->m_cylceCnt); m_stream->println();
  m_stream->print(getResource(PERIOD_SPACES_KEY)); m_stream->print(m_thermometer->getPeriodMeasure()); m_stream->println();
  m_stream->print(getResource(ALPHA_2_SPACES_KEY)); m_stream->print(m_thermometer->getAlpha()); m_stream->println();

  for (unsigned int i = 0; i < NUM_OF_CHANNELS; i++) {
    m_stream->print(getResource(INDEX_KEY)); m_stream->print(i); m_stream->print(getResource(COMMA_SPACE_KEY));
    m_stream->print(getResource(CAL_KEY)); m_stream->print(m_thermometer->getCalibrateFactor(i), 6); m_stream->print(getResource(COMMA_SPACE_KEY));
    m_stream->print(getResource(R_KEY)); m_stream->print(m_thermometer->m_r[i], 2); m_stream->print(getResource(COMMA_SPACE_KEY));
    m_stream->print(getResource(T_KEY)); m_stream->print(m_thermometer->m_temperature[i], 2); m_stream->print(getResource(COMMA_SPACE_KEY));
    m_stream->print(getResource(TS_KEY)); m_stream->print(m_thermometer->m_smoothedTemperature[i], 2);
    m_stream->println();
  }

  return "done";
}

ThermCalibrate::ThermCalibrate(Thermometer *thermometer) : m_thermometer(thermometer), m_enabled(false),
    m_start_highCalibration(false), m_start_zeroCalibration(false), m_r_cal(1000.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->println(getResource(CALI_HELP_1_KEY));
    m_stream->println(getResource(CALI_HELP_2_KEY));
    m_stream->println(getResource(CALI_HELP_3_KEY));
    m_stream->println(getResource(CALI_HELP_4_KEY));
    m_stream->println(getResource(CALI_HELP_5_KEY));
    m_stream->println(getResource(CALI_HELP_6_KEY));
  } else if (params.startsWith("start") && m_enabled) {
    m_start_highCalibration = true;
  } else if (params.startsWith("stop")) {
    m_start_highCalibration = false;
  } else if (params.startsWith("r ") && (space != -1)) {
    m_r_cal = atof(pb1);
  } else if (params.equalsIgnoreCase("show")) {
    m_stream->print(getResource(CALI_SHOW_1_KEY)); m_stream->print(m_enabled); m_stream->println();
    m_stream->print(getResource(CALI_SHOW_2_KEY)); m_stream->print(m_r_cal); m_stream->println();
  } else {
    res = "subcommand not found";
  }
  return res;
}

Thermometer::Thermometer() : 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_calibrationHighMode(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;
}

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::setR(unsigned int index, float r) {
  m_r[index] = r;
  if (getDebug() || getInfo()) {
    Serial.print(getResource(SET_R_KEY)); Serial.print(index); Serial.print(getResource(COMMA_SPACE_KEY));
    Serial.print(getResource(R_EQUALS_KEY)); Serial.print(r, 2); Serial.println();
  }
}

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(getResource(SET_TEMPERATURE_DEBUG_1_KEY)); Serial.print(index); Serial.print(getResource(COMMA_SPACE_KEY));
    Serial.print(getResource(SET_TEMPERATURE_DEBUG_2_KEY)); Serial.print(t); Serial.print(getResource(COMMA_SPACE_KEY));
    Serial.print(getResource(SET_TEMPERATURE_DEBUG_3_KEY)); Serial.print(m_smoothedTemperature[index]);
    Serial.println();
  }
  m_lastSmoothedTemperature[index] = m_smoothedTemperature[index];
}

float Thermometer::getTemperature(unsigned int index) {
  return m_temperature[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(getResource(CONF_COLON_KEY)); 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(getResource(MODE_COLON_KEY)); 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(getResource(MODE_COLON_KEY)); 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);



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


  prepareAdc();
}


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

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

  static unsigned long lastMillis = 0;
  unsigned long currentMillis;


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

    state = 1;
    if (COMPILE_TIME_DEBUG && getDebug()) { Serial.println(getResource(TO_STATE_1_KEY)); }

    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(getResource(TO_STATE_10_KEY)); }
      }

      timeOutCnt = CONV_TIMEOUT;
    }
    timeOutCnt--;
    break;

  case 9:
    if (COMPILE_TIME_DEBUG && getDebug()) { Serial.println(getResource(STATE_9_KEY)); }


    // end cycle
    SPI_Disable(AD7190_SLAVE_ID);

    // increase failure counter
    m_timeOutFailureCnt++;

    if (getDebug() || getInfo()) {
      Serial.print(getResource(STATE_9_DEBUG_1)); Serial.print(m_timeOutFailureCnt); Serial.println();
    }

    state = 0;
    break;

  case 10:
    if (COMPILE_TIME_DEBUG && getDebug()) { Serial.println(getResource(STATE_10_KEY)); }

    SPI_Disable(AD7190_SLAVE_ID);

    if (m_calibrationHighMode || thermCalibrate.m_start_highCalibration) {
      if (getDebug()) { Serial.println(getResource(CALIBRATION_MODE_HINT_KEY)); }
      state = 20;
    } else if (m_calibrationZeroMode || thermCalibrate.m_start_zeroCalibration) {
      Serial.println(getResource(CALIBRATION_ZEOR_MODE_HINT_KEY));
      state = 21;
    } else {
      for (unsigned int i = 0; i < NUM_OF_CHANNELS; i++) {
        if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print("i="); Serial.print(i); Serial.print(getResource(COMMA_SPACE_KEY)); }
        if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print("m_n="); Serial.print(m_n[i]); Serial.print(getResource(COMMA_SPACE_KEY)); }
        if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print((m_n[i] - ((i == 3) ? 0 : m_n[i + 1]))); Serial.print(getResource(COMMA_SPACE_KEY)); }
        if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print("calibrateFactor="); Serial.print(getCalibrateFactor(i), 6); Serial.print(getResource(COMMA_SPACE_KEY)); }
        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(getResource(COMMA_SPACE_KEY)); }
        setR(i, r);
        float t = pt1000(r);
        if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print("t="); Serial.print(t); Serial.print(getResource(COMMA_SPACE_KEY)); }
        if (COMPILE_TIME_DEBUG && getDebug()) { Serial.println(); }
        setTemperature(i, t);
      }

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

      if (COMPILE_TIME_DEBUG && getDebug()) { Serial.println(getResource(TO_STATE_11_KEY)); }
      state = 11;
    }
    break;

  case 11:
    currentMillis = millis();
    if (currentMillis >= (lastMillis + getPeriodMeasure())) {
      lastMillis = currentMillis;
      state = 0;
    }
    break;

  case 20:
    if (thermCalibrate.m_start_highCalibration) {
      if (m_calibrationHighMode) {
        // do the calibration stuff
        (thermCalibrate.m_turn)++;
        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;
          if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print("t="); Serial.print(thermCalibrate.m_turn); Serial.print(", c="); Serial.print(i); }
          if (COMPILE_TIME_DEBUG && getDebug()) { Serial.print(", r="); Serial.print(r, 6); Serial.print(", n="); Serial.print(m_n[i]); }
          thermCalibrate.m_r_sum[i] += r;
          float r_avg = thermCalibrate.m_r_sum[i] / thermCalibrate.m_turn;
          if (getDebug() || getInfo()) { Serial.print(thermCalibrate.m_turn); Serial.print(getResource(COMMA_SPACE_KEY)); Serial.print(getResource(STATE_20_MSG_1_KEY)); Serial.print(i); Serial.print(getResource(COLON_SPACE_KEY)); Serial.print(r_avg, 6); }
          float calFact = thermCalibrate.m_r_cal / r_avg;
          if (getDebug() || getInfo()) { Serial.print(getResource(COMMA_SPACE_KEY)); Serial.print(getResource(STATE_20_MSG_2_KEY)); Serial.print(calFact, 6); Serial.println(); }
        }

        if ((thermCalibrate.m_turn) >= NUM_OF_CALIBRATION_CYCLES) {
          thermCalibrate.m_start_highCalibration = false;
        }
      } else {
        thermCalibrate.m_preserved_alpha = m_alpha;
        m_alpha = 1;
        thermCalibrate.m_preserved_period = m_periodMillis;
        m_periodMillis = CALIBRATION_CYCLE_TIME;
        for (unsigned int i = 0; i < NUM_OF_CHANNELS; i++) {
          thermCalibrate.m_r_sum[i] = 0.0;
        }
        thermCalibrate.m_turn = 0;
        m_calibrationHighMode = true;
      }
    } else {
      // end calibrationMode
      m_calibrationHighMode = false;
      m_alpha = thermCalibrate.m_preserved_alpha;
      m_periodMillis = thermCalibrate.m_preserved_period;
      for (unsigned int i = 0; i < NUM_OF_CHANNELS; i++) {
        Serial.println(getResource(STATE_20_MSG_3_KEY));
        float r_avg = thermCalibrate.m_r_sum[i] / thermCalibrate.m_turn;
        float calFact = thermCalibrate.m_r_cal / r_avg;

        Serial.print(getResource(STATE_20_MSG_4_KEY)); Serial.print(i);
        Serial.print(getResource(COLON_SPACE_KEY)); Serial.print(calFact, 6); Serial.println();
        setCalibrateFactor(i, calFact);
      }
    }

    state = 11;
    break;

  case 21:
    if (thermCalibrate.m_start_zeroCalibration) {
      if (m_calibrationZeroMode) {
        // do the calibration stuff
      } else {
        m_calibrationZeroMode = true;
      }
    } else {
      // end calibrationMode
      m_calibrationZeroMode = false;
    }

    state = 11;
    break;

  default:
    fatal(FATAL_ILLEGAL_STATE);
    break;
  }
}