/*
 * Copyright (c) 2015, Freescale Semiconductor, Inc.
 * Copyright 2016-2019 NXP
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include "fsl_debug_console.h"
#include "fsl_flexcan.h"
#include "board.h"

#include "pin_mux.h"
#include "clock_config.h"
#include "fsl_irqsteer.h"
#include "fsl_lpi2c.h"
#include "fsl_rgpio.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rpmsg_lite.h"
#include "rpmsg_queue.h"
#include "rpmsg_ns.h"
#include "board.h"
#include "fsl_debug_console.h"
#include "FreeRTOS.h"
#include "task.h"

#include "pin_mux.h"
#include "clock_config.h"
#include "fsl_lpuart.h"
#include "fsl_irqsteer.h"
#include "app_srtm.h"
#include "automotive.h"

/*******************************************************************************
 * Definitions
 ******************************************************************************/
#define EXTENDED_FRAME 1

#define EXAMPLE_CAN_IOEXP_LPI2C CM4_1__LPI2C /* I2C instance used for CAN I/O EXPANDER on Base board */
#define EXAMPLE_CAN_IOEXP_I2C_ADDR (0x20)
#define EXAMPLE_CAN_IOEXP_LPI2C_CLOCK_FREQUENCY CLOCK_GetIpFreq(kCLOCK_M4_1_Lpi2c)

#define I2C_RELEASE_SCL_GPIO CM4_1__RGPIO
#define I2C_RELEASE_SDA_GPIO CM4_1__RGPIO
#define I2C_RELEASE_SCL_PIN (2U)
#define I2C_RELEASE_SDA_PIN (3U)

#define RX_MESSAGE_BUFFER_NUM (9)
#define TX_MESSAGE_BUFFER_NUM (8)

/* PCA6416 I2C Register Map */
#define PCA6416_REG_INPUT_PORT_0 (0x0)
#define PCA6416_REG_INPUT_PORT_1 (0x1)
#define PCA6416_REG_OUTPUT_PORT_0 (0x2)
#define PCA6416_REG_OUTPUT_PORT_1 (0x3)
#define PCA6416_REG_POLARITY_INVERSION_PORT_0 (0x4)
#define PCA6416_REG_POLARITY_INVERSION_PORT_1 (0x5)
#define PCA6416_REG_CONFIGURATION_PORT_0 (0x6)
#define PCA6416_REG_CONFIGURATION_PORT_1 (0x7)

#define EXAMPLE_CAN DMA__CAN0
#define EXAMPLE_CAN_CLK_FREQ CLOCK_GetIpFreq(kCLOCK_DMA_Can0)

#define DLC (1)

/* To get most precise baud rate under some circumstances, users need to set
   quantum which is composed of PSEG1/PSEG2/PROPSEG. Because CAN clock prescaler
   = source clock/(baud rate * quantum), for e.g. 84M clock and 1M baud rate, the
   quantum should be .e.g 14=(6+3+1)+4, so prescaler is 6. By default, quantum
   is set to 10=(3+2+1)+4, because for most platforms e.g. 120M source clock/(1M
   baud rate * 10) is an integer. Remember users must ensure the calculated
   prescaler an integer thus to get precise baud rate. */
#define SET_CAN_QUANTUM 1
#define PSEG1 6
#define PSEG2 4
#define PROPSEG 6
#define FPSEG1 6
#define FPSEG2 4
#define FPROPSEG 7
#if (defined(FSL_FEATURE_FLEXCAN_HAS_ERRATA_5829) && FSL_FEATURE_FLEXCAN_HAS_ERRATA_5829)
/* To consider the First valid MB must be used as Reserved TX MB for ERR005829
   If RX FIFO enable(RFEN bit in MCE set as 1) and RFFN in CTRL2 is set default zero, the first valid TX MB Number is 8
   If RX FIFO enable(RFEN bit in MCE set as 1) and RFFN in CTRL2 is set by other value(0x1~0xF), User should consider
   detail first valid MB number
   If RX FIFO disable(RFEN bit in MCE set as 0) , the first valid MB number is zero */
