#include <SPI.h>
#include <EEPROM.h>

#include "Logging.h"

#include "CxMafSensor.h"
#include "CxMafSensorPinDef.h"
#include "PositionSensor.h"

static const double  RES_TOT_18C =
  RES1_OHM + RPOT_TOT_OHM +
  1 / (1.0 / RES2_OHM + 1.0 / (76.0 + RTEMP_ADJUSTMENT));

static const double VREF_18C_127V = 12.7 * RPOT_TOT_OHM / RES_TOT_18C;


namespace Cx {

/*--------------------------------------------------------------------*//**
*
*//*--------------------------------------------------------------------*/

CxMafSensor::CxMafSensor()
  :
  m_ledOn( false ),
  m_prevTime( 0 ),
  m_airTempSensor(
    Util::ResistanceSensor::TOP_SENSOR,
    OPERATING_VOLTAGE, AIR_TEMP_FIXED_RESISTANCE
  ),

  m_airTempConverter( 94, 11.5, 68, 21, 31, 45 ),
  m_displayMode( BASIC ),
  m_calibrationState( IDLE ),
  m_opampOutToDacInConverter( 1.22, 400, 10.74, 3596 ),
  m_minPosAdValue( MIN_POS_DEF_ADVALUE ),
  m_maxPosAdValue( MAX_POS_DEF_ADVALUE ),
  m_posSensorValue( 0 ),
  m_xPos( 0.0 ),
  m_temperatureCelcius( 0.0 ),
  m_temperatureOhm( 0 ),
  m_battSensorValue( 0 ),
  m_batteryVoltage( 0.0 ),
  m_vPos( 0.0 ),
  m_vRef( 0.0 ),
  m_adjustmentVoltLow( 0.0 ),
  m_adjustmentVoltHigh( 0.0 ),
  m_adjustmentVoltRef( 0.0 )
{
  return;
}


/*--------------------------------------------------------------------*//**
*
*//*--------------------------------------------------------------------*/

void CxMafSensor::Setup()
{
  delay( 100 );

  pinMode( LED_PIN, OUTPUT);
  digitalWrite( LED_PIN, LOW );

  pinMode( DAC_SS_PIN, OUTPUT );
  digitalWrite( DAC_SS_PIN, HIGH );

  SPI.begin();

  m_lcd.Init( LCD_SS_PIN, LCD_REG_SELECT_PIN );
  m_dac.Init( DAC_SS_PIN );
  m_displaySelectButton.Init( DISPLAY_SELECT_PIN );
  m_calibrationButton.Init( CALIBRATION_PIN );

  uint16_t minPos = 0;
  uint16_t maxPos = 0;

  EEPROM.get( 0, minPos );
  EEPROM.get( sizeof( minPos ), maxPos );

  if ( 0xFFFF == minPos ){ minPos = m_minPosAdValue; }
  if ( 0xFFFF == maxPos ){ maxPos = m_maxPosAdValue; }

  if ( m_minPosAdValue < m_maxPosAdValue ){
    m_minPosAdValue = minPos;
    m_maxPosAdValue = maxPos;
  }

  EEPROM.put( 0, m_minPosAdValue );
  EEPROM.put( sizeof( m_minPosAdValue ), m_maxPosAdValue );

  return;
}



/*--------------------------------------------------------------------*//**
* Called in busy loop
*//*--------------------------------------------------------------------*/

void CxMafSensor::DoThings()
{
  UpdatePosition();
  UpdateAirTemp();
  UpdateBatteryVoltage();

  CalculateOutVoltages();

  UpdateAdjustments();
  AdjustRefVolt();
  AdjustPosVolt();

  WriteDac();

  CalibratePositionRange();
  UpdateDisplay();

  return;
}


/*--------------------------------------------------------------------*//**
*
*//*--------------------------------------------------------------------*/

void CxMafSensor::CalibratePositionRange()
{
  if ( CALIBRATE != m_displayMode ){
    m_calibrationState = IDLE;
    return;
  }

  if ( true != m_calibrationButton.IsClicked() ){ return; }

  switch( m_calibrationState ){
    case  IDLE :
      m_calibrationState = WAITING_MIN_VALUE;
      break;

    case  WAITING_MIN_VALUE :
      // if ( m_posSensorValue < m_maxPosAdValue ){
      m_minPosAdValue    = m_posSensorValue;
      // }
      m_calibrationState = WAITING_MAX_VALUE;
      break;

    case  WAITING_MAX_VALUE :
      // if ( m_posSensorValue > m_minPosAdValue ){
        m_maxPosAdValue    = m_posSensorValue;
      // }
      m_calibrationState = IDLE;

      EEPROM.put( 0, m_minPosAdValue );
      EEPROM.put( sizeof( m_minPosAdValue ), m_maxPosAdValue );

      break;
  }

  return;
}


/*--------------------------------------------------------------------*//**
* This is for development time calibration
*//*--------------------------------------------------------------------*/

void CxMafSensor::DirectPosToDac()
{
  uint32_t daValue = (m_posSensorValue * 0xFFFUL) / 1023UL;

  m_dac.SetValue( daValue, 0 );
  m_dac.SetValue( daValue, 1 );

  m_lcd.SetPosition( 0, 0 );
  m_lcd.print( m_posSensorValue );
  m_lcd.print( "  " );

  m_lcd.SetPosition( 0, 8 );
  m_lcd.print( (int)daValue );
  m_lcd.print( "   " );

  m_lcd.SetPosition( 1, 0 );
  m_lcd.print( m_batteryVoltage, 1 );
  m_lcd.print( "V  " );

  #ifdef __x86_64__
    LOG_STDOUT(
      "\n 0123456789ABCDEF\n" <<
      "\"" << m_lcd.m_screen[ 0 ] << "\"\n" <<
      "\"" << m_lcd.m_screen[ 1 ] << "\""
    );
  #endif

  return;
}


/*--------------------------------------------------------------------*//**
*
*//*--------------------------------------------------------------------*/

void CxMafSensor::WriteDac()
{
  uint16_t vRefDac = ConvertOpampOutToDacIn( m_vRef );
  uint16_t vPosDac = ConvertOpampOutToDacIn( m_vPos );

  #if (false)
    LOG_STDOUT(
      "\nvRef=" << m_vRef << ", vRefDac=" << vRefDac <<
      "\nvPos=" << m_vPos << ", vRefDac=" << vPosDac
    );
  #endif

#if ( true )
  m_dac.SetValue( vRefDac, 0 );
  m_dac.SetValue( vPosDac, 1 );
#else
  // for testing with only one DAC output
  m_dac.SetValue( vRefDac, 1 );
  m_dac.SetValue( vPosDac, 0 );
#endif

  return;
}


/*--------------------------------------------------------------------*//**
*
*//*--------------------------------------------------------------------*/

uint16_t  CxMafSensor::ConvertOpampOutToDacIn( double a_volt )
{
  return m_opampOutToDacInConverter.GetValue( a_volt );
}


/*--------------------------------------------------------------------*//**
*
*//*--------------------------------------------------------------------*/

void CxMafSensor::UpdatePosition()
{
  m_posSensorValue =
    m_posValueSuppressor.SetValue( analogRead( POS_SENSOR_ANALOG_IN_PIN ) );

  uint16_t posSensorVal = m_posSensorValue;

  if ( m_minPosAdValue < m_maxPosAdValue ){
    if ( posSensorVal < m_minPosAdValue ){ posSensorVal = m_minPosAdValue; }
    if ( posSensorVal > m_maxPosAdValue ){ posSensorVal = m_maxPosAdValue; }
  }
  else{
    // Inverted range; i.e. min pos value > max pos value
    if ( posSensorVal > m_minPosAdValue ){ posSensorVal = m_minPosAdValue; }
    if ( posSensorVal < m_maxPosAdValue ){ posSensorVal = m_maxPosAdValue; }
  }


  double posPercentage =
           double( posSensorVal - m_minPosAdValue )
              /
           double( m_maxPosAdValue - m_minPosAdValue );

  double  subRangeBeginPercentage = 0.0;
  double  subRangeEndPercentage   = 1.0;
  double  subRangeBeginXPos       = 0.0;
  double  subRangeEndXPos         = 110.0;

  if ( 0.063 >= posPercentage ){
    subRangeBeginPercentage = 0.0;
    subRangeEndPercentage   = 0.063;
    subRangeBeginXPos       = 0.0;
    subRangeEndXPos         = 10.0;
  }
  else if ( 0.133 >= posPercentage ){
    subRangeBeginPercentage = 0.063;
    subRangeEndPercentage   = 0.133;
    subRangeBeginXPos       = 10.0;
    subRangeEndXPos         = 20.0;
  }
  else{
    subRangeBeginPercentage = 0.133;
    subRangeEndPercentage   = 1.0;
    subRangeBeginXPos       = 20.0;
    subRangeEndXPos         = 110.0;
  }

  int16_t rangeLen     = m_maxPosAdValue - m_minPosAdValue;
  double  subRangeXLen = subRangeEndXPos - subRangeBeginXPos;

  int16_t subRangeBeginPos =
            subRangeBeginPercentage * rangeLen + m_minPosAdValue;

  int16_t subRangeEndPos =
            subRangeEndPercentage * rangeLen + m_minPosAdValue;

  int16_t subRangeLen = subRangeEndPos - subRangeBeginPos;
  int16_t posInSubRange = posSensorVal - subRangeBeginPos;

  double posSubRangePercentage = double( posInSubRange ) / subRangeLen;

  m_xPos = subRangeBeginXPos + posSubRangePercentage * subRangeXLen;

  #if ( false )
    LOG_STDOUT(
      "\nm_posSensorValue      = " << m_posSensorValue <<
      "\nposPercentage         = " << posPercentage <<
      "\nposInSubRange         = " << posInSubRange <<
      "\nsubRangeBeginPos      = " << subRangeBeginPos <<
      "\nsubRangeEndPos        = " << subRangeEndPos <<
      "\nsubRangeBeginXPos     = " << subRangeBeginXPos <<
      "\nposSubRangePercentage = " << posSubRangePercentage <<
      "\nxPos                  = " << m_xPos
    );
  #endif

  return;
}


/*--------------------------------------------------------------------*//**
*
*//*--------------------------------------------------------------------*/

void CxMafSensor::UpdateAirTemp()
{
  long sensorValue  =
    m_tempValueSuppressor.SetValue(
      analogRead( AIR_THERMISTOR_ANALOG_IN_PIN )
    );

  m_temperatureOhm  = m_airTempSensor.GetResistance( sensorValue );

  // LOG_STDOUT("m_temperatureOhm=" << m_temperatureOhm );
  // LOG_STDOUT( "sensorValue=" << sensorValue );

  m_temperatureCelcius = m_airTempConverter.GetValue( m_temperatureOhm );

  return;
}


/*--------------------------------------------------------------------*//**
*
*//*--------------------------------------------------------------------*/

void CxMafSensor::UpdateAdjustments()
{
  UpdateAdjustment( ADJUST_LOW_PIN,  m_adjustmentVoltLow );
  UpdateAdjustment( ADJUST_HIGH_PIN, m_adjustmentVoltHigh );
  UpdateAdjustment( ADJUST_REF_PIN,  m_adjustmentVoltRef );
  return;
}


/*--------------------------------------------------------------------*//**
*
*//*--------------------------------------------------------------------*/

void CxMafSensor::UpdateAdjustment(
  uint16_t   a_pin,
  double    &a_adjVolt
){
  int  adValue  = analogRead( a_pin );

  if ( 12 > adValue ){ adValue = 12; }          // allow it get to both ends
  else if ( 1012 < adValue ){ adValue = 1012; } // i.e. 500 to both dirs

  a_adjVolt =
    ADJUSTMENT_MAX_VOLT *
    (double( adValue ) - double( ADJUSTMENT_MID_AD_VALUE ) )
      /
    500.0;

  #if ( false )
    LOG_STDOUT(
      "pin=" << a_pin << ", adValue=" << adValue << ", adjV=" << a_adjVolt
    );
  #endif

  return;
}


/*--------------------------------------------------------------------*//**
*
*//*--------------------------------------------------------------------*/

void CxMafSensor::CalculateOutVoltages()
{
  PositionSensor posSen;

  double  rPos = posSen.GetResistenceForVisualPosition( m_xPos );
  double  rTot =
    RES1_OHM + RPOT_TOT_OHM +
    1 / (1.0 / RES2_OHM + 1.0 / (m_temperatureOhm + RTEMP_ADJUSTMENT));


  #if (false)
    LOG_STDOUT(
      m_xPos << ": batt=" << m_batteryVoltage  <<
      ", rTot=" << rTot << ", rPos=" << rPos
    );
  #endif

  m_vPos = m_batteryVoltage * rPos / rTot;
  m_vRef = m_batteryVoltage * RPOT_TOT_OHM / rTot;

  return;
}


/*--------------------------------------------------------------------*//**
*
*//*--------------------------------------------------------------------*/

void CxMafSensor::AdjustRefVolt()
{
  double adjustScale = m_vRef / VREF_18C_127V;
  m_vRef = m_vRef + m_adjustmentVoltRef * adjustScale;
  // LOG_STDOUT( "adjustScale=" << adjustScale );

  return;
}


/*--------------------------------------------------------------------*//**
*
*//*--------------------------------------------------------------------*/

void CxMafSensor::AdjustPosVolt()
{
  double adjustScale = m_vRef / VREF_18C_127V;
  double lowEndScale  = ( 110.0 - m_xPos ) / 110.0;
  double highEndScale = ( 110.0 - ( 110.0 - m_xPos )) / 110.0;

  m_vPos =
    m_vPos +
    m_adjustmentVoltLow * lowEndScale * adjustScale +
    m_adjustmentVoltHigh * highEndScale * adjustScale;

  if ( m_vPos > m_vRef ){ m_vPos = m_vRef; }

  #if ( false )
    LOG_STDOUT(
      "\nadjustScale       =" << adjustScale <<
      "\nlowEndScale       =" << lowEndScale <<
      "\nhighEndScale      =" << highEndScale <<
      "\nadjustmentVoltLow =" << m_adjustmentVoltLow <<
      "\nadjustmentVoltHigh=" << m_adjustmentVoltHigh
    );
  #endif

  return;
}


/*--------------------------------------------------------------------*//**
*
*//*--------------------------------------------------------------------*/

void CxMafSensor::UpdateBatteryVoltage()
{
  m_battSensorValue =
    m_battValueSuppressor.SetValue(
      analogRead( BATTERY_VOLTAGE_ANALOG_IN_PIN )
    );

  double adVolt     = ( m_battSensorValue / 1023.0) * OPERATING_VOLTAGE;
  m_batteryVoltage   =
    adVolt * ( BATT_VOLT_BOTTOM_RESISTANCE + BATT_VOLT_TOP_RESISTANCE )
      /
    BATT_VOLT_BOTTOM_RESISTANCE;

  return;
}


/*--------------------------------------------------------------------*//**
*
*//*--------------------------------------------------------------------*/

void CxMafSensor::UpdateDisplay()
{
  if ( true == m_displaySelectButton.IsClicked() ){
    m_displayMode = Mode( uint8_t(m_displayMode) + 1 );
    if ( DEBUG < m_displayMode ){ m_displayMode = BASIC; }
    m_lcd.ClearDisplay();
  }

  switch( m_displayMode ){
    case BASIC     : ShowBasicDisplay(); break;
    case ADJUST    : ShowAdjustmentDisplay(); break;
    case CALIBRATE : ShowCalibrationDisplay(); break;
    case DEBUG      : ShowDebugDisplay(); break;
  }


  return;
}


/*--------------------------------------------------------------------*//**
*
*//*--------------------------------------------------------------------*/

void CxMafSensor::ShowBasicDisplay()
{
  m_lcd.SetPosition( 0, 0 );
  m_lcd.print( m_xPos, 1 );
  m_lcd.print( "  " );

  m_lcd.SetPosition( 0, 6 );
  m_lcd.print( m_batteryVoltage, 1 );
  m_lcd.print( "V " );

  m_lcd.SetPosition( 0, 12 );
  m_lcd.print( (int)m_temperatureCelcius );
  m_lcd.print( "C " );

  m_lcd.SetPosition( 1, 0 );
  m_lcd.print( "r:" );
  m_lcd.print( m_vRef, 2 );
  m_lcd.print( "V " );

  m_lcd.SetPosition( 1, 8 );
  m_lcd.print( "p:" );
  m_lcd.print( m_vPos, 2 );
  m_lcd.print( "V " );

  return;
}


/*--------------------------------------------------------------------*//**
* In 10mV, i.e. 1V -> 100
*//*--------------------------------------------------------------------*/

void CxMafSensor::ShowAdjustmentDisplay()
{
  m_lcd.SetPosition( 0, 0 );
  m_lcd.print( "Low  High  Ref  " );

  m_lcd.SetPosition( 1, 0 );
  m_lcd.print( int(m_adjustmentVoltLow * 100.0)  );
  m_lcd.print( "  " );

  m_lcd.SetPosition( 1, 5 );
  m_lcd.print( int(m_adjustmentVoltHigh * 100.0) );
  m_lcd.print( "  " );

  m_lcd.SetPosition( 1, 11 );
  m_lcd.print( int(m_adjustmentVoltRef * 100.0) );
  m_lcd.print( "  " );

  return;
}


/*--------------------------------------------------------------------*//**
*
*//*--------------------------------------------------------------------*/

void CxMafSensor::ShowCalibrationDisplay()
{
  m_lcd.SetPosition( 0, 0 );
  m_lcd.print( "minPos  maxPos" );

  m_lcd.SetPosition( 1, 0 );
  if ( WAITING_MIN_VALUE == m_calibrationState ){
    m_lcd.print( "?" );
    m_lcd.print( m_posSensorValue );
  }
  else{
    m_lcd.print( m_minPosAdValue );
  }

  m_lcd.print( "  " );

  m_lcd.SetPosition( 1, 8 );
  if ( WAITING_MAX_VALUE == m_calibrationState ){
    m_lcd.print( "?" );
    m_lcd.print( m_posSensorValue );
  }
  else{
    m_lcd.print( m_maxPosAdValue );
  }

  m_lcd.print( "  " );

  return;
}


/*--------------------------------------------------------------------*//**
*
*//*--------------------------------------------------------------------*/

void CxMafSensor::ShowDebugDisplay()
{
  m_lcd.SetPosition( 0, 0 );
  m_lcd.print( m_posSensorValue );
  m_lcd.print( " " );

  m_lcd.SetPosition( 0, 5 );
  m_lcd.print( m_temperatureOhm );
  m_lcd.print( "ohm " );

  m_lcd.SetPosition( 0, 12 );
  m_lcd.print( m_battSensorValue );
  m_lcd.print( " " );

  return;
}



} // end of namespace
   
