#include "GyroL3GD20H.h"
#include "stm32h7xx_spi_dma_msp.h"
#include "spiConf.h"
#include "../spi/spiDefinitions.h"

#include "fsfw/tasks/TaskFactory.h"
#include "fsfw/serviceinterface/ServiceInterface.h"

#include "stm32h7xx_nucleo.h"
#include "stm32h7xx_hal_spi.h"

std::array<uint8_t, GyroL3GD20H::recvBufferSize> GyroL3GD20H::rxBuffer;
TransferStates transferState = TransferStates::IDLE;
DMA_HandleTypeDef txDmaHandle;
DMA_HandleTypeDef rxDmaHandle;

GyroL3GD20H::GyroL3GD20H(SPI_HandleTypeDef *spiHandle): spiHandle(spiHandle) {
    setDmaHandles(&txDmaHandle, &rxDmaHandle);
}

ReturnValue_t GyroL3GD20H::initialize() {
    // Configure the SPI peripheral
    spiHandle->Instance               = SPI1;
    spiHandle->Init.BaudRatePrescaler = SPI_GetPrescaler(HAL_RCC_GetHCLKFreq(), 3900000);
    spiHandle->Init.Direction         = SPI_DIRECTION_2LINES;
    spi::assignSpiMode(spi::SpiModes::MODE_3, spiHandle);
    spiHandle->Init.DataSize          = SPI_DATASIZE_8BIT;
    spiHandle->Init.FirstBit          = SPI_FIRSTBIT_MSB;
    spiHandle->Init.TIMode            = SPI_TIMODE_DISABLE;
    spiHandle->Init.CRCCalculation    = SPI_CRCCALCULATION_DISABLE;
    spiHandle->Init.CRCPolynomial     = 7;
    spiHandle->Init.CRCLength         = SPI_CRC_LENGTH_8BIT;
    spiHandle->Init.NSS               = SPI_NSS_SOFT;
    spiHandle->Init.NSSPMode          = SPI_NSS_PULSE_DISABLE;
    // Recommended setting to avoid glitches
    spiHandle->Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_ENABLE;
    spiHandle->Init.Mode = SPI_MODE_MASTER;
    if(HAL_SPI_Init(spiHandle) != HAL_OK) {
        sif::printWarning("Error initializing SPI\n");
        return HasReturnvaluesIF::RETURN_FAILED;
    }
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GyroL3GD20H::performOperation() {

    transferState = TransferStates::WAIT;
    // Start SPI transfer via DMA
    if(HAL_SPI_TransmitReceive_DMA(spiHandle, txBuffer.data(), rxBuffer.data(), 2) != HAL_OK) {
        // Transfer error in transmission process
        sif::printWarning("Error transmitting SPI with DMA\n");
    }

    // Wait for the transfer to complete
    while (transferState == TransferStates::WAIT) {
        TaskFactory::delayTask(1);
    }

    // Invalidate cache prior to access by CPU
    SCB_InvalidateDCache_by_Addr ((uint32_t *)rxBuffer.data(), recvBufferSize);

    switch(transferState) {
    case(TransferStates::SUCCESS): {
        sif::printInfo("Transfer success\n");
        transferState = TransferStates::IDLE;
        break;
    }
    case(TransferStates::FAILURE): {
        sif::printWarning("Transfer failure\n");
        transferState = TransferStates::FAILURE;
        break;
    }
    default: {
        break;
    }
    }
    return HasReturnvaluesIF::RETURN_OK;
}

/**
 * @brief  TxRx Transfer completed callback.
 * @param  hspi: SPI handle
 * @note   This example shows a simple way to report end of DMA TxRx transfer, and
 *         you can add your own implementation.
 * @retval None
 */
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
    transferState = TransferStates::SUCCESS;
}

/**
 * @brief  SPI error callbacks.
 * @param  hspi: SPI handle
 * @note   This example shows a simple way to report transfer error, and you can
 *         add your own implementation.
 * @retval None
 */
void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi) {
    transferState = TransferStates::FAILURE;
}