#include <math.h>

#include "imu.h"

#include "ori.h"

#include "med2.h"

static const float PI = 3.14159265359f;
static const float TO_DEG = 57.2957795130f;

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

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

static const long IMU_Iner_Fil = 64;
static float IMU_Iner[3];

static float GetCompass(struct ORI_Data& Data)
{
  float x=-Data.Mag.Y/1000.0f, y=-Data.Mag.X/1000.0f, z=-Data.Mag.Z/1000.0f;
  
  float sp = Data.SinPitch;
  float sr = Data.SinRoll;
  
  float cp = Data.CosPitch;
  float cr = Data.CosRoll;
  
  float cx = x * cp + z * sp;
  float cy = x * sr * sp + y * cr - z * sr * cp;
 
  float n=-atan2f(cy, cx)*TO_DEG;
  float north=360+n;
  
  Data.North=Data.Upside;
  if(n>90) n=180-n;
  else if(n<-90) n=-180-n;
  else Data.North=!Data.Upside;
  
  static long nnn;
  nnn=n;
  
  Data.Yaw = Data.Yaw * (1.0f - FK_Y) + n * FK_Y;
  
  return north;
}

inline float GetAngle(float a1, float a2, float az)
{
  if(a2 == 0.0f && az == 0.0f) return a1>0 ? 90.0f : -90.0f;
  return atanf(a1/(sqrtf(a2*a2+az*az))) * TO_DEG;
}

static void GetAngle(ORI_Data& Data, const unsigned long Freq, short Iner[3])
{
  const float sr=Data.SinRoll;
  const float sp=Data.SinPitch;
  const float cr=Data.CosRoll;
  const float cp=Data.CosPitch;

  float gx = Data.Gyr.X / 16.0f / Freq;
  float gy = Data.Gyr.Y / 16.0f / Freq;
  float gz = Data.Gyr.Z / 16.0f / Freq;

  float ax = Data.Acc.X;
  float ay = Data.Acc.Y;
  float az = Data.Acc.Z;
  
  float sum_xyz;
  sum_xyz = sqrtf((ax*ax)+(ay*ay)+(az*az)) / 1000.0f;
  
  Data.Acc.Sum=sum_xyz*1000;

  if(Data.Upside) gz=-gz;
  
  float gn=gz*cp*cr-gx*sr-gy*sp;
  
  if(Data.Upside) { gx=-gx; gy=-gy; gz=-gz; }
  
  float gp=gx*cr+gz*sr;
  float gr=gy*cp+gz*sp;
  
  static float pitch=0;
  static float roll=0;
  float yaw=Data.Yaw;
  pitch -= gp;
  roll += gr;
  yaw += gn;
  
  if(pitch>90.0f) { Data.Upside=!Data.Upside; pitch=180.0f-pitch; }
  else if(pitch<-90.0f) { Data.Upside=!Data.Upside; pitch=-180.0f-pitch; }
  
  if(roll>90.0f) { Data.Upside=!Data.Upside; roll=180.0f-roll; }
  else if(roll<-90.0f) { Data.Upside=!Data.Upside; roll=-180.0f-roll; }
  
  if(yaw>90.0f) { Data.North=!Data.North; yaw=180.0f-yaw; }
  else if(yaw<-90.0f) { Data.North=!Data.North; yaw=-180.0f-yaw; }

  if((sum_xyz>0.98f) && (sum_xyz<1.02f))
  {
    float ap=GetAngle(ay, ax, az);
    float ar=GetAngle(ax, ay, az);
    
    Data.Upside=az>0;
    pitch = pitch * (1.0f - FK_PR) + ap * FK_PR;
    roll = roll * (1.0f - FK_PR) + ar * FK_PR;
  }

  Data.Pitch = pitch;
  Data.Roll = roll;
  Data.Yaw = yaw;
  
  Data.SinPitch=sinf(pitch*PI/180.0f);
  Data.SinRoll=sinf(roll*PI/180.0f);
  Data.SinYaw=sinf(yaw*PI/180.0f);
  
  Data.CosPitch=sqrtf(1.0f-Data.SinPitch*Data.SinPitch);
  Data.CosRoll=sqrtf(1.0f-Data.SinRoll*Data.SinRoll);
  Data.CosYaw=sqrtf(1.0f-Data.SinYaw*Data.SinYaw);
}

static short GyroShift[3]{20, 10, -2};

static short NormAcc_X[2]{-4090, 4110}, NormAcc_Y[2]{-4130, 4075}, NormAcc_Z[2]{-4182, 4070};

static short NormMag_X[2]{-360, 270}, NormMag_Y[2]{-260, 340}, NormMag_Z[2]{-350, 320};

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;
}

IMU_Data DataIMU;

void ORI_Get(ORI_Data& Data, const unsigned long Freq, short Iner[3])
{
  IMU_Get(DataIMU);
  
  short ax = DataIMU.Acc.X;
  short ay = DataIMU.Acc.Y;
  short az = DataIMU.Acc.Z;
  
  NormalizeAcc(ax, ay, az, 1000);
  
  Data.Acc.X = ax;
  Data.Acc.Y = ay;
  Data.Acc.Z = az;
  
  Data.Gyr.X = DataIMU.Gyr.X + GyroShift[0];
  Data.Gyr.Y = DataIMU.Gyr.Y + GyroShift[1];
  Data.Gyr.Z = DataIMU.Gyr.Z + GyroShift[2];
  
  short mx = DataIMU.Mag.X;
  short my = DataIMU.Mag.Y;
  short mz = DataIMU.Mag.Z;
  
  NormalizeMag(mx, my, mz, 1000);
  
  Data.Mag.X = mx;
  Data.Mag.Y = my;
  Data.Mag.Z = mz;
  
  GetAngle(Data, Freq, Iner);
  GetCompass(Data);
}

short CompAcc[3];

void ORI_TiltCompAcc(float Pitch, float Roll, short Acc[3], IMU_Data& Data)
{
  float sin_r = sinf(Roll*PI/180.0f);
  float sin_p = sinf(Pitch*PI/180.0f);
  
  float cos_r = sqrtf(1.0f-sin_r*sin_r); // cosf(Roll*PI/180.0f);
  float cos_p = sqrtf(1.0f-sin_p*sin_p); // cosf(Pitch*PI/180.0f);
  
  Acc[0] = Data.Acc.X - 4000 * sin_r;
  Acc[1] = Data.Acc.Y + 4000 * sin_p;
  Acc[2] = Data.Acc.Z - 4000 * fabs(cos_r) * fabs(cos_p);
  
  CompAcc[0]=Acc[0];
  CompAcc[1]=Acc[1];
  CompAcc[2]=Acc[2];
}