#include <stm32f302xc.h>
#include <stm32f3xx_hal.h>
#include <stm32f3xx_hal_conf.h>
#include <vn-interface/helper.h>
#include <vn-interface/registers.h>
#include "canhalal.h"
#include "can1.h"
#include "endec.hpp"


template <typename payload_t>
HAL_StatusTypeDef spi_read(SPI_HandleTypeDef *hspi, vn::pkg_request_read_t *pRequestMOSI, vn::pkg_response_t<payload_t> *pResponseMISO){
    HAL_StatusTypeDef status = HAL_OK;
    vn::header_t::response_t requestMISO;

    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET);
    status = HAL_SPI_TransmitReceive(hspi,
                                     (uint8_t *)(pRequestMOSI),
                                     (uint8_t *)(&requestMISO), // not relevant, but there to function
                                     sizeof(*pRequestMOSI), 100);

    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_SET);

    HAL_Delay(1);

    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET);

    vn::pkg_response_t<payload_t> responseMOSI;

    status = HAL_SPI_TransmitReceive(hspi,
                                     (uint8_t *)(&responseMOSI), // just empty byte to allow the slave to transmit
                                     (uint8_t *)(pResponseMISO),
                                     sizeof(*pResponseMISO), 100);

    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_SET);
	return status;
}

template <typename payload_t>
HAL_StatusTypeDef spi_write(SPI_HandleTypeDef *hspi, vn::pkg_request_write_t<payload_t> *pRequestMOSI, vn::pkg_response_t<payload_t> *pResponseMISO){
    HAL_StatusTypeDef status = HAL_OK;
    vn::header_t::response_t requestMISO;

    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET);
    status = HAL_SPI_TransmitReceive(hspi,
                                     (uint8_t *)(pRequestMOSI),
                                     (uint8_t *)(&requestMISO), // not relevant, but there to function
                                     sizeof(*pRequestMOSI), 100);

    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_SET);

    HAL_Delay(1);

    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET);

    vn::pkg_response_t<payload_t> responseMOSI;

    status = HAL_SPI_TransmitReceive(hspi,
                                     (uint8_t *)(&responseMOSI), // just empty byte to allow the slave to transmit
                                     (uint8_t *)(pResponseMISO),
                                     sizeof(*pResponseMISO), 100);

    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_SET);
	return status;
}

template <typename payload_t>
HAL_StatusTypeDef spi2can(SPI_HandleTypeDef *hspi, CAN_HandleTypeDef *hcan, const uint16_t& id){
  auto request = vn::pkg_request_read_t(id);
  auto response = vn::pkg_response_t<payload_t>();
  size_t datalen = 8;
  uint16_t can_id = 0;
  uint8_t *data = nullptr;

  spi_read(hspi, &request, &response);

  switch (id) {
	case vn::YawPitchRollTrueBodyAccelerationAndAngularRatesRegisterID:
	{
		can_id = CAN1_VN200_YPR_FRAME_ID;
		vn::YawPitchRollTrueBodyAccelerationAndAngularRatesRegister payload;
		payload = response.payload;
		//canlib::encode::can1::vn200_ypr(canlib::frame::decoded::can1::vn200_ypr_t((double) payload.yawPitchRoll.x, (double) payload.yawPitchRoll.y, (double) payload.yawPitchRoll.z));
	}
		break;
	default:
		break;
  }
  ftcan_transmit(hcan, can_id, data, datalen);
  return HAL_OK;
}