/*
 * Copyright (c) 2015, Freescale Semiconductor, Inc.
 * Copyright 2016-2017 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_i2c.h"
#include "BatteryCharger.h"
#include "fsl_gpt.h"

#include "fsl_common.h"
#include "FuelGaugeSensor.h"
#include "TempSensor.h"
#include "AccelerometerSensor.h"
#include "fsl_uart.h"

#include "fsl_gpio.h"

/*******************************************************************************
 * Definitions
 ******************************************************************************/
#define EXAMPLE_LED_GPIO     GPIO3
#define EXAMPLE_LED_GPIO_PIN 21U

//**************************************************************************
#define PGA450_UART UART3
#define PGA450_UART_CLK_FREQ BOARD_PGA450_UART_CLK_FREQ
#define PGA450_UART_BAUDRATE 19200U
   
//***************************************************************************

//***********************PDC Coomand***********************************************************
uint8_t Cmd1_PDC_UART_TEST_CMD[4]={0x00, 0x55, 0x01,0x00};
uint8_t Cmd2_BURST_SIGNAL_FOR_SHORT_DISTANCE[5]={0x00, 0x55, 0x51,0x01,0x00};
uint8_t Cmd3_BURST_SIGNAL_FOR_LONG_DISTANCE[5]={0x00, 0x55, 0x51,0x02,0x00};
uint8_t Cmd4_Read_Threshold_for_Close[4]={0x00, 0x55, 0x21,0x00};
uint8_t PDC_Flag =0;

uint8_t PDC_UART3_OK_Flag = 0;
 uint8_t Data[3];

/*******************************************************************************
 * Definitions
 ******************************************************************************/
#define EXAMPLE_I2C_MASTER_BASEADDR I2C3
#define I2C_MASTER_CLK_FREQ                                                                 \
    (CLOCK_GetPllFreq(kCLOCK_SystemPll1Ctrl) / (CLOCK_GetRootPreDivider(kCLOCK_RootI2c3)) / \
     (CLOCK_GetRootPostDivider(kCLOCK_RootI2c3)) / 5) /* SYSTEM PLL1 DIV5 */

//#define I2C_MASTER_SLAVE_ADDR_7BIT 0x7EU
#define I2C_BAUDRATE               100000U
#define I2C_DATA_LENGTH            32U

#define GPT_IRQ_ID  GPT1_IRQn
#define EXAMPLE_GPT GPT1
#define EXAMPLE_GPT_CLK_FREQ                                                                \
    (CLOCK_GetPllFreq(kCLOCK_SystemPll1Ctrl) / (CLOCK_GetRootPreDivider(kCLOCK_RootGpt1)) / \
     (CLOCK_GetRootPostDivider(kCLOCK_RootGpt1)) / 2) /* SYSTEM PLL1 DIV2 */
#define EXAMPLE_GPT_IRQHandler GPT1_IRQHandler

/*******************************************************************************
 * Prototypes
 ******************************************************************************/

/*******************************************************************************
 * Variables
 ******************************************************************************/

uint8_t g_master_txBuff[I2C_DATA_LENGTH];
uint8_t g_master_rxBuff[I2C_DATA_LENGTH];
volatile bool g_MasterCompletionFlag = false;
volatile uint8_t gptIsrFlag = 0;
/*******************************************************************************
 * Code
 ******************************************************************************/
void EXAMPLE_GPT_IRQHandler(void)
{
    /* Clear interrupt flag.*/
    GPT_ClearStatusFlags(EXAMPLE_GPT, kGPT_OutputCompare1Flag);

    gptIsrFlag++;
/* Add for ARM errata 838869, affects Cortex-M4, Cortex-M4F, Cortex-M7, Cortex-M7F Store immediate overlapping
  exception return operation might vector to incorrect interrupt */
#if defined __CORTEX_M && (__CORTEX_M == 4U || __CORTEX_M == 7U)
    __DSB();
#endif
}



/*!
 * @brief Main function
 */

void ReadData(UART_Type *base, uint8_t *data, uint8_t length)
{
   assert(data != NULL);

    uint8_t i;

    /* The Non Blocking read data API assume user have ensured there is enough space in
     * peripheral to write. The URXD holds the received character,In 7-bit mode,
     * the most significant bit (MSB) is forced to 0.In 8-bit mode, all bits are active.
     */
    for (i = 0; i < length; i++)
    {
        data[i] = (uint8_t)((base->URXD & UART_URXD_RX_DATA_MASK) >> UART_URXD_RX_DATA_SHIFT);
    }
}

