/**************************************************************************
MODULE:    MAIN
CONTAINS:  Example application using MicroCANopen, Accelerometer
           NXP LPC55xx derivatives with CAN interface and I2C accelerometer
COPYRIGHT: (c) Embedded Systems Academy (EmSA) 2002-2020
           All rights reserved. www.em-sa.com/nxp
DISCLAIM:  Read and understand our disclaimer before using this code!
           www.esacademy.com/disclaim.htm
           This software was written in accordance to the guidelines at
           www.esacademy.com/software/softwarestyleguide.pdf
LICENSE:   THIS IS THE NXP SDK VERSION OF MICROCANOPEN PLUS
           Licensed under a modified BSD License. See LICENSE.INFO
           file in the project root for full license information.
VERSION:   7.011, EmSA 17-APR-20
           $LastChangedDate: 2020-04-17 17:30:41 +0200 (Fr, 17 Apr 2020) $
           $LastChangedRevision: 4909 $
***************************************************************************/ 

#include "mcop_inc.h"

#include "board.h"
#include "fsl_debug_console.h"
#include "fsl_i2c.h"
#include "pin_mux.h"
#include "clock_config.h"
#include "fsl_gpio.h"

/*******************************************************************************
 * Definitions
 ******************************************************************************/
#define ACCEL_I2C_CLK_SRC I2C4_BASE
#define ACCEL_I2C_CLK_FREQ 12000000

#define I2C_RELEASE_BUS_COUNT 100U
#define I2C_BAUDRATE 100000U
#define FXOS8700_WHOAMI 0xC7U
#define MMA8451_WHOAMI 0x1AU
#define MMA8652_WHOAMI 0x4AU
#define ACCEL_STATUS 0x00U
#define ACCEL_XYZ_DATA_CFG 0x0EU
#define ACCEL_CTRL_REG1 0x2AU
/* FXOS8700 and MMA8451 have the same who_am_i register address. */
#define ACCEL_WHOAMI_REG 0x0DU
#define ACCEL_READ_TIMES 10U

/*******************************************************************************
 * Prototypes
 ******************************************************************************/
void Init_HW(void);

void BOARD_I2C_ReleaseBus(void);

static bool I2C_ReadAccelWhoAmI(void);
static bool I2C_WriteAccelReg(I2C_Type *base, uint8_t device_addr, uint8_t reg_addr, uint8_t value);
static bool I2C_ReadAccelRegs(I2C_Type *base, uint8_t device_addr, uint8_t reg_addr, uint8_t *rxBuff, uint32_t rxSize);

/*******************************************************************************
 * Variables
 ******************************************************************************/
/*  FXOS8700 and MMA8451 device address */
const uint8_t g_accel_address[] = {0x1CU, 0x1DU, 0x1EU, 0x1FU};

i2c_master_handle_t g_m_handle;

uint8_t g_accel_addr_found = 0x00;

volatile bool completionFlag = false;
volatile bool nakFlag        = false;

uint8_t databyte  = 0;
uint8_t write_reg = 0;
uint8_t readBuff[7];
uint8_t status0_value = 0;


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

static void i2c_release_bus_delay(void)
{
    uint32_t i = 0;
    for (i = 0; i < I2C_RELEASE_BUS_COUNT; i++)
    {
        __NOP();
    }
}

void BOARD_I2C_ReleaseBus(void)
{
    uint8_t i = 0;

    IOCON->PIO[1][20] &= 0xFFF1;
    IOCON->PIO[1][21] &= 0xFFF1;

    /* Drive SDA low first to simulate a start */
    GPIO_PinWrite(GPIO, 1U, 21U, 0U);
    i2c_release_bus_delay();

    /* Send 9 pulses on SCL and keep SDA high */
    for (i = 0; i < 9; i++)
    {
        GPIO_PinWrite(GPIO, 1U, 20U, 0U);
        i2c_release_bus_delay();

        GPIO_PinWrite(GPIO, 1U, 21U, 1U);
        i2c_release_bus_delay();

        GPIO_PinWrite(GPIO, 1U, 20U, 1U);
        i2c_release_bus_delay();
        i2c_release_bus_delay();
    }

    /* Send stop */
    GPIO_PinWrite(GPIO, 1U, 20U, 0U);
    i2c_release_bus_delay();

    GPIO_PinWrite(GPIO, 1U, 21U, 0U);
    i2c_release_bus_delay();

    GPIO_PinWrite(GPIO, 1U, 20U, 1U);
    i2c_release_bus_delay();

    GPIO_PinWrite(GPIO, 1U, 21U, 1U);
    i2c_release_bus_delay();
}

