#include <string.h>

#include "tick.h"

#include "ori.h"

#include "uart.h"
#include "gps.h"

#include "pwm.h"

#include "quat.h"

#include "prot.h"
#include "prot_head.h"

Point PointGPS{0,0,0};

MAIN_FLY_MODE MainFlyMode=MAIN_FLY_MODE::WAIT;

STATUS_MODE Main_StatusMode;
SYS_MODE Main_SysMode;

const unsigned char SRC_ID = 1;
const unsigned char DST_ID = 254;

bool HeaderBegin::CheckCRC()
{
  unsigned char test = 0;
  for (unsigned short a = 0; a < len1; a++) test ^= data[a];
  return crc == test;
}

void HeaderBegin::SetCRC()
{
  unsigned char test = 0;
  for (unsigned short a = 0; a < len1; a++) test ^= data[a];
  crc = test;
}

enum class DataMode : unsigned char { Begin, Head, Data };

static unsigned char ProtoData[MaxLength];
static DataMode Mode = DataMode::Begin;
static unsigned long Length, Index;

static const unsigned long Wait = 100; // ms
static unsigned long Timer;

static void ProcessingMessage(HeaderMessages*);

bool MainUpdate(unsigned char byte)
{
  const unsigned long tick = TICK_GetCount();

  //if ((tick - Timer > 100)) // ?????? ?????? ?????????
  //{
  //  printf("------------> Warning: %d\n", tick - Timer);
  //}

  if ((Mode != DataMode::Begin) && (tick - Timer > Wait))
  {
    Mode = DataMode::Begin;
    //printf("Error: timeout\n");
  }

  Timer = tick;

  switch (Mode)
  {
    case DataMode::Begin:
    {
      if (byte != Global_stx) return true;
      
      Index = 0;
      ProtoData[Index++] = byte;
      Length = sizeof(HeaderBegin);
      Mode = DataMode::Head;
      
      return true;
    }

    case DataMode::Head:
    {
      ProtoData[Index++] = byte;
      if (Index < Length) return true;

      HeaderBegin& begin = *(HeaderBegin*)ProtoData;

      if ((begin.len1 != begin.len2) || (begin.len1 > MaxLength)
          || (begin.len1 < sizeof(HeaderMessages)) || Index < sizeof(HeaderBegin))
      {
        //printf("Error: HeaderBegin -> size=%d\n", begin.len1);
        Mode = DataMode::Begin;
        return true;
      }

      Mode = DataMode::Data;
      Length += begin.len1;

      return true;
    }

    case DataMode::Data:
    {
      ProtoData[Index++] = byte;
      if (Index < Length) return true;

      HeaderBegin& begin = *(HeaderBegin*)ProtoData;
      Mode = DataMode::Begin;
      if (!begin.CheckCRC())
      {
        //printf("Error: HeaderBegin -> CRC\n");
        return true;
      }
      ProcessingMessage((HeaderMessages*)(ProtoData + sizeof(HeaderBegin)));
      return true;
    }
  }

  return false;
}

void SendMes(HeaderBegin* begin)
{
    begin->SetCRC();
    //SendData(begin, sizeof(HeaderBegin) + begin->len1);
    
    UART2_Send(begin, sizeof(HeaderBegin) + begin->len1);
}
//??????? ???????? ????????? ?????????

static unsigned char MessageData[MaxLength];

void* CreateMessageHeaders(HeaderBegin*& begin, unsigned long size, MESSAGES_ID msg_id)
{
    
    begin = (HeaderBegin*)MessageData;
    begin->len1 = begin->len2 = sizeof(HeaderMessages) + size;
    begin->stx = Global_stx;
    HeaderMessages& message = *(HeaderMessages*)begin->data;
    message.msgId = msg_id;
    message.srcId = SRC_ID;
    message.dstId = DST_ID;
    message.len = size;
    message.timeusec = TICK_GetCount();
    
    return message.data;
}
//?????????? ??????? ????????? ??????????. ?????????? ??????? ? ???????, ????????? ????????? BatteryInfo
void BatteryInfo()
{
  //????? ???????? ? ??????
  HeaderBegin* begin;
  PayloadBatteryInfo& info = *(PayloadBatteryInfo*) CreateMessageHeaders(begin, sizeof(PayloadBatteryInfo), MESSAGES_ID::BatteryInfo);
  
  info.perCharge = 95;
  info.voltage = 14.5f;
  info.amperage = 20.8f;
  info.temp = 36.6f;
  info.totalPower = 100.0f;
  info.timeRemaining = 3600;
  //printf("MessageTelemetry: BatteryInfo -> Send\n");
  SendMes(begin);
}

