#include "MeasureUnit.h"
//#define DEBUG

MeasureUnit::MeasureUnit(uint8_t *analogInput, 
uint16_t thermistorCount, 
uint64_t precResistor, 
ThermistorSetting thermistorSetting, 
Adc &adc) : _analogInput(analogInput),
_thermistorCount(thermistorCount), 
_precResistor(precResistor),
_thermistorSetting(thermistorSetting), 
_adc(adc), 
_globalOffset(0), 
_error(OK), 
_state(IDLING),
_channel(0),
_offsetComputeIte(7),
_offsetCounter(7),
_courant(0.0),
_tension(0.0),
_triggerLevelOff(false)
{
  //Allocation dynamique des différent tableaux
  _temperatures = (double*) calloc(_thermistorCount, sizeof(double));
  _rOffsetMap = (double*) calloc(_thermistorCount, sizeof(double));
  _resistanceMap = (double*) malloc(_thermistorCount * sizeof(double));
  _rOffsetBuffer = (double*) malloc(_thermistorCount * sizeof(double));

  if(_temperatures == NULL || _rOffsetMap == NULL || _resistanceMap == NULL || _rOffsetBuffer == NULL)
  {
    _error = MALLOC_ERR;
    _temperatures != NULL ? free(_temperatures):(void)_temperatures;
    _rOffsetMap != NULL ? free(_rOffsetMap):(void)_rOffsetMap;
    _resistanceMap != NULL ? free(_resistanceMap):(void)_resistanceMap;
    _rOffsetBuffer != NULL ? free(_rOffsetBuffer):(void)_rOffsetBuffer;

    _temperatures = NULL;
    _rOffsetMap = NULL;
    _resistanceMap = NULL;
    _rOffsetBuffer = NULL;
  }

  _adc.begin();
}

MeasureUnit::~MeasureUnit()
{
  if(_error != MALLOC_ERR)
  {
    free(_temperatures);
    free(_rOffsetMap);
    free(_resistanceMap);
    free(_rOffsetBuffer);
  }
}

