#include <assert.h>
#include "main.h"
#include "pcanpro_can.h"

extern CAN_HandleTypeDef hcan1;

#define CAN_TX_FIFO_SIZE (256)
static struct t_can_dev
{
  void *dev;
  uint32_t tx_msgs;
  uint32_t tx_errs;
  uint32_t tx_ovfs;

  uint32_t rx_msgs;
  uint32_t rx_errs;
  uint32_t rx_ovfs;

  struct t_can_msg tx_fifo[CAN_TX_FIFO_SIZE];
  uint32_t tx_head;
  uint32_t tx_tail;
  uint32_t esr_reg;
  int (*rx_isr)( uint8_t, struct  t_can_msg* );
  int (*tx_isr)( uint8_t, struct  t_can_msg* );
  void (*err_handler)( int bus, uint32_t esr );
}
can_dev_array[CAN_BUS_TOTAL] = 
{       
  [CAN_BUS_1] = { .dev = &hcan1 },
//  [CAN_BUS_2] = { .dev = &hcan[CAN_BUS_2] },
};

uint32_t pcan_can_msg_time( const struct t_can_msg *pmsg, uint32_t nt, uint32_t dt )
{
  const uint32_t data_bits = pmsg->size<<3;
  const uint32_t control_bits = ( pmsg->flags & MSG_FLAG_EXT ) ? 67:47;
 
  if( pmsg->flags & MSG_FLAG_BRS )
    return (control_bits*nt) + (data_bits*dt);
  else
    return (control_bits+data_bits)*nt;
}

int pcan_can_set_filter_mask( int bus, int num, int format, uint32_t id, uint32_t mask )
{
  CAN_FilterTypeDef filter = { 0 };
  CAN_HandleTypeDef *p_can = can_dev_array[bus].dev;
  
  if( num >= CAN_INT_FILTER_MAX )
    return -1;
  
  /* CAN1 & CAN2 filter shared 28 filters, we use 14 for each one */
  /*if( p_can->Instance == CAN2 )
  {
    num += CAN2_FILTER_START;
  }*/
  
  filter.FilterMode = CAN_FILTERMODE_IDMASK;
  filter.FilterScale = CAN_FILTERSCALE_32BIT;
  
  if( format == MSG_FLAG_EXT )
  {
    id &= 0x1FFFFFFF;
    
    /* EXTID[28:13] */
    filter.FilterIdHigh = id >> 13;
    /* EXTID[12:0] + IDE */
    filter.FilterIdLow = ((id << 3)&0xFFFF) | CAN_ID_EXT;
    filter.FilterMaskIdHigh = mask >> 13;
    filter.FilterMaskIdLow = ((mask << 3)&0xFFFF) | CAN_ID_EXT;
  }
  else
  {
    id &= 0x7FF;
    filter.FilterIdHigh = id << 5;
    filter.FilterIdLow =  0x0;
    filter.FilterMaskIdHigh = mask << 5;
    filter.FilterMaskIdLow = 0x0;
  }
  
  filter.FilterFIFOAssignment = CAN_FILTER_FIFO0;
  filter.FilterActivation = ENABLE;
  filter.FilterBank = num;
  filter.SlaveStartFilterBank = CAN2_FILTER_START;
  
  if( HAL_CAN_ConfigFilter( p_can, &filter ) != HAL_OK )
    return -1;
  
  return 0;
}