NAV_SYS_MODE PROT_Nav=NAV_SYS_MODE::gps;

//?????????? ??????? ????????? ??????????. ?????????? ??????? ? ???????, ????????? ????????? SysInfo
void SysInfo()
{
    //????? ???????? ? ??????
    unsigned int count_engine = 4;
    HeaderBegin* begin;
    PayloadSysInfo& info = *(PayloadSysInfo*)CreateMessageHeaders(begin, sizeof(PayloadSysInfo) + count_engine * sizeof(EnginePowerInfo), MESSAGES_ID::SysInfo);

    info.mode = Main_SysMode;   //  !!!!!!!!!!!!
    info.navSys = PROT_Nav;
    info.pressureBaro = DataORI.Bar.Bar;
    info.statusMode = Main_StatusMode;   // !!!!!!!!!!
    info.engineStatus = PWM_Enable ? ENGINE_STATUS::enable : ENGINE_STATUS::disable;   //  !!!!!!!!!!!
    info.engineCount = count_engine;
    info.tempBaro = DataORI.Temp.Bar;

    for (int i = 0; i < info.engineCount; i++)
    {
      EnginePowerInfo& pow = info.enginePower[i];
      pow.power = 50;
      pow.current = 1.0;
      pow.engineSpeed = 100;
      pow.voltage = 12.5;
      pow.temp = 34.5;
    }
    //printf("MessageTelemetry: SysInfo -> Send\n");
    
    SendMes(begin);
}
//?????????? ??????? ????????? ??????????. ?????????? ??????? ? ???????, ????????? ????????? GyroInfo
void GyroInfo()
{
    //????? ???????? ? ??????
    HeaderBegin* begin;
    PayloadGyroInfo& info = *(PayloadGyroInfo*)CreateMessageHeaders(begin, sizeof(PayloadGyroInfo), MESSAGES_ID::GyroInfo);

    info.pitchGyroVel = DataORI.Gyr.X;
    info.rollGyroVel = DataORI.Gyr.Y;
    info.yawGyroVel = DataORI.Gyr.Z;
    //printf("MessageTelemetry: GyroInfo -> Send\n");
    SendMes(begin);
}
//?????????? ??????? ????????? ??????????. ?????????? ??????? ? ???????, ????????? ????????? AccelInfo
void AccelInfo()
{
    //????? ???????? ? ??????
    HeaderBegin* begin;
    PayloadAccelInfo& info = *(PayloadAccelInfo*)CreateMessageHeaders(begin, sizeof(PayloadAccelInfo), MESSAGES_ID::AccelInfo);

    info.aX = DataORI.Acc.X;
    info.aY = DataORI.Acc.Y;
    info.aZ = DataORI.Acc.Z;
    info.pitchAccelVel = 0.0;
    info.rollAccelVel = 0.0;
    info.yawAccelVel = 0.0;
    info.tempAccel = DataORI.Temp.Acc;
    //printf("MessageTelemetry: AccelInfo -> Send\n");
    SendMes(begin);
}

static GPS_BaseInfo Main_GPS;

