/*************************************************************
  Höhen Sensor
  
  Sensor: MS5611 (GY-63)
  I2C Adrtesse 77h

  MS5611 ist wesentlich genauer

  MS5611:  24bit ADC, 4096 osr, best update rate: 50Hz, Auflösung 10cm
  BMP180:  19bit ADC, 8 osr, best update rate: 33Hz
  ------------------------------------------------------------
  
  Copyright (C) 2017 Thomas Lehmann
  
  Version history:
  1.00   19.06.2017  created
  1.01   01.07.2017  BMP180 hinzugefügt

***************************************************************/


#include <Wire.h>

#include "SensAlt.h"


#if _SENS_ALT_ == 1


// ====
// Init
// ====
bool SensorAlt::Init( uint8_t Id, bool VarioActiv )
{
  if( TmoRefresh )                                // bereits gestartet ?
    return( true );

  #if _SENS_VARIO_  == 1
  Vario.Activ = VarioActiv;
  #endif

  // MS5611 Sensor vorhanden / ansprechbar ?
  Wire.begin();                                   // I2C als Master starten
  Wire.beginTransmission( MS5611_ADDRESS );       // Sensor andingsen
  if( Wire.endTransmission() )                    // 0 == korrekte Antwort
    return( false );                              // Sensor nicht angeschlossen

  WriteCmd( MS5611_CMD_RESET );
  delay( 100 );

  //readPROM();
  for( uint8_t offset = 0; offset < 6; offset++ )
    MS5611.fc[offset] = ReadReg(MS5611_CMD_READ_PROM + (offset * 2), 2);

  MS5611.Delay = 1;                               // zykl. MS5611 Abfragen starten

  TmoRefresh = ALT_TMO_REFRESH + Id;              // 10mS Timer asynchron zu anderen Sensoren bedienen
  return( true );
}



// ==================================================
// zyklischer Aufruf aus Loop()
// ==================================================
void SensorAlt::Loop()
{
 static int64_t D1;                               // vom Sensor gelesener Druck

  // zyklisch Temp und Druck abfragen
  if( millis() < MS5611.Delay )
    return;

  // höchste Auflösung die der Sensor zu bieten hat für Variofunktion
  switch( MS5611.State )
    {
    case 0:                                       // read raw Pressure and Delay(10)
      WriteCmd( MS5611_CMD_CONV_D1 + MS5611_ULTRA_HIGH_RES );
      break;
    case 1:                                       // Verzögerung beendet, Pressure auslesen
  D1 = ReadReg( MS5611_CMD_ADC_READ, 3 );
                                                  // read raw Temperature and Delay(10)
      WriteCmd( MS5611_CMD_CONV_D2 + MS5611_ULTRA_HIGH_RES );
      break;
    case 2:                                       // Verzögerung beendet, Temp auslesen und Druck berechnen
      {
      int64_t D2   = ReadReg( MS5611_CMD_ADC_READ, 3 );

      int64_t dT   = D2 - ((uint64_t)MS5611.fc[4] << 8);
      int64_t OFF  = (((uint64_t)MS5611.fc[1]) << 16) + ( ((uint64_t)MS5611.fc[3] * dT) >> 7);
      int64_t SENS = (((uint64_t)MS5611.fc[0]) << 15) + ( ((uint64_t)MS5611.fc[2] * dT) >> 8);
      // ohne Temperatur Kompensation
      // Druck in Pascal (100 Pa = 1 hPa = 1 mbar)
      // 1 digit ~ 8cm
      // NN   = 101325
      // Home ~ 101014 (25m) > offiziell 40m
      uint32_t RawPressure = ((((((D1) * SENS) >> 21) - OFF) ) >> 15);
      // kleiner Smoothing Filter um Spikes auszufiltern
      // Anteil neuer Wert 25%, alter Wert 75%
      MS5611.RawPressure = ((float)RawPressure*0.25) + ((float)MS5611.RawPressure*0.75);

      if( MS5611.ReadCnt < (5000/30) )            // die ersten 5 Sek. (a' 3x10mS) zum Einpegeln
        {
        MS5611.ReadCnt++;
        MS5611.RefPressure = MS5611.RawPressure;  // akt. Druck als Referenz für relative Höhe ggü Start
        }
      else
        MS5611.ReadCnt = 0xff;
      MS5611.State = -1;
      break;
      }
    }

  MS5611.State++;
  MS5611.Delay = millis() + TMO_ULTRA_HIGH_RES;
}

// Calculate altitude from Pressure & Sea level pressure
double SensorAlt::getAltitude( double pressure, double seaLevelPressure )
{
  return (44330.0f * (1.0f - pow((double)pressure / (double)seaLevelPressure, 0.1902949f)));
}

// MS5611 I2C read/write
void SensorAlt::WriteCmd( uint8_t Cmd )
{
  Wire.beginTransmission( MS5611_ADDRESS );
  Wire.write( Cmd );
  Failure = Wire.endTransmission();
}

