#include <string.h>

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

#include "ibus.h"

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

#define IBUS_LEN_MAX 32 // DATA(0..32)
#define IBUS_LEN_PRO 3  // CMD(1)+CHSM(2)

void IBUS_Init(unsigned long Baud)
{
  UART2_Init(Baud);
}

static const unsigned long Size = IBUS_LEN_MAX + IBUS_LEN_PRO;
static char Buffer[Size];

static char Pos = 0, Len = 0;
static unsigned long CheckSum;

static unsigned long Time;

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

  if (!Len)
  {
    if (byte > IBUS_LEN_MAX || byte < IBUS_LEN_PRO) return false;
    Pos = 0;
    Len = byte - 1;
    CheckSum = 0xFFFFUL - byte;
    Time = TICK_GetCount();
    return false;
  }

  Buffer[Pos++] = byte;
  if (Pos <= Len - 2) CheckSum -= byte;

  if (Pos < Len) return false;

  unsigned long len = Len;
  Len = 0;

  unsigned short chsm = *(unsigned short*)(Buffer + (len - 2));

  if (chsm != CheckSum) return false;

  switch (Buffer[0])
  {
  case 0x40:
  {
    memcpy(&Data, Buffer + 1, len - 3 > sizeof(Data) ? sizeof(Data) : len - 3);
    return true;
  }
  }
  return false;
}

bool IBUS_Update(IBUS_Data& Data, bool& Off)
{
  char buf[Size];
  unsigned long size = UART2_Recv(buf, Size);

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