static void i2c_master_callback(I2C_Type *base, i2c_master_handle_t *handle, status_t status, void *userData)
{
    /* Signal transfer success when received success status. */
    if (status == kStatus_Success)
    {
        completionFlag = true;
    }
    /* Signal transfer success when received success status. */
    if ((status == kStatus_I2C_Nak) || (status == kStatus_I2C_Addr_Nak))
    {
        nakFlag = true;
    }
}

static bool I2C_ReadAccelWhoAmI(void)
{
    /*
    How to read the device who_am_I value ?
    Start + Device_address_Write , who_am_I_register;
    Repeart_Start + Device_address_Read , who_am_I_value.
    */
    uint8_t who_am_i_reg          = ACCEL_WHOAMI_REG;
    uint8_t who_am_i_value        = 0x00;
    uint8_t accel_addr_array_size = 0x00;
    bool find_device              = false;
    uint8_t i                     = 0;
    uint32_t sourceClock          = 0;

    i2c_master_config_t masterConfig;

    /*
     * masterConfig.baudRate_Bps = 100000U;
     * masterConfig.enableStopHold = false;
     * masterConfig.glitchFilterWidth = 0U;
     * masterConfig.enableMaster = true;
     */
    I2C_MasterGetDefaultConfig(&masterConfig);

    masterConfig.baudRate_Bps = I2C_BAUDRATE;

    sourceClock = ACCEL_I2C_CLK_FREQ;

    I2C_MasterInit(BOARD_ACCEL_I2C_BASEADDR, &masterConfig, sourceClock);

    i2c_master_transfer_t masterXfer;
    memset(&masterXfer, 0, sizeof(masterXfer));

    masterXfer.slaveAddress   = g_accel_address[0];
    masterXfer.direction      = kI2C_Write;
    masterXfer.subaddress     = 0;
    masterXfer.subaddressSize = 0;
    masterXfer.data           = &who_am_i_reg;
    masterXfer.dataSize       = 1;
    masterXfer.flags          = kI2C_TransferNoStopFlag;

    accel_addr_array_size = sizeof(g_accel_address) / sizeof(g_accel_address[0]);

    for (i = 0; i < accel_addr_array_size; i++)
    {
        masterXfer.slaveAddress = g_accel_address[i];

        I2C_MasterTransferNonBlocking(BOARD_ACCEL_I2C_BASEADDR, &g_m_handle, &masterXfer);

        /*  wait for transfer completed. */
        while ((!nakFlag) && (!completionFlag))
        {
        }

        nakFlag = false;

        if (completionFlag == true)
        {
            completionFlag     = false;
            find_device        = true;
            g_accel_addr_found = masterXfer.slaveAddress;
            break;
        }
    }

    if (find_device == true)
    {
        masterXfer.direction      = kI2C_Read;
        masterXfer.subaddress     = 0;
        masterXfer.subaddressSize = 0;
        masterXfer.data           = &who_am_i_value;
        masterXfer.dataSize       = 1;
        masterXfer.flags          = kI2C_TransferRepeatedStartFlag;

        I2C_MasterTransferNonBlocking(BOARD_ACCEL_I2C_BASEADDR, &g_m_handle, &masterXfer);

        /*  wait for transfer completed. */
        while ((!nakFlag) && (!completionFlag))
        {
        }

        nakFlag = false;

        if (completionFlag == true)
        {
            completionFlag = false;
            if (who_am_i_value == FXOS8700_WHOAMI)
            {
                PRINTF("Found an FXOS8700 on board , the device address is 0x%x . \r\n", masterXfer.slaveAddress);
                return true;
            }
            else if (who_am_i_value == MMA8451_WHOAMI)
            {
                PRINTF("Found an MMA8451 on board , the device address is 0x%x . \r\n", masterXfer.slaveAddress);
                return true;
            }
            else if (who_am_i_value == MMA8652_WHOAMI)
            {
                PRINTF("Found an MMA8652 on board , the device address is 0x%x . \r\n", masterXfer.slaveAddress);
                return true;
            }
            else
            {
                PRINTF("Found a device, the WhoAmI value is 0x%x\r\n", who_am_i_value);
                PRINTF("It's not MMA8451 or FXOS8700 or MMA8652. \r\n");
                PRINTF("The device address is 0x%x. \r\n", masterXfer.slaveAddress);
                return false;
            }
        }
        else
        {
            PRINTF("Not a successful i2c communication \r\n");
            return false;
        }
    }
    else
    {
        PRINTF("\r\n Do not find an accelerometer device ! \r\n");
        return false;
    }
}