void MeasureUnit::run()
{
  switch(_state)
  {
    case MEASURING:
      _adc.startSample(_channel);
      
      if(_channel == 0) //Calcule du courant
      {
        if(_adc.isSampleReady())
        {
          _tension = _adc.getSampleVoltage();
          _courant =  _tension / (double) _precResistor;
          #ifdef DEBUG
          Serial.print("Tension prec : ");Serial.println(_tension);
          #endif
           _channel++;
        }
      }
      else //Calcule des niveaux de tensions
      {
        if(_adc.isSampleReady())
        {
          _resistanceMap[_channel-1] = _adc.getSampleVoltage();
          #ifdef DEBUG
          Serial.print("Tension thermistances : ");Serial.println(_resistanceMap[_channel-1]);
          #endif
          _channel++;
        }
      }
      
      //Fin de la partie d'acquisition
      if(_channel == _thermistorCount)
      {
        _state = COMPUTING;
        _resistanceMap[_channel-1] = _adc.getAdcSetting().getVref();
      }
      break;
    case COMPUTING :
      //Ici nous calculons les temperatures
      for(int i(_thermistorCount-1); i > 0; i--)
      {
        //Calcule de delta :
        _resistanceMap[i] -= _resistanceMap[i-1];
        #ifdef DEBUG
        Serial.printf("Debug voltage delta : %u -> ",i);Serial.println(_resistanceMap[i]);
        #endif
      }

      //Ne pas oublier de déduire la chute de tension de la resistance de precision pour la première thermistance
      _resistanceMap[0] -= _tension;
      #ifdef DEBUG
      Serial.printf("Debug voltage delta : 0 -> ");Serial.println(_resistanceMap[0]);
      #endif
      
      for(int i(0); i < _thermistorCount; i++)
      {
        //3) Nous en déduisons la résistance
        //Serial.print("Resistance ");Serial.print(i);Serial.print(" ");Serial.println(_resistanceMap[i]);
        _resistanceMap[i] /= _courant;
        //4) Nous en déduisons la temperature
        _temperatures[i] = computeTemperature(_thermistorSetting.getBeta(), _resistanceMap[i], _thermistorSetting.getRat25());

        //On effectue un étalonnage
        if(_triggerLevelOff)
        {
          double averageTemp(0);
          //We reset the offset
          for(int i(0); i < _thermistorCount; i++)
          {
            _rOffsetMap[i] = 0;
          }
          
          for(int i(0); i < _thermistorCount; i++)
          {
            averageTemp += _temperatures[i];
          }
        
          averageTemp /= (double)_thermistorCount;
        
          for(int i(0); i < _thermistorCount; i++)
          {
            _rOffsetMap[i] = averageTemp - _temperatures[i];
          }
          _triggerLevelOff = false;
        }
        
        _temperatures[i] += _rOffsetMap[i] + _globalOffset;
        #ifdef DEBUG_TEMP
        Serial.print("Temperature ");Serial.print(i);Serial.print(" : ");Serial.println(_temperatures[i]);
        #endif
      }
       _state = MEASUREMENT_READY;
      break;
  }
  
  if(_offsetCounter < _offsetComputeIte && isMeasurementReady())
  {
    //We reset the offset array
    if(_offsetCounter == 0)
    {
      Serial.printf("Initiating average \n");
      for(int i(0); i < _thermistorCount; i++)
      {
        _rOffsetBuffer[i] = 0;
        _rOffsetMap[i] = 0;
      }
    }
    else if(_offsetCounter == _offsetComputeIte-1)
    {
      double averageTemp(0);
      
      for(int i(0); i < _thermistorCount; i++)
      {
        _rOffsetBuffer[i] += _temperatures[i];
      }
      //We compute the average for each thermistor:
      for(int i(0); i < _thermistorCount; i++)
      {
        _rOffsetBuffer[i] /= _offsetCounter;
        averageTemp += _rOffsetBuffer[i];
      }
      
      averageTemp /= _thermistorCount;
      for(int i(0); i < _thermistorCount; i++)
      {
        _rOffsetMap[i] = averageTemp - _rOffsetBuffer[i];
      }

      Serial.println("Offset done");
      Serial.print("|");
      for(int i(0); i < _thermistorCount; i++)
      {
        if(i != 7)
        {
          Serial.print("  ");Serial.print(_rOffsetMap[i],2);Serial.print("  |");
        }
        else
        {
          Serial.print("  ");Serial.print(_rOffsetMap[i],2);Serial.print("  |");
        }
      }
      Serial.println("");
    }
    else
    {
      for(int i(0); i < _thermistorCount; i++)
      {
        _rOffsetBuffer[i] += _temperatures[i];
      }
    }
    
    _offsetCounter++;
  }
}

void MeasureUnit::setOffsetIteration(uint8_t iteration)
{
  _offsetComputeIte = iteration;
  _offsetCounter = iteration;
}

void MeasureUnit::startOffsetComputing()
{
  _offsetCounter = 0;
}

/**
 * Methode permettant d'effectuer les mesures de température et de les récupérer
 */