int pcan_can_filter_init_stdid_list( int bus, const uint16_t *id_list, int id_len  )
{
  CAN_FilterTypeDef filter = { 0 };
  CAN_HandleTypeDef *p_can = can_dev_array[bus].dev;
 
  int i, offset = 0;

  //offset = ( p_can->Instance == CAN2 ) ? CAN2_FILTER_START: 0;
  
  /* STDID[10:3] | STDID[2:0] RTR IDE EXID[17:15] */
  /* IDE = 0, RTR = 0, EXID[17:15] = { 0 } */
  filter.FilterMode = CAN_FILTERMODE_IDLIST;
  filter.FilterScale = CAN_FILTERSCALE_16BIT;
  
  i = 0;
  while( i < id_len )
  {
    filter.FilterBank = ((i)>>2)+offset;
    filter.SlaveStartFilterBank = CAN2_FILTER_START;
    
    filter.FilterMaskIdLow = 0;
    filter.FilterMaskIdHigh = 0;
    filter.FilterIdLow = 0;
    filter.FilterIdHigh = 0;
    
    switch( (id_len-i) )
    {
    /* fallthrough */
    default:
      filter.FilterMaskIdLow = id_list[i++]<<5;
    case 3:
      filter.FilterMaskIdHigh = id_list[i++]<<5;
    case 2:
      filter.FilterIdLow = id_list[i++]<<5;
    case 1:
      filter.FilterIdHigh = id_list[i++]<<5;
    }
    filter.FilterFIFOAssignment = CAN_FILTER_FIFO0;
    filter.FilterActivation = ENABLE;
    
    if( HAL_CAN_ConfigFilter( p_can, &filter ) != HAL_OK )
      return -1;
  }
  
  return id_len;
}

static int _can_send( CAN_HandleTypeDef *p_can, struct t_can_msg *p_msg )
{
  CAN_TxHeaderTypeDef msg = { .TransmitGlobalTime = DISABLE };
  uint32_t txMailbox = 0;

  if( HAL_CAN_IsTxMessagePending( p_can, CAN_TX_MAILBOX0 ) )
    return -1;

  if( p_msg->flags & MSG_FLAG_EXT )
  {
    msg.ExtId = p_msg->id & 0x1FFFFFFF;
    msg.IDE = CAN_ID_EXT;
  }
  else
  {
    msg.StdId = p_msg->id & 0x7FF;
    msg.IDE = CAN_ID_STD;
  }
  
  msg.DLC = p_msg->size;
  msg.RTR = (p_msg->flags & MSG_FLAG_RTR)?CAN_RTR_REMOTE:CAN_RTR_DATA;
  
  if( HAL_CAN_AddTxMessage( p_can, &msg, (void*)p_msg->data, &txMailbox ) != HAL_OK )
    return -1;

  return txMailbox;
}

static void pcan_can_flush_tx( int bus )
{
  struct t_can_dev *p_dev = &can_dev_array[bus];
  struct t_can_msg *p_msg;

  /* empty fifo */
  if( p_dev->tx_head == p_dev->tx_tail )
    return;
  
  p_msg = &p_dev->tx_fifo[p_dev->tx_tail];
  if( _can_send( p_dev->dev, p_msg ) < 0 )
    return;

  if( p_dev->tx_isr )
  {
    (void)p_dev->tx_isr( bus, p_msg );
  }

  /* update fifo index */
  p_dev->tx_tail = (p_dev->tx_tail+1)&(CAN_TX_FIFO_SIZE-1);
}

int pcan_can_write( int bus, struct t_can_msg *p_msg )
{
  struct t_can_dev *p_dev = &can_dev_array[bus];

  if( !p_msg )
    return 0;

  uint32_t  tx_head_next = (p_dev->tx_head+1)&(CAN_TX_FIFO_SIZE-1);
  /* overflow ? just skip it */
  if( tx_head_next == p_dev->tx_tail )
  {
    ++p_dev->tx_ovfs;
    return -1;
  }

  p_dev->tx_fifo[p_dev->tx_head] = *p_msg;
  p_dev->tx_head = tx_head_next;

  return 0;
}

void pcan_can_install_rx_callback( int bus, int (*cb)( uint8_t, struct  t_can_msg* ) )
{
  struct t_can_dev *p_dev = &can_dev_array[bus];
  p_dev->rx_isr = cb;
}

void pcan_can_install_tx_callback( int bus, int (*cb)( uint8_t, struct  t_can_msg* ) )
{
  struct t_can_dev *p_dev = &can_dev_array[bus];
  p_dev->tx_isr = cb;
}

