/* Board */
#include "pin_mux.h"

/* SDK drivers */
#include "fsl_clock.h"
#include "fsl_common.h"
#include "fsl_common_arm.h"
#include "fsl_gpio.h"

/* App */
#include "app_i2c_if.h"
#include "app_reg_if.h"

#define APP_I2C_INSTANCE I2C0
#define APP_I2C_ADDR     (0x28U)

#define APP_I2C_WRITE_LEN (3)
#define APP_I2C_READ_LEN  (2)

uint8_t s_app_i2c_write_ptr;
uint8_t s_app_i2c_write_buffer[APP_I2C_WRITE_LEN];

uint8_t s_app_i2c_read_ptr;
uint8_t s_app_i2c_read_reg_addr;
uint8_t s_app_i2c_read_buffer[APP_I2C_READ_LEN];

int app_i2c_if_init(void) {
    CLOCK_Select(kI2C0_Clk_From_MainClk);
    CLOCK_EnableClock(kCLOCK_I2c0);

    RESET_PeripheralReset(kI2C0_RST_N_SHIFT_RSTn);

    APP_I2C_INSTANCE->CLKDIV = 2U;

    APP_I2C_INSTANCE->SLVADR[0] = I2C_SLVADR_SLVADR(APP_I2C_ADDR);  // Use slave address 0
    APP_I2C_INSTANCE->SLVQUAL0  = 0UL;                              // Use the address as-is

    APP_I2C_INSTANCE->INTENCLR =
        I2C_INTENCLR_MSTPENDINGCLR(1) | I2C_INTENCLR_MSTARBLOSSCLR(1) | I2C_INTENCLR_MSTSTSTPERRCLR(1) |
        I2C_INTENCLR_SLVPENDINGCLR(1) | I2C_INTENCLR_SLVNOTSTRCLR(1) | I2C_INTENCLR_SLVDESELCLR(1) |
        I2C_INTENCLR_MONRDYCLR(1) | I2C_INTENCLR_MONIDLECLR(1) | I2C_INTENCLR_EVENTTIMEOUTCLR(1) |
        I2C_INTENCLR_SCLTIMEOUTCLR(1);  // Disable all interrupts

    APP_I2C_INSTANCE->STAT = I2C_STAT_MSTARBLOSS(1) | I2C_STAT_MSTSTSTPERR(1) | I2C_STAT_SLVDESEL(1) |
                             I2C_STAT_MONOV(1) | I2C_STAT_MONIDLE(1) | I2C_STAT_EVENTTIMEOUT(1) |
                             I2C_STAT_SCLTIMEOUT(1);  // Clear all interrupt flags

    APP_I2C_INSTANCE->INTENSET = I2C_INTENSET_SLVPENDINGEN(1) | I2C_INTENSET_SLVDESELEN(1);  // Enable slave interrupts

    APP_I2C_INSTANCE->CFG = I2C_CFG_SLVEN(1);  // Enable Slave mode

    s_app_i2c_write_ptr = 0U;
    s_app_i2c_read_ptr  = 0U;

    NVIC_SetPriority(I2C0_IRQn, 0);
    EnableIRQ(I2C0_IRQn);

    return 0;
}

void I2C0_IRQHandler(void) {
    uint32_t flags = APP_I2C_INSTANCE->STAT;

    /* Clear interrupt flags */
    APP_I2C_INSTANCE->STAT |= flags;
    /* Slave requires software servicing */
    if (flags & I2C_STAT_SLVPENDING_MASK) {
        uint32_t slave_state = (flags & I2C_STAT_SLVSTATE_MASK);

        if (slave_state == I2C_STAT_SLVSTATE(0)) {
            /* A new address cycle has started, slave is selected with address match */

            /* Ack the address */
            APP_I2C_INSTANCE->SLVCTL = I2C_SLVCTL_SLVCONTINUE(1);
        } else if (slave_state == I2C_STAT_SLVSTATE(1)) {
            /* Data is ready to be read out */
            if (s_app_i2c_write_ptr == APP_I2C_WRITE_LEN) {
                /* Master sent too much data, Nack. */
                APP_I2C_INSTANCE->SLVCTL = I2C_SLVCTL_SLVNACK(1);
            } else {
                s_app_i2c_write_buffer[s_app_i2c_write_ptr] = APP_I2C_INSTANCE->SLVDAT;
                s_app_i2c_write_ptr++;

                APP_I2C_INSTANCE->SLVCTL = I2C_SLVCTL_SLVCONTINUE(1);
            }
        } else if (slave_state == I2C_STAT_SLVSTATE(2)) {
            /* Data needs to be written to host */

            if (s_app_i2c_write_ptr < APP_I2C_READ_LEN) {
                if (s_app_i2c_read_ptr == 0U) {
                    /* First read, read register to buffer */
                    uint16_t reg_data = app_reg_if_read(s_app_i2c_read_reg_addr);

                    s_app_i2c_read_buffer[0] = reg_data & 0xFFU;
                    s_app_i2c_read_buffer[1] = (reg_data >> 8U) & 0xFFU;
                }

                APP_I2C_INSTANCE->SLVDAT = s_app_i2c_read_buffer[s_app_i2c_read_ptr];
                s_app_i2c_read_ptr++;
            } else {
                /* Max length reached, start a new register read with self-increased address */
                s_app_i2c_read_reg_addr++;
                s_app_i2c_read_ptr = 0U;

                uint16_t reg_data = app_reg_if_read(s_app_i2c_read_reg_addr);

                s_app_i2c_read_buffer[0] = reg_data & 0xFFU;
                s_app_i2c_read_buffer[1] = (reg_data >> 8U) & 0xFFU;

                APP_I2C_INSTANCE->SLVDAT = s_app_i2c_read_buffer[s_app_i2c_read_ptr];
                s_app_i2c_read_ptr++;
            }

            APP_I2C_INSTANCE->SLVCTL = I2C_SLVCTL_SLVCONTINUE(1);
        }
    }

    if (flags & I2C_STAT_SLVDESEL_MASK) {
        /* Slave is de-selected due to another address cycle or STOP event */

        if (s_app_i2c_write_ptr == APP_I2C_WRITE_LEN) {
            /* Complete write, write register. */

            uint16_t reg_data = (s_app_i2c_write_buffer[2] << 8U) | s_app_i2c_write_buffer[1];

            app_reg_if_write(s_app_i2c_write_buffer[0], reg_data);

        } else if (s_app_i2c_write_ptr == 1) {
            /* Select register to read */
            s_app_i2c_read_reg_addr = s_app_i2c_write_buffer[0];
        }

        s_app_i2c_write_ptr = 0U;
        s_app_i2c_read_ptr  = 0U;
    }
}