#include <nuttx/config.h>
#include "app_config.h"

#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <limits.h>

#include "chip.h"
#include "stm32.h"
#include <chip/stm32_can.h>
#include "nvic.h"

#include "bl_macros.h"
#include "driver.h"
#include "crc.h"
#include "timer.h"

#include <arch/board/board.h>

#define INAK_TIMEOUT 65535

#define CAN_1MBAUD_SJW 0
#define CAN_1MBAUD_BS1 6u
#define CAN_1MBAUD_BS2 0
#define CAN_1MBAUD_PRESCALER 4u

#define CAN_500KBAUD_SJW 0
#define CAN_500KBAUD_BS1 6u
#define CAN_500KBAUD_BS2 0
#define CAN_500KBAUD_PRESCALER 8u

#define CAN_250KBAUD_SJW 0
#define CAN_250KBAUD_BS1 6u
#define CAN_250KBAUD_BS2 0
#define CAN_250KBAUD_PRESCALER 16u

#define CAN_125KBAUD_SJW 0
#define CAN_125KBAUD_BS1 6u
#define CAN_125KBAUD_BS2 0
#define CAN_125KBAUD_PRESCALER 32u

#define CAN_BTR_LBK_SHIFT 30

#define WAIT_TX_READY_MS ((TIMER_HRT_CYCLES_PER_MS-(TIMER_HRT_CYCLES_PER_MS/4))

/* Number of CPU cycles for a single bit time at the supported speeds */

#define CAN_1MBAUD_BIT_CYCLES   (1*(TIMER_HRT_CYCLES_PER_US))
#define CAN_500KBAUD_BIT_CYCLES (2*(TIMER_HRT_CYCLES_PER_US))
#define CAN_250KBAUD_BIT_CYCLES (4*(TIMER_HRT_CYCLES_PER_US))
#define CAN_125KBAUD_BIT_CYCLES (8*(TIMER_HRT_CYCLES_PER_US))

/* All the mesured samples were off byt 28 counts this implys that
 * the CAN_MSR_RX is sampled on a differnt clock then the CPU */

#define CAN_BAUD_ADJUST 28

#define CAN_BAUD_TIME_IN_MS             200
#define CAN_BAUD_SAMPLES_NEEDED         32
#define CAN_BAUD_SAMPLES_DISCARDED      8

