#include "Arduino.h"

#include "Metro.h"
#include "Streaming.h"

#include "ads1210.h"
#include "led.h"
#include "SimpleModbusSlave.h"
#include "Thermometer.h"
#include "Config.h"




const uint8_t LED_PIN = 8;

const uint8_t ADC_1_CS_PIN = 9;
const uint8_t ADC_1_RDY_PIN = 7;

const uint8_t ADC_2_CS_PIN = 2;
const uint8_t ADC_2_RDY_PIN = 3;

const uint8_t DEBUG = 14;
const uint8_t CAL_OFFSET_ENABLE = 15;
const uint8_t CAL_FACTOR_ENABLE = 16;

const uint8_t MODBUS_TX_ENABLE_PIN = 6;
const uint8_t MODBUS_ID = 3;
const uint32_t MODBUS_BAUD = 1200;

const uint16_t CALIBRATION_CYCLES = 30;

const uint8_t NUM_OF_CHANNELS = 2;

ADS1210 ads1210[NUM_OF_CHANNELS];
Thermometer thermometer[NUM_OF_CHANNELS];
LED led;
Metro secondTick = Metro(1000);
uint32_t uptimeSeconds = 0;


typedef enum { e_CAL_IDLE, e_CAL_RUNNING, e_CAL_SET, e_CAL_COMPLETE } tCalibrationState;

bool debug = false;
bool calibrationOffsetEnable = false;
bool calibrationFactorEnable = false;
tCalibrationState calibrationState = e_CAL_IDLE;
uint16_t calibrationCycleCnt = 0;
float calibrationValueSum[NUM_OF_CHANNELS];



struct {
  struct {
	  union {
	 	  uint32_t in;
		  uint16_t modbusRegisters[2];
	  } adcValue;
	  union {
		  float in;
		  uint16_t modbusRegisters[2];
 	  } adcU;
	  union {
		  float in;
		  uint16_t modbusRegisters[2];
 	  } adcR;
	  union {
		  float in;
		  uint16_t modbusRegisters[2];
 	  } calOffset;
	  union {
		  float in;
		  uint16_t modbusRegisters[2];
 	  } calFactor;
	  union {
		  float in;
		  uint16_t modbusRegisters[2];
	  } temperatureRaw;
	  union {
		  float in;
		  uint16_t modbusRegisters[2];
	  } temperature;
	  union {
		  float in;
		  uint16_t modbusRegisters[2];
	  } alpha;
  } channelVariables[NUM_OF_CHANNELS];	
  union {
	  uint32_t in;
	  uint16_t modbusRegisters[2];
  } uptimeSeconds;
} modbusHoldingRegisters;


void setup() {  
  pinMode(DEBUG, INPUT_PULLUP);
  pinMode(CAL_FACTOR_ENABLE, INPUT_PULLUP);
  pinMode(CAL_OFFSET_ENABLE, INPUT_PULLUP);

  delay(100);

  debug = (digitalRead(DEBUG) == 0);
  calibrationOffsetEnable = debug && (digitalRead(CAL_OFFSET_ENABLE) == 0);
  calibrationFactorEnable = debug && (digitalRead(CAL_FACTOR_ENABLE) == 0);

  bool initializeConfig = Config::initialize();
  led.begin(LED_PIN);
  ads1210[0].begin(ADC_1_CS_PIN, ADC_1_RDY_PIN, initializeConfig, Config::ADC1START);
  ads1210[1].begin(ADC_2_CS_PIN, ADC_2_RDY_PIN, initializeConfig, Config::ADC2START);
  thermometer[0].begin(initializeConfig, Config::THERMO1START);
  thermometer[1].begin(initializeConfig, Config::THERMO2START);

  if (debug) {
    Serial.begin(9600);
  } else {
    modbus_configure(&Serial, MODBUS_BAUD, SERIAL_8N2, MODBUS_ID, MODBUS_TX_ENABLE_PIN,
                    sizeof(modbusHoldingRegisters), (uint16_t*)(&modbusHoldingRegisters));
  }
  
  for (uint8_t i = 0; i < NUM_OF_CHANNELS; i++) {
    modbusHoldingRegisters.channelVariables[i].calOffset.in = ads1210[i].getCalOffset();
    modbusHoldingRegisters.channelVariables[i].calFactor.in = ads1210[i].getCalFactor();
    modbusHoldingRegisters.channelVariables[i].alpha.in = thermometer[i].getAlpha();
    calibrationValueSum[i] = 0.0;
  }

}