//?????????? ??????? ????????? ??????????. ?????????? ??????? ? ???????, ????????? ????????? GpsInfo
void GpsInfo()
{
    
    GPS_GetBaseInfo(Main_GPS);
  
    //????? ???????? ? ??????
    HeaderBegin* begin;
    PayloadGpsInfo& info = *(PayloadGpsInfo*)CreateMessageHeaders(begin, sizeof(PayloadGpsInfo), MESSAGES_ID::GpsInfo);

    info.lat = Main_GPS.lat;
    info.lon = Main_GPS.lon;
    info.absAlt = Main_GPS.absAlt;
    info.realAlt = Main_GPS.realAlt;
    info.fixType = Main_GPS.fixType;
    info.hdop = Main_GPS.hdop;
    info.pdop = Main_GPS.pdop;
    info.vdop = Main_GPS.vdop;
    info.satUsed = Main_GPS.satUsed;
    info.satVisible = Main_GPS.satVisible;
    info.noise = Main_GPS.noise;
    info.jamming = 0.0;
    info.speed = Main_GPS.speed;
    info.timeUTC = Main_GPS.timeUTC;
    //printf("MessageTelemetry: GpsInfo -> Send\n");
    SendMes(begin);
}

extern Vec3 test_iner;

//?????????? ??????? ????????? ??????????. ?????????? ??????? ? ???????, ????????? ????????? InertialInfo
void InertialInfo()
{
    //????? ???????? ? ??????
    HeaderBegin* begin;
    PayloadInertialInfo& info = *(PayloadInertialInfo*)CreateMessageHeaders(begin, sizeof(PayloadInertialInfo), MESSAGES_ID::InertialInfo);

    info.x = test_iner.x;
    info.y = test_iner.y;
    info.z = test_iner.z;
    info.baroAlt = DataORI.Bar.Alt;
    info.pitch = DataORI.Pitch;
    info.yaw = DataORI.Yaw;
    info.roll = DataORI.Roll;
    info.headingDeg = DataORI.Yaw;
    info.speed = Main_GPS.speed;
    info.pitchVel = 1.0;
    info.yawVel = 1.0;
    info.rollVel = 1.0;
    //printf("MessageTelemetry: InertialInfo -> Send\n");
    SendMes(begin);
}
//????????? ????? ?? ??????? ?? ???????? ???????? ??????
void CommandAck(PayloadCommandAck& ack)
{
    HeaderBegin* begin;
    PayloadCommandAck& info = *(PayloadCommandAck*)CreateMessageHeaders(begin, sizeof(PayloadCommandAck), MESSAGES_ID::Ack);
    info = ack;
    //printf("MessageCommandACK: Send\n");
    SendMes(begin);
}
//?????????? ???????? ???????????
static void ProcessingCommandLand()
{
    MainFlyMode=MAIN_FLY_MODE::STOP;
    
    // ????????? ???????...
    PayloadCommandAck info;
    info.commandId = COMMANDS_NAME::Land;
    info.errorCode = ERROR_CODE_COMMAND::No_error;
    info.status = FEASIBILITY_CODE_COMMAND::accepted;
    CommandAck(info);

}
//?????????? ???????? ????????? ?????? (?????????)
static void ProcessingCommandPause()
{
    // ????????? ???????...
    PayloadCommandAck info;
    info.commandId = COMMANDS_NAME::Pause;
    info.errorCode = ERROR_CODE_COMMAND::No_error;
    info.status = FEASIBILITY_CODE_COMMAND::accepted;
    CommandAck(info);

}
//?????????? ???????? ???????? ? ????? ??????
static void ProcessingCommandGoHome()
{
    // ????????? ???????...
    PayloadCommandAck info;
    info.commandId = COMMANDS_NAME::GoHome;
    info.errorCode = ERROR_CODE_COMMAND::No_error;
    info.status = FEASIBILITY_CODE_COMMAND::accepted;
    CommandAck(info);

}
//?????????? ???????? ??????????? ??????
static void ProcessingCommandContinue()
{
    // ????????? ???????...
    PayloadCommandAck info;
    info.commandId = COMMANDS_NAME::Continue;
    info.errorCode = ERROR_CODE_COMMAND::No_error;
    info.status = FEASIBILITY_CODE_COMMAND::accepted;
    CommandAck(info);

}
//?????????? ???????? ??????? ???????
static void ProcessingCommandStartEngine()
{
    if(MainFlyMode!=MAIN_FLY_MODE::STOP) MainFlyMode=MAIN_FLY_MODE::START;
  
    // ????????? ???????...
    PayloadCommandAck info;
    info.commandId = COMMANDS_NAME::StartEngine;
    info.errorCode = ERROR_CODE_COMMAND::No_error;
    info.status = FEASIBILITY_CODE_COMMAND::accepted;
    CommandAck(info);

}
//?????????? ???????? ????????? ???????
static void ProcessingCommandStopEngine()
{
    MainFlyMode=MAIN_FLY_MODE::WAIT;
  
    // ????????? ???????...
    PayloadCommandAck info;
    info.commandId = COMMANDS_NAME::StopEngine;
    info.errorCode = ERROR_CODE_COMMAND::No_error;
    info.status = FEASIBILITY_CODE_COMMAND::accepted;
    CommandAck(info);

}
//?????????? ???????? ????????? ???????? ??????
static void ProcessingCommandChangeSpeed(PayloadCommandChangeSpeed* payload)
{
    // ????????? ???????...
    //payload->speed;

    PayloadCommandAck info;
    info.commandId = COMMANDS_NAME::ChangeSpeed;
    info.errorCode = ERROR_CODE_COMMAND::No_error;
    info.status = FEASIBILITY_CODE_COMMAND::accepted;
    CommandAck(info);

}
//?????????? ???????? ????? ????????????? ???????
static void ProcessingCommandChangeNav(PayloadCommandChangeNav* payload)
{
    PROT_Nav=(NAV_SYS_MODE)payload->nav_id;
    // ????????? ???????...
    //payload->nav_id;
    PayloadCommandAck info;
    info.commandId = COMMANDS_NAME::ChangeNav;
    info.errorCode = ERROR_CODE_COMMAND::No_error;
    info.status = FEASIBILITY_CODE_COMMAND::accepted;
    CommandAck(info);
}
//?????????? ???????? ??????????? ? ????? ?? ?????????? ???????????
static void ProcessingCommandGoToGlobal(PayloadCommandGoToGlobal* payload)
{
    PointGPS.Latitude=payload->lat;
    PointGPS.Longitude=payload->lon;
    PointGPS.Altitude=payload->abs_alt;
   
    if(MainFlyMode!=MAIN_FLY_MODE::STOP) MainFlyMode=MAIN_FLY_MODE::FLY;
  
    // ????????? ???????...
    PayloadCommandAck info;
    info.commandId = COMMANDS_NAME::GoToGlobal;
    info.errorCode = ERROR_CODE_COMMAND::No_error;
    info.status = FEASIBILITY_CODE_COMMAND::accepted;
    CommandAck(info);

}
//?????????? ???????? ??????????? ? ????? ?? ????????? ???????????
static void ProcessingCommandGoToLocal(PayloadCommandGoToLocal* payload)
{
    // ????????? ???????...
    PayloadCommandAck info;
    info.commandId = COMMANDS_NAME::GoToLocal;
    info.errorCode = ERROR_CODE_COMMAND::No_error;
    info.status = FEASIBILITY_CODE_COMMAND::accepted;
    CommandAck(info);

}

