#include "Drone.h"

namespace DroneClient {

    // Реализация статического метода GetBytes
    array<Byte>^ Drone::GetBytes(Object^ data)
    {
        int size = Marshal::SizeOf(data);   
        array<Byte>^ arr = gcnew array<Byte>(size);

        IntPtr ptr = IntPtr::Zero;
        try
        {
            ptr = Marshal::AllocHGlobal(size);
            Marshal::StructureToPtr(data, ptr, true);
            Marshal::Copy(ptr, arr, 0, size);
        }
        finally
        {
            Marshal::FreeHGlobal(ptr);
        }
        return arr;
    }

    // Реализация статического метода FromBytes
    Object^ Drone::FromBytes(array<Byte>^ arr, Type^ type)
    {
        Object^ mem = gcnew Object();

        int size = Marshal::SizeOf(type);
        IntPtr ptr = IntPtr::Zero;
        try
        {
            ptr = Marshal::AllocHGlobal(size);
            Marshal::Copy(arr, 0, ptr, size);
            mem = Marshal::PtrToStructure(ptr, type);
        }
        finally
        {
            Marshal::FreeHGlobal(ptr);
        }
        return mem;
    }

    // Реализация приватного метода SendDataMotor4
    array<Byte>^ Drone::SendDataMotor4()
    {

        DroneData::DataMotor4 mot4;

        mot4.Head.Size = Marshal::SizeOf(DroneData::DataMotor4::typeid);
        mot4.Head.Mode = DroneData::DataMode::Response;
        mot4.Head.Type = DroneData::DataType::DataMotor4;
        updateData();
        setMotors();
        mot4.UL = MotorUL;
        mot4.UR = MotorUR;
        mot4.DL = MotorDL;
        mot4.DR = MotorDR;

        return GetBytes(mot4);
    }

    array<Byte>^ Drone::RecvDataAcc(array<Byte>^ data)
    {
        DroneData::DataAcc imu = (DroneData::DataAcc)FromBytes(data, DroneData::DataAcc::typeid);

        AccX = imu.Acc.X; AccY = imu.Acc.Y; AccZ = imu.Acc.Z;
        TimeAcc = imu.Time;

        return gcnew array<Byte>(0);
    }

    array<Byte>^ Drone::RecvDataGyr(array<Byte>^ data)
    {
      DroneData::DataGyr imu = (DroneData::DataGyr)FromBytes(data, DroneData::DataGyr::typeid);

      GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z;
      TimeGyr = imu.Time;

      return gcnew array<Byte>(0);
    }

    array<Byte>^ Drone::RecvDataRange(array<Byte>^ data)
    {
      DroneData::DataRange pos = (DroneData::DataRange)FromBytes(data, DroneData::DataRange::typeid);

      LaserRange = pos.LiDAR;
      TimeRange = pos.Time;

      return gcnew array<Byte>(0);
    }

    array<Byte>^ Drone::RecvDataLocal(array<Byte>^ data)
    {
        DroneData::DataLocal pos = (DroneData::DataLocal)FromBytes(data, DroneData::DataLocal::typeid);

        PosX = pos.Local.X; PosY = pos.Local.Y;

        return gcnew array<Byte>(0);
    }