int32_t SensorAlt::ReadReg( uint8_t Cmd, int ByteCnt )
{
  WriteCmd( Cmd );
  Wire.requestFrom( MS5611_ADDRESS, ByteCnt );

  int32_t Res = 0L;
  //while( ByteCnt-- )
  while( Wire.available() )
    {
    Res <<= 8;
    Res |= Wire.read();
    }
  return( Res );
}






// ==========
// 10mS Timer
// ==========
void SensorAlt::Tim10mS()
{
  // Sensor aktiv ?
  if( TmoRefresh == 0 )                           // Sensor nicht angeschlossen
    return;

  // Sensor noch nicht eingepegelt ?
  if( MS5611.ReadCnt != 0xff )
    return;

  if( --TmoRefresh )
    return;
  TmoRefresh = ALT_TMO_REFRESH;

  #if VARIO_TEST == 1   // steigen/sinken simulieren
  // je digit 8cm * 5 = 0.40m/s
  // steigen 1 m/s = 120 cm/s > 120/8 = 15 digits/s 
  // alle 200mS = 15/5 = 3
  static long TmpPressure = 101325;   // NN
  // 0,40m/s
  //TmpPressure -= 1;                  // je digit 8cm * 5 = 0.40m/s
  // 1,20m/s
  TmpPressure -= 3;                    // je digit 8cm * 5/s
  MS5611.RawPressure = TmpPressure;
  #endif



  // ====
  // Höhe
  // ====
                                                  // aktuelle relative Höhe ggü Start in Meter
  TVal.AltitudeRelativ = getAltitude( MS5611.RawPressure, MS5611.RefPressure );

  // Gesamthöhe, nur steigen, summieren
  // ----------------------------------
  #define ALT_HYSTERESE   10                      // Gesamthöhe alle 10m summieren
  if( TVal.AltitudeRelativ > LastAltTot + ALT_HYSTERESE ) // Höhe steigt um HYSTERESE Meter
    {
    GData.EEprom.NV.AltitudeTot += TVal.AltitudeRelativ - LastAltTot;
    GData.Signal |= SIG_SAVE2NV;                  // neue Gesamthöhe, speichern
    LastAltTot = TVal.AltitudeRelativ;
    }
  else if( TVal.AltitudeRelativ + ALT_HYSTERESE < LastAltTot )  // sinken
    LastAltTot = TVal.AltitudeRelativ;

  // Gesamt Höhe in km, 1234m = 1,23km
  TVal.AltitudeTotDez2 = GData.EEprom.NV.AltitudeTot / 1000;


  // =====
  // VARIO
  // =====
  #if _SENS_VARIO_  == 1

  if( !Vario.Activ )                   // Vario aktiv geschaltet ?
    return;                            // nö, dann sparen wir uns etwas Performance
                                       // akt. Höhe in cm
  long Altitude = getAltitude( MS5611.RawPressure, MS5611.RefPressure ) * 100;
  long uVario;
/*
TermPut(" RawPress: ");
TermPut(MS5611.RawPressure);
TermPut(" Alt: ");
TermPut( Altitude );
TermPut(" lastAlt: ");
TermPut( Vario.LastAlt );
*/
  if( Vario.LastAlt )
    uVario = (Altitude - Vario.LastAlt) * (1000 / float(millis() - Vario.LastTime));
  Vario.LastAlt = Altitude;
  Vario.LastTime = millis();

//TermPut(" uVario: ");
//TermPut(uVario);

  // Vario Filter
  // IIR Low Pass Filter
  // y[i] := α * x[i] + (1-α) * y[i-1]
  //      := y[i-1] + α * (x[i] - y[i-1])
  // mit α = 1- β
  // y[i] := (1-β) * x[i] + β * y[i-1]
  //      := x[i] - β * x[i] + β * y[i-1]
  //      := x[i] + β (y[i-1] - x[i])
  // see: https://en.wikipedia.org/wiki/Low-pass_filter#Simple_infinite_impulse_response_filter
  // Danke an Rainer Stransky
  
  float SmoothFilt = GData.EEprom.Data.CfgVarioSmoothing / 100.0;
  uVario = uVario + (SmoothFilt * (Vario.LastFilter - uVario));
  Vario.LastFilter = uVario;

  // Dead zone filter
  //  if( uVario > GData.EEprom.Data.CfgVarioDeadzone || uVario < (GData.EEprom.Data.CfgVarioDeadzone * -1) )
  //    uVario = 0;
  if( uVario > GData.EEprom.Data.CfgVarioDeadzone )
    uVario -= GData.EEprom.Data.CfgVarioDeadzone;
  else if( uVario < (GData.EEprom.Data.CfgVarioDeadzone * -1) )
    uVario += GData.EEprom.Data.CfgVarioDeadzone;
  else
    uVario = 0;

  TVal.VarioDez2 = uVario;

//TermPut(" Filt: ");
//TermPut(SmoothFilt);
//TermPut(" uVarioRes: ");
//TermPutln(uVario);
  #endif
}