static bool I2C_WriteAccelReg(I2C_Type *base, uint8_t device_addr, uint8_t reg_addr, uint8_t value)
{
    i2c_master_transfer_t masterXfer;
    memset(&masterXfer, 0, sizeof(masterXfer));

    masterXfer.slaveAddress   = device_addr;
    masterXfer.direction      = kI2C_Write;
    masterXfer.subaddress     = reg_addr;
    masterXfer.subaddressSize = 1;
    masterXfer.data           = &value;
    masterXfer.dataSize       = 1;
    masterXfer.flags          = kI2C_TransferDefaultFlag;

    /*  direction=write : start+device_write;cmdbuff;xBuff; */
    /*  direction=recive : start+device_write;cmdbuff;repeatStart+device_read;xBuff; */

    I2C_MasterTransferNonBlocking(BOARD_ACCEL_I2C_BASEADDR, &g_m_handle, &masterXfer);

    /*  wait for transfer completed. */
    while ((!nakFlag) && (!completionFlag))
    {
    }

    nakFlag = false;

    if (completionFlag == true)
    {
        completionFlag = false;
        return true;
    }
    else
    {
        return false;
    }
}

static bool I2C_ReadAccelRegs(I2C_Type *base, uint8_t device_addr, uint8_t reg_addr, uint8_t *rxBuff, uint32_t rxSize)
{
    i2c_master_transfer_t masterXfer;
    memset(&masterXfer, 0, sizeof(masterXfer));
    masterXfer.slaveAddress   = device_addr;
    masterXfer.direction      = kI2C_Read;
    masterXfer.subaddress     = reg_addr;
    masterXfer.subaddressSize = 1;
    masterXfer.data           = rxBuff;
    masterXfer.dataSize       = rxSize;
    masterXfer.flags          = kI2C_TransferDefaultFlag;

    /*  direction=write : start+device_write;cmdbuff;xBuff; */
    /*  direction=recive : start+device_write;cmdbuff;repeatStart+device_read;xBuff; */

    I2C_MasterTransferNonBlocking(BOARD_ACCEL_I2C_BASEADDR, &g_m_handle, &masterXfer);

    /*  wait for transfer completed. */
    while ((!nakFlag) && (!completionFlag))
    {
    }

    nakFlag = false;

    if (completionFlag == true)
    {
        completionFlag = false;
        return true;
    }
    else
    {
        return false;
    }
}

/*******************************************************************************
 * DOES:    USER Process, here simple I/O example
 * RETURNS: nothing
 ******************************************************************************/
