#include "radio_receiver.h"


volatile uint16_t sbus_channels[16] = {0};
volatile uint8_t sbus_buffer[SBUS_FRAME_SIZE] = {0};
volatile uint8_t sbus_index = 0;
volatile uint8_t sbus_failsafe = 0;
volatile uint8_t sbus_frame_lost = 0;
volatile uint8_t sbus_frame_ready = 0;

void receiver_gpio_init()
{
  RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
  
  GPIOA->MODER &= ~(3 << (3 * 2));
  GPIOA->MODER |= 2 << (3 * 2);
  
  GPIOA->AFR[0] &= ~(0xF << (3 * 4));
  GPIOA->AFR[0] |= 12 << (3 * 4);
  
  // pull-up
  GPIOA->PUPDR &= ~(3 << (3 * 2));
  GPIOA->PUPDR |= 1 << (3 * 2);
  
}

void receiver_lpuart_clock_init()
{
  RCC->CCIPR &= ~(RCC_CCIPR_LPUART1SEL);
  RCC->CCIPR |= 1 << RCC_CCIPR_LPUART1SEL_Pos;
  RCC->APB1ENR2 |= RCC_APB1ENR2_LPUART1EN;
}

void receiver_uart_init()
{
  receiver_lpuart_clock_init();
  
  LPUART1->CR1 = 0;
  LPUART1->CR2 = 0;
  LPUART1->CR3 = 0;
  
  LPUART1->BRR = (256 * 16000000UL) / 100000UL;
  
  // parity control enable
  LPUART1->CR1 |= USART_CR1_PCE | USART_CR1_M0;
  
  // word length M = 01 - 9 bit
  LPUART1->CR1 &= ~USART_CR1_M1;
  LPUART1->CR1 |= USART_CR1_M0;
  
  // even parity
  LPUART1->CR1 &= ~USART_CR1_PS;
  
  // 2 stop bits
  LPUART1->CR2 &= ~USART_CR2_STOP;
  LPUART1->CR2 |= 2 << USART_CR2_STOP_Pos;
  
  // invertion enabled
  LPUART1->CR2 |= USART_CR2_RXINV;
  
  // receiver enable
  // interrupt generated whenever ORE = 1 or RXNE = 1
  LPUART1->CR1 |= USART_CR1_RE | USART_CR1_RXNEIE;
  
  // uart enable
  LPUART1->CR1 |= USART_CR1_UE;
  
  NVIC_EnableIRQ(LPUART1_IRQn);
}

void receiver_init()
{
  receiver_gpio_init();
  receiver_uart_init();
}

void LPUART1_IRQHandler()
{ 
  if (LPUART1->ISR & USART_ISR_RXNE)
  {
    uint8_t b = LPUART1->RDR;
    
    if (b == SBUS_START_BYTE)
      sbus_index = 0;
    
    if (sbus_index < SBUS_FRAME_SIZE)
      sbus_buffer[sbus_index++] = b;
    
    if (sbus_index == SBUS_FRAME_SIZE)
    {
      sbus_index = 0;
      sbus_frame_ready = 1;
    }
  }
}

void receiver_update(rc_channels* chs)
{
  if (!sbus_frame_ready)
    return;
    
  sbus_frame_ready = 0;
  
  if (sbus_buffer[0] != SBUS_START_BYTE)
    return;
  
  sbus_failsafe   = sbus_buffer[23] & (1 << 3);
  sbus_frame_lost = sbus_buffer[23] & (1 << 2);
  
  if (sbus_failsafe || sbus_frame_lost)
    return;
      
  receiver_parse_frame();
  
  chs->rc_roll = sbus_channels[0];
  chs->rc_pitch = sbus_channels[1];
  chs->rc_throttle = sbus_channels[2];
  chs->rc_armed = sbus_channels[4];
}

