#include "IRS.h"
#include <math.h>


static constexpr float PI = 3.14159265359f;
static constexpr float TO_DEG = 180.0f / PI;
static constexpr float TO_RAD = PI / 180.0f;
static constexpr float G = 9.80665f; // m/c^2

IRS::IRS():
  RecordGyro(RecGyr.Rec),
  RecordSpeed(RecSpd.Rec),
  RecordPosit(RecPos.Rec),
  RecordQuat(RecQua.Rec)
{
  RecordGyro.Init(RecGyr.Buffer, Freq, RecGyr.Past);
  RecordSpeed.Init(RecSpd.Buffer, Freq, RecSpd.Past);
  RecordPosit.Init(RecPos.Buffer, Freq, RecPos.Past);
  RecordQuat.Init(RecQua.Buffer, Freq, RecQua.Past);
}


// ======================Orientation======================
void IRS::UpdateGyro(const Vector3& Gyr)
{
  if(!Gyr.IsFinite()) return;
  
  IMU.Gyr = Gyr;

  Vector3 gyr = Gyr * (Period * TO_RAD);

  RecordGyro.Add(Gyr * TO_RAD);

  Quaternion g = gyr;
  g = (OriQuat * g);

  OriQuat += g * 0.5f;
  OriQuat = OriQuat.Norm();

  RecordQuat.Add(OriQuat);

  OriPRT = OriQuat.Conjugate() * Quaternion(0, 0, 1, 0) * OriQuat;
}

void IRS::UpdateAccel(const Vector3& Acc)
{
  if(!Acc.IsFinite()) return;
  
  Vector3 accel = ShiftAccelPRY * Acc * ShiftAccelPRY.Conjugate();
  
  IMU.Acc = accel;

  Quaternion ine = OriQuat * accel * OriQuat.Conjugate();

  Vector3 acc = { ine.X, ine.Y, ine.Z - 1.0f }; // без гравитации
  
  Inertial.Acc = acc;//OriQuat.RotateAroundZ(acc, true);

  Inertial.Spd += acc * (G * Period); // интегрирование скорости
  
  Vector3 spd = Inertial.Spd * Period;
  
  Inertial.Pos += spd; // интегрирование позиции
  
  RecordSpeed.Add(Inertial.Spd);
  RecordPosit.Add(Inertial.Pos);

  RestoreQuat(accel);
}

void IRS::RestoreQuat(const Vector3& Acc)
{
  float len = Acc.Length();

  static float quat_acc_alpha = 0.03f;
  static float quat_acc_max = 0.02f;
  
  float dyn = fabsf(len - 1.0f);
  if(dyn>quat_acc_max) dyn=1.0f; else dyn/=quat_acc_max;

  float gain = quat_acc_alpha * (1.0f - dyn);
  
  if (gain < 0.0001f) return;
  
  Vector3 acc = Acc.Norm();
  
  Vector3 est;
  est.X = 2.0f * (OriQuat.X * OriQuat.Z - OriQuat.W * OriQuat.Y);
  est.Y = 2.0f * (OriQuat.W * OriQuat.X + OriQuat.Y * OriQuat.Z);
  est.Z = OriQuat.W * OriQuat.W - OriQuat.X * OriQuat.X - OriQuat.Y * OriQuat.Y + OriQuat.Z * OriQuat.Z;

  Vector3 cross = acc.Cross(est); 
  float dot = acc.Dot(est);
  
  if (dot < 0.0f) 
  {
    float error_len = cross.Length();
    if (error_len < 0.001f) cross = {1.0f, 0.0f, 0.0f}; 
    else cross = cross * (1.0f / error_len);
  }
  
  Vector3 axis = cross * (gain * 0.5f);
  
  Quaternion correction 
  {
    axis.X,
    axis.Y,
    axis.Z,
    1.0f // Для малых углов cos(0) = 1. При нормализации это сработает корректно.
  };

  OriQuat = OriQuat * correction;
  
  OriQuat = OriQuat.Norm();
}

void IRS::UpdateMagnet(const Vector3& Mag)
{
  if(!Mag.IsFinite()) return;
  
  IMU.Mag = Mag; // Сохраняем сырые данные (если нужно для отладки)
  
  static int init_count = 100;
  
  float alpha = 0.001f; 
  

  if(init_count > 0) 
  {
    alpha = 0.05f;
    init_count--;
  }

  Vector3 mag_norm = Mag.Norm();
  
  Quaternion mW = OriQuat * mag_norm * OriQuat.Conjugate();
  
  Vector3 mW_horizontal = { mW.X, mW.Y, 0.0f };
  
  if (mW_horizontal.LengthSquared() < 0.0001f) return;
  mW_horizontal = mW_horizontal.Norm();

  float error_z = mW_horizontal.Y * NorthDeclination.X - mW_horizontal.X * NorthDeclination.Y;
  
  float half_error_step = error_z * alpha * 0.5f;
  
  Quaternion corr
  {
      0.0f,
      0.0f,
      half_error_step, 
      1.0f 
  };
  
  OriQuat = corr * OriQuat;
  OriQuat = OriQuat.Norm();
}
// ======================/Orientation======================


