/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file           : main.c
 * @brief          : Main program body
 ******************************************************************************
 * @attention
 *
 * <h2><center>&copy; Copyright (c) 2022 STMicroelectronics.
 * All rights reserved.</center></h2>
 *
 * This software component is licensed by ST under BSD 3-Clause license,
 * the "License"; You may not use this file except in compliance with the
 * License. You may obtain a copy of the License at:
 *                        opensource.org/licenses/BSD-3-Clause
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "display.h"

#include "string.h"
#include "base64.h"
#include "libSerial/serial.H"
#include "libSerial/serialUtils.H"
#include "libSmallPrintf/small_printf.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

typedef enum
{
  FAIL,
  REJECT,
  ACCEPT,
  COMPLETE
} appendStatus;

typedef struct
{
  uint8_t payloadBuffer[233];
  uint32_t payloadId;
  uint8_t payloadRemaining;  // number of bytes remaining
  uint8_t payloadTotal;      // number total number of bytes
  uint8_t payloadOffset;     // byte offset
  uint8_t payloadNextSeq;    // next sequence frame expected
  uint32_t payloadTimestamp; // timestamp of reception
  appendStatus status;
} contextType;

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
#define False (0)
#define True (1)
/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
CAN_HandleTypeDef hcan;

SPI_HandleTypeDef hspi1;

UART_HandleTypeDef huart1;

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_CAN_Init(void);
static void MX_USART1_UART_Init(void);
static void MX_SPI1_Init(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

#define CONTEXTS 6
contextType contexts[CONTEXTS];

CAN_TxHeaderTypeDef TxHeader;
CAN_RxHeaderTypeDef RxHeader;

uint8_t TxData[8];
uint8_t RxData[8];

// Storage for the data

uint32_t TxMailbox;

char const version[] = "$PDGY,Mike_James_Converter_#00001_Mode:15\r\n";
char const keepawake[] = "$PDGY,000000,1,,4,%ld.%03ld,,,\r\n";

// reset a search context
void resetContext(contextType *c)
{
  c->payloadRemaining = 0; // number of bytes remaining
  c->payloadTotal = 0;     // number total number of bytes
  c->payloadOffset = 0;
  c->payloadNextSeq = 0;
  c->payloadTimestamp = 0;
  c->status = FAIL;
  c->payloadId = 0;
}

uint8_t singleFrame(uint32_t pgn)
{

  switch (pgn)
  {
  case 129029:
  case 129038:
  case 126996:
  case 127489:
    return False;
  default:
    break;
  }

  if (pgn >= 130816 && pgn <= 131071)
    return False;
  if (pgn >= 65536 && pgn <= 126975)
    return False;
  return True;
}

// called from CAN frame callback
appendStatus append(contextType *c, CAN_RxHeaderTypeDef *packet)
{
  uint32_t extId = RxHeader.ExtId;

  if (c->payloadId != 0 && c->payloadId != extId)
    return REJECT;
  uint8_t newId = c->payloadId == 0; // set a flag if this is a newly discovered frame
  c->payloadId = extId;

  uint32_t pgn = (c->payloadId >> 8) & ((1 << 18) - 1);
  uint8_t pf = (pgn >> 8) & 0xFF;

  if (pf < 240)
  {
    // process PS if in PDU1 , address is low byte
    pgn = pgn & 0x3FF00;
  }

  uint8_t packetLen = RxHeader.DLC;

  if (singleFrame(pgn))
  {
    c->payloadTimestamp = HAL_GetTick();
    memcpy(c->payloadBuffer, RxData, packetLen);
    c->payloadRemaining = 0;
    c->payloadTotal = packetLen;
    c->status = COMPLETE;
    return c->status; // successfully filled frame
  }
  else
  {
    // sort out fastpacket new ID
    if (newId)
    {
      if ((RxData[0] & 0x1f) != 0)
      {
        resetContext(c);
        return FAIL;
      }
      c->payloadTimestamp = HAL_GetTick();
      c->payloadNextSeq = RxData[0] + 1;
      c->payloadId = extId;
      c->payloadRemaining = RxData[1];
      c->payloadTotal = RxData[1];
      // no data , return
      if (c->payloadTotal == 0)
      {
        resetContext(c);
        return FAIL;
      }
      uint8_t numBytes = packetLen - 2;
      memcpy(c->payloadBuffer, RxData + 2, numBytes);

      // CAN frame can be longer than remaining bytes, do not make payloadRemaining wrap around !
      if (numBytes < c->payloadRemaining)
        c->payloadRemaining -= numBytes;
      else
        c->payloadRemaining = 0;
      c->payloadOffset = numBytes;
    }
    else
    {
      if (RxData[0] != c->payloadNextSeq)
      {
        resetContext(c);
        return FAIL;
      }
      // predict next payload sequence
      c->payloadNextSeq++;
      uint8_t numBytes = packetLen - 1;
      memcpy(c->payloadBuffer + c->payloadOffset, RxData + 1, numBytes);
      c->payloadRemaining -= numBytes;
      c->payloadOffset += numBytes;
    }
  }

  appendStatus stat = (c->payloadRemaining != 0) ? ACCEPT : COMPLETE;
  c->status = stat;
  return stat;
}

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
  HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
  HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, RxData);

  int i;
  for (i = 0; i < CONTEXTS; ++i)
  {
    contextType *ctx = &contexts[i];
    appendStatus stat = append(ctx, &RxHeader);
    if (stat == ACCEPT || stat == COMPLETE)
      break; // accepted data, stop loop
  }
}