#ifdef RX_MESSAGE_BUFFER_NUM
#undef RX_MESSAGE_BUFFER_NUM
#define RX_MESSAGE_BUFFER_NUM (10)
#endif
#ifdef TX_MESSAGE_BUFFER_NUM
#undef TX_MESSAGE_BUFFER_NUM
#define TX_MESSAGE_BUFFER_NUM (9)
#endif
#endif
#ifndef DEMO_FORCE_CAN_SRC_OSC
#define DEMO_FORCE_CAN_SRC_OSC 0
#endif
/*******************************************************************************
 * Prototypes
 ******************************************************************************/
void can_send(uint8_t len, uint8_t *data, uint8_t id);
extern int console_run_command(const char *);
/*******************************************************************************
 * Variables
 ******************************************************************************/
flexcan_handle_t flexcanHandle;
volatile bool txComplete = false;
volatile bool rxComplete = false;
volatile bool wakenUp    = false;
volatile bool canDoneInit = false;
volatile unsigned long remote_addr;
/* message queue to contain vehicle states*/
extern QueueHandle_t xVStateQueue;
flexcan_mb_transfer_t txXfer, rxXfer;
#if (defined(USE_CANFD) && USE_CANFD)
flexcan_fd_frame_t frame, frame_rx, frame_tx;
#else
flexcan_frame_t frame, frame_rx, frame_tx;
#endif
uint32_t txIdentifier;
uint32_t rxIdentifier;

bool pcan_connected=0;
bool packet_received=1;

void app_nameservice_isr_cb(unsigned int new_ept, const char *new_ept_name, unsigned long flags, void *user_data)
{
}

/*******************************************************************************
 * Code
 ******************************************************************************/
static bool PCA6416_WriteReg(
    LPI2C_Type *base, const uint8_t dev_addr, const uint8_t reg_offset, const uint8_t *txBuff, uint32_t txSize)
{
    status_t reVal = kStatus_Fail;

    if (kStatus_Success == LPI2C_MasterStart(base, dev_addr, kLPI2C_Write))
    {
        while (LPI2C_MasterGetStatusFlags(base) & kLPI2C_MasterNackDetectFlag)
        {
        }

        reVal = LPI2C_MasterSend(base, (void *)&reg_offset, 1);
        if (reVal != kStatus_Success)
        {
            return -1;
        }

        reVal = LPI2C_MasterSend(base, (void *)txBuff, txSize);
        if (reVal != kStatus_Success)
        {
            return -1;
        }

        reVal = LPI2C_MasterStop(base);
        if (reVal != kStatus_Success)
        {
            return -1;
        }
    }
    return kStatus_Success;
}

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

void BOARD_I2C_ReleaseBus(void)
{
    uint8_t i = 0;
    rgpio_pin_config_t pin_config;

    pin_config.pinDirection = kRGPIO_DigitalOutput;
    pin_config.outputLogic  = 1U;

    RGPIO_PinInit(I2C_RELEASE_SCL_GPIO, I2C_RELEASE_SCL_PIN, &pin_config); /* I2C SCL */
    RGPIO_PinInit(I2C_RELEASE_SDA_GPIO, I2C_RELEASE_SDA_PIN, &pin_config); /* I2C SDA */

    /* Drive SDA low first to simulate a start */
    RGPIO_WritePinOutput(I2C_RELEASE_SDA_GPIO, I2C_RELEASE_SDA_PIN, 0U);
    i2c_release_bus_delay();

    /* Send 9 pulses on SCL and keep SDA high */
    for (i = 0; i < 9; i++)
    {
        RGPIO_WritePinOutput(I2C_RELEASE_SCL_GPIO, I2C_RELEASE_SCL_PIN, 0U);
        i2c_release_bus_delay();

        RGPIO_WritePinOutput(I2C_RELEASE_SDA_GPIO, I2C_RELEASE_SDA_PIN, 1U);
        i2c_release_bus_delay();

        RGPIO_WritePinOutput(I2C_RELEASE_SCL_GPIO, I2C_RELEASE_SCL_PIN, 1U);
        i2c_release_bus_delay();
        i2c_release_bus_delay();
    }

    /* Send stop */
    RGPIO_WritePinOutput(I2C_RELEASE_SCL_GPIO, I2C_RELEASE_SCL_PIN, 0U);
    i2c_release_bus_delay();

    RGPIO_WritePinOutput(I2C_RELEASE_SDA_GPIO, I2C_RELEASE_SDA_PIN, 0U);
    i2c_release_bus_delay();

    RGPIO_WritePinOutput(I2C_RELEASE_SCL_GPIO, I2C_RELEASE_SCL_PIN, 1U);
    i2c_release_bus_delay();

    RGPIO_WritePinOutput(I2C_RELEASE_SDA_GPIO, I2C_RELEASE_SDA_PIN, 1U);
    i2c_release_bus_delay();
}

