#include "stm32l1xx_hal.h"
#include "core/csect.h"
#include "core/config.h"
#include "core/config_pins.h"
#include "drivers/switchboard_control/switchboard_control.h"
#include "drivers/spi/spi.h"

static bool SB_Init();
static void SB_DeInit();
static void SB_SetNSS();
static void SB_ClrNSS();
static void SB_Transmit( uint8_t * pTxBuf, size_t size );
static bool SB_Receive( uint8_t * pRxBuf, size_t size );
static void SB_TransmitReceive( uint8_t * pTxBuf, uint8_t * pRxBuf, size_t size );

 const SB_Handle_t SBHandle = {

    SB_Init,
    SB_DeInit,
    SB_SetNSS,
    SB_ClrNSS,
    SB_Transmit,
    SB_Receive,
    SB_TransmitReceive,
 };
 
 static SPI_HandleTypeDef handle_spi = {
  #if CONFIG_PERIF_SBCONTROLSPI == CONFIG_PERIF_SPI1
  .Instance = SPI1
  #elif CONFIG_PERIF_SBCONTROLSPI == CONFIG_PERIF_SPI2
  .Instance = SPI2
  #else
  #error Invalid SPI specified in 'config_pins.h': see 'CONFIG_PERIF_SBCONTROLSPI' 
  #endif
};

static void SB_SetNSS()
{
  HAL_GPIO_WritePin( CONFIG_PORT__SBCONTROL__CHS, CONFIG_PIN__SBCONTROL__CHS, GPIO_PIN_SET );
}

static void SB_ClrNSS()
{
  HAL_GPIO_WritePin( CONFIG_PORT__SBCONTROL__CHS, CONFIG_PIN__SBCONTROL__CHS, GPIO_PIN_RESET );
}

static void SB_Transmit( uint8_t * pTxBuf, size_t size )
{
  HAL_SPI_Transmit( &handle_spi, pTxBuf, size, HAL_MAX_DELAY );
}

static bool SB_Receive( uint8_t * pRxBuf, size_t size )
{
  HAL_StatusTypeDef request = HAL_SPI_Receive( &handle_spi, pRxBuf, size, HAL_MAX_DELAY );
  return HAL_OK == request;
}