#endif




/*
//****************
//openXSensor Code
//****************

TermPut(" press: ");
TermPut(realPressure);
TermPut(" curAlt: ");
TermPut( SensMS5611->getAltitude(realPressure/10000) );


static int32_t altitude, rawAltitude ; // in cm * 100 
static int32_t altitudeLowPass  ;
static int32_t altitudeHighPass  ;
static float climbRate2AltFloat  ;
static float abs_deltaClimbRate ;
static float climbRateFloat  ;  // in cm/sec but as float

// pressureMicros = micros();                  // pressureMicros is the timestamp to calculate climbrate between 2 pressuresD2=result;

      //                1009106180
      if ( realPressure > 954610000) {
        rawAltitude = ( 1013250000 - realPressure ) * 0.08526603 ; // = 500 / (101325 - 95461)  // returned value 1234567 means 123,4567 m (temp is fixed to 15 degree celcius)
      } else if ( realPressure > 898760000) {
        rawAltitude = 5000000 + ( 954610000 - realPressure ) * 0.089525515  ; 
      } else if ( realPressure > 845980000) {
        rawAltitude = 10000000 + ( 898760000 - realPressure ) * 0.094732853  ; 
      } else if ( realPressure > 794980000) {
        rawAltitude = 15000000 + ( 845980000 - realPressure ) *  0.098039216 ; 
      } else if ( realPressure > 746860000) {
        rawAltitude = 20000000 + ( 794980000 - realPressure ) *  0.103906899 ; 
      } else if ( realPressure > 701120000) {
        rawAltitude = 25000000 + ( 746860000 - realPressure ) *  0.109313511 ; 
      } else if ( realPressure > 657680000) {
        rawAltitude = 30000000 + ( 701120000 - realPressure ) *  0.115101289 ; 
      } else if ( realPressure > 616450000) {
        rawAltitude = 35000000 + ( 657680000 - realPressure ) *  0.121270919 ; 
      } else if ( realPressure > 577330000) {
        rawAltitude = 40000000 + ( 616450000 - realPressure ) *  0.127811861 ;
      } else
        rawAltitude = 45000000 + ( 577330000 - realPressure ) *  0.134843581 ;

TermPut(" rawAlt: ");
TermPut(rawAltitude);

// here the classical way to calculate Vspeed with high and low pass filter      
      if (altitude == 0) {
        altitudeLowPass = altitudeHighPass = altitude = rawAltitude ;
      }
      altitude += 0.04 * (rawAltitude - altitude) ;
//      varioData.altitudeAt20MsecAvailable = true ; // inform openxsens.ino that calculation of dTE can be performed
TermPut(" alt: ");
TermPut( altitude );

      altitudeLowPass += 0.085 * ( rawAltitude - altitudeLowPass) ;
      altitudeHighPass += 0.1  * ( rawAltitude - altitudeHighPass) ;
      climbRate2AltFloat = ((altitudeHighPass - altitudeLowPass )  * 5666.685 ) / 20000 ; 

      abs_deltaClimbRate =  abs(climbRate2AltFloat - climbRateFloat) ;

TermPut(" low: ");
TermPut(altitudeLowPass);
TermPut(" high: ");
TermPut(altitudeHighPass);
TermPut(" climb: ");
TermPut(climbRate2AltFloat);
TermPut(" delta: ");
TermPut(abs_deltaClimbRate);

#define SENSITIVITY_MIN 50
#define SENSITIVITY_MAX 300
#define SENSITIVITY_MIN_AT 100
#define SENSITIVITY_MAX_AT 1000

static int sensitivityMin = SENSITIVITY_MIN ; // set the min smoothing to the default value
static int32_t sensVal, climbRate;

//      if ( varioData.sensitivityPpm  > 0)
//        sensitivityMin =   varioData.sensitivityPpm ; 
      if ( (abs_deltaClimbRate <= SENSITIVITY_MIN_AT) || (sensitivityMin >= SENSITIVITY_MAX) ) {
         sensVal = sensitivityMin ;  
      } else if (abs_deltaClimbRate >= SENSITIVITY_MAX_AT)  {
         sensVal = SENSITIVITY_MAX ; 
      } else {
         sensVal = sensitivityMin + ( SENSITIVITY_MAX - sensitivityMin ) * (abs_deltaClimbRate - SENSITIVITY_MIN_AT) / (SENSITIVITY_MAX_AT - SENSITIVITY_MIN_AT) ;
      }
      climbRateFloat += sensVal * (climbRate2AltFloat - climbRateFloat)  * 0.001 ; // sensitivity is an integer and must be divided by 1000
      
#define VARIOHYSTERESIS 5
      if ( abs((int32_t)  climbRateFloat - climbRate) > VARIOHYSTERESIS ) {
          climbRate = (int32_t)  climbRateFloat  ;
      }    

TermPut(" climbRate: ");
TermPut(climbRate);

TermPut("\n");

  TVal.VarioDez2 = climbRate;

return;
*/