void heartBeat()
{
  char lineBuff[80];
  uint32_t timestamp = HAL_GetTick();
  size_t pos = small_sprintf(lineBuff, keepawake,
                             timestamp / 1000, // seconds
                             timestamp % 1000  // milliseconds
  );
  HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
  if (pos < SerialTransmitSpace(&uc1))
  {
    __disable_irq();
    sendString(&uc1, (char *)lineBuff, pos);
    __enable_irq();
  }
}

// utility function to set up the filter mask
void CAN_FilterMaskEXT(CAN_FilterTypeDef *filter_, uint32_t id_, uint32_t mask_)
{
  filter_->FilterIdHigh = id_ >> 13;
  filter_->FilterIdLow = ((id_ & 0x1FFF) << 3) | CAN_ID_EXT;
  filter_->FilterMaskIdHigh = mask_ >> 13;
  filter_->FilterMaskIdLow = ((mask_ & 0x1FFF) << 3) | CAN_ID_EXT;
}

void processCmd(char *buff, int len)
{
  buff[len] = 0; // terminate in case of error
  char lineBuff[100];
  int pos = small_sprintf(lineBuff, "$PDGY,ACK,%s\r\n", buff + 6);
  if (pos < SerialTransmitSpace(&uc1))
  {
    __disable_irq();
    sendString(&uc1, (char *)lineBuff, pos);
    __enable_irq();
  }
}

/* USER CODE END 0 */

