/*
 * Copyright 2021 NXP
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

/*  Standard C Included Files */
#include <string.h>
/*  SDK Included Files */
#include "pin_mux.h"
#include "clock_config.h"
#include "board.h"
#include "fsl_debug_console.h"
#include "fsl_component_i3c_adapter.h"
#include "fsl_sensor_common.h"
#include "fsl_icm42688p.h"
#include "fsl_p3t11xx.h"

/*******************************************************************************
 * Definitions
 ******************************************************************************/
#define EXAMPLE_MASTER             I3C0
#define EXAMPLE_I2C_BAUDRATE       400000
#define I3C_MASTER_CLOCK_FREQUENCY CLOCK_GetI3cClkFreq()
#define I3C_MASTER_SLAVE_ADDR_7BIT 0x01A

#define ICM42688_STATIC_I2C_ADDR   0x69U
#define P3T1175_STATIC_I2C_ADDR    0x4CU
#define P3T1108_STATIC_I2C_ADDR    0x48U

#define ITRC_STATUS *((unsigned int *)0x5000F020)
#define DISABLE_ITR_RESET *((unsigned int *)0x5000F028)
#define DISABLE_ITR_RESET1 *((unsigned int *)0x5000F02C)
#define DISABLE_GDET_ITR *((unsigned int *)0x50030040)

/*******************************************************************************
 * Prototypes
 ******************************************************************************/
void icm42688p_ibi_callback(i3c_device_t *dev, const void *ibiData, uint32_t ibiLen);
void p3t1175_ibi_callback(i3c_device_t *dev, const void *ibiData, uint32_t ibiLen);
void p3t1108_ibi_callback(i3c_device_t *dev, const void *ibiData, uint32_t ibiLen);

/*******************************************************************************
 * Variables
 ******************************************************************************/
i3c_bus_t demo_i3cBus;
i3c_device_t demo_masterDev;
i3c_device_t *demo_icm42688pDev;
i3c_device_t *demo_p3t1175Dev;
i3c_device_t *demo_p3t1108Dev;

volatile bool icm42688p_ibiFlag = false;
uint8_t icm42688p_ibiData[16];
uint8_t icm42688p_ibiLen;
static i3c_device_ibi_info_t dev_icm42688pIbi = {
    .maxPayloadLength = 1, .enabled = true, .ibiHandler = icm42688p_ibi_callback};

volatile bool p3t1175_ibiFlag = false;
static i3c_device_ibi_info_t dev_p3t1175Ibi = {
    .maxPayloadLength = 1, .enabled = true, .ibiHandler = p3t1175_ibi_callback};

volatile bool p3t1108_ibiFlag = false;
static i3c_device_ibi_info_t dev_p3t1108Ibi = {
    .maxPayloadLength = 1, .enabled = true, .ibiHandler = p3t1108_ibi_callback};

uint8_t icm42688p_sensorAddr = 0x0;
uint8_t p3t1175_sensorAddr = 0x0;
uint8_t p3t1108_sensorAddr = 0x0;

icm42688p_handle_t icmp42688p_handle;
p3t11xx_handle_t p3t1175_handle;
p3t11xx_handle_t p3t1108_handle;

i3c_master_adapter_resource_t demo_masterResource = {.base      = EXAMPLE_MASTER,
                                                     .transMode = kI3C_MasterTransferInterruptMode};
i3c_device_control_info_t i3cMasterCtlInfo        = {
    .funcs = (i3c_device_hw_ops_t *)&master_ops, .resource = &demo_masterResource, .isSecondary = false};

/*******************************************************************************
 * Code
 ******************************************************************************/

void icm42688p_ibi_callback(i3c_device_t *dev, const void *ibiData, uint32_t ibiLen)
{
    icm42688p_ibiFlag = true;
    icm42688p_ibiLen  = ibiLen;
    memcpy(icm42688p_ibiData, ibiData, icm42688p_ibiLen);
}

void p3t1175_ibi_callback(i3c_device_t *dev, const void *ibiData, uint32_t ibiLen)
{
    p3t1175_ibiFlag = true;
}

void p3t1108_ibi_callback(i3c_device_t *dev, const void *ibiData, uint32_t ibiLen)
{
    p3t1108_ibiFlag = true;
}

