#include "Pico_I2C.h"

#include <hardware/gpio.h>
#include <hardware/i2c.h>
#include <pico/i2c_slave.h>

#include "Config.h"

#include <stdio.h>

void init_i2c_master(i2c_master_t *i2c_master, i2c_inst_t *i2c_inst)
{
    i2c_master->i2c_inst = i2c_inst;

    // Init GPIO pins
    gpio_init(I2C_MASTER_SDA_PIN);
    gpio_init(I2C_MASTER_SCL_PIN);
    gpio_set_function(I2C_MASTER_SDA_PIN, GPIO_FUNC_I2C);
    gpio_set_function(I2C_MASTER_SCL_PIN, GPIO_FUNC_I2C);
    gpio_pull_up(I2C_MASTER_SDA_PIN);
    gpio_pull_up(I2C_MASTER_SCL_PIN);

    i2c_init(i2c_master->i2c_inst, I2C_MASTER_BAUD_RATE);
}

void exit_i2c_master(i2c_master_t *i2c_master)
{
    // Reset GPIO pins
    gpio_deinit(I2C_MASTER_SDA_PIN);
    gpio_deinit(I2C_MASTER_SCL_PIN);
    gpio_set_function(I2C_MASTER_SDA_PIN, GPIO_FUNC_NULL);
    gpio_set_function(I2C_MASTER_SCL_PIN, GPIO_FUNC_NULL);

    i2c_deinit(i2c_master->i2c_inst);
}

static i2c_slave_t *i2c_slaves[2];

static inline void i2c_slave_buffer_handler(i2c_inst_t *i2c, i2c_slave_event_t event)
{
    i2c_slave_t *i2c_slave = i2c_slaves[i2c_get_index(i2c)];

    switch(event)
    {
      case I2C_SLAVE_RECEIVE: // master has written some data
        if(!i2c_slave->reg_written)
        {
            // writes always start with the memory address
            i2c_slave->reg = i2c_read_byte_raw(i2c);
            i2c_slave->reg_written = true;
        }
        else
        {
            // save into memory
            i2c_slave->buffer[i2c_slave->reg] = i2c_read_byte_raw(i2c);
            i2c_slave->reg++;
        }

        break;

      case I2C_SLAVE_REQUEST: // master is requesting data
        // load from memory
        i2c_write_byte_raw(i2c, i2c_slave->buffer[i2c_slave->reg]);
        i2c_slave->reg++;

        break;

      case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
        i2c_slave->reg_written = false;

        break;

      default:
        break;
    }
}

void init_i2c_slave(i2c_slave_t *i2c_slave, i2c_inst_t *i2c_inst, uint8_t address)
{
    i2c_slave->i2c_inst = i2c_inst;

    i2c_slaves[i2c_get_index(i2c_inst)] = i2c_slave;

    // Init GPIO pins
    gpio_init(I2C_SLAVE_SDA_PIN);
    gpio_init(I2C_SLAVE_SCL_PIN);
    gpio_set_function(I2C_SLAVE_SDA_PIN, GPIO_FUNC_I2C);
    gpio_set_function(I2C_SLAVE_SCL_PIN, GPIO_FUNC_I2C);

    i2c_init(i2c_slave->i2c_inst, 0);

    // New SDK method to init i2c slave
    i2c_slave_init(i2c_slave->i2c_inst, address, &i2c_slave_buffer_handler);
}

void exit_i2c_slave(i2c_slave_t *i2c_slave)
{
    // Reset GPIO pins
    gpio_deinit(I2C_SLAVE_SDA_PIN);
    gpio_deinit(I2C_SLAVE_SCL_PIN);
    gpio_set_function(I2C_SLAVE_SDA_PIN, GPIO_FUNC_NULL);
    gpio_set_function(I2C_SLAVE_SCL_PIN, GPIO_FUNC_NULL);

    // New SDK method to reset i2c slave
    i2c_slave_deinit(i2c_slave->i2c_inst);
    i2c_deinit(i2c_slave->i2c_inst);
}