//
static void ProcessingCommandSetParameter(PayloadCommandSetParameter* payload)
{
    unsigned char cat_id = payload->cat_id; // ID ?????????
    unsigned char param_id = payload->param_id; // ID ?????????
    DATA_TYPES param_type = payload->param_type_code; // ??? ??????
    unsigned char param_len = payload->param_len; // ?????? ??????? ??????
    //payload->param_data; // ?????? ???? ?????????

    PayloadCommandAck info;
    info.commandId = COMMANDS_NAME::SetParameter;
    info.status = FEASIBILITY_CODE_COMMAND::accepted;
    info.errorCode = ERROR_CODE_COMMAND::No_error;
    CommandAck(info);
}

//????? ?????????? ????????? ? ????????
static void ProcessingCommand(CommandObj* commandObject) {
    switch (commandObject->commandId)
    {
        case COMMANDS_NAME::Land: ProcessingCommandLand(); return;
        case COMMANDS_NAME::Pause: ProcessingCommandPause(); return;
        case COMMANDS_NAME::GoHome: ProcessingCommandGoHome(); return;
        case COMMANDS_NAME::Continue: ProcessingCommandContinue(); return;
        case COMMANDS_NAME::StartEngine: ProcessingCommandStartEngine(); return;
        case COMMANDS_NAME::StopEngine: ProcessingCommandStopEngine(); return;
        case COMMANDS_NAME::ChangeSpeed: ProcessingCommandChangeSpeed((PayloadCommandChangeSpeed*)commandObject->data); return;
        case COMMANDS_NAME::ChangeNav: ProcessingCommandChangeNav((PayloadCommandChangeNav*)commandObject->data); return;
        case COMMANDS_NAME::GoToGlobal: ProcessingCommandGoToGlobal((PayloadCommandGoToGlobal*)commandObject->data); return;
        case COMMANDS_NAME::GoToLocal: ProcessingCommandGoToLocal((PayloadCommandGoToLocal*)commandObject->data); return;
        case COMMANDS_NAME::SetParameter: ProcessingCommandSetParameter((PayloadCommandSetParameter*)commandObject->data); return;
    }
}
//?????????? ????????? ???????????? ?????
static void ConnectionTest()
{
    HeaderBegin* begin;
    CreateMessageHeaders(begin, sizeof(PayloadBatteryInfo), MESSAGES_ID::ConnectionTest);

    begin->SetCRC();

    UART2_Send(begin, sizeof(HeaderBegin) + begin->len1);
}
//?????????? ????????? ??????? ??????? ???????
static void StatusCommand(PayloadStatusCommand* payload) 
{
    // payload->commandId; ??? ?????? ???? ?????-?? ???????? ???????? ????????? ?????????? ??????? ?? ?? ID
    HeaderBegin* begin;
    PayloadStatusCommand& info = *(PayloadStatusCommand*)CreateMessageHeaders(begin, sizeof(PayloadStatusCommand), MESSAGES_ID::StatusCommand);
    info.commandId = payload->commandId;
    info.executionCode = EXEC_CODE_COMMAND::completed;
    info.errorCode = ERROR_CODE_COMMAND::No_error;
    SendMes(begin);
}