void IRS::UpdatePositionSpeed(const Vector3& ShiftPosition, const Vector3& ShiftSpeed)
{
  Shift.Pos += ShiftPosition;
  Shift.Spd += ShiftSpeed;
}

void IRS::UpdateQuaternion(const Quaternion& ShiftQuaternion, float Alpha)
{
  Shift.Pos = Shift.Qua * (1.0f - Alpha) + ShiftQuaternion * Alpha;
}

void IRS::RestoreAllShift(Vector3& ShiftPos)
{
  Vector3 spd = Shift.Spd;
  Shift.Spd = 0;
  RecordSpeed.Mix(spd);
  Inertial.Spd += spd;

  Vector3 pos = Shift.Pos;
  Shift.Pos = 0;
  
  RecordPosit.Mix(pos);
  Inertial.Pos += pos;
  ShiftPos=pos;

  Quaternion qua = Shift.Qua;
  Shift.Qua = 0;
  RecordQuat.Mix(qua);
  OriQuat += qua;
  OriQuat = OriQuat.Norm();
}

void IRS::SetAccelShift(float Pitch, float Roll, float Yaw)
{
  float h_roll  = (Roll * TO_RAD)  / 2.0f;
  float h_pitch = (Pitch * TO_RAD) / 2.0f;
  float h_yaw   = (Yaw * TO_RAD)   / 2.0f;
  
  Quaternion q_roll =  {0.0f, sinf(h_roll), 0.0f, cosf(h_roll)};
  Quaternion q_pitch = {sinf(h_pitch), 0.0f, 0.0f, cosf(h_pitch)};
  //   Как найти поворот: arcsin(ось/макс_ось) * TO_GRAD
  Quaternion q_yaw = {0.0f, 0.0f, sinf(h_yaw), cosf(h_yaw)}; 
  
  ShiftAccelPRY = q_pitch * q_roll * q_yaw;
}

void IRS::SetNorthDeclination(const float Yaw)
{
  float dec = Yaw * TO_RAD; // Укажите ваше склонение
  Vector3 mag_north_target;
  NorthDeclination = { sinf(dec), cosf(dec) };
}

bool IRS::SetGroundHeight(float* Height, float MaxHeight, float Alpha)
{
  if(Height)
  {
    float shift = Inertial.Pos.Z - *Height;
    GroundShift = GroundShift*(1.0f-Alpha) + shift*Alpha;
  }
  else
  {
    float height = Inertial.Pos.Z - GroundShift;
    
    if(height<MaxHeight)
    {
      float shift = Inertial.Pos.Z - MaxHeight;
      GroundShift = GroundShift*(1.0f-Alpha) + shift*Alpha;
    }
  }
  
  float height = Inertial.Pos.Z - GroundShift;
  if (height < 0.0f) GroundShift+=height;
}

void IRS::GetSinXYCosZ(Vector3& SinXYCosZ)
{
  SinXYCosZ = OriPRT;
}

void IRS::GetPitchRollYaw(Vector3& PitchRollYaw, bool& Reversed)
{
  Reversed = OriPRT.Z < 0;
  
  float yaw = atan2f(2.0f * (OriQuat.X * OriQuat.Y - OriQuat.W * OriQuat.Z), 1.0f - 2.0f * (OriQuat.Y * OriQuat.Y + OriQuat.Z * OriQuat.Z)) * TO_DEG;
  if (yaw < 0.0f) yaw += 360.0f;

  PitchRollYaw =
  {
    GetAngle(OriPRT.Y, OriPRT.X, OriPRT.Z),  // Pitch
    GetAngle(-OriPRT.X, OriPRT.Y, OriPRT.Z), // Roll
    yaw                                      // Yaw
  };
}

void IRS::GetInertial(Vector3* Pos, Vector3* Spd, Vector3* Acc, float* Gnd)
{
  if (Acc) *Acc = Inertial.Acc;
  if (Spd) *Spd = Inertial.Spd;
  if (Pos) *Pos = Inertial.Pos;
  if (Gnd) *Gnd = GroundShift;
}