int can_init(can_speed_t speed, uint8_t mode)
{
  int speedndx = speed -1;
  /* 
   * TODO: use full-word writes to reduce the number of loads/stores.
   * 
   * Also consider filter use -- maybe set filters for all the message types we 
   * want. */
   
   
  const uint32_t bitrates[] = {
      (CAN_125KBAUD_SJW << 24) |
      (CAN_125KBAUD_BS1 << 16) |
      (CAN_125KBAUD_BS2 << 20) | (CAN_125KBAUD_PRESCALER - 1),

      (CAN_250KBAUD_SJW << 24) |
      (CAN_250KBAUD_BS1 << 16) |
      (CAN_250KBAUD_BS2 << 20) | (CAN_250KBAUD_PRESCALER - 1),

      (CAN_500KBAUD_SJW << 24) |
      (CAN_500KBAUD_BS1 << 16) |
      (CAN_500KBAUD_BS2 << 20) | (CAN_500KBAUD_PRESCALER - 1),

      (CAN_1MBAUD_SJW << 24) |
      (CAN_1MBAUD_BS1 << 16) |
      (CAN_1MBAUD_BS2 << 20) | (CAN_1MBAUD_PRESCALER - 1)
  };
  /* Remove Unknow Offset */
  if (speedndx < 0 || speedndx > (int)arraySize(bitrates)) {
      return -EINVAL;
  }
  uint32_t timeout;
  /* 
   *  Reset state is 0x0001 0002 CAN_MCR_DBF|CAN_MCR_SLEEP
   *  knock down Sleep and raise CAN_MCR_INRQ
   */

  putreg32(CAN_MCR_DBF | CAN_MCR_INRQ, STM32_CAN1_MCR);

  /* Wait until initialization mode is acknowledged */

  for (timeout = INAK_TIMEOUT; timeout > 0; timeout--)
    {
      if ((getreg32(STM32_CAN1_MSR) & CAN_MSR_INAK) != 0)
        {
          /* We are in initialization mode */

          break;
        }
    }

  if (timeout < 1)
    {
      /* 
       * Initialization failed, not much we can do now other than try a normal
       * startup. */
      return -ETIME;
    }


  putreg32(bitrates[speedndx]| mode << CAN_BTR_LBK_SHIFT, STM32_CAN1_BTR);

  putreg32(CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_DBF, STM32_CAN1_MCR);

  for (timeout = INAK_TIMEOUT; timeout > 0; timeout--)
    {
      if ((getreg32(STM32_CAN1_MSR) & CAN_MSR_INAK) == 0)
        {
          /* We are in initialization mode */

          break;
        }
    }
  if (timeout < 1)
    {
      return -ETIME;
    }

  /* 
   * CAN filter initialization -- accept everything on RX FIFO 0, and only
   * GetNodeInfo requests on RX FIFO 1. */
  putreg32(CAN_FMR_FINIT, STM32_CAN1_FMR);
  putreg32(0, STM32_CAN1_FA1R); /* Disable all filters */
  putreg32(3, STM32_CAN1_FS1R); /* Enable 32-bit mode for filters 0 and 1 */

  /* Filter 0 masks -- data type ID 551 only */
  putreg32(UAVCAN_GETNODEINFO_DTID << 22, STM32_CAN1_FIR(0, 1));
  /* Top 10 bits of ID */
  putreg32(0xFFC00000, STM32_CAN1_FIR(0, 2));

  /* Filter 1 masks -- everything is don't-care */
  putreg32(0, STM32_CAN1_FIR(1, 1));
  putreg32(0, STM32_CAN1_FIR(1, 2));

  putreg32(0, STM32_CAN1_FM1R); /* Mask mode for all filters */
  putreg32(1, STM32_CAN1_FFA1R);        /* FIFO 1 for filter 0, FIFO 0 for the
                                         * rest of filters */
  putreg32(3, STM32_CAN1_FA1R); /* Enable filters 0 and 1 */

  putreg32(0, STM32_CAN1_FMR);  /* Leave init Mode */

  return OK;
}

void can_tx(uint32_t message_id, size_t length,
            const uint8_t * message, uint8_t mailbox)
{
  uint32_t data[2];

  memcpy(data, message, sizeof(data));

  /* 
   * Just block while waiting for the mailbox. Give it an extra 0.75 ms per
   * frame to avoid an issue Ben was seeing with packets going missing on a USBtin. */

  uint32_t mask = CAN_TSR_TME0 << mailbox;
  time_hrt_cycles_t begin = timer_hrt_read();

  while (((getreg32(STM32_CAN1_TSR) & mask) == 0) ||
      timer_hrt_elapsed(begin, timer_hrt_read()) < WAIT_TX_READY_MS));

  putreg32(length & CAN_TDTR_DLC_MASK, STM32_CAN1_TDTR(mailbox));
  putreg32(data[0], STM32_CAN1_TDLR(mailbox));
  putreg32(data[1], STM32_CAN1_TDHR(mailbox));
  putreg32((message_id << CAN_TIR_EXID_SHIFT) | CAN_TIR_IDE | CAN_TIR_TXRQ,
           STM32_CAN1_TIR(mailbox));
}


uint8_t can_rx(uint32_t * out_message_id, size_t * out_length,
               uint8_t * out_message, uint8_t fifo)
{
  uint32_t data[2];
  uint8_t rv = 0;
  const uint32_t fifos[] = { STM32_CAN1_RF0R, STM32_CAN1_RF1R };

  if (getreg32(fifos[fifo & 1]) & CAN_RFR_FMP_MASK)
    {

      rv = 1;
      /* If so, process it */

      *out_message_id =
        getreg32(STM32_CAN1_RIR(fifo) & CAN_RIR_EXID_MASK) >>
        CAN_RIR_EXID_SHIFT;
      *out_length =
        getreg32(STM32_CAN1_RDTR(fifo) & CAN_RDTR_DLC_MASK) >>
        CAN_RDTR_DLC_SHIFT;
      data[0] = getreg32(STM32_CAN1_RDLR(fifo));
      data[1] = getreg32(STM32_CAN1_RDHR(fifo));

      putreg32(CAN_RFR_RFOM, fifos[fifo & 1]);

      memcpy(out_message, data, sizeof(data));
    }

  return rv;
}