uint16_t ShortDistance_Check()
{
   uint16_t  ReadShortPdc_Data =0;
   UART_WriteBlocking(PGA450_UART,Cmd2_BURST_SIGNAL_FOR_SHORT_DISTANCE,5);
   SDK_DelayAtLeastUs(200000, SDK_DEVICE_MAXIMUM_CPU_CLOCK_FREQUENCY);  
   UART_WriteBlocking(PGA450_UART,Cmd4_Read_Threshold_for_Close,4);
   SDK_DelayAtLeastUs(500000, SDK_DEVICE_MAXIMUM_CPU_CLOCK_FREQUENCY);
   ReadData(PGA450_UART,Data,3);
   
  // PRINTF("Short distance Data_Byte1 = %x\r\n",Data[0]);
//   PRINTF("Short distance Data_Byte2 = %x\r\n",Data[1]);
 //  PRINTF("Short distance Data_Byte3 = %x\r\n",Data[2]);
   
   ReadShortPdc_Data =  ((Data[0]<<8)&0xFF00)|((Data[1]<<0)&0x00FF);
   
   
   PRINTF("ShortDistance_Check value = %x\r\n",ReadShortPdc_Data);
   return(ReadShortPdc_Data);
  
}

uint16_t LongDistance_Check()
{
   uint16_t  ReadLongPdc_Data=0;
   
  UART_WriteBlocking(PGA450_UART,Cmd3_BURST_SIGNAL_FOR_LONG_DISTANCE,5);
  SDK_DelayAtLeastUs(200000, SDK_DEVICE_MAXIMUM_CPU_CLOCK_FREQUENCY); 
  UART_WriteBlocking(PGA450_UART,Cmd4_Read_Threshold_for_Close,4);
  SDK_DelayAtLeastUs(500000, SDK_DEVICE_MAXIMUM_CPU_CLOCK_FREQUENCY);
  
  ReadData(PGA450_UART,Data,3);
 // PRINTF("Long  distance Data_Byte1 = %x\r\n",Data[0]);
 // PRINTF("Long distance Data_Byte2 = %x\r\n",Data[1]);
  //PRINTF("Long distance Data_Byte3 = %x\r\n",Data[2]); 
  
  ReadLongPdc_Data = ((Data[0]<<8)&0xFF00)|((Data[1]<<0)&0x00FF);
  
  PRINTF("LongDistance_Check value = %x\r\n",ReadLongPdc_Data);
  return(ReadLongPdc_Data);
 
}

void PDC(void)
{
  uint16_t data_received = 0;
  data_received = ShortDistance_Check();
   PRINTF("ShortDistance_Check value = %x\r\n",data_received);
   PDC_Flag = 0;
   if(data_received <= 0x1238)//80m
  {
     PRINTF("Short Range Obstacle Detected....\r\n");
    
     PDC_Flag = 1;
  }
  else if(data_received <= 0x147f)//90cm
  {
     PRINTF("Middle Range Obstacle Detected....\r\n");
     PDC_Flag = 1;
  }
  
  
  if(PDC_Flag != 1)
  {
    PDC_Flag = 0;
    data_received = LongDistance_Check();
    PRINTF("LongDistance_Check value = %x\r\n",data_received);
  if(data_received <= 0x1D9C)//1.3m
  {
     PRINTF("Long Range Obstacle Detected....\r\n");
  }
  else
  {
     PRINTF(" No Obstacle Detected And Safe Zone....\r\n");
  }
  }
  
   
  
    
}