static void SB_TransmitReceive( uint8_t * pTxBuf, uint8_t * pRxBuf, size_t size )
{
  HAL_SPI_TransmitReceive( &handle_spi, pTxBuf, pRxBuf, size, HAL_MAX_DELAY );
}

 static bool SB_Init()
 {
   bool bRet = true;
   // Initialize SPI handle
   //DI();
   #if CONFIG_PERIF_SBCONTROLSPI == CONFIG_PERIF_SPI1
   handle_spi.Instance = SPI1;
   #elif CONFIG_PERIF_SBCONTROLSPI == CONFIG_PERIF_SPI2
   handle_spi.Instance = SPI2;
   #else
   #error Invalid SPI specified in 'config_pins.h': see 'CONFIG_PERIF_EXTMEMSPI' 
   #endif
   handle_spi.Init.Mode = SPI_MODE_MASTER;
   handle_spi.Init.Direction = SPI_DIRECTION_2LINES;
   handle_spi.Init.DataSize = SPI_DATASIZE_16BIT;
   handle_spi.Init.CLKPolarity = SPI_POLARITY_LOW;
   handle_spi.Init.CLKPhase = SPI_PHASE_1EDGE;
   handle_spi.Init.NSS = SPI_NSS_SOFT;
   handle_spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16;
   handle_spi.Init.FirstBit = SPI_FIRSTBIT_MSB;
   handle_spi.Init.TIMode = SPI_TIMODE_DISABLED;
   handle_spi.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLED;
   handle_spi.Init.CRCPolynomial = 7;
   
   {
   GPIO_InitTypeDef GPIO_InitStruct = {0};
   // Configure GPIO pins for SPI
   GPIO_InitStruct.Pin = CONFIG_PIN__SBCONTROL__CK|CONFIG_PIN__SBCONTROL__SO|CONFIG_PIN__SBCONTROL__SI;  
   GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
   GPIO_InitStruct.Pull = GPIO_NOPULL;
   #if CONFIG_PERIF_SBCONTROLSPI == CONFIG_PERIF_SPI1
   GPIO_InitStruct.Alternate = GPIO_AF5_SPI1;
   #elif CONFIG_PERIF_SBCONTROLSPI == CONFIG_PERIF_SPI2  
   GPIO_InitStruct.Alternate = GPIO_AF5_SPI2;
   #else
   #error Invalid SPI specified in 'config_pins.h': see 'CONFIG_PERIF_SBCONTROLSPI' 
   #endif
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
   HAL_GPIO_Init(CONFIG_PORT__SBCONTROL__SO_SI_CK, &GPIO_InitStruct);
   }
   {
   GPIO_InitTypeDef GPIO_InitStruct = {0};
   // Configure GPIO pins for SPI : PB11 = Software NSS
   GPIO_InitStruct.Pin = CONFIG_PIN__SBCONTROL__CHS;
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
   GPIO_InitStruct.Pull = GPIO_PULLUP;
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
   HAL_GPIO_Init(CONFIG_PORT__SBCONTROL__CHS, &GPIO_InitStruct);
   HAL_GPIO_WritePin( CONFIG_PORT__SBCONTROL__CHS, CONFIG_PIN__SBCONTROL__CHS, GPIO_PIN_SET );
   }
   
   #if CONFIG_PERIF_SBCONTROLSPI == CONFIG_PERIF_SPI1
   // Enable SPI1 Clocking
   __HAL_RCC_SPI1_CLK_ENABLE();
   #elif CONFIG_PERIF_SBCONTROLSPI == CONFIG_PERIF_SPI2
   // Enable SPI2 Clocking
   __HAL_RCC_SPI2_CLK_ENABLE();
   #else
   #error Invalid SPI specified in 'config_pins.h': see 'CONFIG_PERIF_SBCONTROLSPI' 
   #endif
   
   // Initialize SPI HAL
   HAL_SPI_Init( &handle_spi );
   
   // Set NSS to HIGH by default
   SB_SetNSS();
   
   //EI();

   return bRet;
 }

static void SB_DeInit()
{
    #if CONFIG_PERIF_SBCONTROLSPI == CONFIG_PERIF_SPI1
   // Disable SPI1 Clocking
   handle_spi.Instance = SPI1;
   __HAL_RCC_SPI1_CLK_DISABLE();
   #elif CONFIG_PERIF_SBCONTROLSPI == CONFIG_PERIF_SPI2
   // Disable SPI2 Clocking
   handle_spi.Instance = SPI2;
   __HAL_RCC_SPI2_CLK_DISABLE();
   #else
   #error Invalid SPI specified in 'config_pins.h': see 'CONFIG_PERIF_SBCONTROLSPI' 
   #endif

   // Deinitialize ports
   // It is required to pull NSS (CS) to VCC to prevent
   // the slave activating!!!!

    // Configure GPIO pins for SPI 
   {
   GPIO_InitTypeDef GPIO_InitStruct = {0};
   GPIO_InitStruct.Pin = CONFIG_PIN__SBCONTROL__CK|CONFIG_PIN__SBCONTROL__SO|CONFIG_PIN__SBCONTROL__SI;
   GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
   GPIO_InitStruct.Pull = GPIO_NOPULL;   
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
   HAL_GPIO_Init(CONFIG_PORT__SBCONTROL__SO_SI_CK, &GPIO_InitStruct);
   }

   {
   GPIO_InitTypeDef GPIO_InitStruct = {0};
   GPIO_InitStruct.Pin = CONFIG_PIN__SBCONTROL__CHS;
   #if SPI2_KEEP_NSS_PULLEDUP_SB 
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
   GPIO_InitStruct.Pull = GPIO_PULLUP; 
   #else
   GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
   GPIO_InitStruct.Pull = GPIO_NOPULL;   
   #endif
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
   HAL_GPIO_Init(CONFIG_PORT__SBCONTROL__CHS, &GPIO_InitStruct);
   }

   // DeInitialize SPI HAL
   HAL_SPI_DeInit( &handle_spi );
}