void loop() {
  for (uint8_t i = 0; i < NUM_OF_CHANNELS; i++) {
    ads1210[i].exec();
    modbusHoldingRegisters.channelVariables[i].adcValue.in = ads1210[i].getValue();
    modbusHoldingRegisters.channelVariables[i].adcU.in = ads1210[i].getU();
    modbusHoldingRegisters.channelVariables[i].adcR.in = ads1210[i].getR();
    
    thermometer[i].exec(ads1210[i].getR());
    modbusHoldingRegisters.channelVariables[i].temperatureRaw.in = thermometer[i].getTemperatureRaw();
    modbusHoldingRegisters.channelVariables[i].temperature.in = thermometer[i].getTemperature();
    
    if (modbusHoldingRegisters.channelVariables[i].alpha.in != thermometer[i].getAlpha()) {
      thermometer[i].setAlpha(modbusHoldingRegisters.channelVariables[i].alpha.in);
    }
  }


  if (secondTick.check() == 1) {
    if (debug) {
      for (uint8_t i = 0; i < NUM_OF_CHANNELS; i++) {
        Serial << "Last measurement: Offset: " << ads1210[i].getCalOffset() << ", Factor: " << ads1210[i].getCalFactor();
        Serial << ", R_Raw: " << ads1210[i].getRRaw() << ", R: " << ads1210[i].getR() << endl;
      }
    }

    if ((calibrationOffsetEnable || calibrationFactorEnable) &&
        (calibrationState != e_CAL_COMPLETE)) {
      Serial << "Calibration enabled" << endl;
      led.on();


      switch (calibrationState) {
        case e_CAL_IDLE:
          Serial << "Calibration state idle" << endl;
          for (uint8_t i = 0; i < NUM_OF_CHANNELS; i++) {
        	if (calibrationOffsetEnable) {
              ads1210[i].setCalOffset(0.0);
              ads1210[i].setCalFactor(1.0);
        	}
        	if (calibrationFactorEnable) {
              ads1210[i].setCalFactor(1.0);
        	}
          }
          calibrationState = e_CAL_RUNNING;
          break;
        case e_CAL_RUNNING:
          Serial << "Calibration state running" << endl;
          calibrationCycleCnt++;
          Serial << "  Cnt: "  << calibrationCycleCnt << endl;
          for (uint8_t i = 0; i < NUM_OF_CHANNELS; i++) {
            float r = ads1210[i].getR();
            calibrationValueSum[i] += r;
            Serial << "  Channel: " << i << ", r: " << r << ", Sum: " << calibrationValueSum[i] << endl;
          }
          if (calibrationCycleCnt >= CALIBRATION_CYCLES) {
            calibrationState = e_CAL_SET;
          }
          break;
        case e_CAL_SET:
          Serial << "Calibration state set" << endl;
          // calculate and set according to selected calibration mode
          if (calibrationOffsetEnable) {
            // for offset calibration, the terminals needs to be shorten
            // offset calibration needs to be performed first
            for (uint8_t i = 0; i < NUM_OF_CHANNELS; i++) {
              Serial << "Sum: " << calibrationValueSum[i] << ", Cnt: " << (float)calibrationCycleCnt << endl;
              float offset = calibrationValueSum[i] / ((float)calibrationCycleCnt);
              ads1210[i].setCalOffset(offset);
              Serial << "Setting offset, Channel: " << i << ", Offset: " << offset << endl;
            }
          }
          if (calibrationFactorEnable) {
            // for factor calibration, a 1000R resistor needs to be connected
            // to the terminals
            for (uint8_t i = 0; i < NUM_OF_CHANNELS; i++) {
              Serial << "Sum: " << calibrationValueSum[i] << ", Cnt: " << (float)calibrationCycleCnt << endl;
              float factor = 1000.0 / (calibrationValueSum[i] / ((float)calibrationCycleCnt));
              ads1210[i].setCalFactor(factor);
              Serial << "Setting factor, Channel: " << i << ", Factor: " << factor << endl;
            }
          }
          calibrationState = e_CAL_COMPLETE;
          break;
        default:
          Serial << "Calibration state default" << endl;
          calibrationState = e_CAL_COMPLETE;
          break;
      }
    } else {
      led.toggle();
    }

    uptimeSeconds++;
    modbusHoldingRegisters.uptimeSeconds.in = uptimeSeconds;
  }

  if (! debug) {
    modbus_update();
  }
}