void ShortDistance(void)
{
  uint16_t Short_Set = 0x1238; //0x1238;//0.8m
  uint16_t Short_Middle_Set = 0x16c6;//0x16c6;//1m
  
  uint16_t Data_short = 0;
  uint16_t Data_middle = 0;
 
  UART_WriteBlocking(PGA450_UART,Cmd2_BURST_SIGNAL_FOR_SHORT_DISTANCE,5);
  SDK_DelayAtLeastUs(200000, SDK_DEVICE_MAXIMUM_CPU_CLOCK_FREQUENCY); 
  
  UART_WriteBlocking(PGA450_UART,Cmd4_Read_Threshold_for_Close,4);
 // SDK_DelayAtLeastUs(3000000, SDK_DEVICE_MAXIMUM_CPU_CLOCK_FREQUENCY);
  
   ReadData(PGA450_UART,Data,3);

   Data_short = ((Data[1]<<8)&0xFF)|((Data[0]<<0)&0xFF);
   Data_middle = ((Data[1]<<8)&0xFF)|((Data[0]<<0)&0xFF);
      if(Data_middle>Short_Middle_Set)
     {
         PRINTF("Middle DETECTED \r\n");
     }
     else if(Data_short>Short_Set)
     {
         PRINTF("SHORT DETECTED \r\n");
     }
     else
   {
     PRINTF("Middle DETECTED \r\n");
   }
     
   char Receive_data[] = "Short distance Data ";
   PRINTF("Receive_data = %s\r\n",Receive_data);
   PRINTF("Short distance Data_Byte1 = %x\r\n",Data[0]);
   PRINTF("Short distance Data_Byte2 = %x\r\n",Data[1]);
   PRINTF("Short distance Data_Byte3 = %x\r\n",Data[2]);
  
}

void LongDistance(void)
{
  uint16_t Long_Set = 0x1D9C;//1.3m
  uint16_t DATA_Long_Set;//1.3m
  UART_WriteBlocking(PGA450_UART,Cmd3_BURST_SIGNAL_FOR_LONG_DISTANCE,5);
  SDK_DelayAtLeastUs(200000, SDK_DEVICE_MAXIMUM_CPU_CLOCK_FREQUENCY); 
  UART_WriteBlocking(PGA450_UART,Cmd4_Read_Threshold_for_Close,4);
 // SDK_DelayAtLeastUs(3000000, SDK_DEVICE_MAXIMUM_CPU_CLOCK_FREQUENCY);
  
  ReadData(PGA450_UART,Data,3);
    DATA_Long_Set = ((Data[1]<<8)&0xFF)|((Data[0]<<0)&0xFF);
    if(DATA_Long_Set>Long_Set)
    {
       PRINTF("LONG Distance obstacle Detected\r\n");
    }
    else
    {
       PRINTF("No obstacle Detected for Long Sequence\r\n");
   }
   char Receive_data[] = "Long distance Data ";
   PRINTF("Receive_data = %s\r\n",Receive_data);
   PRINTF("Long distance Data_Byte1 = %x\r\n",Data[0]);
   PRINTF("Long distance Data_Byte2 = %x\r\n",Data[1]);
   PRINTF("Long distance Data_Byte3 = %x\r\n",Data[2]);
  
}

