#ifndef COMPLIB_SERVER_SPI_HPP
#define COMPLIB_SERVER_SPI_HPP

#include <mutex>

//SPI_MODE_0 (0,0) 	CPOL = 0, CPHA = 0, Clock idle low, data is clocked in on rising edge, output data (change) on falling edge
//SPI_MODE_1 (0,1) 	CPOL = 0, CPHA = 1, Clock idle low, data is clocked in on falling edge, output data (change) on rising edge
//SPI_MODE_2 (1,0) 	CPOL = 1, CPHA = 0, Clock idle high, data is clocked in on falling edge, output data (change) on rising edge
//SPI_MODE_3 (1,1) 	CPOL = 1, CPHA = 1, Clock idle high, data is clocked in on rising, edge output data (change) on falling edge

#define SPI_BUFFER_SIZE 20
#define SPI_SPEED 2000000 // 2 MHz
#define SPI_BITS_PER_WORD 8

class Spi {
public:
    static Spi& getInstance()
    {
        static Spi instance;
        return instance;
    }

    Spi(Spi const&) = delete;
    void operator=(Spi const&) = delete;

    int read(uint8_t reg, uint8_t length);
    void read_array(uint8_t reg, uint8_t length, uint8_t* data);
    
    void write(uint8_t reg, uint8_t length, int value);
    void write_array(uint8_t reg, uint8_t length, const uint8_t* data);
    
    enum Register : uint8_t {
        IDENTIFICATION_MODEL_ID = 1,
        IDENTIFICATION_MODEL_REV_MAJOR = 2,
        IDENTIFICATION_MODEL_REV_MINOR = 3,
        IDENTIFICATION_MODEL_REV_PATCH = 4,

        // Motor encoder positions
        MOTOR_1_POS_B3 = 10,
        MOTOR_1_POS_B2 = 11,
        MOTOR_1_POS_B1 = 12,
        MOTOR_1_POS_B0 = 13,
        MOTOR_2_POS_B3 = 14,
        MOTOR_2_POS_B2 = 15,
        MOTOR_2_POS_B1 = 16,
        MOTOR_2_POS_B0 = 17,
        MOTOR_3_POS_B3 = 18,
        MOTOR_3_POS_B2 = 19,
        MOTOR_3_POS_B1 = 20,
        MOTOR_3_POS_B0 = 21,
        MOTOR_4_POS_B3 = 22,
        MOTOR_4_POS_B2 = 23,
        MOTOR_4_POS_B1 = 24,
        MOTOR_4_POS_B0 = 25,

        // PWM Control Modes
        PWM_1_CTRL = 26,
        PWM_2_CTRL = 27,
        PWM_3_CTRL = 28,
        PWM_4_CTRL = 29,

        // Motor pwm speed
        MOTOR_1_PWM_H = 30,
        MOTOR_1_PWM_L = 31,
        MOTOR_2_PWM_H = 32,
        MOTOR_2_PWM_L = 33,
        MOTOR_3_PWM_H = 34,
        MOTOR_3_PWM_L = 35,
        MOTOR_4_PWM_H = 36,
        MOTOR_4_PWM_L = 37,

        // Servo goal position
        SERVO_1_PWM_H = 38,
        SERVO_1_PWM_L = 39,
        SERVO_2_PWM_H = 40,
        SERVO_2_PWM_L = 41,
        SERVO_3_PWM_H = 42,
        SERVO_3_PWM_L = 43,
        SERVO_4_PWM_H = 44,
        SERVO_4_PWM_L = 45,
        SERVO_5_PWM_H = 46,
        SERVO_5_PWM_L = 47,
        SERVO_6_PWM_H = 48,
        SERVO_6_PWM_L = 49,
        SERVO_7_PWM_H = 50,
        SERVO_7_PWM_L = 51,
        SERVO_8_PWM_H = 52,
        SERVO_8_PWM_L = 53,

        // IR Sensor value
        IR_1_H = 54,
        IR_1_L = 55,
        IR_2_H = 56,
        IR_2_L = 57,
        IR_3_H = 58,
        IR_3_L = 59,
        IR_4_H = 60,
        IR_4_L = 61,
        IR_5_H = 62,
        IR_5_L = 63,
        IR_1_LED = 64,
        IR_2_LED = 65,
        IR_3_LED = 66,
        IR_4_LED = 67,
        IR_5_LED = 68,

        // Display registers
        DISPLAY_LINE_1_C0 = 69,
        DISPLAY_LINE_2_C0 = 85,
        DISPLAY_LINE_3_C0 = 101,
        DISPLAY_LINE_4_C0 = 117,

        // Motor encoder velocities
        MOTOR_1_VEL_H = 118,
        MOTOR_1_VEL_L = 119,
        MOTOR_2_VEL_H = 120,
        MOTOR_2_VEL_L = 121,
        MOTOR_3_VEL_H = 122,
        MOTOR_3_VEL_L = 123,
        MOTOR_4_VEL_H = 124,
        MOTOR_4_VEL_L = 125
    };

private:
    Spi();

    int spi_file_descriptor{};
    uint8_t tx_buffer[SPI_BUFFER_SIZE] = {0};
    uint8_t rx_buffer[SPI_BUFFER_SIZE] = {0};

    std::recursive_mutex spi_mutex;

    void transfer();
    void clear_buffers();

    uint8_t calculate_checksum(uint8_t* data, uint8_t length);
};


#endif //COMPLIB_SERVER_SPI_HPP