/**
 * @brief  The application entry point.
 * @retval int
 */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */
  cc_init();

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_CAN_Init();
  MX_USART1_UART_Init();
  MX_SPI1_Init();
  /* USER CODE BEGIN 2 */

  HAL_CAN_Start(&hcan);

  init_usart_ctl(&uc1, &huart1);

  EnableSerialRxInterrupt(&uc1);

  // Activate the notification
  HAL_CAN_ActivateNotification(&hcan, CAN_IT_RX_FIFO0_MSG_PENDING);

  // send out a version string on the serial port
  sendString(&uc1, (char *)version, sizeof(version));

  uint32_t sendTime = HAL_GetTick() + 1000;
  char cmdBuff[100];

  // serial utils library command editor 
  editBuffer cmdEdit;
  
  initReadLine( &cmdEdit, cmdBuff, 100, READLINES_CR);

  for (int i = 0; i < CONTEXTS; ++i)
    resetContext(&contexts[i]);

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */

 editBufferReturn ret = readLine(&uc1, &cmdEdit);

  if (ret == EDIT_CR)
  {
      processCmd (cmdBuff,charCount(&cmdEdit));
  }

    if (HAL_GetTick() > sendTime)
    {
      sendTime += 1000;
      heartBeat();
      cc_display(2);
    }

    for (int i = 0; i < CONTEXTS; ++i)
    {
      contextType *ctx = &contexts[i];
      /// check for too old
      //   if (ctx->status == ACCEPT && ctx->payloadTimestamp < old)
      //   {
      //     resetContext(ctx);
      //     continue;
      //   }

      if (ctx->status == COMPLETE)
      {
        __disable_irq();

        uint8_t prio = (ctx->payloadId >> 26) & 0x7;
        uint8_t src = ctx->payloadId & 0xFF;
        uint8_t dst = 255;
        // mask out the PGN field as 18 bits.
        uint32_t pgn = (ctx->payloadId >> 8) & ((1 << 18) - 1);
        uint8_t pf = (pgn >> 8) & 0xFF;

        if (pf < 240)
        {
          // process PS if in PDU1 , address is low byte
          dst = pgn & 0xFF;
          pgn = pgn & 0x3FF00;
        }

        char lineBuff[300];
        size_t pos = small_sprintf(lineBuff, "!PDGY,%ld,%d,%d,%d,%ld.%03ld,",
                                   pgn,  // PGN
                                   prio, // priority
                                   src,  // source address
                                   dst,  // destination address

                                   ctx->payloadTimestamp / 1000, // milliseconds
                                   ctx->payloadTimestamp % 1000);

        size_t base64_len;
        base64_encode(ctx->payloadBuffer,
                      ctx->payloadTotal,
                      lineBuff + pos,
                      &base64_len);

        pos += base64_len;
        lineBuff[pos++] = '\r';
        lineBuff[pos++] = '\n';
        __enable_irq();

        // skip sending if this would overrun the buffer
        if (pos < SerialTransmitSpace(&uc1))
          sendString(&uc1, lineBuff, pos);
        __disable_irq();

        resetContext(ctx);
        __enable_irq();
      }
    }
  }
  /* USER CODE END 3 */
}

/**
 * @brief System Clock Configuration
 * @retval None
 */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
   * in the RCC_OscInitTypeDef structure.
   */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI_DIV2;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL16;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
   */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/**
 * @brief CAN Initialization Function
 * @param None
 * @retval None
 */
