#include <string.h>

#include "gpio.h"

#include "uart.h"
#include "tick.h"

#include "tele.h"

enum class RecvModeEnum : unsigned char { Begin, Head, Titles, Data, Auto, SetPID, GetPID, Done };

struct RecvHead
{
  RecvHead(){};
  RecvHead(RecvModeEnum mode, unsigned char size, unsigned char crc)
  {
    Mode = mode;
    DataSize = size;
    DataCRC8 = crc;
    CRC8 = (unsigned char)((unsigned char)Mode ^ DataSize ^ DataCRC8);
  }

  RecvModeEnum Mode;
  unsigned char DataSize;
  unsigned char DataCRC8;

  unsigned char CRC8;
  
  bool Check()
  {
    return CRC8 == (unsigned char)((unsigned char)Mode ^ DataSize ^ DataCRC8);
  }
};

unsigned char GetCRC8(const void* arr, int size)
{
  unsigned char crc = 0;
  for (int a = 0; a < size; a++) crc ^= ((unsigned char*)arr)[a];
  return crc;
}

static RecvHead Tele_Mode(RecvModeEnum::Begin,0,0);

void TELE_Init()
{
  UART3_Init(57600);
}

static unsigned long TELE_LastTime=0;
static unsigned long TELE_WaitTimer=0;

static unsigned long TELE_Count=0;

static bool DataAuto=true;

void TELE_Update(const void* info, unsigned long size, unsigned long update)
{
  
  switch(Tele_Mode.Mode)
  {
    case RecvModeEnum::Begin:
    {
      unsigned char begin;
      int len=UART3_Recv(&begin, 1);
      TELE_WaitTimer=TICK_GetCount();
      if(len && begin==0xAA) Tele_Mode.Mode=RecvModeEnum::Head;
      break;
    }
    
    case RecvModeEnum::Head:
    {
      int len=UART3_Recv(0, 0);
      if(len<sizeof(RecvHead)) break;
      
      RecvHead head;
      UART3_Recv(&head, sizeof(RecvHead));
      
      if(!head.Check()) Tele_Mode.Mode=RecvModeEnum::Begin;
      else Tele_Mode=head;
      
      break;
    }
    
    case RecvModeEnum::Titles:
    {
      const char* titles="Acc X|AccY|Acc Z|Acc S|Gyr X|Gyr Y|Gyr Z|Pitch|Roll|Yaw";
      int len=strlen(titles);
      
      unsigned char h=0xAA;
      UART3_Send(&h, 1);
      
      RecvHead head(RecvModeEnum::Titles, len, GetCRC8(titles, len));
      UART3_Send(&head, sizeof(head));
      UART3_Send(titles, len);
      
      Tele_Mode.Mode=RecvModeEnum::Begin;
      
      break;
    }
    
    /*case RecvModeEnum::GetPID:
    {
      float send[20]={PID_X.Min, PID_X.Max, 
      
      PID_X.P.Min, PID_X.P.Max, PID_X.P.C,
      PID_X.I.Min, PID_X.I.Max, PID_X.I.C,
      PID_X.D.Min, PID_X.D.Max, PID_X.D.C,
      
      PID_Z.P.Min, PID_Z.P.Max, PID_Z.P.C,
      PID_Z.I.Min, PID_Z.I.Max, PID_Z.I.C,
      PID_Z.D.Min, PID_Z.D.Max, PID_Z.D.C
      };
      
      unsigned char h=0xAA;
      UART3_Send(&h, 1);
      
      RecvHead head(RecvModeEnum::GetPID, sizeof(send), GetCRC8(send, sizeof(send)));
      UART3_Send(&head, sizeof(head));
      UART3_Send(send, sizeof(send));
      
      Tele_Mode.Mode=RecvModeEnum::Begin;
      
      break;
    }*/
    
    case RecvModeEnum::Data:
    {
      unsigned char h=0xAA;
      UART3_Send(&h, 1);
      
      RecvHead head(RecvModeEnum::Data, size, GetCRC8(info, size));
      UART3_Send(&head, sizeof(head));
      UART3_Send(info, size);
      
      Tele_Mode.Mode=RecvModeEnum::Begin;
      
      break;
    }
    
    /*case RecvModeEnum::SetPID:
    {
      float recv[20];
      
      unsigned long len=UART3_Recv(0, 0);
      if(len<sizeof(recv)) break;
      
      UART3_Recv(recv, sizeof(recv));
      
      unsigned char crc=GetCRC8(recv, sizeof(recv));
      if(Tele_Mode.DataCRC8!=crc) break;
      
      PID_X.Min=recv[0]; PID_X.Max=recv[1];
      
      PID_X.P.Min=recv[2]; PID_X.P.Max=recv[3]; PID_X.P.C=recv[4];
      PID_X.I.Min=recv[5]; PID_X.I.Max=recv[6]; PID_X.I.C=recv[7];
      PID_X.D.Min=recv[8]; PID_X.D.Max=recv[9]; PID_X.D.C=recv[10];
      
      PID_Z.P.Min=recv[11]; PID_Z.P.Max=recv[12]; PID_Z.P.C=recv[13];
      PID_Z.I.Min=recv[14]; PID_Z.I.Max=recv[15]; PID_Z.I.C=recv[16];
      PID_Z.D.Min=recv[17]; PID_Z.D.Max=recv[18]; PID_Z.D.C=recv[19];
      
      unsigned char h=0xAA;
      UART3_Send(&h, 1);
      
      RecvHead head(RecvModeEnum::Done, 0, 0);
      UART3_Send(&head, sizeof(head));
      
      Tele_Mode.Mode=RecvModeEnum::Begin;
      
      break;
    }*/
    
    default:
    {
      Tele_Mode.Mode=RecvModeEnum::Begin;
      break;
    }
  }
  
  if(TICK_GetCount()-TELE_WaitTimer>100) Tele_Mode.Mode=RecvModeEnum::Begin;
  
  if(DataAuto && (TICK_GetCount()-TELE_LastTime>update))
  {
    TELE_LastTime=TICK_GetCount();
    
    unsigned char h=0xAA;
    UART3_Send(&h, 1);
    
    RecvHead head(RecvModeEnum::Data, size, GetCRC8(info, size));
    UART3_Send(&head, sizeof(head));
    UART3_Send(info, size);
  }
}