#include <SPI.h>

/* Common Frame Size */
#define MAX149X6_FRAME_SIZE 2
#define CS 10

/* Common Registers */
#define MAX149X6_CHIP_ADDR_MASK  ((1 << 7) | (1 << 6))
#define MAX149X6_ADDR_MASK       ((1 << 4) | (1 << 3) | (1 << 2) | (1 << 1))
#define MAX149X6_RW_MASK         (1 << 0)

/* Error Codes */
#define EINVAL 22
#define ENOMEM 12
#define ENODEV 19

struct max149x6_init_param {
    uint32_t chip_address = 0;
    uint8_t cs_pin = 10;
    uint8_t en_pin;
    uint8_t fault_pin;
    uint8_t ready_pin;
    uint8_t synch_pin;
    bool crc_en;
};

/**
 * @brief Device descriptor for MAX149X6.
 */
struct max149x6_desc {
    uint32_t chip_address = 0;
    uint8_t cs_pin = 10;
    uint8_t en_pin;
    uint8_t fault_pin;
    uint8_t ready_pin;
    uint8_t synch_pin;
    uint8_t buff[MAX149X6_FRAME_SIZE + 1];
    bool crc_en;
};

#define MAX14906_CHANNELS 4

#define MAX14906_SETOUT_REG 0x0
#define MAX14906_SETLED_REG 0x1
#define MAX14906_DOILEVEL_REG 0x2
#define MAX14906_INT_REG 0x3
#define MAX14906_OVR_LD_REG 0x4
#define MAX14906_OPN_WIR_FLT_REG 0x5
#define MAX14906_SHD_VDD_FLT_REG 0x6
#define MAX14906_GLOBAL_FLT_REG 0x7
#define MAX14906_CONFIG1_REG 0xA
#define MAX14906_CONFIG2_REG 0xB
#define MAX14906_CONFIG_DI_REG 0xC
#define MAX14906_CONFIG_DO_REG 0xD
#define MAX14906_CONFIG_CURR_LIM 0xE
#define MAX14906_CONFIG_MASK 0xF

/* DoiLevel register */
#define MAX14906_DOI_LEVEL_MASK(x) (1 << (x))

/* SetOUT register */
#define MAX14906_HIGHO_MASK(x) (1 << (x))

#define MAX14906_DO_MASK(x) ((3 << (2 * (x))))
#define MAX14906_CH_DIR_MASK(x) (1 << ((x) + 4))
#define MAX14906_CH(x) (x)
#define MAX14906_IEC_TYPE_MASK (1 << 7)
#define MAX14906_CL_MASK(x) ((3 << (2 * (x))))

enum max14906_iec_type {
    MAX14906_TYPE_1_3,
    MAX14906_TYPE_2,
};

/**
 * @brief Channel configuration options.
 */
enum max14906_function {
    MAX14906_OUT,
    MAX14906_IN,
    MAX14906_HIGH_Z
};

/**
 * @brief Configuration options for the output driver (on each channel).
 */
enum max14906_do_mode {
    MAX14906_HIGH_SIDE,
    MAX14906_HIGH_SIDE_INRUSH,
    MAX14906_PUSH_PULL_CLAMP,
    MAX14906_PUSH_PULL
};

/**
 * @brief Current limit options for output channels.
 */
enum max14906_climit {
    MAX14906_CL_600 = 0,
    MAX14906_CL_130 = 1,
    MAX14906_CL_300 = 2,
    MAX14906_CL_1200 = 3,
};


class Max14906 {
public:
    Max14906(uint8_t csPin);
    ~Max14906();

    int max149x6_reg_read(struct max149x6_desc *, uint32_t, uint32_t *);
    int max149x6_reg_update(struct max149x6_desc *, uint32_t, uint32_t, uint32_t);
    int max14906_ch_get(struct max149x6_desc *, uint32_t, uint32_t *);
    int max14906_ch_set(struct max149x6_desc *, uint32_t, uint32_t);
    int max14906_ch_func(struct max149x6_desc *, uint32_t, enum max14906_function);
    int max14906_climit_set(struct max149x6_desc *, uint32_t, enum max14906_climit);
    int max14906_climit_get(struct max149x6_desc *, uint32_t, enum max14906_climit *);
    int max14906_init(struct max149x6_desc **, struct max149x6_init_param *);
    int max14906_remove(struct max149x6_desc *);

private:
    uint8_t csPin;

    void spiWrite(uint8_t reg, uint8_t value);
    uint8_t spiRead(uint8_t reg);
};

Max14906::Max14906(uint8_t csPin) : csPin(csPin) {
    // Constructor implementation (if any)
}

Max14906::~Max14906() {
    // Destructor implementation (if any)
}