void USER_Process(void)
{
// buffers to hold CANopen in and out data
static uint8_t command_byte;
static uint8_t last_command_byte;
static uint16_t pos_x, pos_y, pos_z;
static uint32_t loop_cnt;

  // work on loop cnt
  loop_cnt++;

  // Process output data, received from CANopen
  MCO_ReadProcessData(&(command_byte),1,P620001_CommandByte);
  if (command_byte != last_command_byte)
  {
    if (command_byte == 1)
    {
	    MCO_WriteProcessData(P201000_128byte_Buffer_Segmentation_Test,128,(uint8_t *)
			  "This is string ONE, copied when command byte comes in with the value '1'. See www.em-sa.com/nxp for sources to this demo.      ");
    }
    else if (command_byte == 2)
    {
	    MCO_WriteProcessData(P201000_128byte_Buffer_Segmentation_Test,128,(uint8_t *)
			  "This is string TWO, copied when command byte comes in with the value '2'. See www.em-sa.com/video for more online courses.     ");
    }
    else
    {
	    MCO_WriteProcessData(P201000_128byte_Buffer_Segmentation_Test,128,(uint8_t *)
			  "Default string in the 128byte buffer used to visualize segmented CANopen (FD) transfers. Visit us online at www.em-sa.com!     ");
    }
  }

  I2C_ReadAccelRegs(BOARD_ACCEL_I2C_BASEADDR, g_accel_addr_found, ACCEL_STATUS, &status0_value, 1);
  /*  new data ready. */
  if (status0_value == 0xff)
  {

    /*  Multiple-byte Read from STATUS (0x00) register */
    I2C_ReadAccelRegs(BOARD_ACCEL_I2C_BASEADDR, g_accel_addr_found, ACCEL_STATUS, readBuff, 7);

    status0_value = readBuff[0];
    pos_x         = ((int16_t)(((readBuff[1] * 256U) | readBuff[2]))) / 4U;
    pos_y         = ((int16_t)(((readBuff[3] * 256U) | readBuff[4]))) / 4U;
    pos_z         = ((int16_t)(((readBuff[5] * 256U) | readBuff[6]))) / 4U;

    // Process all input data, send to CANopen
    MCO_WriteProcessData(P640101_PosX,2,(uint8_t *)&(pos_x));
    MCO_WriteProcessData(P640102_PosY,2,(uint8_t *)&(pos_y));
    MCO_WriteProcessData(P640103_PosZ,2,(uint8_t *)&(pos_z));
  }

  // Process all input data, send to CANopen
  MCO_WriteProcessData(P220000_LoopCount,4,(uint8_t *)&(loop_cnt));
}


/*******************************************************************************
 * DOES:    The main function
 * RETURNS: nothing
 ******************************************************************************/
int main(
  void
  )
{
  // Initialize hardware components needed for this example
  Init_HW();

#if USE_CANOPEN_FD
  PRINTF("\r\nStarting CANopen FD Library slave example\r\n");
#else
  PRINTF("\r\nStarting CANopen Library slave example\r\n");
#endif
  PRINTF("Provided by EmSA - www.em-sa.com/nxp\r\n");

  // Reset/Initialize CANopen communication
  MCOUSER_ResetCommunication();
  
  // foreground loop
  while(1)
  {
    // Operate on CANopen protocol stack
    MCO_ProcessStack();

    // Operate on application
    if (MY_NMT_STATE == NMTSTATE_OP)
    { // only when we are operational
      USER_Process();
    }
    else if (MY_NMT_STATE == NMTSTATE_PREOP)
    { // Here Self-Autostart
    	MCO_HandleNMTRequest(NMTMSG_OP);
    }

    // Check for CAN Err, auto-recover
    if (MCOHW_GetStatus() & HW_BOFF)
    {
      MCOUSER_FatalError(0xF6F6);
    }

  } // end of while(1)
} // end of main


/*******************************************************************************
 * DOES:    Initialize hardware components needed for this example
 * RETURNS: nothing
 ******************************************************************************/
