#include <sta/raspi/spi.hpp>

#ifdef STA_PLATFORM_RASPI

#include <sta/raspi/gpio_pin.hpp>
#include <sta/spi/settings.hpp>
#include <sta/assert.hpp>
#include <sta/endian.hpp>
#include <sta/lang.hpp>

#include <cstdlib>
#include <string.h>
#include <cstdio>

// Imports needed for SPI handling on Linux.
#include <linux/spi/spidev.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <errno.h>

#define STA_DEBUG_IOCTL_WRITE(result) STA_ASSERT_MSG(result >= 0, "ioctl writing failed!")
#define STA_DEBUG_IOCTL_READ(result)  STA_ASSERT_MSG(result >= 0, "ioctl reading failed!")
#define STA_DEBUG_IOCTL_SEND(result)  STA_ASSERT_MSG(result >= 0, "ioctl sending spi message failed!")

#define STA_DEBUG_FD_OPEN() STA_ASSERT_MSG(open_, "File descriptor wasn't opened!")
#define STA_DEBUG_NULLPTR(ptr) STA_ASSERT_MSG(ptr != nullptr, "Pointer is nullptr!")


namespace sta
{

    RaspiSPI::RaspiSPI(SPINode node, const SPISettings & settings, Mutex * mutex, bool persistent_open)
        : SPI(settings, mutex), open_{ false }, persistent_open_{ persistent_open }, settings_{ settings }
    {
        // Check validity of parameters.
        STA_DEBUG_NULLPTR(mutex);

        open_ = false;

        // Safer version of malloc + strcpy
        spidev_ = strdup(node == SPINode::DEV_0_0 ? "/dev/spidev0.0" : "/dev/spidev0.1");
        
        // Check if memory allocation was successful.
        STA_DEBUG_NULLPTR(spidev_);
    }

    RaspiSPI::~RaspiSPI()
    {
        if (spidev_ != NULL ) {
            free(spidev_);
	        spidev_ = NULL;
        }
        
        if (open_) {
            close(spifd_);
        }
    }

    void RaspiSPI::transfer(uint8_t value)
    {
        STA_DEBUG_FD_OPEN();

        struct spi_ioc_transfer spi_message[1];

        // According to the documentation, spi_message should be zero intialized.
        memset(spi_message, 0, sizeof(spi_message));
        spi_message[0].tx_buf = (unsigned long)&value;
        spi_message[0].len = 1;

        int result = ioctl(spifd_, SPI_IOC_MESSAGE(1), spi_message);

        STA_DEBUG_IOCTL_SEND(result);
    }

    void RaspiSPI::transfer16(uint16_t value)
    {
        STA_DEBUG_FD_OPEN();

        struct spi_ioc_transfer spi_message[1];

        // According to the documentation, spi_message should be zero intialized.
        memset(spi_message, 0, sizeof(spi_message));
        spi_message[0].tx_buf = (unsigned long)&value;
        spi_message[0].len = 1;

        int result = ioctl(spifd_, SPI_IOC_MESSAGE(1), spi_message);

        STA_DEBUG_IOCTL_SEND(result);
    }

    void RaspiSPI::transfer(const uint8_t * buffer, size_t size)
    {
        STA_DEBUG_FD_OPEN();
        STA_DEBUG_NULLPTR(buffer);
        STA_ASSERT_MSG(size != 0, "Buffer size cannot be 0!");

        struct spi_ioc_transfer spi_message[1];

        // According to the documentation, spi_message should be zero intialized.
        memset(spi_message, 0, sizeof(spi_message));
        spi_message[0].tx_buf = (unsigned long)buffer;
        spi_message[0].len = size;

        printf("Sending ");
        for (int i = 0; i < size; i++) {
            printf("%x, ", buffer[i]);
        }
        printf("\n");

        int result = ioctl(spifd_, SPI_IOC_MESSAGE(1), spi_message);

        STA_DEBUG_IOCTL_SEND(result);
    }

    void RaspiSPI::transfer(const uint8_t * txBuffer, uint8_t * rxBuffer, size_t size)
    {
        STA_DEBUG_FD_OPEN();
        STA_DEBUG_NULLPTR(txBuffer);
        STA_DEBUG_NULLPTR(rxBuffer);
        STA_ASSERT_MSG(size != 0, "Buffer size cannot be 0!");
        
        struct spi_ioc_transfer spi_message[1];

        // According to the documentation, spi_message should be zero intialized.
        memset(spi_message, 0, sizeof(spi_message));
    
        spi_message[0].rx_buf = (unsigned long)rxBuffer;
        spi_message[0].tx_buf = (unsigned long)txBuffer;
        spi_message[0].len = size;

        int result = ioctl(spifd_, SPI_IOC_MESSAGE(1), spi_message);

        if (result == -1) {
            printf("Sending failed with error '%s'! \n", strerror(errno));
        }

        STA_DEBUG_IOCTL_SEND(result);
    }