void BOARD_ConfigureExpansionIO(void)
{
    uint32_t i;
    lpi2c_master_config_t masterConfig;
    uint8_t txBuffer[4] = {0};

    /* Configure CAN IO EXPANDER PCA6416 to initialize TJA1043T */
    LPI2C_MasterGetDefaultConfig(&masterConfig);
    LPI2C_MasterInit(EXAMPLE_CAN_IOEXP_LPI2C, &masterConfig, EXAMPLE_CAN_IOEXP_LPI2C_CLOCK_FREQUENCY);
    txBuffer[0] = 0;
    PCA6416_WriteReg(EXAMPLE_CAN_IOEXP_LPI2C, EXAMPLE_CAN_IOEXP_I2C_ADDR, PCA6416_REG_CONFIGURATION_PORT_0, txBuffer,
                     1);
    txBuffer[0] = 0;
    PCA6416_WriteReg(EXAMPLE_CAN_IOEXP_LPI2C, EXAMPLE_CAN_IOEXP_I2C_ADDR, PCA6416_REG_OUTPUT_PORT_0, txBuffer, 1);
    i = 0;
    while (i < 150000)
    {
        __ASM("nop");
        i++;
    }
    txBuffer[0] = 0x28;
    PCA6416_WriteReg(EXAMPLE_CAN_IOEXP_LPI2C, EXAMPLE_CAN_IOEXP_I2C_ADDR, PCA6416_REG_OUTPUT_PORT_0, txBuffer, 1);

    LPI2C_MasterDeinit(EXAMPLE_CAN_IOEXP_LPI2C);
}

/*!
 * @brief FlexCAN Call Back function
 */
static void flexcan_callback(CAN_Type *base, flexcan_handle_t *handle, status_t status, uint32_t result, void *userData)
{
    switch (status)
    {
        case kStatus_FLEXCAN_RxIdle:
            if (RX_MESSAGE_BUFFER_NUM == result)
            {
		PRINTF("Rx status=%x\r\n",status);
                rxComplete = true;
            }
            break;

        case kStatus_FLEXCAN_TxIdle:
            if (TX_MESSAGE_BUFFER_NUM == result)
            {
		PRINTF("Tx status=%x\r\n",status);
                txComplete = true;
            }
            break;

	case kStatus_FLEXCAN_TxBusy:
	    PRINTF("kStatus_FLEXCAN_TxBusy\r\n");
	    break;

	case kStatus_FLEXCAN_RxBusy:
	    PRINTF("kStatus_FLEXCAN_RxBusy\r\n");
	    break;

	case kStatus_FLEXCAN_RxOverflow:
	    PRINTF("kStatus_FLEXCAN_RxOverflow\r\n");
	    break;

        case kStatus_FLEXCAN_WakeUp:
		PRINTF("wakeup status=%x\r\n",status);
            wakenUp = true;
            break;

        default:
	    PRINTF(" Default status=%x\trxComplete=%x\r\n",status,rxComplete);
            //rxComplete = true;
            break;
    }
}

