#include "stm32g4xx.h"

#include <string.h>

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

#include "sbus.h"

#pragma pack(push,1)
union SBUS_Struct
{
  unsigned char data[25];
  struct
  {
    unsigned start : 8;
    unsigned ch1 : 11;
    unsigned ch2 : 11;
    unsigned ch3 : 11;
    unsigned ch4 : 11;
    unsigned ch5 : 11;
    unsigned ch6 : 11;
    unsigned ch7 : 11;
    unsigned ch8 : 11;
    unsigned ch9 : 11;
    unsigned ch10 : 11;
    unsigned ch11 : 11;
    unsigned ch12 : 11;
    unsigned ch13 : 11;
    unsigned ch14 : 11;
    unsigned ch15 : 11;
    unsigned ch16 : 11;
    unsigned flags : 8;
    unsigned end : 8;
  } bus;
};
#pragma pack(pop)

// IBUS: LEN(1)+CMD(1)+DATA(0..32)+CHSM(2)

#define SBUS_START 0x0F

#define SBUS_CH17 0x01
#define SBUS_CH18 0x02

#define SBUS_FRAMELOST 0x04
#define SBUS_FAILSAFE 0x08

#define SBUS_LENGTH 25

void SBUS_Init()
{
  UART1_Init(100'000);
  USART1->CR1 = USART_CR1_TE | USART_CR1_RE | USART_CR1_RXNEIE;
  USART1->CR2 = USART_CR2_STOP_1 | USART_CR2_RXINV;
  USART1->CR1 |= USART_CR1_PCE | USART_CR1_PS | USART_CR1_UE;
}

static const unsigned long Size = SBUS_LENGTH;
static char Buffer[Size];

static char Length = 0;

static unsigned long Time;

static bool Parse(SBUS_Data& Data, char byte)
{
  unsigned long tick=TICK_GetCount();
  
  unsigned long wait = tick - Time;
  if (wait > 4) Length = 0; // Protocol synchronization lost !!!

  Time=tick;
  
  if (!Length && (byte != SBUS_START)) return false;

  Buffer[Length++] = byte;
  
  if(Length<Size) return false;
  Length=0;
  
  SBUS_Struct* frame=(SBUS_Struct*)Buffer;
  if(frame->bus.end!=0) return false;
  
  Data.X=frame->bus.ch1;
  Data.Y=frame->bus.ch2;
  Data.Z=frame->bus.ch3;
  Data.W=frame->bus.ch4;
  
  Data.SWA=frame->bus.ch5;
  Data.SWB=frame->bus.ch6;
  Data.SWC=frame->bus.ch7;
  Data.SWD=frame->bus.ch8;
  
  Data.VRA=frame->bus.ch9;
  Data.VRB=frame->bus.ch10;
  
  Data.OTHER[0]=frame->bus.ch11;
  Data.OTHER[1]=frame->bus.ch12;
  Data.OTHER[2]=frame->bus.ch13;
  Data.OTHER[3]=frame->bus.ch14;
  Data.OTHER[4]=frame->bus.ch15;
  Data.OTHER[5]=frame->bus.ch16;
  
  Data.OTHER[6]=(bool)(frame->bus.flags & SBUS_CH17);
  Data.OTHER[7]=(bool)(frame->bus.flags & SBUS_CH18);
  
  Data.FailSafe=frame->bus.flags & SBUS_FAILSAFE;
  Data.FrameLost=frame->bus.flags & SBUS_FRAMELOST;
  
  return true;
}

bool SBUS_Update(SBUS_Data& Data, bool& Off)
{
  char buf[Size];
  unsigned long size = UART1_Recv(buf, Size);

  bool done = false;
  for (long a = 0; a < size; a++) done = Parse(Data, buf[a]);
  Off=Data.FailSafe;
  return done;
}