//*** end of file ***

    Vref              not                  Vin (batt)
     (8)           connected                (16)
      |                |                      |
      |    ,------,    |     ,------,         |
  ,---'----|  R1  |----|-----|  R2  |---------|
  |        '------'    |     '------'         |
  |                    |                      |
  |                    |     ,---/--,         |
  |                    '-----|  Rt  |---------'
  |                          '-/----'
  |          ,-------------------------------,
  '----------|           Rpos                |-------,
             '-------------------------------'       |
                            A                        |
                            |                        |
                           (7)                     (gnd)
                           Vpos                     

  R1 = 148 ohm (alkujaan 150?)
  R2 = 150 ohm

  - Rt, ilman lämpötila. Vastus on herkkä virralle, pitää huomioida
    MCU:n etuvastuksen mitoituksessa/softassa.
     temp/C   ohm
     13       100
     18       85
     21       79
     25       67
     30       59
     35       48
     40       41
     45       37

  - Rpos. Potikan ja vastusverkon yhdistelmä. Huom; 0-10 ja 10-20
    välit ovat lyhyempiä kuin muut. Pitää huomioida softassa.
     ohm   pos
      80   0
      80   5
      80   10
      95   15
     145   20
     187   25
     218   30
     247   35
     268   40
     288   45
     306   50
     321   55
     330   60
     337   65
     343   70
     346   75
     349   80
     352   85
     355   90
     356   95
     357   100
     358   105
     359   110