    array<Byte>^ Drone::ClientRequestResponse(DroneData::DataHead head, array<Byte>^ body)
    {
        array<Byte>^ zero = gcnew array<Byte>(0);

        switch (head.Type)
        {
        case DroneData::DataType::DataAcc:
            if (head.Mode == DroneData::DataMode::Request)
            {
                return zero; // Запрос данных (не реализовано)
            }
            else
            {
                return RecvDataAcc(body); // Пришли данные
            }

        case DroneData::DataType::DataGyr:
          if (head.Mode == DroneData::DataMode::Request)
          {
            return zero; // Запрос данных (не реализовано)
          }
          else
          {
            return RecvDataGyr(body); // Пришли данные
          }

        case DroneData::DataType::DataRange:
            if (head.Mode == DroneData::DataMode::Request)
            {
                return zero; // Запрос данных (не реализовано)
            }
            else
            {
                return RecvDataRange(body); // Пришли данные
            }

        case DroneData::DataType::DataLocal:
          if (head.Mode == DroneData::DataMode::Request)
          {
            return zero; // Запрос данных (не реализовано)
          }
          else
          {
            return RecvDataLocal(body); // Пришли данные
          }

        case DroneData::DataType::DataMotor4:
            if (head.Mode == DroneData::DataMode::Request)
            {
                return SendDataMotor4(); // Запрос данных
            }
            else
            {
                return zero; // Пришли данные (не реализовано)
            }
        }

        return zero;
    }



    //void Drone::setMotors()
    //{
    //    array<UInt16>^ ch = joy->Channels;   // ссылка на тот же массив
    //    UInt16 pow = (ch[2] - 1000) / 10;
    //    float fpow = float(pow)/ 100.0f;

    //   const float maxAngle = 20.0f;
    //  

    //   desPitch = (ch[1] - 1500) * maxAngle / 500;
    //   desRoll = (ch[0] - 1499) * maxAngle / 500;


    //   float errorPitch, errorRoll, forceRoll, forcePitch;
    //    errorPitch = desPitch -pitch;
    //    errorRoll = desRoll - roll;

    //    static float PRegulator = 0.001f;
 
    //    forcePitch = PRegulator * errorPitch;
    //    forceRoll = PRegulator * errorRoll;


    //   MotorUL = fpow-forcePitch + forceRoll;
    //   MotorUR = fpow - forcePitch - forceRoll;
    //   MotorDL = fpow + forcePitch + forceRoll;
    //   MotorDR = fpow + forcePitch - forceRoll;
    //}


    /* ───────────── вспомогательная функция ───────────── */
    static float Clamp01(float v)
    {
        if (v < 0.0f) return 0.0f;
        if (v > 1.0f) return 1.0f;
        return v;
    }

    /* ───────────── PD-регулятор и микширование ───────── */
    void Drone::setMotors()
    {
        /* ---------- входные каналы --------------------- */
        array<UInt16>^ ch = joy->Channels;
        const float fpow = (ch[2] - 1000) * 0.001f;       // 0…1

        /* ---------- желаемые углы ---------------------- */
        const float maxAngle = 20.0f;
         desPitch = (ch[1] - 1500) * maxAngle / 500.0f;
         desRoll = (ch[0] - 1499) * maxAngle / 500.0f;

         
        /* ---------- PD-регулятор ----------------------- */
        static float prevErrPitch = 0.0f, prevErrRoll = 0.0f;
        const float errPitch = desPitch - pitch;
        const float errRoll = desRoll - roll;

        const float dt = 0.01f;                // период 10 мс (100 Гц)
        const float dPitch = (errPitch - prevErrPitch) / dt;
        const float dRoll = (errRoll - prevErrRoll) / dt;

        const float Kp = 0.115f;               
        const float Kd = 0.0f;

        const float forcePitch = Kp * errPitch + Kd * dPitch;
        const float forceRoll = Kp * errRoll + Kd * dRoll;

        prevErrPitch = errPitch;
        prevErrRoll = errRoll;

        /* ---------- распределение на моторы ------------ */
        MotorUL = fpow - forcePitch + forceRoll;
        MotorUR = fpow - forcePitch - forceRoll;
        MotorDL = fpow + forcePitch + forceRoll;
        MotorDR = fpow + forcePitch - forceRoll;

        //MotorUL = Clamp01(MotorUL);
        //MotorUR = Clamp01(MotorUR);
        //MotorDL = Clamp01(MotorDL);
        //MotorDR = Clamp01(MotorDR);
    }




