/** 
 * @file    i2c.c
 * @brief   I2C Library
 * @author  Kenny Tran
 * @date    Jul 05 2022
 * @version 0.0.1
 *
 * @section LICENSES
 * Copyright (C) Morgan Advanced Programmable Systems, Inc.
 */


/*--------------------------------------------------------------------------------------------------
 * INCLUDE FILES
 *------------------------------------------------------------------------------------------------*/
#include <stdint.h>
#include <string.h>

#include <zephyr/kernel.h>
#include <zephyr/pm/device.h>
#include <zephyr/drivers/i2c.h>

#include "../lib/errno.h"
#include "../lib/debug.h"
#include "../bsp/imu_reg.h"

#include "i2c.h"


/*--------------------------------------------------------------------------------------------------
 * MACROS & CONSTANTS
 *------------------------------------------------------------------------------------------------*/
/* Ideally should be part of ctx structure but using Zephyr defines is easier. */
K_MUTEX_DEFINE(_i2c0_mutex);
K_MUTEX_DEFINE(_i2c1_mutex);

/*--------------------------------------------------------------------------------------------------
 * TYPE DEFINITIONS & ENUMERATIONS
 *------------------------------------------------------------------------------------------------*/
typedef struct _i2c_ctx
{
    const struct device *p_dev;
          bool           init;
          struct k_mutex *p_mutex;
} _i2c_ctx_s;


/*--------------------------------------------------------------------------------------------------
 * PRIVATE CONSTANTS & VARIABLES
 *------------------------------------------------------------------------------------------------*/
/* NOTE: SPI instance 0 and 1 share the same base address as TWI instance 0 and 1.
         Reserve 0 and 1 for TWI and use 2 and 3 for SPI. */
static _i2c_ctx_s _i2c_ctx_arr[I2C_CH_COUNT] =
{
    {
        .p_dev     = DEVICE_DT_GET(DT_NODELABEL(i2c0)),
        .init      = false,
        .p_mutex   = &_i2c0_mutex
    },
    {
        .p_dev     = DEVICE_DT_GET(DT_NODELABEL(i2c1)),
        .init      = false,
        .p_mutex   = &_i2c1_mutex
    },
};


/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTION PROTOTYPES
 *------------------------------------------------------------------------------------------------*/


/*--------------------------------------------------------------------------------------------------
 * PUBLIC FUNCTION DEFINITIONS
 *------------------------------------------------------------------------------------------------*/
ERRNO_e I2C_init (I2C_ch_e ch)
{
    if (ch >= I2C_CH_COUNT)
    {
        return ERRNO_LIMIT_REACHED;
    }
    else if (_i2c_ctx_arr[ch].init)
    {
        return ERRNO_ALREADY_INITIALIZED;
    }

    if (!device_is_ready (_i2c_ctx_arr[ch].p_dev))
    {
        return ERRNO_NOT_READY;
    }

    /* Set init flag upon successful initialization. */
    _i2c_ctx_arr[ch].init = true;

    return ERRNO_SUCCESS;
}

ERRNO_e I2C_suspend (void)
{
    uint8_t i;

    for (i = 0; i < I2C_CH_COUNT; i++)
    {
        if (!_i2c_ctx_arr[i].init)
        {
            return ERRNO_NOT_INITIALIZED;
        }

        /* Lock mutex */
        k_mutex_lock(_i2c_ctx_arr[i].p_mutex, K_FOREVER);

        if (pm_device_action_run (_i2c_ctx_arr[i].p_dev, PM_DEVICE_ACTION_SUSPEND) < 0)
        {
            return ERRNO_PWR_MGMT_FAILURE;
        }
    }
    
    return ERRNO_SUCCESS;
}

ERRNO_e I2C_resume (void)
{
    uint8_t i;

    for (i = 0; i < I2C_CH_COUNT; i++)
    {
        if (!_i2c_ctx_arr[i].init)
        {
            return ERRNO_NOT_INITIALIZED;
        }

        if (pm_device_action_run (_i2c_ctx_arr[i].p_dev, PM_DEVICE_ACTION_RESUME) < 0)
        {
            return ERRNO_PWR_MGMT_FAILURE;
        }

        /* Unlock mutex */
        k_mutex_unlock(_i2c_ctx_arr[i].p_mutex);
    }
    
    return ERRNO_SUCCESS;
}

ERRNO_e I2C_words_read (I2C_ch_e ch, uint8_t dev_addr, uint8_t reg_addr, uint16_t *p_dest, uint16_t len)
{
    uint16_t i;

    if (ch >= I2C_CH_COUNT)
    {
        return ERRNO_INVALID_PARAM;
    }
    else if (p_dest == NULL)
    {
        return ERRNO_BAD_POINTER;
    }
    else if (!_i2c_ctx_arr[ch].init)
    {
        return ERRNO_NOT_INITIALIZED;
    }

    /* Lock mutex */
    k_mutex_lock(_i2c_ctx_arr[ch].p_mutex, K_FOREVER);
    
    /* Write the reg_addr. */
    if (i2c_write (_i2c_ctx_arr[ch].p_dev, &reg_addr, 1, dev_addr))
    {
        k_mutex_unlock(_i2c_ctx_arr[ch].p_mutex);
        return ERRNO_I2C_FAILURE;
    }

    /* Loop through to read words. */
    for (i = 0; i < len; i++)
    {
        /* Transfer data */
        if (i2c_read (_i2c_ctx_arr[ch].p_dev, (uint8_t *) &p_dest[i], 2, dev_addr))
        {
            k_mutex_unlock(_i2c_ctx_arr[ch].p_mutex);
            return ERRNO_I2C_FAILURE;
        }
    }

    /* Unlock mutex */
    k_mutex_unlock(_i2c_ctx_arr[ch].p_mutex);
        
    return ERRNO_SUCCESS;
}