/*!
 * @brief Main function
 */
void can_main_task(void)
{
    flexcan_config_t flexcanConfig;
    flexcan_rx_mb_config_t mbConfig;

    /* assigne CAN msg id's */
    txIdentifier = 0x456;
    rxIdentifier = 0x321;

    /* Get FlexCAN module default Configuration. */
    /*
     * flexcanConfig.clkSrc = kFLEXCAN_ClkSrcOsc;
     * flexcanConfig.baudRate = 1000000U;
     * flexcanConfig.baudRateFD = 2000000U;
     * flexcanConfig.maxMbNum = 16;
     * flexcanConfig.enableLoopBack = false;
     * flexcanConfig.enableSelfWakeup = false;
     * flexcanConfig.enableIndividMask = false;
     * flexcanConfig.enableDoze = false;
     * flexcanConfig.timingConfig = timingConfig;
     */
    FLEXCAN_GetDefaultConfig(&flexcanConfig);
    flexcanConfig.baudRate = 500000U;

    PRINTF("New Can BR = 0x%x \r\n", flexcanConfig.baudRate);
/* Init FlexCAN module. */
#if (!defined(DEMO_FORCE_CAN_SRC_OSC)) || !DEMO_FORCE_CAN_SRC_OSC
#if (!defined(FSL_FEATURE_FLEXCAN_SUPPORT_ENGINE_CLK_SEL_REMOVE)) || !FSL_FEATURE_FLEXCAN_SUPPORT_ENGINE_CLK_SEL_REMOVE
    flexcanConfig.clkSrc = kFLEXCAN_ClkSrcPeri;
#else
#if defined(CAN_CTRL1_CLKSRC_MASK)
    if (!FSL_FEATURE_FLEXCAN_INSTANCE_SUPPORT_ENGINE_CLK_SEL_REMOVEn(EXAMPLE_CAN))
    {
        flexcanConfig.clkSrc = kFLEXCAN_ClkSrcPeri;
    }
#endif
#endif /* FSL_FEATURE_FLEXCAN_SUPPORT_ENGINE_CLK_SEL_REMOVE */
#endif /* DEMO_FORCE_CAN_SRC_OSC */
/* If special quantum setting is needed, set the timing parameters. */
#if (defined(SET_CAN_QUANTUM) && SET_CAN_QUANTUM)
    flexcanConfig.timingConfig.phaseSeg1 = PSEG1;
    flexcanConfig.timingConfig.phaseSeg2 = PSEG2;
    flexcanConfig.timingConfig.propSeg   = PROPSEG;
#if (defined(FSL_FEATURE_FLEXCAN_HAS_FLEXIBLE_DATA_RATE) && FSL_FEATURE_FLEXCAN_HAS_FLEXIBLE_DATA_RATE)
    flexcanConfig.timingConfig.fphaseSeg1 = FPSEG1;
    flexcanConfig.timingConfig.fphaseSeg2 = FPSEG2;
    flexcanConfig.timingConfig.fpropSeg   = FPROPSEG;
#endif
#endif

#if (defined(USE_IMPROVED_TIMING_CONFIG) && USE_IMPROVED_TIMING_CONFIG)
    flexcan_timing_config_t timing_config;
#if (defined(USE_CANFD) && USE_CANFD)
    if (FLEXCAN_FDCalculateImprovedTimingValues(flexcanConfig.baudRate, flexcanConfig.baudRateFD, EXAMPLE_CAN_CLK_FREQ,
                                                &timing_config))
    {
        /* Update the improved timing configuration*/
        memcpy(&(flexcanConfig.timingConfig), &timing_config, sizeof(flexcan_timing_config_t));
    }
    else
    {
        PRINTF("No found Improved Timing Configuration. Just used default configuration\r\n\r\n");
    }
#else
    if (FLEXCAN_CalculateImprovedTimingValues(flexcanConfig.baudRate, EXAMPLE_CAN_CLK_FREQ, &timing_config))
    {
        /* Update the improved timing configuration*/
        memcpy(&(flexcanConfig.timingConfig), &timing_config, sizeof(flexcan_timing_config_t));
    }
    else
    {
        PRINTF("No found Improved Timing Configuration. Just used default configuration\r\n\r\n");
    }
#endif
#endif

#if (defined(USE_CANFD) && USE_CANFD)
    FLEXCAN_FDInit(EXAMPLE_CAN, &flexcanConfig, EXAMPLE_CAN_CLK_FREQ, BYTES_IN_MB, true);
#else
    FLEXCAN_Init(EXAMPLE_CAN, &flexcanConfig, EXAMPLE_CAN_CLK_FREQ);
#endif

    /* Create FlexCAN handle structure and set call back function. */
    FLEXCAN_TransferCreateHandle(EXAMPLE_CAN, &flexcanHandle, flexcan_callback, NULL);

    /* Set Rx Masking mechanism. */
    FLEXCAN_SetRxMbGlobalMask(EXAMPLE_CAN, 0);
    /* Setup Rx Message Buffer. */
#if(EXTENDED_FRAME)
    PRINTF("Extended enabled \r\n");
    mbConfig.format = kFLEXCAN_FrameFormatExtend;
    mbConfig.id     = FLEXCAN_ID_EXT(rxIdentifier);
#else
    mbConfig.format = kFLEXCAN_FrameFormatStandard;
    mbConfig.id     = FLEXCAN_ID_STD(rxIdentifier);
#endif
    mbConfig.type   = kFLEXCAN_FrameTypeData;

#if (defined(USE_CANFD) && USE_CANFD)
    FLEXCAN_SetFDRxMbConfig(EXAMPLE_CAN, RX_MESSAGE_BUFFER_NUM, &mbConfig, true);
#else
    FLEXCAN_SetRxMbConfig(EXAMPLE_CAN, RX_MESSAGE_BUFFER_NUM, &mbConfig, true);
#endif

/* Setup Tx Message Buffer. */
#if (defined(USE_CANFD) && USE_CANFD)
    FLEXCAN_SetFDTxMbConfig(EXAMPLE_CAN, TX_MESSAGE_BUFFER_NUM, true);
#else
    FLEXCAN_SetTxMbConfig(EXAMPLE_CAN, TX_MESSAGE_BUFFER_NUM, true);
#endif

    canDoneInit = true;
    PRINTF("CAN Init Done...\r\n");

    while (1)
    {
    }
}