    // Реализация конструктора
    Drone::Drone()
    {
        DroneStreamData = gcnew array<Byte>(DroneStreamCount); // Инициализация массива
        DroneStreamIndex = 0;
        DroneStreamHead.Mode = DroneData::DataMode::None;
        DroneStreamHead.Size = 0;
        DroneStreamHead.Type = DroneData::DataType::None;

        this->joy = gcnew Joypad();

        this->joy->Start("COM7", 115200);
          
    }

    // Реализация метода DataStream
    List<array<Byte>^>^ Drone::DataStream(array<Byte>^ data, int size)
    {
        System::Collections::Generic::List<array<Byte>^>^ ret = gcnew System::Collections::Generic::List<array<Byte>^>();

        if (data == nullptr) return ret; // Последовательность не сформирована

        if (size + DroneStreamIndex > DroneStreamCount) return nullptr; // Ошибка переполнения

        Array::Copy(data, 0, DroneStreamData, DroneStreamIndex, size);
        DroneStreamIndex += size;

        while (true)
        {
            if (DroneStreamHead.Size == 0) // Заголовок еще не получен
            {
                if (DroneStreamIndex < DroneData::DataHead::StrLen) return ret;
                DroneStreamHead = (DroneData::DataHead)FromBytes(DroneStreamData, DroneData::DataHead::typeid);
            }

            if (DroneStreamHead.Size > DroneStreamIndex) break; // Пакет еще не полный

            array<Byte>^ body = gcnew array<Byte>(DroneStreamHead.Size);
            Array::Copy(DroneStreamData, 0, body, 0, DroneStreamHead.Size);

            int shift = DroneStreamHead.Size;

            DroneStreamIndex -= shift;
            Array::Copy(DroneStreamData, shift, DroneStreamData, 0, DroneStreamIndex); // Сдвиг массива

            DroneStreamHead.Size = 0; // Сброс заголовка

            ret->Add(ClientRequestResponse(DroneStreamHead, body));
        }

        return ret;
    }

    // Реализация метода SendRequest
    array<Byte>^ Drone::SendRequest()
    {
      DroneData::DataHead^ acc = gcnew DroneData::DataHead();
      acc->Size = DroneData::DataHead::StrLen;
      acc->Mode = DroneData::DataMode::Request;
      acc->Type = DroneData::DataType::DataAcc;

      DroneData::DataHead^ gyr = gcnew DroneData::DataHead();
      gyr->Size = DroneData::DataHead::StrLen;
      gyr->Mode = DroneData::DataMode::Request;
      gyr->Type = DroneData::DataType::DataGyr;

      DroneData::DataHead^ range = gcnew DroneData::DataHead();
      range->Size = DroneData::DataHead::StrLen;
      range->Mode = DroneData::DataMode::Request;
      range->Type = DroneData::DataType::DataRange;

      DroneData::DataHead^ local = gcnew DroneData::DataHead();
      local->Size = DroneData::DataHead::StrLen;
      local->Mode = DroneData::DataMode::Request;
      local->Type = DroneData::DataType::DataLocal;

      List<array<byte>^>^ list = gcnew List<array<byte>^>();

      list->Add(GetBytes(acc));
      list->Add(GetBytes(gyr));
      list->Add(GetBytes(range));
      list->Add(GetBytes(local));
      list->Add(SendDataMotor4());

      int count = 0;

      for each(array<byte>^ d in list) count += d->Length;

      array<byte>^ send = gcnew array<byte>(count);

      count = 0;
      for each (array<byte> ^ d in list)
      {
        d->CopyTo(send, count);
        count += d->Length;
      }

      return send;
    }

    void Drone::updateData(){
        Vec3 acc{ this->AccX,this->AccY, this->AccZ };
        Vec3 gyr{ this->GyrX,this->GyrY, this->GyrZ };
        Vec3 mag{ 1,0, 0};
        ORI result = WorkAccGyroMag(acc, gyr, mag, 0, 0.01);
        this->pitch = result.Pitch;
        this->roll = -result.Roll;
        this->yaw = result.Yaw;


    }
}