#include <math.h>

#include "imu.h"
#include "bar.h"

#include "med2.h"

#include "ori.h"
#include "quat.h"

static const float PI = 3.14159265359f;
static const float TO_DEG = 180.0f/PI;
static const float TO_RAD = PI/180.0f;

static float FK_PR = 0.01f;
static float FK_Y = 0.01f;

static float MEGA_BAR;
static float MEGA_BAR_MIN=0.00001f;
static float MEGA_BAR_MAX=0.1f;
static float MEGA_BAR_MUL = 100.0f;
static float MEGA_BAR_POW = 2.0f;
static float MEGA_BAR_CNT = 7.0f;

ORI_Data DataORI;

const int BAR_MedCount=50;
long BAR_MedData[BAR_MedCount];
unsigned char BAR_MedIndex[BAR_MedCount];
MED2_Data BAR_Med = {BAR_MedCount, 0, BAR_MedIndex, BAR_MedData, 20};

static float MegaFilterAlt(const long Freq, const float Alt, float& Speed, float& Acc)
{
  static float falt=0;
  falt=falt*(1.0f-MEGA_BAR)+(Alt)*MEGA_BAR;
  
  static float alt_speed=0, alt_acc=0;
  static int count=0;
  if(count>=MEGA_BAR_CNT)
  {
    static float last_b=0, last_s=0;
    alt_speed=(falt-last_b)*Freq/MEGA_BAR_CNT;
    alt_acc=(alt_speed-last_s);
    last_b=falt;
    last_s=alt_speed;
    count=0;
  }
  else count++;
  
  float coef_w, coef_s;
  
  coef_w=MEGA_BAR_MIN;
  float sub=fabsf(Alt-falt);
  coef_w*=powf(sub*MEGA_BAR_MUL, MEGA_BAR_POW);
  if(coef_w>MEGA_BAR_MAX) coef_w=MEGA_BAR_MAX;
  if(coef_w<MEGA_BAR_MIN) coef_w=MEGA_BAR_MIN;
  
  
  MEGA_BAR=coef_w;
  
  //static float test_speed=0;
  //test_speed=(test_speed*15.0f+alt_speed)/16.0f;
  
  Speed=MED2_Update(alt_speed*1000, BAR_Med)/1000.0f;
  
  Acc=alt_acc;
  
  return falt;
}
//------------------------------------------------------------------------------

/*const int BAR_MedCount=200;
long BAR_MedData[BAR_MedCount];
unsigned char BAR_MedIndex[BAR_MedCount];
MED2_Data BAR_Med = {BAR_MedCount, 0, BAR_MedIndex, BAR_MedData, 100};

const int BAR_MedCount2=50;
long BAR_MedData2[BAR_MedCount2];
unsigned char BAR_MedIndex2[BAR_MedCount2];
MED2_Data BAR_Med2 = {BAR_MedCount2, 0, BAR_MedIndex2, BAR_MedData2, 10};

static float MegaFilterAlt(const long Freq, const float Alt, float& Speed, float& Acc)
{
  static float alt_speed=0, last_b=0;
  
  float falt=MED2_Update(Alt*1000.0f, BAR_Med)/1000.0f;
  
  static int count=0;
  if(count>=MEGA_BAR_CNT)
  {
    static float last_b=0;
    alt_speed=(falt-last_b)*Freq/MEGA_BAR_CNT;
    last_b=falt;
    count=0;
  }
  else count++;
  
  Speed=MED2_Update(alt_speed*1000.0f, BAR_Med2)/1000.0f;
  
  return falt;
}*/
//------------------------------------------------------------------------------



static float MEGA_ACC=0.01f;

static const long MEGA_Count=60; // 600ms at 100Hz
static float MEGA_Alt_Buff[MEGA_Count];
static float MEGA_Spd_Buff[MEGA_Count];
static long MEGA_Index=0;

static float MegaFilterAcc(struct ORI_Data& Data)
{
  float acc;
    
  acc=fabsf(Data.Bar.Acc)*100.0f;
  float filt = MEGA_ACC;
  if(acc>1) filt/=acc;
  
  acc=fabsf(Data.Iner.Z)*100.0f;
  if(acc>1) filt/=acc;
  
  const float g=9.80665f;
  
  static float i_speed=0;
  i_speed+=Data.Iner.Z*g/Data.Freq;
  
  
  MEGA_Alt_Buff[MEGA_Index++]=i_speed;
  if(MEGA_Index>=MEGA_Count) MEGA_Index=0;
  static float delay_speed;
  delay_speed=(delay_speed+MEGA_Alt_Buff[MEGA_Index])/2.0f;
  
  float shift=Data.Bar.Speed-delay_speed;
  
  for(int a=0; a<MEGA_Count; a++) MEGA_Alt_Buff[a] = MEGA_Alt_Buff[a]*(1.0f-filt) + (MEGA_Alt_Buff[a]+shift)*filt;
  
  i_speed = i_speed*(1.0f-filt) + (i_speed+shift)*filt;
  
  Data.Speed.Z=i_speed;
  
  Data.Pos.Z+=i_speed/Data.Freq;
  
  return 0;
}
//------------------------------------------------------------------------------