void can_receive_task(void)
{
    char app_buf[8] = {0};
    vehicle_state_t vstate = {0};
    uint16_t msg_id;
    uint8_t rxInProgress = 0;


    while(!canDoneInit)
           ;

    while(1)
    {

        /* Start receive data through Rx Message Buffer. */
        rxXfer.mbIdx = RX_MESSAGE_BUFFER_NUM;
#if (defined(USE_CANFD) && USE_CANFD)
        rxXfer.framefd = &frame_rx;
        if(rxInProgress == 0){
            FLEXCAN_TransferFDReceiveNonBlocking(EXAMPLE_CAN, &flexcanHandle, &rxXfer);
        }
#else
        rxXfer.frame = &frame_rx;
	/*if (packet_received == 1)
	{
		packet_received = 0;
	}*/
    if(rxInProgress == 0){
        FLEXCAN_TransferReceiveNonBlocking(EXAMPLE_CAN, &flexcanHandle, &rxXfer);
    }
#endif

        /* Wait until Rx receive full. */
        while (!rxComplete)
         {
	
         };
        //rxComplete = false;
	/* Some packets has been received from PCAN so setting up pcan_connected to 1 this is one time operation packet_received is never made 1 again */
/*	if (packet_received)
	{
		packet_received = 0;
		pcan_connected = 1;
	}*/
#if(EXTENDED_FRAME)
        msg_id = frame_rx.id >> CAN_ID_EXT_SHIFT;
        PRINTF("msg_id EXT = %x\n", msg_id);
        PRINTF("frame_rx.id EXT = %x\n", frame_rx.id);

#else
        msg_id = frame_rx.id >> CAN_ID_STD_SHIFT;
        PRINTF("msg_id STD = %x\n", msg_id);
        PRINTF("frame_rx.id STD = %x\n", frame_rx.id);

#endif

    if(msg_id == 0x335 || msg_id == 0x360 || msg_id == 0x361){
        //do nothing 
    }else{
        rxInProgress = 1;
        /* Some packets has been received from PCAN so setting up pcan_connected to 1 this is one time operation packet_received is never made 1 again */
      	if (packet_received)
      	{
            packet_received = 0;  //AR: it will have a consequences if user has not simulated any msg and just open the PCAN tx won't get transmitted
            pcan_connected = 1;
      	}
        app_buf[0] = frame_rx.dataByte0;
        app_buf[1] = frame_rx.dataByte1;
        app_buf[2] = frame_rx.dataByte2;
        app_buf[3] = frame_rx.dataByte3;
        app_buf[4] = frame_rx.dataByte4;
        app_buf[5] = frame_rx.dataByte5;
        app_buf[6] = frame_rx.dataByte6;
        app_buf[7] = frame_rx.dataByte7;

        vstate.value = 0;
        PRINTF("CAN Receive : CAN ID : %d\r\n",msg_id);
        PRINTF("app_buf[0] = %x\rapp_buf[1] = %x\rapp_buf[2] = %x\n", app_buf[0],app_buf[1],app_buf[2]);

    }


        rxInProgress = 0; //setting it back to 0 to read the next rx msg.
    }

}

