#include <stdio.h>
//#include "pico/stdlib.h"
#include "hardware/i2c.h"
#include "hardware/spi.h"
#include "pico/cyw43_arch.h"

static const unsigned int I2C_SDA = 4;
static const unsigned int I2C_SCL = 5;
const uint LED_PIN = 14;

// Pins spi
const uint cs_pin = 17;
const uint sck_pin = 18;
const uint mosi_pin = 19;
const uint miso_pin = 16;

// Registers
static const uint8_t REG_DEVID = 0x00;
static const uint8_t REG_POWER_CTL = 0x2D;
static const uint8_t REG_DATAX0 = 0x32;

enum bank_t{bank0 = 0, bank1 = 1};


void error()
{
    cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1);
    gpio_put(LED_PIN, 0);
    while (true)
        ;
}

// Write 1 byte to the specified register 
// Quelle: https://www.digikey.de/en/maker/projects/raspberry-pi-pico-rp2040-spi-example-with-micropython-and-cc

void spi_init(){
    // Pins
    const uint cs_pin = 17;
    const uint sck_pin = 18;
    const uint mosi_pin = 19;
    const uint miso_pin = 16;

    // Buffer to store raw reads
    uint8_t data[6];

    // Ports
    spi_inst_t *spi = spi0;

    // Initialize CS pin high
    gpio_init(cs_pin);
    gpio_set_dir(cs_pin, GPIO_OUT);
    gpio_put(cs_pin, 1);

    // Initialize SPI port at 1 MHz
    spi_init(spi, 1000 * 1000);

    // Set SPI format
    spi_set_format( spi0,   // SPI instance
                    8,      // Number of bits per transfer
                    SPI_CPOL_0,      // Polarity (CPOL)
                    SPI_CPHA_0,      // Phase (CPHA)
                    SPI_MSB_FIRST);

    // Initialize SPI pins
    gpio_set_function(sck_pin, GPIO_FUNC_SPI);
    gpio_set_function(mosi_pin, GPIO_FUNC_SPI);
    gpio_set_function(miso_pin, GPIO_FUNC_SPI);

    // Workaround: perform throw-away read to make SCK idle high
    // reg_read(spi, cs_pin, REG_DEVID, data, 1);
}
void reg_write( spi_inst_t *spi, 
                const uint cs, 
                const uint8_t reg, 
                const uint8_t data) {

    uint8_t msg[2];
                
    // Construct message (set ~W bit low, MB bit low)
    msg[0] = 0x00 | reg;
    msg[1] = data;

    // Write to register
    gpio_put(cs, 0);
    spi_write_blocking(spi, msg, 2);
    gpio_put(cs, 1);
}

int spi_readGpio(spi_inst_t *spi,
                 const uint8_t io_expander,
                 const uint cs,
                 const bank_t bank,
                 uint8_t *inBuffer) {

    int num_bytes_read = 0;
    uint8_t mcpSelect[2] = {0,0};

    mcpSelect[0] = io_expander + 0x20;
    switch (bank)
    {
    case bank0:
        mcpSelect[1] = 0x09;
        break;
    
    case bank1:
        mcpSelect[1] = 0x19;
        break;
    }

    // Read from register
    gpio_put(cs, 0);
    spi_write_blocking(spi, mcpSelect, 2);
    num_bytes_read = spi_read_blocking(spi, 0, inBuffer, 1);
    gpio_put(cs, 1);

    return num_bytes_read;
}

uint8_t i2c_writeRegister(uint8_t io_expander, uint8_t mcp_register, uint8_t mcp_value)
{
    uint8_t src[2];
    uint8_t result = -1;
    src[0] = mcp_register; // 0x15;
    src[1] = mcp_value;    // 0x01;
    result = i2c_write_blocking(i2c_default, io_expander, src, 2, false);
    printf("i2c %x.%x: %x result: %x\n", io_expander, mcp_register, mcp_value, result);
    return result;
}

uint8_t i2c_writeGpio(uint8_t io_expander,
                      uint8_t mcp_register, 
                      uint8_t mcp_value)
{
    return -1;
}