void receiver_parse_frame()
{
  uint16_t b[22];
  
  for (uint8_t i = 0; i < 22; ++i)
    b[i] = sbus_buffer[i + 1];

  sbus_channels[0]  = ( b[0]        | (b[1] << 8) )                  & 0x07FF;
  sbus_channels[1]  = ( (b[1] >> 3) | (b[2] << 5) )                  & 0x07FF;
  sbus_channels[2]  = ( (b[2] >> 6) | (b[3] << 2) | (b[4] << 10) )   & 0x07FF;
  sbus_channels[3]  = ( (b[4] >> 1) | (b[5] << 7) )                  & 0x07FF;
  sbus_channels[4]  = ( (b[5] >> 4) | (b[6] << 4) )                  & 0x07FF;
  sbus_channels[5]  = ( (b[6] >> 7) | (b[7] << 1) | (b[8] << 9) )    & 0x07FF;
  sbus_channels[6]  = ( (b[8] >> 2) | (b[9] << 6) )                  & 0x07FF;
  sbus_channels[7]  = ( (b[9] >> 5) | (b[10] << 3) )                 & 0x07FF;

  sbus_channels[8]  = ( b[11]       | (b[12] << 8) )                 & 0x07FF;
  sbus_channels[9]  = ( (b[12] >> 3)| (b[13] << 5) )                 & 0x07FF;
  sbus_channels[10] = ( (b[13] >> 6)| (b[14] << 2) | (b[15] << 10) ) & 0x07FF;
  sbus_channels[11] = ( (b[15] >> 1)| (b[16] << 7) )                 & 0x07FF;
  sbus_channels[12] = ( (b[16] >> 4)| (b[17] << 4) )                 & 0x07FF;
  sbus_channels[13] = ( (b[17] >> 7)| (b[18] << 1) | (b[19] << 9) )  & 0x07FF;
  sbus_channels[14] = ( (b[19] >> 2)| (b[20] << 6) )                 & 0x07FF;
  sbus_channels[15] = ( (b[20] >> 5)| (b[21] << 3) )                 & 0x07FF;

  sbus_frame_lost = sbus_buffer[23] & (1 << 2);
  sbus_failsafe   = sbus_buffer[23] & (1 << 3);
}

rc_channels normalize_channels(rc_channels chs)
{
  chs.rc_roll = int_mapping(chs.rc_roll, 240, 1807, -500, 500);
  chs.rc_pitch = int_mapping(chs.rc_pitch, 240, 1807, -500, 500);
  chs.rc_throttle = int_mapping(chs.rc_throttle, 240, 1807, 1000, 2000);
  //chs.rc_yaw = int_mapping(chs.rc_yaw, 240, 1807, -10, 10);
  chs.rc_armed = bool_mapping_gt(chs.rc_armed, 1500);
  
  return chs;
}

int16_t int_mapping(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max)
{
  return out_min + (x - in_min) * (out_max - out_min) / (in_max - in_min);
}

int8_t bool_mapping_gt(int16_t x, int16_t boundary)
{
  return x >= boundary;
}

//------------------------------------------------------------------------------

void toggle_led()
{
    if (GPIOA->ODR & (1 << 15)) 
    {
        GPIOA->BSRR = 1 << (15 + 16);
    }
    else
    {
        GPIOA->BSRR = 1 << 15;
    }
}

void led_init(void)
{
    /* Enable GPIOA clock */
    RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;

    /* PA15 -> Output mode */
    GPIOA->MODER &= ~(3U << (15 * 2));
    GPIOA->MODER |=  (1U << (15 * 2));

    /* Push-pull */
    GPIOA->OTYPER &= ~(1U << 15);

    /* Low speed (?????????? ??? LED) */
    GPIOA->OSPEEDR &= ~(3U << (15 * 2));

    /* No pull-up / pull-down */
    GPIOA->PUPDR &= ~(3U << (15 * 2));

    /* Start with LED OFF */
    GPIOA->BSRR = (1U << (15 + 16));
}