static inline uint32_t read_msr_rx(void)
{
  return getreg32(STM32_CAN1_MSR) & CAN_MSR_RX;
}

static uint32_t read_msr(time_hrt_cycles_t  *now)
{
  __asm__ __volatile__ ("\tcpsid  i\n");
  *now = timer_hrt_read();
  uint32_t msr = read_msr_rx();
  __asm__ __volatile__ ("\tcpsie  i\n");
  return msr;
}

static int read_bits_times(time_hrt_cycles_t *times, size_t max)
{
  uint32_t samplecnt = 0;
  bl_timer_id ab_timer= timer_allocate(modeTimeout|modeStarted, CAN_BAUD_TIME_IN_MS, 0);
  time_ref_t ab_ref =  timer_ref(ab_timer);
  uint32_t msr;
  uint32_t last_msr = read_msr(times);

  while(samplecnt < max && !timer_ref_expired(ab_ref))
  {
    do {
        msr = read_msr(&times[samplecnt]);
    } while(!(msr ^ last_msr) && !timer_ref_expired(ab_ref));
    last_msr = msr;
    samplecnt++;
  }
  timer_free(ab_timer);
  return samplecnt;
}

int can_autobaud(can_speed_t *can_speed, bl_timer_id tboot)
{

  *can_speed = CAN_UNKNOWN;

 volatile int attempt = 0;
  /* Threshold are at 1.5 Bit times */


  /*
   * We are here because there was a reset or the app invoked
   * the bootloader with no bit rate set.
   */

  time_hrt_cycles_t bit_time;
  time_hrt_cycles_t min_cycles;
  int sample;
  can_speed_t speed = CAN_125KBAUD;

  time_hrt_cycles_t samples[128];

  while(1) {


      while(1) {

          min_cycles = ULONG_MAX;
          int samplecnt = read_bits_times(samples, arraySize(samples));

          if (timer_expired(tboot)) {
              return CAN_BOOT_TIMEOUT;
          }

          if ((getreg32(STM32_CAN1_RF0R) | getreg32(STM32_CAN1_RF1R)) &
               CAN_RFR_FMP_MASK) {
              *can_speed = speed;
              return CAN_OK;
          }

          if (samplecnt < CAN_BAUD_SAMPLES_NEEDED ) {
              continue;
          }

          for (sample = 0; sample < samplecnt; sample += 2) {
              bit_time = samples[sample] = timer_hrt_elapsed(samples[sample], samples[sample+1]);
              if (sample > CAN_BAUD_SAMPLES_DISCARDED && bit_time < min_cycles) {
                  min_cycles = bit_time;
              }
          }
          break;
      }

       uint32_t bit34 = CAN_125KBAUD_BIT_CYCLES - CAN_125KBAUD_BIT_CYCLES/4;
       samples[1] = min_cycles;
       speed = CAN_125KBAUD;
       while(min_cycles < bit34 && speed < CAN_1MBAUD) {
           speed++;
           bit34 /= 2;
       }

       attempt++;
       can_init(speed, CAN_Mode_Silent);


  } /* while(1) */

  return CAN_OK;
}

int can_speed2freq(can_speed_t speed)
{
  return 1000000 >> (CAN_1MBAUD-speed);
}

can_speed_t can_freq2speed(int freq)
{
  return (freq == 1000000u ? CAN_1MBAUD : freq == 500000u ? CAN_500KBAUD : freq == 250000u ? CAN_250KBAUD : CAN_125KBAUD);
}