static void MX_CAN_Init(void)
{

  /* USER CODE BEGIN CAN_Init 0 */

  /* USER CODE END CAN_Init 0 */

  /* USER CODE BEGIN CAN_Init 1 */

  /* USER CODE END CAN_Init 1 */
  hcan.Instance = CAN1;
  hcan.Init.Prescaler = 16;
  hcan.Init.Mode = CAN_MODE_NORMAL;
  hcan.Init.SyncJumpWidth = CAN_SJW_2TQ;
  hcan.Init.TimeSeg1 = CAN_BS1_3TQ;
  hcan.Init.TimeSeg2 = CAN_BS2_4TQ;
  hcan.Init.TimeTriggeredMode = DISABLE;
  hcan.Init.AutoBusOff = DISABLE;
  hcan.Init.AutoWakeUp = DISABLE;
  hcan.Init.AutoRetransmission = ENABLE;
  hcan.Init.ReceiveFifoLocked = DISABLE;
  hcan.Init.TransmitFifoPriority = DISABLE;
  if (HAL_CAN_Init(&hcan) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN CAN_Init 2 */
  /* USER CODE BEGIN CAN_Init 2 */

  // allow absolutely everything into filter
  CAN_FilterTypeDef filterConfig;
  filterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
  filterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
  filterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0;
  filterConfig.FilterActivation = CAN_FILTER_ENABLE;
  filterConfig.FilterBank = 0;

  CAN_FilterMaskEXT(&filterConfig, 0, 0);

  HAL_CAN_ConfigFilter(&hcan, &filterConfig);

  /* USER CODE END CAN_Init 2 */
}

/**
 * @brief SPI1 Initialization Function
 * @param None
 * @retval None
 */
static void MX_SPI1_Init(void)
{

  /* USER CODE BEGIN SPI1_Init 0 */

  /* USER CODE END SPI1_Init 0 */

  /* USER CODE BEGIN SPI1_Init 1 */

  /* USER CODE END SPI1_Init 1 */
  /* SPI1 parameter configuration*/
  hspi1.Instance = SPI1;
  hspi1.Init.Mode = SPI_MODE_MASTER;
  hspi1.Init.Direction = SPI_DIRECTION_2LINES;
  hspi1.Init.DataSize = SPI_DATASIZE_8BIT;
  hspi1.Init.CLKPolarity = SPI_POLARITY_LOW;
  hspi1.Init.CLKPhase = SPI_PHASE_1EDGE;
  hspi1.Init.NSS = SPI_NSS_SOFT;
  hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
  hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
  hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
  hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
  hspi1.Init.CRCPolynomial = 10;
  if (HAL_SPI_Init(&hspi1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN SPI1_Init 2 */

  /* USER CODE END SPI1_Init 2 */
}

/**
 * @brief USART1 Initialization Function
 * @param None
 * @retval None
 */
static void MX_USART1_UART_Init(void)
{

  /* USER CODE BEGIN USART1_Init 0 */

  /* USER CODE END USART1_Init 0 */

  /* USER CODE BEGIN USART1_Init 1 */

  /* USER CODE END USART1_Init 1 */
  huart1.Instance = USART1;
  huart1.Init.BaudRate = 460800;
  huart1.Init.WordLength = UART_WORDLENGTH_8B;
  huart1.Init.StopBits = UART_STOPBITS_1;
  huart1.Init.Parity = UART_PARITY_NONE;
  huart1.Init.Mode = UART_MODE_TX_RX;
  huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart1.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART1_Init 2 */

  /* USER CODE END USART1_Init 2 */
}

/**
 * @brief GPIO Initialization Function
 * @param None
 * @retval None
 */
static void MX_GPIO_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};
  /* USER CODE BEGIN MX_GPIO_Init_1 */
  /* USER CODE END MX_GPIO_Init_1 */

  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOC_CLK_ENABLE();
  __HAL_RCC_GPIOD_CLK_ENABLE();
  __HAL_RCC_GPIOA_CLK_ENABLE();

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOA, SPI1_CS_Pin | SPI1_CD_Pin | SPI1_RESET_Pin, GPIO_PIN_RESET);

  /*Configure GPIO pin : LED_Pin */
  GPIO_InitStruct.Pin = LED_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
  HAL_GPIO_Init(LED_GPIO_Port, &GPIO_InitStruct);

  /*Configure GPIO pins : SPI1_CS_Pin SPI1_CD_Pin SPI1_RESET_Pin */
  GPIO_InitStruct.Pin = SPI1_CS_Pin | SPI1_CD_Pin | SPI1_RESET_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  /* USER CODE BEGIN MX_GPIO_Init_2 */
  /* USER CODE END MX_GPIO_Init_2 */
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
 * @brief  This function is executed in case of error occurrence.
 * @retval None
 */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */

  /* USER CODE END Error_Handler_Debug */
}

#ifdef USE_FULL_ASSERT
/**
 * @brief  Reports the name of the source file and the source line number
 *         where the assert_param error has occurred.
 * @param  file: pointer to the source file name
 * @param  line: assert_param error line source number
 * @retval None
 */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */