#include "../windentelemetry.h"
#include "airspeed.h"
#include <ms4525do.h>
#include <airdata.h>
#include <units.h>
#include <Wire.h>
#include <Adafruit_BMP085.h>

extern TwoWire Wire1;
bfs::Ms4525do pres;
Adafruit_BMP085 bmp;

float pressure_avg[10];
float basicaltitude = 0.0f;
bool pressure_sensor_ready = false;

bool init_2ndi2c(){
  if (!Wire1.begin(dp_sda, dp_scl))
  {
    Serial.println("Error starting I2C");
    return false;
  }
  Wire1.setClock(400000);
  return true;
}
bool init_airspeed_sensor(void)
{

  /*
   * I2C address of 0x28, on bus 0, with a -1 to +1 PSI range
   */
  pres.Config(&Wire1, 0x28, 1.0f, -1.0f);
  /* Starting communication with the pressure transducer */
  if (!pres.Begin())
  {
    Serial.println("Error communicating with sensor");
    return false;
  }
  return true;
}

int get_airspeed_kmh(void)
{
  int vel_kmh = round(bfs::convvel(get_airspeed(), bfs::LinVelUnit::MPS, bfs::LinVelUnit::KPH));
  return vel_kmh;
}
float get_airspeed(void)
{
  if (pres.Read())
  {
    float ms = bfs::Ias_mps(pres.pres_pa());

    return ms;
  }
  else
  {
    return 0;
  }
}
bool init_pressure_sensor(void)
{ 
  if (!bmp.begin(BMP085_ULTRAHIGHRES, &Wire1))
  {
    Serial.println("Error communicating with pressure sensor");
    return false;
  }
  basicaltitude=bmp.readPressure();
  if (basicaltitude <= 0.0f)
  {
    Serial.println("Invalid initial pressure");
    return false;
  }
  for (float &sample : pressure_avg)
    sample = basicaltitude;
  pressure_sensor_ready = true;
  Serial.print("Pressure = ");
  Serial.print(basicaltitude);
  Serial.println(" Pa");

  // Calculate altitude assuming 'standard' barometric
  // pressure of 1013.25 millibar = 101325 Pascal
  Serial.print("Altitude = ");
  Serial.print(get_altitude(basicaltitude));
  Serial.println(" meters");
  return true;
}

float get_pressure()
{
  if (!pressure_sensor_ready)
    return 0.0f;

  float pressure;
  pressure = bmp.readPressure();
  if (pressure <= 0.0f)
    return pressure_avg[9];

  memmove(pressure_avg, &pressure_avg[1], sizeof(pressure_avg) - sizeof(pressure_avg[0]));
  pressure_avg[9] = pressure;
  return pressure;
}

float get_pressure_avg(void)
{
  float total = 0.0f;
  int i;
  for (i = 0; i <10; i++)
  {
    total = total + pressure_avg[i];
  }
  return (total / 10);
}
float get_vario(void)
{
  float vario;
  vario = get_altitude(pressure_avg[9]) - get_altitude(pressure_avg[8]);
  return vario;
}

float get_vario_avg(void)
{
  float vario;

  int i;
  float total = 0.0f;
  for (i = 0; i <9; i++)
  {
    int n=i+1;
    vario = get_altitude(pressure_avg[n]) - get_altitude(pressure_avg[i]);
    total = total + vario;
  }
  return (total / 9);
}

float get_altitude(float pressure)
{
  float altitude;
  altitude = 44330 * (1.0 - pow(pressure / 101325, 0.1903));
  return altitude;
}

float get_altitude_diff(void){
  return get_altitude(pressure_avg[9]) - get_altitude(basicaltitude);
}