static void CountMenuCategories() 
{
    //
    unsigned char count_categories = 4;
    unsigned char categoriesNamesLen[4] = { 5, 6, 5, 7 };
    HeaderBegin* begin;
    PayloadCountMenuCategories& info = *(PayloadCountMenuCategories*)CreateMessageHeaders(begin, sizeof(PayloadCountMenuCategories) + count_categories * sizeof(unsigned char), MESSAGES_ID::CountMenuCategories);

    info.countCategories = count_categories;
    for (int i = 0; i < info.countCategories; i++) {
        info.data[i] = categoriesNamesLen[i];
    }
    //printf("MessageTelemetry: SysInfo -> Send\n");
    SendMes(begin);
}

static void MenuCategories(PayloadMenuCategories* payload)
{
    const char* names[4] = { "Main1","PID_S1","MOTOR", "ACCELER"}; // ????? ?????????
    unsigned char categoriesNamesLen[4] = { 5, 6, 5, 7 }; // ????? ???? ????????? (????? ? ????????? utf-8)
    unsigned char req_count = payload->countCategories;
    unsigned char total_len_names = 0;
    for (int i = 0; i < req_count; i++) 
    {
        unsigned char cat_id = (payload->categories)[i];
        unsigned char cat_name_len = categoriesNamesLen[(payload->categories)[i]];
        total_len_names += cat_name_len;
    }
    HeaderBegin* begin;
    PayloadMenuCategories& info = *(PayloadMenuCategories*)CreateMessageHeaders(begin, sizeof(PayloadMenuCategories) + req_count * sizeof(MenuCategori) + total_len_names * sizeof(unsigned char), MESSAGES_ID::CategoriesMenu);
    info.countCategories = req_count;
    MenuCategori* menu = (MenuCategori*)info.categories;
    for (int i = 0; i < req_count; i++) 
    {
        menu->id = (payload->categories)[i];
        menu->count_param = 1;
        menu->len_name = categoriesNamesLen[menu->id];
        memcpy(menu->name, names[menu->id], menu->len_name);
        menu = (MenuCategori*)(((char*)menu) + sizeof(MenuCategori) + menu->len_name);
    }
    SendMes(begin);
}

