#include <SCCB.hpp>
#include <camera.hpp>
#include "hardware/i2c.h"
#include "pico/stdlib.h"
#include "stdio.h"
#include "src/pio/i2c/pio_i2c.h"

SCCB::SCCB(uint8_t sda_pin, uint8_t scl_pin) {
    this->sda = sda_pin;
    this->scl = scl_pin;
}

SCCB::~SCCB() {}

// ctrl: 0 - i2c0, 1 - i2c1, 2 - pio0, 3 - pio1, 4 - pio1
// TODO: add proper return values when failure
// TODO: panic if scl != sda + 1
int SCCB::begin(uint8_t ctrl)
{   
    // Set up XCLK out unless it's a self-clocking camera. This must be done
    // BEFORE any I2C commands, as cam may require clock for I2C timing.
    this->ctrl = ctrl;
    if (ctrl < 2) {
        this->i2cc = ctrl == 0 ? i2c0 : i2c1;
        i2c_init(this->i2cc, 100 * 1000);
        gpio_set_function(this->sda, GPIO_FUNC_I2C);
        gpio_set_function(this->scl, GPIO_FUNC_I2C);
        gpio_pull_up(this->sda);
        gpio_pull_up(this->scl);
    }
    else {
        PIO pio;
        switch (ctrl) {
        case 2:
            this->pio = pio0;
            break;
        case 3:
            this->pio = pio1;
            break;
        case 4:
            this->pio = pio1;
            break;
        default:
            this->pio = pio0;
            break;
        };
        
        uint sm = pio_claim_unused_sm(this->pio, true);
        uint offset = pio_add_program(this->pio, &i2c_program);
        i2c_program_init(this->pio, this->sm, offset, this->sda, this->scl);
    }

    return 0;
}

int SCCB::readRegister(uint8_t reg)
{
    uint8_t buf[1];
    if (this->ctrl < 2) {
        i2c_write_blocking(this->i2cc, OV2640_ADDR, &reg, 1, true); // TODO: check if true or false is correct here
        i2c_read_blocking(this->i2cc, OV2640_ADDR, buf, 1, false);  // false - finished with bus
    }else {
        //pio_i2c_write_blocking(this->pio, this->sm, OV2640_ADDR, &reg, 1);
        //pio_i2c_read_blocking(this->pio, this->sm, OV2640_ADDR, buf, 1);
    }
    return buf[0];
}

void SCCB::writeRegister(uint8_t reg, uint8_t value)
{
    uint8_t buf[2];
    buf[0] = reg;
    buf[1] = value;

    if (this->ctrl < 2) {
        int ret = i2c_write_blocking(this->i2cc, OV2640_ADDR, buf, 2, false);
        if(ret == PICO_ERROR_GENERIC) {
            printf("Problem i2c write failed!");
            sleep_ms(1000);
        }
    }else {
        //pio_i2c_write_blocking(this->pio, this->sm, OV2640_ADDR, buf, 2);
    }
}

void SCCB::writeMaskRegister(uint8_t reg, uint8_t offset, uint8_t mask, uint8_t value)
{
    uint8_t buf[2];
    buf[0] = reg;
    buf[1] = value;

    uint8_t old_val = this->readRegister(reg);
    uint8_t new_value = (old_val & ~(mask << offset)) | ((value & mask) << offset);
    this->writeRegister(reg, new_value);
}

void SCCB::writeList(const register_val_t *cfg,
                     uint16_t len)
{
    for (int i = 0; i < len; i++)
    {
        writeRegister(cfg[i].reg, cfg[i].value);
        sleep_ms(2); // Some cams require, else lockup on init
    }
}