int Max14906::max149x6_reg_read(struct max149x6_desc *desc, uint32_t reg, uint32_t *val) {
    // Implementation of register read
        digitalWrite(desc->cs_pin, LOW);

    // Send the register address we want to read from
    SPI.transfer(reg);

    // Read the value from the register
    uint32_t read_val = SPI.transfer(0x00);

    // Deselect the device by setting the chip select pin high
    digitalWrite(desc->cs_pin, HIGH);

    // Debug print to show the register being read and the value obtained
    Serial.print("Reading register ");
    Serial.print(reg, HEX);
    Serial.print(": value = ");
    Serial.println(read_val, HEX);

    *val = read_val;
    return 0; // Return 0 for success
}

int Max14906::max149x6_reg_update(struct max149x6_desc *desc, uint32_t reg, uint32_t mask, uint32_t val) {
    // Read the current register value
    uint32_t current_val;
    int ret = Max14906::max149x6_reg_read(desc, reg, &current_val);
    if (ret) {
        Serial.println("Failed to read register");
        return ret;
    }

    // Apply the mask and update the value
    uint32_t new_val = (current_val & ~mask) | (val & mask);
    Serial.print("Updating register ");
    Serial.print(reg, HEX);
    Serial.print(": current value = ");
    Serial.print(current_val, HEX);
    Serial.print(", new value = ");
    Serial.println(new_val, HEX);

    // Write the new value back to the register
    Max14906::spiWrite(reg, new_val);
    return 0; // Return 0 for success
}


int Max14906::max14906_ch_func(struct max149x6_desc *desc, uint32_t ch, enum max14906_function function) {
    uint8_t setout_reg_val;
    int ret = 0;

    switch (function) {
    case MAX14906_HIGH_Z:
        setout_reg_val = MAX14906_IN;
        ret = max149x6_reg_update(desc, MAX14906_CONFIG_DO_REG, MAX14906_DO_MASK(ch),
                                  MAX14906_PUSH_PULL);
        break;

    case MAX14906_IN:
        setout_reg_val = MAX14906_IN;
        ret = max149x6_reg_update(desc, MAX14906_CONFIG_DO_REG, MAX14906_DO_MASK(ch),
                                  MAX14906_HIGH_SIDE);
        break;

    case MAX14906_OUT:
        setout_reg_val = MAX14906_OUT;
        break;

    default:
        return -EINVAL;
    }

    if (ret)
        return ret;

    return max149x6_reg_update(desc, MAX14906_SETOUT_REG, MAX14906_CH_DIR_MASK(ch),
                               setout_reg_val);
}

int Max14906::max14906_ch_get(struct max149x6_desc *desc, uint32_t ch, uint32_t *val) {
    int ret;

    if (ch >= MAX14906_CHANNELS)
        return -EINVAL;

    ret = max149x6_reg_read(desc, MAX14906_DOILEVEL_REG, val);
    if (ret)
        return ret;

    *val = (MAX14906_DOI_LEVEL_MASK(ch) & *val) ? 1 : 0;

    return 0;
}

int Max14906::max14906_ch_set(struct max149x6_desc *desc, uint32_t ch, uint32_t val) {
    if (ch >= MAX14906_CHANNELS)
        return -EINVAL;

    return max149x6_reg_update(desc, MAX14906_SETOUT_REG,
                               MAX14906_HIGHO_MASK(ch), (val) ?
                               MAX14906_HIGHO_MASK(ch) : 0);
}

int Max14906::max14906_climit_set(struct max149x6_desc *desc, uint32_t ch, enum max14906_climit climit) {
    Serial.print("Setting current limit for channel ");
    Serial.print(ch);
    Serial.print(" to ");
    Serial.println(climit);

    uint32_t mask = MAX14906_CL_MASK(ch);
    uint32_t val = climit << (2 * ch);

    Serial.print("Mask: ");
    Serial.println(mask, HEX);
    Serial.print("Value to set: ");
    Serial.println(val, HEX);

    return max149x6_reg_update(desc, MAX14906_CONFIG_CURR_LIM, mask, val);
}

int Max14906::max14906_climit_get(struct max149x6_desc *desc, uint32_t ch, enum max14906_climit *climit) {
    uint32_t reg_val;
    int ret;

    if (!climit)
        return -EINVAL;

    ret = max149x6_reg_read(desc, MAX14906_CONFIG_CURR_LIM, reg_val);
    if (ret)
        return ret;

    *climit = (enum max14906_climit)((reg_val & MAX14906_CL_MASK(ch)) >> (2 * ch));

    Serial.print("Current limit for channel ");
    Serial.print(ch);
    Serial.print(" is ");
    Serial.println(*climit);

    return 0;
}