void PGA450_TestCommand_1(uint8_t *Cmd,uint8_t CmdLength, uint8_t Cmd_No)
{
 
UART_WriteBlocking(PGA450_UART,Cmd,CmdLength);
 SDK_DelayAtLeastUs(3000000, SDK_DEVICE_MAXIMUM_CPU_CLOCK_FREQUENCY);

  ReadData(PGA450_UART,Data,3);

 
if(Data[0] == 0x12 && Data[1] == 0x34 && Data[2] == 0xB9)
{
  PDC_UART3_OK_Flag = 1;
   char Receive_data[] = "Received Data ";
   PRINTF("Receive_data = %s\r\n",Receive_data);
   PRINTF("Command1_Response_Byte1 = %x\r\n",Data[0]);
   PRINTF("Command1_Response_Byte2 = %x\r\n",Data[1]);
   PRINTF("Command1_Response_Byte3 = %x\r\n",Data[2]);
 
} 
}
int main(void)
{
  status_t status;
  status_t status_1;
  uart_config_t config;
  uint32_t gptFreq;
  gpt_config_t gptConfig;
  uint8_t Data = 0, Result;
    
      gpio_pin_config_t led_config = {kGPIO_DigitalInput, 0, kGPIO_NoIntmode};
    
      uint32_t InterruptStatus = 0x00000000 ;
      int32_t read_data = 0x00000000;

      i2c_master_config_t masterConfig;
      uint32_t sourceClock;
      i2c_master_transfer_t masterXfer;

    /* Board specific RDC settings */
    BOARD_RdcInit();

    BOARD_InitPins();
    BOARD_BootClockRUN();
    BOARD_InitDebugConsole();
    BOARD_InitMemory();
    
    //UART3
    UART_GetDefaultConfig(&config);
    config.baudRate_Bps = PGA450_UART_BAUDRATE;
    config.enableTx = true;
    config.enableRx = true;

    CLOCK_SetRootMux(kCLOCK_RootI2c3, kCLOCK_I2cRootmuxSysPll1Div5); /* Set I2C source to SysPLL1 Div5 160MHZ */
    CLOCK_SetRootDivider(kCLOCK_RootI2c3, 1U, 10U);                  /* Set root clock to 160MHZ / 10 = 16MHZ */

    PRINTF("\r\nI2C board2board polling example -- Master transfer.\r\n");

    I2C_MasterGetDefaultConfig(&masterConfig);
    masterConfig.baudRate_Bps = I2C_BAUDRATE;

    sourceClock = I2C_MASTER_CLK_FREQ;

    I2C_MasterInit(EXAMPLE_I2C_MASTER_BASEADDR, &masterConfig, sourceClock);
    GPIO_PinInit(EXAMPLE_LED_GPIO, EXAMPLE_LED_GPIO_PIN, &led_config);

    memset(&masterXfer, 0, sizeof(masterXfer));
    init_battery_charger();
    config_battery_charger();
    PRINTF("\r\nEnd of I2C example .\r\n");

    CLOCK_SetRootMux(kCLOCK_RootGpt1, kCLOCK_GptRootmuxSysPll1Div2); /* Set GPT1 source to SYSTEM PLL1 DIV2 400MHZ */
    CLOCK_SetRootDivider(kCLOCK_RootGpt1, 1U, 4U);                   /* Set root clock to 400MHZ / 4 = 100MHZ */

    GPT_GetDefaultConfig(&gptConfig);

    /* Initialize GPT module */
    GPT_Init(EXAMPLE_GPT, &gptConfig);

    /* Divide GPT clock source frequency by 3 inside GPT module */
    GPT_SetClockDivider(EXAMPLE_GPT, 3);

    /* Get GPT clock frequency */
    gptFreq = EXAMPLE_GPT_CLK_FREQ;

    /* GPT frequency is divided by 3 inside module */
    gptFreq /= 3;

    /* Set both GPT modules to 1 second duration */
    GPT_SetOutputCompareValue(EXAMPLE_GPT, kGPT_OutputCompare_Channel1, gptFreq);

    /* Enable GPT Output Compare1 interrupt */
    GPT_EnableInterrupts(EXAMPLE_GPT, kGPT_OutputCompare1InterruptEnable);

    /* Enable at the Interrupt */
    EnableIRQ(GPT_IRQ_ID);

    /* Start Timer */
    PRINTF("\r\nStarting GPT timer ...");
    GPT_StartTimer(EXAMPLE_GPT);
    
    //****************************uart3******************************************
    status_1 = UART_Init(PGA450_UART, &config, PGA450_UART_CLK_FREQ);
    if (kStatus_Success != status_1)
    {
       PRINTF("UART3 FAIL BAUD RATE NOT SUPPORTED\r\n");
       return kStatus_Fail;
    }
    else
    {
       PRINTF("UART3 OK WORKING WITH BAUD RATE 19200 BPS\r\n");
    }
//************************************************************************************************************
    
    Init_AccelerometerSensor();
    Init_TempSensor();
    Init_Fuel_G_Sensor();
    ReadAccelerometerSensor();
    ReadFuel_G_Sensor();
   ReadChargeStatusRegister();
   ReadTempSensor();
uint8_t CheckFlag=0;
uint8_t Count=0;
//PRINTF("UART3 PDC TESTING.......................\r\n");
//PGA450_TestCommand_1(Cmd1_PDC_UART_TEST_CMD,4,1);


    while (1)
    {
                                 
      if (gptIsrFlag >= 15)
      {
        PRINTF("statement...................\r\n");
        gptIsrFlag = 0;
        Data = (BQ2416X_REG_STATUS_TMR_RST_MASK | BQ2416X_REG_STATUS_SUPPLY_SEL_MASK);
        Result = write_data_to_battery_charger(Data, STATUS_CONTROL_REGISTER, 1, kI2C_Write);
                                                
       }
                                                    
      read_data= GPIO_PinRead(EXAMPLE_LED_GPIO,EXAMPLE_LED_GPIO_PIN);
      PRINTF("  read_data_new = %x\r\n",read_data);
                                                 
/*
    while (1)
    {
     
     PRINTF("Hello PDC\r\n",Count);
     PDC();
    
                                                       
    }*/
}
}