status_t I3C_WriteSensor(i3c_device_t *device_handle, uint8_t regAddress, uint8_t *regData, size_t dataSize)
{
    status_t result = kStatus_Success;
    i3c_bus_transfer_t busXfer;
    memset(&busXfer, 0, sizeof(busXfer));

    busXfer.regAddr     = regAddress;
    busXfer.regAddrSize = 1;
    busXfer.isRead      = false;
    busXfer.data        = regData;
    busXfer.dataSize    = dataSize;

    result = I3C_BusMasterDoTransferToI3CDev(&demo_masterDev, device_handle, &busXfer);
    return result;
}

status_t I3C_ReadSensor(i3c_device_t *device_handle, uint8_t regAddress, uint8_t *regData, size_t dataSize)
{
    status_t result = kStatus_Success;
    i3c_bus_transfer_t busXfer;
    memset(&busXfer, 0, sizeof(busXfer));

    busXfer.regAddr     = regAddress;
    busXfer.regAddrSize = 1;
    busXfer.isRead      = true;
    busXfer.data        = regData;
    busXfer.dataSize    = dataSize;

    result = I3C_BusMasterDoTransferToI3CDev(&demo_masterDev, device_handle, &busXfer);
    return result;
}

static status_t I3C_BusMasterGetBCR(i3c_device_t *master, i3c_device_information_t *info)
{
    uint8_t bcr;
    i3c_ccc_cmd_t getBCRCmd = {0};
    status_t result         = kStatus_Success;

    getBCRCmd.isRead   = true;
    getBCRCmd.cmdId    = I3C_BUS_CCC_GETBCR;
    getBCRCmd.destAddr = info->dynamicAddr;
    getBCRCmd.data     = &bcr;
    getBCRCmd.dataSize = 1U;

    result = I3C_BusMasterSendCCC(master, &getBCRCmd);

    info->bcr = bcr;

    return result;
}

static status_t I3C_BusMasterGetMaxReadLength(i3c_device_t *master, i3c_device_information_t *info)
{
    i3c_ccc_cmd_t getMRLCmd = {0};
    status_t result         = kStatus_Success;

    getMRLCmd.isRead   = true;
    getMRLCmd.cmdId    = I3C_BUS_CCC_GETMRL;
    getMRLCmd.destAddr = info->dynamicAddr;
    getMRLCmd.data     = malloc(3U);
    getMRLCmd.dataSize = 3U;

    /*
     * When the device does not have IBI payload GETMRL only returns 2
     * bytes of data.
     */
    if ((info->bcr & I3C_BUS_DEV_BCR_IBI_PAYLOAD_MASK) == 0U)
    {
        getMRLCmd.dataSize -= 1U;
    }

    result = I3C_BusMasterSendCCC(master, &getMRLCmd);

    uint8_t *pData = getMRLCmd.data;
    if ((info->bcr & I3C_BUS_DEV_BCR_IBI_PAYLOAD_MASK) != 0U)
    {
        info->maxIBILength = pData[2];
    }

    info->maxReadLength = (uint16_t)pData[0] << 8UL | (uint16_t)pData[1];

    free(getMRLCmd.data);

    return result;
}

/*!
 * @brief Main function
 */
