/**
 * \file
 * \brief Timer driver
 * \author Erich Styger, erich.styger@hslu.ch
 * \license SPDX-License-Identifier: BSD-3-Clause
 * This module implements the driver for all our timers.
 */

#include "platform.h"
#include "Timer.h"
#if PL_CONFIG_USE_EVENTS
  #include "Event.h"
#endif
#include "Trigger.h"
#include "fsl_ftm.h"
#if PL_CONFIG_USE_QUADRATURE
  #include "fsl_pit.h"
  #include "QuadCounter.h"
#endif
#include "Tacho.h"

void TMR_OnInterrupt(void) { /* called from RTOS tick hook */
  /* this one gets called from an interrupt!!!! */
#if PL_CONFIG_USE_EVENTS
  static unsigned int cntr = 0;
  #define BLINK_PERIOD_MS 2000
  /* this one gets called from an interrupt!!!! */
  cntr++;
  if ((cntr%(BLINK_PERIOD_MS/TMR_TICK_MS))==0) { /* every two seconds */
    EVNT_SetEvent(EVNT_LED_HEARTBEAT);
  }
#endif
  TRG_AddTick();
  TACHO_Sample();
}

#if PL_CONFIG_USE_LINE_SENSOR
/*------------------------------------------------------------------------------------------- */
/* Reflectance Sensor free running timer */
/*------------------------------------------------------------------------------------------- */
#define REFLECTANCE_FTM_BASEADDR        FTM1  /* FTM0 and FTM1 works, but FTM2 crashes? */
#define REFLECTANCE_FTM_CLOCK_SRC       kFTM_SystemClock
#define REFLECTANCE_FTM_IRQ_NUM         FTM1_IRQn
#define REFLECTANCE_FTM_IRQ_HANDLER     FTM1_IRQHandler
#define REFLECTANCE_FTM_SOURCE_CLOCK    (CLOCK_GetFreq(kCLOCK_BusClk))

#if 0
void REFLECTANCE_FTM_IRQ_HANDLER(void) { /* called every 1 ms */
  /* Clear interrupt flag.*/
  FTM_ClearStatusFlags(REFLECTANCE_FTM_BASEADDR, kFTM_TimeOverflowFlag);
  __DSB();
}
#endif

void TMR_StartReflectanceTimer(void) {
  FTM_StartTimer(REFLECTANCE_FTM_BASEADDR, REFLECTANCE_FTM_CLOCK_SRC);
}

void TMR_StopReflectanceTimer(void) {
  FTM_StopTimer(REFLECTANCE_FTM_BASEADDR);
}

void TMR_ResetReflectanceTimerCounter(void) {
  REFLECTANCE_FTM_BASEADDR->CNT = 0; /* there is no SDK function for this */
}

uint32_t TMR_GetReflectanceTimerCounter(void) {
  return FTM_GetCurrentTimerCount(REFLECTANCE_FTM_BASEADDR);
}

uint32_t TMR_GetRefelectanceTimerFrequencyHz(void) {
  return REFLECTANCE_FTM_SOURCE_CLOCK;
}

static void InitReflectanceTimer(void) {
  ftm_config_t ftmInfo;

  FTM_GetDefaultConfig(&ftmInfo);
  /* Divide FTM clock by 4 */
  ftmInfo.prescale = kFTM_Prescale_Divide_1;

  /* Initialize FTM module */
  FTM_Init(REFLECTANCE_FTM_BASEADDR, &ftmInfo);
  FTM_SetTimerPeriod(REFLECTANCE_FTM_BASEADDR, 0xFFFF /*USEC_TO_COUNT(10000U), REFLECTANCE_FTM_SOURCE_CLOCK)*/);

//  FTM_EnableInterrupts(REFLECTANCE_FTM_BASEADDR, kFTM_TimeOverflowInterruptEnable);
//  EnableIRQ(REFLECTANCE_FTM_IRQ_NUM);

  FTM_StartTimer(REFLECTANCE_FTM_BASEADDR, REFLECTANCE_FTM_CLOCK_SRC);
}
#endif /* PL_CONFIG_USE_LINE_SENSOR */
/*------------------------------------------------------------------------------------------- */
#if PL_CONFIG_USE_MOTORS
/*
 * Left motor: PTC5, FTM0, Ch2
 * Right motor: PTC4, FTM0, Ch3
 */
#define MOTOR_PWM_FTM_BASEADDR        FTM0
#define MOTOR_FTM_SOURCE_CLOCK        CLOCK_GetFreq(kCLOCK_BusClk)
#define MOTOR_PWM_LEFT_FTM_CHANNEL    2U
#define MOTOR_PWM_RIGHT_FTM_CHANNEL   3U

void TMR_SetLeftMotorPWMDutyPercent(uint8_t percent) {
  FTM_UpdatePwmDutycycle(MOTOR_PWM_FTM_BASEADDR, (ftm_chnl_t)MOTOR_PWM_LEFT_FTM_CHANNEL, kFTM_EdgeAlignedPwm, 100-percent);
  /* Software trigger to update registers */
  FTM_SetSoftwareTrigger(MOTOR_PWM_FTM_BASEADDR, true);
}

void TMR_SetRightMotorPWMDutyPercent(uint8_t percent) {
  FTM_UpdatePwmDutycycle(MOTOR_PWM_FTM_BASEADDR, (ftm_chnl_t)MOTOR_PWM_RIGHT_FTM_CHANNEL, kFTM_EdgeAlignedPwm, 100-percent);
  /* Software trigger to update registers */
  FTM_SetSoftwareTrigger(MOTOR_PWM_FTM_BASEADDR, true);
}

void TMR_SetLeftMotorPWMRatio(uint16_t val) {
  TMR_SetLeftMotorPWMDutyPercent(val>>8);
}

void TMR_SetRightMotorPWMRatio(uint16_t val) {
  TMR_SetRightMotorPWMDutyPercent(val>>8);
}

void TMR_MotorPWMStart(void) {
  FTM_StartTimer(MOTOR_PWM_FTM_BASEADDR, kFTM_SystemClock);
}

void TMR_MotorPWMStop(void) {
  FTM_StopTimer(MOTOR_PWM_FTM_BASEADDR);
}

static void InitMotorPWMTimer(void) {
  ftm_chnl_pwm_signal_param_t ftmParam[2];
  ftm_config_t ftmInfo;

  /* Configure ftm params with frequency 24kHZ */
  ftmParam[0].chnlNumber = (ftm_chnl_t)MOTOR_PWM_LEFT_FTM_CHANNEL;
  ftmParam[0].level = kFTM_LowTrue;
  ftmParam[0].dutyCyclePercent = 0U;
  ftmParam[0].firstEdgeDelayPercent = 0U;

  ftmParam[1].chnlNumber = (ftm_chnl_t)MOTOR_PWM_RIGHT_FTM_CHANNEL;
  ftmParam[1].level = kFTM_LowTrue;
  ftmParam[1].dutyCyclePercent = 0U;
  ftmParam[1].firstEdgeDelayPercent = 0U;

  /*
    * ftmInfo.prescale = kFTM_Prescale_Divide_1;
    * ftmInfo.bdmMode = kFTM_BdmMode_0;
    * ftmInfo.pwmSyncMode = kFTM_SoftwareTrigger;
    * ftmInfo.reloadPoints = 0;
    * ftmInfo.faultMode = kFTM_Fault_Disable;
    * ftmInfo.faultFilterValue = 0;
    * ftmInfo.deadTimePrescale = kFTM_Deadtime_Prescale_1;
    * ftmInfo.deadTimeValue = 0;
    * ftmInfo.extTriggers = 0;
    * ftmInfo.chnlInitState = 0;
    * ftmInfo.chnlPolarity = 0;
    * ftmInfo.useGlobalTimeBase = false;
    */
   FTM_GetDefaultConfig(&ftmInfo);
   /* Initialize FTM module */
   FTM_Init(MOTOR_PWM_FTM_BASEADDR, &ftmInfo);

   FTM_SetupPwm(MOTOR_PWM_FTM_BASEADDR, ftmParam, 2U, kFTM_EdgeAlignedPwm, 24000U, MOTOR_FTM_SOURCE_CLOCK);
   TMR_SetLeftMotorPWMDutyPercent(0);
   TMR_SetRightMotorPWMDutyPercent(0);
 //  FTM_StartTimer(MOTOR_PWM_FTM_BASEADDR, kFTM_SystemClock);
}
#endif /* PL_CONFIG_USE_MOTORS */
/*------------------------------------------------------------------------------------------- */
#if PL_CONFIG_USE_QUADRATURE

#define QUADRATURE_PIT_BASEADDR       PIT
#define QUADRATURE_PIT_SOURCE_CLOCK   CLOCK_GetFreq(kCLOCK_BusClk)
#define QUADRATURE_PIT_CHANNEL        kPIT_Chnl_0
#define QUADRATURE_PIT_HANDLER        PIT0_IRQHandler
#define QUADRATURE_PIT_IRQ_ID         PIT0_IRQn

void QUADRATURE_PIT_HANDLER(void) {
  PIT_ClearStatusFlags(QUADRATURE_PIT_BASEADDR, QUADRATURE_PIT_CHANNEL, kPIT_TimerFlag);
  QuadCounter_Sample();
  __DSB();
}

static void InitQuadratureTimer(void) {
  pit_config_t config;

  PIT_GetDefaultConfig(&config);
  config.enableRunInDebug = false;
  PIT_Init(QUADRATURE_PIT_BASEADDR, &config);
  PIT_SetTimerPeriod(QUADRATURE_PIT_BASEADDR, QUADRATURE_PIT_CHANNEL, USEC_TO_COUNT(200U, QUADRATURE_PIT_SOURCE_CLOCK));
  PIT_EnableInterrupts(QUADRATURE_PIT_BASEADDR, QUADRATURE_PIT_CHANNEL, kPIT_TimerInterruptEnable);
  NVIC_SetPriority(QUADRATURE_PIT_IRQ_ID, 0);
  EnableIRQ(QUADRATURE_PIT_IRQ_ID);
  PIT_StartTimer(QUADRATURE_PIT_BASEADDR, QUADRATURE_PIT_CHANNEL);
}
#endif /* PL_CONFIG_USE_QUADRATURE */

/*------------------------------------------------------------------------------------------- */
void TMR_Init(void) {
#if PL_CONFIG_USE_LINE_SENSOR
  InitReflectanceTimer();
#endif
#if PL_CONFIG_USE_MOTORS
  InitMotorPWMTimer();
#endif
#if PL_CONFIG_USE_QUADRATURE
  InitQuadratureTimer();
#endif
}

void TMR_Deinit(void) {
}