void Init_HW()
{
  /* Initialize board hardware. */
  bool isThereAccel = false;
  /* attach 12 MHz clock to FLEXCOMM0 (debug console) */
  CLOCK_AttachClk(BOARD_DEBUG_UART_CLK_ATTACH);

  /* attach 12 MHz clock to FLEXCOMM4 (I2C master) */
  CLOCK_AttachClk(kFRO12M_to_FLEXCOMM4);

  /* reset FLEXCOMM for I2C */
  RESET_PeripheralReset(kFC4_RST_SHIFT_RSTn);

  BOARD_InitPins();
#if defined(CPU_LPC54628)
  BOARD_BootClockPLL160M();
#if USE_CANOPEN_FD
  /* Set MCAN clock 160Mhz/2=80MHz for CAN FD. */
  CLOCK_SetClkDiv(kCLOCK_DivCan0Clk, 2U, true);
#else
  /* Set MCAN clock 160Mhz/8=20MHz for legacy CAN. */
  CLOCK_SetClkDiv(kCLOCK_DivCan0Clk, 8U, true);
#endif
#elif defined(CPU_LPC54608) || defined(CPU_LPC54618) || defined(CPU_LPC54S018)
  BOARD_BootClockPLL80M();
#if USE_CANOPEN_FD
  /* Set MCAN clock 80Mhz/1=80MHz for CAN FD. */
  CLOCK_SetClkDiv(kCLOCK_DivCan0Clk, 1U, true);
#else
  /* Set MCAN clock 80Mhz/4=20MHz for legacy CAN. */
  CLOCK_SetClkDiv(kCLOCK_DivCan0Clk, 4U, true);
#endif
#elif defined(LPC55S16_SERIES)
  BOARD_BootClockPLL80M();
#if USE_CANOPEN_FD
  /* Set MCAN clock 80Mhz/1=80MHz for CAN FD. */
  CLOCK_SetClkDiv(kCLOCK_DivCanClk, 1U, true);
#else
  /* Set MCAN clock 80Mhz/4=20MHz for legacy CAN. */
  CLOCK_SetClkDiv(kCLOCK_DivCanClk, 4U, true);
#endif
  CLOCK_AttachClk(kMCAN_DIV_to_MCAN);
  /* Enable FRO 1M clock for WWDT module. */
  SYSCON->CLOCK_CTRL |= SYSCON_CLOCK_CTRL_FRO1MHZ_CLK_ENA_MASK;
  /* Set clock divider for WWDT clock source. */
  CLOCK_SetClkDiv(kCLOCK_DivWdtClk, 1U, true);
#endif
  BOARD_InitDebugConsole();

#if USE_LEDS
  // Initialize status LEDs
  LIBCB_InitLeds();
#endif

  I2C_MasterTransferCreateHandle(BOARD_ACCEL_I2C_BASEADDR, &g_m_handle, i2c_master_callback, NULL);
  isThereAccel = I2C_ReadAccelWhoAmI();

  /*  read the accel xyz value if there is accel device on board */
  if (true == isThereAccel)
  {
      /*  please refer to the "example FXOS8700CQ Driver Code" in FXOS8700 datasheet. */
      /*  write 0000 0000 = 0x00 to accelerometer control register 1 */
      /*  standby */
      /*  [7-1] = 0000 000 */
      /*  [0]: active=0 */
      write_reg = ACCEL_CTRL_REG1;
      databyte  = 0;
      I2C_WriteAccelReg(BOARD_ACCEL_I2C_BASEADDR, g_accel_addr_found, write_reg, databyte);

      /*  write 0000 0001= 0x01 to XYZ_DATA_CFG register */
      /*  [7]: reserved */
      /*  [6]: reserved */
      /*  [5]: reserved */
      /*  [4]: hpf_out=0 */
      /*  [3]: reserved */
      /*  [2]: reserved */
      /*  [1-0]: fs=01 for accelerometer range of +/-4g range with 0.488mg/LSB */
      /*  databyte = 0x01; */
      write_reg = ACCEL_XYZ_DATA_CFG;
      databyte  = 0x01;
      I2C_WriteAccelReg(BOARD_ACCEL_I2C_BASEADDR, g_accel_addr_found, write_reg, databyte);

      /*  write 0000 1101 = 0x0D to accelerometer control register 1 */
      /*  [7-6]: aslp_rate=00 */
      /*  [5-3]: dr=001 for 200Hz data rate (when in hybrid mode) */
      /*  [2]: lnoise=1 for low noise mode */
      /*  [1]: f_read=0 for normal 16 bit reads */
      /*  [0]: active=1 to take the part out of standby and enable sampling */
      /*   databyte = 0x0D; */
      write_reg = ACCEL_CTRL_REG1;
      databyte  = 0x0d;
      I2C_WriteAccelReg(BOARD_ACCEL_I2C_BASEADDR, g_accel_addr_found, write_reg, databyte);
  }

  // initialize timer interrupt - 1ms period
  SysTick_Config(SystemCoreClock / 1000U);

  return;
}

/*******************************************************************************
 * DOES:    System Tick handler, execute every 1 ms
 * RETURNS: nothing
 ******************************************************************************/
void SysTick_Handler (void)
{
  MCOHW_Tick(); // Call CANopen stack tick function
}