double *MeasureUnit::getTemperatures()
{
  double courant(0), rPrecTension(0);
  //1) Nous calculons le courant présent dans la branche grace à la résistance de précision
  #ifdef DEBUG
  Serial.println("-------------");
  #endif
  rPrecTension = _adc.sampleVoltage(0);
  #ifdef DEBUG
  Serial.println("-------------");
  Serial.print("R prec voltage mV : ");Serial.println(rPrecTension,6);
  #endif
  
  courant = rPrecTension / (double) _precResistor;
  #ifdef DEBUG
  Serial.print("R prec current mA : ");Serial.println(courant,6);
  #endif
  
  
  //2) Nous calculons le delta de tensions pour chaque thermistances
  for(int i(1); i < _thermistorCount; i++)
  {  
    _resistanceMap[i-1] = _adc.sampleVoltage(_analogInput[i]);
    #ifdef DEBUG
    Serial.print("Voltage steps ");Serial.print(i-1);Serial.print(" : ");Serial.println(_resistanceMap[i-1]);
    #endif
  }
  _resistanceMap[7] = _adc.getAdcSetting().getVref();
  #ifdef DEBUG
  Serial.print("Voltage steps 7 : ");Serial.println(_resistanceMap[7]);
  #endif
  
  for(int i(_thermistorCount-1); i > 0; i--)
  {
    //Calcule de delta :
    _resistanceMap[i] -= _resistanceMap[i-1];
    #ifdef DEBUG
    Serial.print("Debug voltage delta : ");Serial.println(_resistanceMap[i]);
    #endif
  }

  //Ne pas oublier de déduire la chute de tension de la resistance de precision pour la première thermistance
  _resistanceMap[0] -= rPrecTension;
  Serial.printf("Debug voltage delta : 0 -> ");Serial.println(_resistanceMap[0]);
  
  for(int i(0); i < _thermistorCount; i++)
  {
    //3) Nous en déduisons la résistance
    //Serial.print("Resistance ");Serial.print(i);Serial.print(" ");Serial.println(_resistanceMap[i]);
    _resistanceMap[i] /= courant;
    //4) Nous en déduisons la temperature
    _temperatures[i] = computeTemperature(_thermistorSetting.getBeta(), _resistanceMap[i], _thermistorSetting.getRat25());
    _temperatures[i] += _rOffsetMap[i] + _globalOffset;
    #ifdef DEBUG_TEMP
    Serial.print("Temperature ");Serial.print(i);Serial.print(" : ");Serial.println(_temperatures[i]);
    #endif
  }
  
  return _temperatures;
}

double MeasureUnit::computeTemperature(double beta, double resistance, double rAt25)
{
  return (((25.0+273.15) * beta) / (beta + (25.0+273.15)*log(resistance / rAt25))) - 273.15;
}

void MeasureUnit::setGlobalTempOffset(double offset)
{
  _globalOffset = offset;
}

double MeasureUnit::getGlobalTempOffset()
{
  return _globalOffset;
}

/**
 * Cette méthode permet de calibrer toutes les temperatures en faisans la moyenne et appliquant un offset individuel
 */
void MeasureUnit::levelTemperaturesOff()
{
  double averageTemp(0);
  //We reset the offset
  for(int i(0); i < _thermistorCount; i++)
  {
    _rOffsetMap[i] = 0;
  }

  getTemperatures();
  
  for(int i(0); i < _thermistorCount; i++)
  {
    averageTemp += _temperatures[i];
  }

  averageTemp /= _thermistorCount;

  for(int i(0); i < _thermistorCount; i++)
  {
    _rOffsetMap[i] = averageTemp - _temperatures[i];
  }
}

double *MeasureUnit::getROffsetMap()
{
  return _rOffsetMap;
}

boolean MeasureUnit::startTemperatureMeasurement()
{

  if(_state == IDLING)
  {
    _state = MEASURING;
    _channel = 0;
    return true;
  }

  return false;
}

void MeasureUnit::levelAsyncTemperaturesOff()
{
  _triggerLevelOff = true;
}

boolean MeasureUnit::isMeasurementReady()
{
  return _state == MEASUREMENT_READY;
}

double *MeasureUnit::getAsyncTemperatures()
{
  double *p(NULL);

  if(_state == MEASUREMENT_READY)
  {
    p = _temperatures;
    _state = IDLING;
  }
  
  return p;
}

void MeasureUnit::init()
{
  Serial.println(computeTemperature(3380, 18000, 10000));
  Serial.println(computeTemperature(3380, 11700, 10000));
}