int Max14906::max14906_init(struct max149x6_desc **desc, struct max149x6_init_param *param) {
    struct max149x6_desc *descriptor;
    uint32_t reg_val;
    int ret;
    int i;

    descriptor = (struct max149x6_desc *)calloc(1, sizeof(*descriptor));
    if (!descriptor)
        return -ENOMEM;

    descriptor->chip_address = param->chip_address;
    descriptor->cs_pin = param->cs_pin;
    descriptor->en_pin = param->en_pin;
    descriptor->fault_pin = param->fault_pin;
    descriptor->ready_pin = param->ready_pin;
    descriptor->synch_pin = param->synch_pin;
    descriptor->crc_en = param->crc_en;

    pinMode(descriptor->cs_pin, OUTPUT);
    digitalWrite(descriptor->cs_pin, HIGH);

    pinMode(descriptor->en_pin, OUTPUT);
    digitalWrite(descriptor->en_pin, HIGH);
    pinMode(descriptor->fault_pin, INPUT);
    pinMode(descriptor->ready_pin, INPUT);
    pinMode(descriptor->synch_pin, OUTPUT);
    digitalWrite(descriptor->synch_pin, HIGH);

    SPI.begin();

    /* Clear the latched faults generated at power up */
    ret = max149x6_reg_read(descriptor, MAX14906_OVR_LD_REG, reg_val);
    if (ret)
        goto err;

    ret = max149x6_reg_read(descriptor, MAX14906_OPN_WIR_FLT_REG, reg_val);
    if (ret)
        goto err;

    ret = max149x6_reg_read(descriptor, MAX14906_SHD_VDD_FLT_REG, reg_val);
    if (ret)
        goto err;

    ret = max149x6_reg_read(descriptor, MAX14906_GLOBAL_FLT_REG, reg_val);
    if (ret)
        goto err;

    for (i = 0; i < MAX14906_CHANNELS; i++) {
        ret = max14906_ch_func(descriptor, i, MAX14906_HIGH_Z);
        if (ret)
            goto err;
    }

    *desc = descriptor;

    return 0;

err:
    free(descriptor);
    return ret;
}

int Max14906::max14906_remove(struct max149x6_desc *desc) {
    int ret;
    int i;

    if (!desc)
        return -ENODEV;

    for (i = 0; i < MAX14906_CHANNELS; i++) {
        ret = max14906_ch_func(desc, i, MAX14906_HIGH_Z);
        if (ret)
            return ret;
    }

    SPI.end();

    free(desc);

    return 0;
}

void Max14906::spiWrite(uint8_t reg, uint8_t value) {
    digitalWrite(csPin, LOW);
    SPI.transfer(reg);
    SPI.transfer(value);
    digitalWrite(csPin, HIGH);
}

uint8_t Max14906::spiRead(uint8_t reg) {
    digitalWrite(csPin, LOW);
    SPI.transfer(reg);
    uint8_t value = SPI.transfer(0x00);
    digitalWrite(csPin, HIGH);
    return value;
}

struct max149x6_desc *max14906;
struct max149x6_init_param max14906_ip; // Changed from pointer to structure
Max14906 Max14906(CS);



//--------------------Functions --------------------------------------------------------------//

void test_read_channel() {
    uint32_t val;
    int ret = Max14906.max14906_ch_get(max14906, 0, &val); // Read state of channel 0
    if (ret) {
        Serial.println("Failed to read channel state");
    } else {
        Serial.print("Channel 0 state: ");
        Serial.println(val);
    }
}

void test_set_channel() {
    int ret = Max14906.max14906_ch_set(max14906, 0, 1); // Set channel 0 to high
    if (ret) {
        Serial.println("Failed to set channel state");
    } else {
        Serial.println("Channel 0 set to high");
    }
}

void test_configure_channel() {
    int ret = Max14906.max14906_ch_func(max14906, 0, MAX14906_OUT); // Configure channel 0 as output
    if (ret) {
        Serial.println("Failed to configure channel function");
    } else {
        Serial.println("Channel 0 configured as output");
    }
}

void test_set_current_limit() {
    int ret = Max14906.max14906_climit_set(max14906, 0, MAX14906_CL_600); // Set current limit for channel 0
    if (ret) {
        Serial.println("Failed to set current limit");
    } else {
        Serial.println("Current limit for channel 0 set to 600mA");
    }
}

void test_read_current_limit() {
    enum max14906_climit climit;
    int ret = Max14906.max14906_climit_get(max14906, 0, &climit); // Read current limit for channel 0
    if (ret) {
        Serial.println("Failed to read current limit");
    } else {
        Serial.print("Current limit for channel 0: ");
        Serial.println(climit);
    }
}





void setup() {
    char msg[] = "FEI Dev";

    Serial.begin(115200);
    pinMode(CS, OUTPUT);

    // SPI Transaction: sends the contents of buffer, and overwrites it with the received data.
    digitalWrite(CS, LOW);
    SPI.begin();
    SPI.transfer(msg);
    SPI.end();
    digitalWrite(CS, HIGH);

    Serial.println("Data received from eSPI device:");
    Serial.println(msg);

    int ret = Max14906.max14906_init(&max14906, &max14906_ip); // Pass the address of the structure
    if (ret) {
        Serial.println("MAX14906 Initialization Failed");
    }
    else{
      Serial.println("MAX14906 Initialized Successful");
    }

    test_read_channel();
    test_set_channel();
    test_configure_channel();
    test_set_current_limit();
    test_read_current_limit();
}

void loop() {
    // Your loop code here
}