void can_send(uint8_t len, uint8_t *data, uint8_t id)
{


        //frame_tx.dataByte0 = id;
	if (len==5) {					//id 361
		frame_tx.dataByte0 = data[2];
		frame_tx.dataByte1 = data[3];
		frame_tx.dataByte2 = data[4];
	} else if (len==9) {				//id 360
		frame_tx.dataByte0 = data[2];
		frame_tx.dataByte1 = data[3];
		frame_tx.dataByte2 = data[4];
		frame_tx.dataByte3 = data[5];
		frame_tx.dataByte4 = data[6];
		frame_tx.dataByte5 = data[7];
		frame_tx.dataByte6 = data[8];
	}else if (len == 10) {
		frame_tx.dataByte0 = data[2];
		frame_tx.dataByte1 = data[3];
		frame_tx.dataByte2 = data[4];
		frame_tx.dataByte3 = data[5];
		frame_tx.dataByte4 = data[6];
		frame_tx.dataByte5 = data[7];
		frame_tx.dataByte6 = data[8];
		frame_tx.dataByte7 = data[9];
	}

	txIdentifier = (data[0] << 8) | data[1];
#if(EXTENDED_FRAME)
        frame_tx.format = kFLEXCAN_FrameFormatExtend;
        frame_tx.id     = FLEXCAN_ID_EXT(txIdentifier);
#else
        frame_tx.format = kFLEXCAN_FrameFormatStandard;
        frame_tx.id     = FLEXCAN_ID_STD(txIdentifier);
#endif
        frame_tx.length = len - 2;
        frame_tx.type   = kFLEXCAN_FrameTypeData;
        txXfer.mbIdx = TX_MESSAGE_BUFFER_NUM;
#if (defined(USE_CANFD) && USE_CANFD)
        frame.brs      = 1;
        txXfer.framefd = &frame_tx;
        FLEXCAN_TransferFDSendNonBlocking(EXAMPLE_CAN, &flexcanHandle, &txXfer);
#else
        txXfer.frame = &frame_tx;
        FLEXCAN_TransferSendNonBlocking(EXAMPLE_CAN, &flexcanHandle, &txXfer);
#endif

        while (!txComplete)
        {
        };
        txComplete = false;
}