#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;
void init_2ndi2c(){
  Wire1.begin(dp_sda, dp_scl);
  Wire1.setClock(400000);
}
void 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");
    while (1)
    {
    }
  }
}

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;
  }
}
void init_pressure_sensor(void)
{ 
  bmp.begin(BMP085_ULTRAHIGHRES, &Wire1);
  basicaltitude=bmp.readPressure();
  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");
}

float get_pressure()
{
  float pressure;
  pressure = bmp.readPressure();
  memcpy(pressure_avg, &pressure_avg[1], sizeof(pressure_avg) - sizeof(int));
  pressure_avg[9] = pressure;
  return pressure;
}

float get_pressure_avg(void)
{
  int i;
  float total;
  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;
  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 / 10);
}

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

float get_altitude_diff(void){
  return bmp.readPressure() - basicaltitude;
}