ERRNO_e I2C_words_write (I2C_ch_e ch, uint8_t dev_addr, uint8_t reg_addr, uint16_t *p_src, uint8_t len)
{
          uint8_t i;
    const uint8_t tx_len = len * sizeof (uint16_t);
          uint8_t tx_buf[tx_len];

    if (ch >= I2C_CH_COUNT)
    {
        return ERRNO_INVALID_PARAM;
    }
    else if (p_src == NULL)
    {
        return ERRNO_BAD_POINTER;
    }
    else if (!_i2c_ctx_arr[ch].init)
    {
        return ERRNO_NOT_INITIALIZED;
    }

    /* Lock mutex */
    k_mutex_lock(_i2c_ctx_arr[ch].p_mutex, K_FOREVER);

    /* Write the reg_addr. */
    if (i2c_write (_i2c_ctx_arr[ch].p_dev, &reg_addr, 1, dev_addr))
    {
        k_mutex_unlock(_i2c_ctx_arr[ch].p_mutex);
        return ERRNO_I2C_FAILURE;
    }

    /* Loop through mask and write words. */
    for (i = 0; i < len; i++)
    {
        tx_buf[(i * sizeof (uint16_t))]     = p_src[i] >> 8;
        tx_buf[(i * sizeof (uint16_t)) + 1] = p_src[i] & 0xFF;
    }

    if (i2c_write (_i2c_ctx_arr[ch].p_dev, tx_buf, tx_len, dev_addr))
    {
        k_mutex_unlock(_i2c_ctx_arr[ch].p_mutex);
        return ERRNO_I2C_FAILURE;
    }

    /* Unlock */
    k_mutex_unlock(_i2c_ctx_arr[ch].p_mutex);

    return ERRNO_SUCCESS;
}

ERRNO_e I2C_bytes_read (I2C_ch_e ch, uint8_t dev_addr, uint8_t reg_addr, uint8_t *p_dest, uint32_t len)
{
    if (ch >= I2C_CH_COUNT)
    {
        return ERRNO_INVALID_PARAM;
    }
    else if (p_dest == NULL)
    {
        return ERRNO_BAD_POINTER;
    }
    else if (!_i2c_ctx_arr[ch].init)
    {
        return ERRNO_NOT_INITIALIZED;
    }

    #ifdef CONFIG_DBG_STATS
    if (reg_addr == LSM6DS3TR_C_FIFO_DATA_OUT_L) {
        atomic_set(&i2c_transfer_active, 1);
    }
    #endif

    /* Lock mutex */
    k_mutex_lock(_i2c_ctx_arr[ch].p_mutex, K_FOREVER);

    if (i2c_write_read (_i2c_ctx_arr[ch].p_dev, dev_addr, &reg_addr, 1, p_dest, len))
    {
        k_mutex_unlock(_i2c_ctx_arr[ch].p_mutex);
        return ERRNO_I2C_FAILURE;
    }

    /* Unlock */
    k_mutex_unlock(_i2c_ctx_arr[ch].p_mutex);

    #ifdef CONFIG_DBG_STATS
    atomic_set(&i2c_transfer_active, 0);
    #endif
        
    return ERRNO_SUCCESS;
}

ERRNO_e I2C_bytes_write (I2C_ch_e ch, uint8_t dev_addr, uint8_t reg_addr, uint8_t *p_src, uint8_t len)
{
    const uint8_t  tx_len         = len + 1;
          uint8_t  tx_buf[tx_len];

    if (ch >= I2C_CH_COUNT)
    {
        return ERRNO_INVALID_PARAM;
    }
    else if (p_src == NULL)
    {
        return ERRNO_BAD_POINTER;
    }
    else if (!_i2c_ctx_arr[ch].init)
    {
        return ERRNO_NOT_INITIALIZED;
    }

    /* Write the register address and data into transmit buffer. */
    tx_buf[0] = reg_addr;
    memcpy (&tx_buf[1], p_src, len);

    /* Lock */
    k_mutex_lock(_i2c_ctx_arr[ch].p_mutex, K_FOREVER);

    if (i2c_write (_i2c_ctx_arr[ch].p_dev, tx_buf, tx_len, dev_addr))
    {
        k_mutex_unlock(_i2c_ctx_arr[ch].p_mutex);
        return ERRNO_I2C_FAILURE;
    }

    /* Unlock */
    k_mutex_unlock(_i2c_ctx_arr[ch].p_mutex);

    return ERRNO_SUCCESS;
}

