#include <fcntl.h>				//Needed for SPI port
#include <sys/ioctl.h>			//Needed for SPI port
#include <linux/spi/spidev.h>	//Needed for SPI port
#include <unistd.h>			    //Needed for SPI port
#include <iostream>
#include <string.h>
#include <spdlog/spdlog.h>

#include "include/spi.hpp"
#include "include/mathUtils.hpp"

void check_for_error(int error_code, std::string error_message) {
    if (error_code < 0) {
        spdlog::error(error_message);
        exit(1);
    }
}

Spi::Spi() {
    int spi_mode = SPI_MODE_0;
    int spi_bits_per_workd = SPI_BITS_PER_WORD;
    int spi_speed = SPI_SPEED;

    check_for_error(spi_file_descriptor = open("/dev/spidev1.2", O_RDWR), "Could not open SPI device");
    check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MODE, &spi_mode), "Could not set SPI_IOC_WR_MODE");
    check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MODE, &spi_mode), "Could not set SPI_IOC_RD_MODE");
    check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_WR_BITS_PER_WORD");
    check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_RD_BITS_PER_WORD");
    check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed), "Could not set SPI_IOC_WR_MAX_SPEED_HZ");
    check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MAX_SPEED_HZ, &spi_mode), "Could not set SPI_IOC_RD_MAX_SPEED_HZ");
}

void Spi::clear_buffers() {
    memset(tx_buffer, 0, SPI_BUFFER_SIZE);
    memset(rx_buffer, 0, SPI_BUFFER_SIZE);
}

uint8_t Spi::calculate_checksum(uint8_t* data, uint8_t length) {
    int sum = 0;
    for (int i = 0; i < length; i++) {
        sum += data[i];
    }
    return sum % 256;
}

int Spi::read(uint8_t reg, uint8_t length) {
    std::lock_guard<std::recursive_mutex> lock(spi_mutex);

    uint8_t read_buffer[SPI_BUFFER_SIZE] = {0};
    read_array(reg, length, read_buffer);

    return mathUtils::int_from_bytes(read_buffer, length);
}

void Spi::read_array(uint8_t reg, uint8_t length, uint8_t* data) {
    std::lock_guard<std::recursive_mutex> lock(spi_mutex);

    clear_buffers();
    tx_buffer[0] = 0;
    tx_buffer[1] = reg;
    tx_buffer[2] = length;

    transfer();

    uint8_t checksum = calculate_checksum(rx_buffer, length + 3);
    if (checksum != rx_buffer[length + 3]) {
        spdlog::error("Received invalid checksum {}. Should be {}", rx_buffer[length +3], checksum);
    }

    memcpy(data, rx_buffer + 2, length);
}

void Spi::write(uint8_t reg, uint8_t length, int value) {
    std::lock_guard<std::recursive_mutex> lock(spi_mutex);

    uint8_t write_buffer[SPI_BUFFER_SIZE] = {0};
    mathUtils::bytes_from_int(value, length, write_buffer);
    write_array(reg, length, write_buffer);
}

void Spi::write_array(uint8_t reg, uint8_t length, const uint8_t* data) {
    std::lock_guard<std::recursive_mutex> lock(spi_mutex);

    clear_buffers();
    tx_buffer[0] = 1;
    tx_buffer[1] = reg;
    tx_buffer[2] = length;
    memcpy(tx_buffer + 3, data, length);

    transfer();
}

void Spi::transfer() {
    std::lock_guard<std::recursive_mutex> lock(spi_mutex);
    
    struct spi_ioc_transfer spi;
    memset(&spi, 0, sizeof(spi));
    
    spi.tx_buf = (unsigned long) tx_buffer;
	spi.rx_buf = (unsigned long) rx_buffer;
	spi.len = SPI_BUFFER_SIZE;
	spi.delay_usecs = 0;
	spi.speed_hz = SPI_SPEED;
    spi.bits_per_word = SPI_BITS_PER_WORD;
    spi.cs_change = 0;
    
    check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data");
    check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data");

    if (tx_buffer[1] != rx_buffer[1]) {
        spdlog::error("SPI error during read/write of register {}. Got reg {} instead!", tx_buffer[1], rx_buffer[1]);
    }
}
