#include "crsf.hpp"
#include <algorithm>

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

uint32_t crsfLastUpdateTime;
constexpr uint32_t crsfSignalLoseTimeout = 100;

void CRSF_Init(void)
{
  UART2_Init(CRSF_BAUDRATE);
  crsfLastUpdateTime = 0;
}

bool CRSF_Update(CRSF_Data& data, bool& off)
{
  uint8_t buf[CRSF_FRAME_SIZE_MAX + 1];
  unsigned long size = UART2_Recv(buf, sizeof(buf));
  if(size == 0)
    return false;
  
  uint16_t num;
  bool isUpdated = crsf_parse(buf, size, reinterpret_cast<uint16_t*>(&data), &num, 18);
  
  if(isUpdated)
    crsfLastUpdateTime = TICK_GetCount();
  else if(TICK_GetCount() - crsfLastUpdateTime > crsfSignalLoseTimeout)
  {
    off = true;
    return true;
  }
  else
    off = false;
  
  return isUpdated;
}