    void RaspiSPI::receive(uint8_t * buffer, size_t size)
    {
        STA_DEBUG_FD_OPEN();
        STA_DEBUG_NULLPTR(buffer);
        STA_ASSERT_MSG(size != 0, "Buffer size cannot be 0!");

        struct spi_ioc_transfer spi_message[1];

        // According to the documentation, spi_message should be zero intialized.
        memset(spi_message, 0, sizeof(spi_message));
        
        spi_message[0].rx_buf = (unsigned long)buffer;
        spi_message[0].len = size;

        int result = ioctl(spifd_, SPI_IOC_MESSAGE(1), spi_message);

        printf("Receiving ");
        for (int i = 0; i < size; i++) {
            printf("%x, ", buffer[i]);
        }
        printf("\n");

        STA_DEBUG_IOCTL_SEND(result);
    }

    void RaspiSPI::fill(uint8_t value, size_t count)
    {
        STA_DEBUG_FD_OPEN();
        STA_ASSERT_MSG(count != 0, "Buffer size cannot be 0!");

        uint8_t * buffer = new uint8_t[count];
        memset(buffer, value, count);

        struct spi_ioc_transfer spi_message[1];
        memset(spi_message, 0, sizeof(spi_message));
        spi_message[0].tx_buf = (unsigned long)buffer;
        spi_message[0].len = count;

        int result = ioctl(spifd_, SPI_IOC_MESSAGE(1), spi_message);

        STA_DEBUG_IOCTL_SEND(result);

        delete[] buffer;
    }

    void RaspiSPI::update_setting(int setting, int value)
    {
        
    }

    void RaspiSPI::get_setting(int setting, void * rslt_ptr)
    {
        STA_DEBUG_FD_OPEN();
        int result = ioctl(spifd_, setting, rslt_ptr);
        STA_DEBUG_IOCTL_READ(result);        
    }

    void RaspiSPI::update_settings()
    {
        STA_DEBUG_FD_OPEN();

        switch (settings_.mode)
        {
        case SPIMode::MODE_0:
            mode_ = SPI_MODE_0;
            break;
        case SPIMode::MODE_1:
            mode_ = SPI_MODE_1;
            break;
        case SPIMode::MODE_2:
            mode_ = SPI_MODE_2;
            break;
        case SPIMode::MODE_3:
            mode_ = SPI_MODE_3;
            break;
        default:
            STA_ASSERT_MSG(false, "Case for SPIMode enum not handled");
            STA_UNREACHABLE();
        }

        dataSize_ = settings_.dataSize == SPIDataSize::SIZE_8 ? 0 : 16;
        bitOrder_ = settings_.bitOrder == SPIBitOrder::MSB ? 0 : 1;
        clkSpeed_ = settings_.clkSpeed;
        
        // Set the spi mode.
        int result = ioctl(spifd_, SPI_IOC_WR_MODE, &mode_);
        STA_DEBUG_IOCTL_WRITE(result);

        // Set the word size. According to the documentation "the value zero signifies eight bits".
        result = ioctl(spifd_, SPI_IOC_WR_BITS_PER_WORD, &dataSize_);
        STA_DEBUG_IOCTL_WRITE(result);

        // Set the bit order. According to the documentation zero means MSB first, everything else means LSB first.
        result = ioctl(spifd_, SPI_IOC_WR_LSB_FIRST, &bitOrder_);
        STA_DEBUG_IOCTL_WRITE(result);

        // Set the maximum clock speed.
        result = ioctl(spifd_, SPI_IOC_WR_MAX_SPEED_HZ, &clkSpeed_);
        STA_DEBUG_IOCTL_WRITE(result);

        #if defined(DEBUG)
            uint8_t r_mode = 0;
            get_setting(SPI_IOC_RD_MODE, &r_mode);
            STA_ASSERT_MSG(r_mode == mode_, "The mode wasn't set correctly!");

            uint8_t r_dataSize = 0;
            get_setting(SPI_IOC_WR_BITS_PER_WORD, &r_dataSize);
            STA_ASSERT_MSG(r_dataSize == (settings_.dataSize == SPIDataSize::SIZE_8 ? 8 : 16), "The data size wasn't set correctly!");

            uint8_t r_bitOrder = 0;
            get_setting(SPI_IOC_RD_LSB_FIRST, &r_bitOrder);
            STA_ASSERT_MSG(r_bitOrder == (settings_.bitOrder == SPIBitOrder::MSB ? 0 : 1), "The data size wasn't set correctly!");

            uint32_t r_clk_Speed;
            get_setting(SPI_IOC_RD_MAX_SPEED_HZ, &r_clk_Speed);
            STA_ASSERT_MSG(r_clk_Speed == settings_.clkSpeed, "The clock speed wasn't set correctly!");
        #endif // STA_DEBUG
    }

    void RaspiSPI::acquire()
    {
        SPI::acquire();

        STA_ASSERT(spidev_ != nullptr);

        /* open spidev device */
        if (open_ == true)
            return;

        spifd_ = open(spidev_, O_RDWR);
        open_ = true;

        STA_ASSERT(spifd_ >= 0);

        update_settings();
    }

    void RaspiSPI::release()
    {
        STA_ASSERT_MSG(open_, "'release' was called despite the device being closed! This has to be a bug!");

        if (!persistent_open_ && open_) {
            close(spifd_);
        }

        SPI::release();
    }

    RaspiSPIDevice::RaspiSPIDevice(RaspiSPI * intf) : SPIDevice { intf, RaspiGpioPin::DUMMY_GPIO }
    {

    }
} // namespace sta


#endif // STA_PlATFORM_RASPI