ERRNO_e I2C_bits_read (I2C_ch_e ch, uint8_t dev_addr, uint8_t reg_addr, uint8_t *p_dest, uint8_t bit_start, uint8_t len)
{
    // 01101001 read byte
    // 76543210 bit numbers
    //    xxx   args: bitStart=4, length=3
    //    010   masked
    //   -> 010 shifted
    ERRNO_e err = ERRNO_I2C_FAILURE;
    uint8_t     byte;

    if (p_dest == NULL)
    {
        return ERRNO_BAD_POINTER;
    }

    err = I2C_bytes_read (ch, dev_addr, reg_addr, &byte, 1);
    if (err == ERRNO_SUCCESS)
    {
        uint8_t mask = ((1 << len) - 1) << (bit_start - len + 1);
        byte &= mask;
        byte >>= (bit_start - len + 1);
        *p_dest = byte;
    }

    return err;
}

ERRNO_e I2C_bits_write (I2C_ch_e ch, uint8_t dev_addr, uint8_t reg_addr, uint8_t data, uint8_t bit_start, uint8_t len)
{
    //      010 value to write
    // 76543210 bit numbers
    //    xxx   args: bitStart=4, length=3
    // 00011100 mask byte
    // 10101111 original value (sample)
    // 10100011 original & ~mask
    // 10101011 masked | value
    ERRNO_e err = ERRNO_I2C_FAILURE;
    uint8_t     byte;
    uint8_t     mask;

    err = I2C_bytes_read (ch, dev_addr, reg_addr, &byte, 1);

    if (err != ERRNO_SUCCESS)
    {
        return err;
    }

    /* Determine correct mask. */
    mask = ((1 << len) - 1) << (bit_start - len + 1);

    /* Shift data into correct position */
    data <<= (bit_start - len + 1);

    /* Zero non-important bits in the data. */
    data &= mask;

    /* Zero all important bits in existing byte. */
    byte &= ~(mask);

    /* Combine new data with existing byte. */
    byte |= data;

    err = I2C_bytes_write (ch, dev_addr, reg_addr, &byte, 1);

    return err;
}

ERRNO_e I2C_field_write(I2C_ch_e ch, uint8_t dev_addr, uint8_t reg_addr, uint8_t field_data, uint8_t field_start, uint8_t len)
{
    //      010 data to write
    // 76543210 bit index
    //    xxx   args: field_start=2, length=3
    // 00011100 mask
    // 00001000 data shifted
    // 00001000 data & mask
    // 10101111 original value (sample)
    // 10100011 original & ~mask
    // 10101011 (original value & ~mask) | (data & mask)
    ERRNO_e err = ERRNO_I2C_FAILURE;
    uint8_t     byte = 0;
    uint8_t     mask = 0;

    err = I2C_bytes_read (ch, dev_addr, reg_addr, &byte, 1);
    if (err != ERRNO_SUCCESS)
    {
        return err;
    }

    /* Determine correct mask. */
    mask = ((1U << len) - 1) << field_start;

    /* Shift data into correct position */
    uint8_t data_shifted = field_data << field_start;

    /* Zero non-important bits in the data. */
    data_shifted &= mask;

    // Isolate the current bits in the register and compare to the new data
    if ((byte & mask) == data_shifted) {
        // The register already holds the correct value.
        // Return SUCCESS immediately and skip the write.
        return ERRNO_SUCCESS;
    }

    /* Zero all important bits in existing byte. */
    byte &= ~(mask);

    /* Combine new data with existing byte. */
    byte |= data_shifted;

    err = I2C_bytes_write (ch, dev_addr, reg_addr, &byte, 1);

    return err;
}

ERRNO_e I2C_field_read(I2C_ch_e ch, uint8_t dev_addr, uint8_t reg_addr, uint8_t *field_data, uint8_t field_start, uint8_t len)
{
    ERRNO_e err = ERRNO_I2C_FAILURE;
    uint8_t byte = 0;
    uint8_t mask = 0;

    // 1. Safety Check
    // Since we are returning data via a pointer, we must ensure it exists.
    if (field_data == NULL) {
        return ERRNO_BAD_POINTER; 
    }

    // 2. READ the full register value
    err = I2C_bytes_read(ch, dev_addr, reg_addr, &byte, 1);
    if (err != ERRNO_SUCCESS) {
        return err;
    }

    // 3. CALCULATE Mask
    // Same logic as the write function: Create a window of 1s at the field location
    mask = ((1U << len) - 1) << field_start;

    // 4. ISOLATE the field bits
    // Clear everything outside the mask (zero out bits we don't care about)
    byte &= mask;

    // 5. NORMALIZE the value (Right Shift)
    // Move the bits down to position 0 so the value makes sense to the user.
    // Example: If reading bits [3:2] and the value is binary '10', 
    // the register holds 0x08. We shift right by 2 to get 0x02.
    byte >>= field_start;

    // 6. RETURN the value
    *field_data = byte;

    return ERRNO_SUCCESS;
}

/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTION DEFINITIONS
 *------------------------------------------------------------------------------------------------*/