void pcan_can_install_err_callback( int bus, void (*cb)( int , uint32_t ) )
{
  struct t_can_dev *p_dev = &can_dev_array[bus];
  p_dev->err_handler = cb;
}

void pcan_can_set_silent( int bus, uint8_t silent_mode )
{
  CAN_HandleTypeDef *p_can = can_dev_array[bus].dev;

  p_can->Init.Mode = silent_mode ? CAN_MODE_SILENT: CAN_MODE_NORMAL;
  if( HAL_CAN_Init( p_can ) != HAL_OK )
  {
    assert( 0 );
  }
}

/* bxCAN does not support FDCAN ISO mode switch */
void pcan_can_set_iso_mode( int bus, uint8_t iso_mode )
{
  (void)bus;
  (void)iso_mode;
}

void pcan_can_set_loopback( int bus, uint8_t loopback )
{
  CAN_HandleTypeDef *p_can = can_dev_array[bus].dev;
  
  p_can->Init.Mode = loopback ? CAN_MODE_LOOPBACK: CAN_MODE_NORMAL;
  if( HAL_CAN_Init( p_can ) != HAL_OK )
  {
    assert( 0 );
  }
}

void pcan_can_set_bus_active( int bus, uint16_t mode )
{
  CAN_HandleTypeDef *p_can = can_dev_array[bus].dev;

  if( mode )
  {
    HAL_CAN_Start( p_can );
    HAL_CAN_AbortTxRequest( p_can, CAN_TX_MAILBOX0 | CAN_TX_MAILBOX1 | CAN_TX_MAILBOX2 );
  }
  else
  {
    HAL_CAN_AbortTxRequest( p_can, CAN_TX_MAILBOX0 | CAN_TX_MAILBOX1 | CAN_TX_MAILBOX2 );
    HAL_CAN_Stop( p_can );
  }
}

static void pcan_can_tx_complete( int bus, int mail_box )
{
  ++can_dev_array[bus].tx_msgs;
}

static void pcan_can_tx_err( int bus, int mail_box )
{
  ++can_dev_array[bus].tx_errs;
}

int pcan_can_stats( int bus, struct t_can_stats *p_stats )
{
  struct t_can_dev *p_dev = &can_dev_array[bus];
  
  p_stats->tx_msgs = p_dev->tx_msgs;
  p_stats->tx_errs = p_dev->tx_errs;
  p_stats->rx_msgs = p_dev->rx_msgs;
  p_stats->rx_errs = p_dev->rx_errs;
  p_stats->rx_ovfs = p_dev->rx_ovfs;

  return sizeof( struct t_can_stats );
}

void pcan_can_poll( void )
{
  static uint32_t err_last_check = 0;
  uint32_t ts_ms;
  
  //ts_ms = HAL_GetTick();
  ts_ms = xTaskGetTickCount() * (1000 / configTICK_RATE_HZ);
#if ( CAN_WITHOUT_ISR == 1 )
  HAL_CAN_IRQHandler( &hcan[CAN_BUS_1] );
//  HAL_CAN_IRQHandler( &hcan[CAN_BUS_2] );
#endif
  
  pcan_can_flush_tx( CAN_BUS_1 );
//  pcan_can_flush_tx( CAN_BUS_2 );

  if( (uint32_t)( err_last_check - ts_ms ) > 250 )
  {
    err_last_check = ts_ms;
    for( int i = 0; i < CAN_BUS_TOTAL; i++ )
    {
      if( !can_dev_array[i].err_handler )
        continue;
      CAN_HandleTypeDef *pcan = can_dev_array[i].dev;
      if( can_dev_array[i].esr_reg != pcan->Instance->ESR )
      {
        can_dev_array[i].esr_reg = pcan->Instance->ESR;
        can_dev_array[i].err_handler( i, can_dev_array[i].esr_reg );
      }
    }
  }
}

/* --------------- HAL PART ------------- */
static int _bus_from_int_dev( CAN_TypeDef *can )
{
//  if( can == CAN1 )
//    return CAN_BUS_1;
//  else if( can == CAN2 )
//    return CAN_BUS_2;
  /* abnormal! */
  return CAN_BUS_1;
}