int main(void)
{
    icm42688p_config_t icm42688p_sensorConfig;
    p3t11xx_config_t   p3t1175_sensorConfig, p3t1108_sensorConfig;
    status_t result    = kStatus_Success;
    
    /* attach 12 MHz clock to FLEXCOMM0 (debug console) */
    CLOCK_SetClkDiv(kCLOCK_DivFlexcom0Clk, 0u, false);
    CLOCK_SetClkDiv(kCLOCK_DivFlexcom0Clk, 1u, true);
    CLOCK_AttachClk(BOARD_DEBUG_UART_CLK_ATTACH);

    /* Attach main clock to I3C, 150MHz / 3 = 50MHz. */
    CLOCK_SetClkDiv(kCLOCK_DivI3cFclk, 0U, true);
    CLOCK_SetClkDiv(kCLOCK_DivI3cFclk, 3U, false);
    CLOCK_AttachClk(kMAIN_CLK_to_I3CFCLK);

    BOARD_InitPins();
	DISABLE_ITR_RESET = 0xAAAAAAAA;
            DISABLE_ITR_RESET1 = 0xAAAAAAAA;
            DISABLE_GDET_ITR = DISABLE_GDET_ITR & 0xFFFFFFFFD;
    BOARD_BootClockPLL150M();
    BOARD_InitDebugConsole();

    demo_masterResource.clockInHz = I3C_MASTER_CLOCK_FREQUENCY;

    PRINTF("\r\nI3C bus master read ICM42688 and NXP Temperature sensor example.\r\n");

    /* Create I3C bus. */
    i3c_bus_config_t busConfig;
    I3C_BusGetDefaultBusConfig(&busConfig);
    
    /* In order to start sensor network discovery, depending on the sensors on the network,
    initiate PP baudrate needs to be slow. PP Baudrate can be set to higher after the discovery is done. */
    busConfig.i3cPushPullBaudRate  = 2000000ULL;
    busConfig.i3cOpenDrainBaudRate = EXAMPLE_I2C_BAUDRATE;
    I3C_BusCreate(&demo_i3cBus, &busConfig);

    i3c_device_information_t masterInfo;
    memset(&masterInfo, 0, sizeof(masterInfo));
    masterInfo.staticAddr  = I3C_MASTER_SLAVE_ADDR_7BIT;
    masterInfo.bcr         = 0x60U;
    masterInfo.dcr         = 0x00U;
    masterInfo.dynamicAddr = I3C_BusGetValidAddrSlot(&demo_i3cBus, 0);
    masterInfo.vendorID    = 0x345U;

    extern i3c_device_control_info_t i3cMasterCtlInfo;
    
    /* If you need do pre-assign init dynamic address to device, need to create device structure and
    add device into bus before master create. Master create will firstly do setdasa to the device list on
    bus which have init dynamic address configured, then it will dynamically probe device on
    bus and create device structure for the newly probed device. */
    I3C_BusMasterCreate(&demo_masterDev, &demo_i3cBus, &masterInfo, &i3cMasterCtlInfo);

    PRINTF("\r\nI3C bus master creates.\r\n");
    
    i3c_device_t *eachDev = NULL;

    for (list_element_handle_t listItem = demo_i3cBus.i3cDevList.head; listItem != NULL; listItem = listItem->next)
    {
        eachDev = (i3c_device_t *)listItem;
        if (eachDev == &demo_masterDev)
        {
            continue;
        }
        else
        {
            /* See sensor Reference Manual for more specific information on part # and vendor info., etc. */
            if (eachDev->info.vendorID == 0x235)
            {
                demo_icm42688pDev = eachDev;
            }
            else if (eachDev->info.vendorID == 0x11B)
            {
                if ( (eachDev->info.partNumber>>16) == 0x152A)
                {
                  demo_p3t1175Dev = eachDev;
                }
                if ( (eachDev->info.partNumber>>16) == 0x1529)
                {
                  demo_p3t1108Dev = eachDev;
                }
            }
        }
    }

    /* Set higher PP Baudrate higher for data communication after DAA. */
    i3c_baudrate_hz_t   baudrate_config;
    baudrate_config.i3cPushPullBaud  = 10000000ULL;
    baudrate_config.i3cOpenDrainBaud = 4000000ULL;
    I3C_MasterSetBaudRate(EXAMPLE_MASTER, &baudrate_config, I3C_MASTER_CLOCK_FREQUENCY);

    if (demo_icm42688pDev != NULL ) {
      icm42688p_sensorAddr = demo_icm42688pDev->info.dynamicAddr;
      PRINTF("\r\nICM42688 Dynamic addresses is %d\n", icm42688p_sensorAddr);
    }
    
    if (demo_p3t1175Dev != NULL ) {
      p3t1175_sensorAddr = demo_p3t1175Dev->info.dynamicAddr;
      PRINTF("\r\nP3T1175 Dynamic addresses is %d\n", p3t1175_sensorAddr);
    }
    
    if (demo_p3t1108Dev != NULL ) {
      p3t1108_sensorAddr = demo_p3t1108Dev->info.dynamicAddr;
      PRINTF("\r\nP3T1108 Dynamic addresses is %d\n", p3t1108_sensorAddr);
    }
    
    if (demo_icm42688pDev != NULL ) {    

      i3c_device_information_t device_info;
      memset(&device_info, 0, sizeof(device_info));
      device_info.dynamicAddr = icm42688p_sensorAddr;

      PRINTF("\r\n");
      PRINTF("I3C_BusMasterGetBCR\r\n");
      I3C_BusMasterGetBCR(&demo_masterDev, &device_info);
      PRINTF("bcr: %d\r\n", device_info.bcr);

      //SDK_DelayAtLeastUs(3 * 1000000, SystemCoreClock);

      PRINTF("I3C_BusMasterGetMaxReadLength begin\r\n");
      I3C_BusMasterGetMaxReadLength(&demo_masterDev, &device_info);
      PRINTF("maxReadLength: %d\r\n", device_info.maxReadLength);
      PRINTF("maxIBILength: %d\r\n", device_info.maxIBILength);

#if 0
      /* Set up ICM42688 Motion Tracking sensor */
      result = I3C_BusMasterRegisterDevIBI(&demo_masterDev, demo_icm42688pDev, &dev_icm42688pIbi);
      if (result != kStatus_Success)
      {
        PRINTF("\r\nICM Sensor IBI register failed.\r\n");
        return -1;
      }
        
      icm42688p_sensorConfig.Sensor_WriteTransfer = (sensor_write_transfer_func_t)I3C_WriteSensor;
      icm42688p_sensorConfig.Sensor_ReadTransfer  = (sensor_read_transfer_func_t)I3C_ReadSensor;
      icm42688p_sensorConfig.device_handle        = demo_icm42688pDev;
//    icm42688p_sensorConfig.isReset              = true;
      icm42688p_sensorConfig.isReset              = false;
    
      result = ICM42688P_Init(&icmp42688p_handle, &icm42688p_sensorConfig);
      if (result != kStatus_Success)
      {
        PRINTF("\r\nSensor reset failed.\r\n");
        return -1;
      }
    
      result = ICM42688P_EnableSensors(&icmp42688p_handle);
      if (result != kStatus_Success)
      {
        PRINTF("\r\nSensor enable failed.\r\n");
        return -1;
      }

      result = ICM42688P_ConfigureTapDetectIBI(&icmp42688p_handle);
      if (result != kStatus_Success)
      {
        PRINTF("\r\nEnable TAP detect IBI failed.\r\n");
        return -1;
      }

      uint8_t bankSel = 0;
      /* Select bank 0 to read sensor data.*/
      result = ICM42688P_WriteReg(&icmp42688p_handle, BANK_SEL, &bankSel, 1);
      if (result != kStatus_Success)
      {
        PRINTF("\r\nSelect sensor bank 0 failure.\r\n");
        return -1;
      }
    }

    if (demo_p3t1175Dev != NULL ) {		
      /* Set up NXP P3T1175 temperature sensor */
      result = I3C_BusMasterRegisterDevIBI(&demo_masterDev, demo_p3t1175Dev, &dev_p3t1175Ibi);
      if (result != kStatus_Success)
      {
        PRINTF("\r\nP3T1175 Sensor IBI register failed.\r\n");
        return -1;
      } 
      p3t1175_sensorConfig.Sensor_WriteTransfer = (sensor_write_transfer_func_t)I3C_WriteSensor;
      p3t1175_sensorConfig.Sensor_ReadTransfer  = (sensor_read_transfer_func_t)I3C_ReadSensor;
      p3t1175_sensorConfig.device_handle        = demo_p3t1175Dev;
      p3t1175_sensorConfig.isReset              = true;
    
      result = P3T1175_Init(&p3t1175_handle, &p3t1175_sensorConfig);
      if (result != kStatus_Success)
      {
        PRINTF("\r\nTemp Sensor reset failed.\r\n");
        return -1;
      }
#endif
    }
    
    do {} while (0);
    
#if 0
    if (demo_p3t1108Dev != NULL ) {
      /* Set up NXP P3T1108 temperature sensor */
      result = I3C_BusMasterRegisterDevIBI(&demo_masterDev, demo_p3t1108Dev, &dev_p3t1108Ibi);
      if (result != kStatus_Success)
      {
        PRINTF("\r\nP3T1108 Sensor IBI register failed.\r\n");
        return -1;
      }    
      p3t1108_sensorConfig.Sensor_WriteTransfer = (sensor_write_transfer_func_t)I3C_WriteSensor;
      p3t1108_sensorConfig.Sensor_ReadTransfer  = (sensor_read_transfer_func_t)I3C_ReadSensor;
      p3t1108_sensorConfig.device_handle        = demo_p3t1108Dev;
      p3t1108_sensorConfig.isReset              = true;

      result = P3T1108_Init(&p3t1108_handle, &p3t1108_sensorConfig);
      if (result != kStatus_Success)
      {
        PRINTF("\r\nTemp Sensor reset failed.\r\n");
        return -1;
      }
    }
    
    icm42688p_sensor_data_t sensorData = {0};    
    uint32_t temp_sensorData;
    uint32_t temp_counter = 0;

    while ((!icm42688p_ibiFlag) && (!p3t1175_ibiFlag) && (!p3t1108_ibiFlag))
    {
      if (demo_icm42688pDev != NULL ) { 
        result = ICM42688P_ReadSensorData(&icmp42688p_handle, &sensorData);
        if (result != kStatus_Success)
        {
            PRINTF("\r\nRead ICM sensor data failed.\r\n");
            return -1;
        }
        PRINTF("\r\nICM42688 Sensor Data: ACCEL X %d, Y %d, Z %d; GYRO X %d, Y %d, Z %d.", sensorData.accelDataX,
               sensorData.accelDataY, sensorData.accelDataZ, sensorData.gyroDataX, sensorData.gyroDataY,
               sensorData.gyroDataZ);
      }

      if (demo_p3t1175Dev != NULL ) {				
        result = P3T1175_ReadSensorData(&p3t1175_handle, (uint32_t *)&temp_sensorData);
        if (result != kStatus_Success)
        {
            PRINTF("\r\nRead P3T1175 TEMP sensor data failed.\r\n");
            return -1;
        }
        /* Check sign bit, if bit 11 is set, it's minus degree temperature. */
        if ( temp_sensorData & (0x1<<11) ) 
        {
          temp_sensorData = ((~temp_sensorData)&0x0FFF)+1;
          if ( temp_sensorData < 16 )
          {
            PRINTF("\r\nP3T1175 Temp Sensor counter: %d, reading %2d degree C", temp_counter, (temp_sensorData/16));
          }
          else 
          {
            PRINTF("\r\nP3T1175 Temp Sensor counter: %d, reading -%2d degree C", temp_counter, (temp_sensorData/16));
          }
        }
        else 
        {
          PRINTF("\r\nP3T1175 Temp Sensor counter: %d, reading %2d degree C", temp_counter, (temp_sensorData/16));
        }
      }
        
      if (demo_p3t1108Dev != NULL ) {
        result = P3T1108_ReadSensorData(&p3t1108_handle, (uint32_t *)&temp_sensorData);
        if (result != kStatus_Success)
        {
            PRINTF("\r\nRead P3T1108 TEMP sensor data failed.\r\n");
            return -1;
        }
        /* Check sign bit, if bit 11 is set, it's minus degree temperature. */
        if ( temp_sensorData & (0x1<<11) ) 
        {
          temp_sensorData = ((~temp_sensorData)&0x0FFF)+1;
          if ( temp_sensorData < 16 )
          {
            PRINTF("\r\nP3T1108 Temp Sensor counter: %d, reading %2d degree C", temp_counter, (temp_sensorData/16));
          }
          else 
          {
            PRINTF("\r\nP3T1108 Temp Sensor counter: %d, reading -%2d degree C", temp_counter, (temp_sensorData/16));
          }
        }
        else 
        {
          PRINTF("\r\nP3T1108 Temp Sensor counter: %d, reading %2d degree C", temp_counter, (temp_sensorData/16));
        }
      }
      temp_counter++;
      SDK_DelayAtLeastUs(100000, SystemCoreClock);
    }
    
    while (1)
    {
        if (icm42688p_ibiFlag)
        {
            PRINTF("\r\nReceived ICM42688 slave IBI request.");
            for (uint8_t count = 0; count < icm42688p_ibiLen; count++)
            {
                PRINTF(" Data 0x%x.", icm42688p_ibiData[count]);
            }
            icm42688p_ibiFlag = false;
        }
        if (p3t1175_ibiFlag)
        {
            PRINTF("\r\nReceived NXP P3T1175 slave IBI request.");
            p3t1175_ibiFlag = false;
        }
        if (p3t1108_ibiFlag)
        {
            PRINTF("\r\nReceived NXP P3T1108 slave IBI request.");
            p3t1108_ibiFlag = false;
        }
    }
#endif
}