int i2c_readGpio(spi_inst_t *spi,
             const uint cs,
             const uint8_t mcp_register,
             uint8_t *buf) {

                return -1;
}
int main()
{

    const int io_expander0 = 0x20;
    const int io_expander1 = 0x21;
    const int io_expander2 = 0x22;
    uint8_t inBuffer[6] = {0x00};

    //stdio_init_all();

    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);
    i2c_init(i2c_default, 48 * 1000); // 48000);
    spi_init();
    gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C);
    gpio_set_function(PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C);
    gpio_pull_up(PICO_DEFAULT_I2C_SDA_PIN);
    gpio_pull_up(PICO_DEFAULT_I2C_SCL_PIN);

    printf("hello blink");
    if (cyw43_arch_init())
    {
        printf("Wi-Fi init failed");
        return -1;
    }
    // This example will use I2C0 on the default SDA and SCL pins (GP4, GP5 on a Pico)

    sleep_ms(500);
    // src[0] = 0x01;
    // src[1] = 0x00;
    // result = i2c_write_blocking(i2c_default, io_expander2, src, 2, false);
    // src[0] = 0x01;
    // src[1] = 0x00;
    // result = i2c_write_blocking(i2c_default, io_expander0, src, 2, false);
    // i2c_writeRegister(io_expander0, 0x00, 0x00);
    // i2c_writeRegister(io_expander0, 0x01, 0x00);

    // i2c_writeRegister(io_expander1, 0x00, 0x00);
    // i2c_writeRegister(io_expander1, 0x01, 0x00);

    // i2c_writeRegister(io_expander2, 0x00, 0x00);
    // i2c_writeRegister(io_expander2, 0x01, 0x00);
    
    printf("Hello, world!");
    // printf("i2c result: %d\n", result);
    // if (2 != result)
    //     error();

    while (true)
    {
        // src[0] = 0x00;
        // src[1] = 0x00;
        // result = i2c_write_blocking(i2c_default, io_expander0, src, 2, false);
        // printf("i2c result: %d\n", result);
        // src[0] = 0x15;
        // src[1] = 0x01;
        // result = i2c_write_blocking(i2c_default, io_expander0, src, 2, false);
        // printf("i2c 0.1.0 ON result: %d\n", result);
        // src[0] = 0x15;
        // src[1] = 0x00;
        // result = i2c_write_blocking(i2c_default, io_expander2, src, 2, false);
        // printf("i2c 2.1.0 OFF result: %d\n", result);
        //  if (2 != result)
        //      error();
        // i2c_writeRegister(io_expander0, 0x15, 0x01);
        // i2c_writeRegister(io_expander1, 0x15, 0x00);
        // i2c_writeRegister(io_expander2, 0x15, 0x00);
        // gpio_put(LED_PIN, 1);
        // cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 0);
        // sleep_ms(1000);
        // i2c_writeRegister(io_expander0, 0x15, 0x00);
        // i2c_writeRegister(io_expander1, 0x15, 0x01);
        // i2c_writeRegister(io_expander2, 0x15, 0x00);
        // gpio_put(LED_PIN, 0);
        // cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1);
        // sleep_ms(1000);
        // i2c_writeRegister(io_expander0, 0x15, 0x00);
        // i2c_writeRegister(io_expander1, 0x15, 0x00);
        // i2c_writeRegister(io_expander2, 0x15, 0x01);

        spi_readGpio(spi0, 0, cs_pin, bank0, inBuffer);
        gpio_put(LED_PIN, 1);
        // cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1);
        sleep_ms(1000);
    }    
        
        
        // src[0] = 0x15;
        // src[1] = 0x00;
        // result = i2c_write_blocking(i2c_default, io_expander2, src, 2, false);
        // printf("i2c 0.1.0 OFF result: %d\n", result);
        // src[0] = 0x15;
        // src[1] = 0x01;
        // result = i2c_write_blocking(i2c_default, io_expander0, src, 2, false);
        // printf("i2c 2.1.0 ON  result: %d\n", result);
        // // if (2 != result)
        // //     error();
        // gpio_put(LED_PIN, 0);
        // cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 0);
        // sleep_ms(1000);
        // printf("blink\n");
    
    return 0;
}