void ORI_Init()
{ 
  IMU_Init();
  MED2_Init(BAR_Med);
}
//------------------------------------------------------------------------------

static void GetPos(struct ORI_Data& Data)
{
  static long land=Data.Bar.Bar;
  float alt=0;
  alt = BAR_GetAltitude(land, Data.Bar.Bar);
  
  float alt_speed, alt_acc;
  
  float falt=MegaFilterAlt(Data.Freq, alt, alt_speed, alt_acc);
  
  Data.Bar.Alt=alt;
  Data.Bar.Filt=falt;
  Data.Bar.Speed=alt_speed;
  Data.Bar.Acc=alt_acc;
  
  MegaFilterAcc(Data);
}
//------------------------------------------------------------------------------


static short NormAcc_X[2]{-7890, 8030}, NormAcc_Y[2]{-8090, 7930}, NormAcc_Z[2]{-7910, 7910};

static short NormMag_X[2]{-160, 160}, NormMag_Y[2]{-155, 140}, NormMag_Z[2]{-150, 140}; // IST8310
static short GyroShift[3]{-2, 7, 17};

static void NormalizeAcc(short& X, short& Y, short& Z, const short G)
{
  short x=(NormAcc_X[1]-NormAcc_X[0])/2;
  short y=(NormAcc_Y[1]-NormAcc_Y[0])/2;
  short z=(NormAcc_Z[1]-NormAcc_Z[0])/2;
  
  X=(X-(NormAcc_X[0]+x))*G/x;
  Y=(Y-(NormAcc_Y[0]+y))*G/y;
  Z=(Z-(NormAcc_Z[0]+z))*G/z;
}

static void NormalizeMag(short& X, short& Y, short& Z, const short N)
{
  short x=(NormMag_X[1]-NormMag_X[0])/2;
  short y=(NormMag_Y[1]-NormMag_Y[0])/2;
  short z=(NormMag_Z[1]-NormMag_Z[0])/2;
  
  X=(X-(NormMag_X[0]+x))*N/x;
  Y=(Y-(NormMag_Y[0]+y))*N/y;
  Z=(Z-(NormMag_Z[0]+z))*N/z;
}

void ORI_Get(ORI_Data& Data, const unsigned long Freq, IMU_Data& IMU, BAR_Data BAR)
{
  Data.Freq=Freq;
    //------------------------------ imu perevernuta, x z inverse
  short ax = IMU.Acc.X;
  short ay = IMU.Acc.Y;
  short az = IMU.Acc.Z;
  
  NormalizeAcc(ax, ay, az, 8000);
  
  Data.Acc.X = ax;
  Data.Acc.Y = ay;
  Data.Acc.Z = az;

  short gx = (IMU.Gyr.X - GyroShift[0]);
  short gy = IMU.Gyr.Y - GyroShift[1];
  short gz = (IMU.Gyr.Z - GyroShift[2]);
  
  Data.Gyr.X = gx;
  Data.Gyr.Y = gy;
  Data.Gyr.Z = gz;
  
  short mx = IMU.Mag.X;
  short my = IMU.Mag.Y;
  short mz = IMU.Mag.Z;
  
  NormalizeMag(mx, my, mz, 1000);
  
  Data.Mag.X = mx;
  Data.Mag.Y = my;
  Data.Mag.Z = mz;
  
  static float bar_zero=0;
  static int zero_count=0;
  
  if(zero_count<500)
  {
    bar_zero=BAR.Pressure;
    zero_count++;
  }
  
  Data.Bar.Bar=BAR.Pressure;
  //Data.Bar.Alt=(bar_zero-BAR.Pressure)*9.0f;
  
  Vec3 acc{((float)ax)/8000, ((float)ay)/8000, ((float)az)/8000};
  Vec3 gyr{(float)gx, (float)gy, (float)gz};
  Vec3 mag{(float)mx, (float)my, -(float)mz};
  
  


  ORI o = WorkAccGyroMag(acc, gyr, mag, 90, 0.01f);
  
  static int test_pitch, test_roll;
  
  Data.Pitch=o.Pitch;
  Data.Roll=o.Roll;
  Data.Yaw=o.Yaw;
  
  Data.SinX=o.sinX;
  Data.SinY=o.sinY;
  Data.CosZ=o.cosZ;
  
  Data.Iner.X=o.IneX;
  Data.Iner.Y=o.IneY;
  Data.Iner.Z=o.IneZ-1;
  
  //Data.Speed.X+=o.IneX*9.8f/100.0f;
  //Data.Speed.Y+=o.IneY*9.8f/100.0f;
  //Data.Speed.Z+=(o.IneZ-1)*9.8f/100.0f;
  
  Data.Temp.Acc=IMU.Temp;
  Data.Temp.Bar=BAR.Temp;
  
  //GetAngle(Data, Freq);
  //GetCompass(Data);
  //GetIner(Data);
  GetPos(Data);
}