static void pcan_can_isr_frame( CAN_HandleTypeDef *hcan, uint32_t fifo )
{
  CAN_RxHeaderTypeDef hdr;
  const int bus = _bus_from_int_dev( hcan->Instance );
  struct t_can_dev * const p_dev = &can_dev_array[bus];
  struct t_can_msg  msg = { 0 };
  
  if( HAL_CAN_GetRxMessage( hcan, fifo, &hdr, msg.data ) != HAL_OK )
    return;

  /* oversize frame ? */
  if( hdr.DLC > CAN_PAYLOAD_MAX_SIZE )
    return;

  if( hdr.IDE == CAN_ID_STD )
  {
    msg.id = hdr.StdId;
  }
  else
  {
    msg.id = hdr.ExtId;
    msg.flags |= MSG_FLAG_EXT;
  }

  if( hdr.RTR != CAN_RTR_DATA )
  {
    msg.flags |= MSG_FLAG_RTR;
  }

  msg.size = hdr.DLC;
  //msg.timestamp = pcan_timestamp_us();
  
  if( p_dev->rx_isr )
  {
    if( p_dev->rx_isr( bus, &msg ) < 0 )
    {
      ++p_dev->rx_ovfs;
      return;
    }
  }
  ++p_dev->rx_msgs;
}

/* CAN HAL subsystem callbacks */
void HAL_CAN_TxMailbox0CompleteCallback( CAN_HandleTypeDef *hcan )
{
  pcan_can_tx_complete( _bus_from_int_dev( hcan->Instance ), 0 );
}

void HAL_CAN_TxMailbox1CompleteCallback( CAN_HandleTypeDef *hcan )
{
  pcan_can_tx_complete( _bus_from_int_dev( hcan->Instance ), 1 ); 
}

void HAL_CAN_TxMailbox2CompleteCallback( CAN_HandleTypeDef *hcan )
{
  pcan_can_tx_complete( _bus_from_int_dev( hcan->Instance ), 2 );
}

void HAL_CAN_TxMailbox0AbortCallback( CAN_HandleTypeDef *hcan ){}
void HAL_CAN_TxMailbox1AbortCallback( CAN_HandleTypeDef *hcan ){}
void HAL_CAN_TxMailbox2AbortCallback( CAN_HandleTypeDef *hcan ){}

void HAL_CAN_RxFifo0MsgPendingCallback( CAN_HandleTypeDef *hcan )
{
  pcan_can_isr_frame( hcan, CAN_RX_FIFO0 );
}


void HAL_CAN_RxFifo1MsgPendingCallback( CAN_HandleTypeDef *hcan )
{
  pcan_can_isr_frame( hcan, CAN_RX_FIFO1 );
}

void HAL_CAN_RxFifo0FullCallback( CAN_HandleTypeDef *hcan )
{
}

void HAL_CAN_RxFifo1FullCallback( CAN_HandleTypeDef *hcan )
{
}

void HAL_CAN_SleepCallback( CAN_HandleTypeDef *hcan ){}
void HAL_CAN_WakeUpFromRxMsgCallback( CAN_HandleTypeDef *hcan ){}

void HAL_CAN_ErrorCallback( CAN_HandleTypeDef *hcan )
{
  /* handle errors */
  uint32_t err = HAL_CAN_GetError( hcan );
  int bus = _bus_from_int_dev( hcan->Instance );
  
  if ( err & HAL_CAN_ERROR_TX_TERR0 ) 
  {
    pcan_can_tx_err( bus, 0 );
  }
  
  if ( err & HAL_CAN_ERROR_TX_TERR1 ) 
  {
    pcan_can_tx_err( bus, 1 );
  }
  
  if ( err & HAL_CAN_ERROR_TX_TERR2 ) 
  {
    pcan_can_tx_err( bus, 2 );
  }
  
  HAL_CAN_ResetError( hcan );
}