static void CategoriesParameters(PayloadCategoriesParameters* payload)
{
    unsigned char cat_id = payload->cat_id; // id ????????? ??? ?????? ?????????
    unsigned char param_id = payload->param_id; // id ????????? ??? ?????? ?????????
    // ????????? ?????????
    const char* name = "PARAMETER1";
    unsigned char len_name = 10;
    float param1 = 0.231;
    DATA_TYPES param_type = DATA_TYPES::dt_float_32;
    //
    
    HeaderBegin* begin;
    PayloadCategoriesParameters& info = *(PayloadCategoriesParameters*)CreateMessageHeaders(begin, sizeof(PayloadCategoriesParameters) + len_name + sizeof(param1), MESSAGES_ID::CategoriesParameters);
    info.cat_id = cat_id;
    info.param_id = param_id;
    info.param_name_len = len_name;
    info.param_len = sizeof(param1);
    info.param_type_code = param_type;
    memcpy(info.param_data, name, len_name);
    memcpy(&info.param_data[len_name], &param1, sizeof(param1));
    SendMes(begin);
}

//void Ping(HeaderMessages* message)
//{
//  HeaderBegin* begin;
//  CreateMessageHeaders(begin, sizeof(PayloadBatteryInfo), MESSAGES_ID::Ping);
//  //---
//  SendMes(begin);
//  //---
//  printf("Ping %d Pong %d \n", message->timeusec, ((HeaderMessages*)begin->data)->timeusec);
//}
//
//void Pong(HeaderMessages* message)
//{
//  HeaderBegin* begin;
//  CreateMessageHeaders(begin, sizeof(PayloadBatteryInfo), MESSAGES_ID::Pong);
//  //---
//  SendMes(begin);
//  //---
//  printf("Pong %d Ping %d \n", message->timeusec, ((HeaderMessages*)begin->data)->timeusec);
//}

static void ProcessingMessage(HeaderMessages* message)
{
  switch (message->msgId)
  {
    /*case MESSAGES_ID::Ping: Pong(message); return;
    case MESSAGES_ID::Pong: Ping(message); return;*/
    case MESSAGES_ID::BatteryInfo: BatteryInfo(); return;
    case MESSAGES_ID::GyroInfo: GyroInfo(); return;
    case MESSAGES_ID::AccelInfo: AccelInfo(); return;
    case MESSAGES_ID::GpsInfo: GpsInfo(); return;
    case MESSAGES_ID::InertialInfo: InertialInfo(); return;
    case MESSAGES_ID::SysInfo: SysInfo(); return;
    case MESSAGES_ID::Command: ProcessingCommand((CommandObj*)message->data); return;
    case MESSAGES_ID::ConnectionTest: ConnectionTest(); return;
    case MESSAGES_ID::StatusCommand: StatusCommand((PayloadStatusCommand*)message->data); return;
    
    case MESSAGES_ID::CountMenuCategories: CountMenuCategories(); return;
    case MESSAGES_ID::CategoriesMenu: MenuCategories((PayloadMenuCategories*)message->data); return;
    case MESSAGES_ID::CategoriesParameters: CategoriesParameters((PayloadCategoriesParameters*)message->data); return;
  }
}

//void TestPing()
//{
//  HeaderMessages message;
//  message.timeusec = GetTickCount();
//  Pong(&message);
//}

void PROT_Update()
{
  char byte;
  unsigned char size = UART2_Recv(&byte, 1);
  if (size) MainUpdate(byte);
}