/*
 * AN13724 SW EXAMPLE
 * STEPPER MOTOR CONTROL
 * V2.1
 *
 * Copyright 2021 NXP
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include "fsl_debug_console.h"
#include "pin_mux.h"
#include "clock_config.h"
#include "board.h"
#include "LPC55S36.h"
#include "fsl_vref.h"
#include "fsl_power.h"
#include "fsl_dac.h"
#include "fsl_usart.h"
#include "freemaster.h"
#include "freemaster_serial_usart.h"
#include "mlib.h"
#include "gflib.h"
#include "gmclib.h"

/*******************************************************************************
 * Definitions
 ******************************************************************************/
#define PWM_FREQUENCY           20000
#define PWM_MODULO              BOARD_BOOTCLOCKPLL150M_CORE_CLOCK/PWM_FREQUENCY

#define ANGLE_0             FRAC16(0.0)
#define ANGLE_45            FRAC16(0.25)
#define ANGLE_90            FRAC16(0.5)
#define ANGLE_270           FRAC16(-0.5)

#define SLOW_DECAY_UNIPOLAR       0
#define FAST_DECAY_BIPOLAR        1

/*******************************************************************************
 * Prototypes
 ******************************************************************************/
static void PinInit(void);
static void SystickInit(void);
static void	Pwm0Init(void);
static void	Pwm1Init(void);
void UpdatePWM0(frac16_t phA, frac16_t phB);
void UpdatePWM1(frac16_t phA, frac16_t phB);
static void Usart0Init(void);

/*******************************************************************************
 * Variables
 ******************************************************************************/
uint16_t ui16SystickIsrCnt,ui16PwmIsrCnt;
uint16_t ui16Switching;
uint16_t ui16Control;
uint32_t ui32FaultStatusReg;
uint16_t ui16ChangeUnipolar;
uint16_t ui16ChangeBipolar;

frac16_t f16Angle;
frac16_t f16Sin;
frac16_t f16AngleInc;
frac16_t f16Amplitude;
frac16_t f16Offset;
frac16_t f16Pha1;
frac16_t f16Pha2;
frac16_t f16Phb1;
frac16_t f16Phb2;

GMCLIB_2COOR_SINCOS_T_F16 sAngle;

frac16_t f16Modulo0;
frac16_t f16Modulo1;
frac16_t f16Duty0;
frac16_t f16Duty1;
frac16_t f16Duty2;
frac16_t f16Duty3;

uint8_t ui8Invert;
static FMSTR_U8 recBuffer[2048];     /* Recorder #1 sampling buffer */

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

/* Init Freemaster and recorder */
void FreemasterInit()
{
	Usart0Init();

	FMSTR_Init();

	FMSTR_REC_BUFF recBuffCfg;
	recBuffCfg.addr          = (FMSTR_ADDR)recBuffer;
	recBuffCfg.size          = sizeof(recBuffer);
	recBuffCfg.basePeriod_ns = 50000;
	recBuffCfg.name          = "FMSTR_REC";
	FMSTR_RecorderCreate(0, &recBuffCfg);
}

/*!
 * @brief Main function
 */
int main(void)
{
    __disable_irq();
    ui16Switching = 1;
    ui16Control   = 0;
    f16AngleInc   = FRAC16(0.01);
    f16Amplitude  = FRAC16(0.1);
    BOARD_BootClockPLL150M();
    BOARD_InitPins();
    Pwm0Init();
    Pwm1Init();
	PWM0->MCTRL |= PWM_MCTRL_RUN(15);
	PWM1->MCTRL |= PWM_MCTRL_RUN(15);
	RTCESL_PQ_Init();
    FreemasterInit();
    __enable_irq();

    while (1)
    {
    	FMSTR_Poll();
    }
}

void FLEXPWM0_RELOAD0_IRQHandler()
{
	/* Clear reload flag */
	PWM0->SM[0].STS |= PWM_STS_RF_MASK;
	/* Isr check counter */
	ui16PwmIsrCnt++;
	/* Make rotation */
	f16Angle = f16Angle + f16AngleInc;
	/* Calculate sin cos and apply amplitude */
	sAngle.f16Sin = MLIB_Mul_F16(f16Amplitude,GFLIB_Sin_F16(f16Angle));
	sAngle.f16Cos = MLIB_Mul_F16(f16Amplitude,GFLIB_Cos_F16(f16Angle));

	if(ui16Switching == FAST_DECAY_BIPOLAR)
	{
		ui16ChangeUnipolar = 0;
		/* bipolar, fast decay */
		if(!ui16ChangeBipolar)
		{
			/* Change polarity */
			PWM0->DTSRCSEL = PWM_DTSRCSEL_SM0SEL23(0)
					       | PWM_DTSRCSEL_SM0SEL45(0)
						   | PWM_DTSRCSEL_SM1SEL23(1)
					       | PWM_DTSRCSEL_SM1SEL45(0)
						   | PWM_DTSRCSEL_SM2SEL23(0)
						   | PWM_DTSRCSEL_SM2SEL45(0)
						   | PWM_DTSRCSEL_SM3SEL23(0)
						   | PWM_DTSRCSEL_SM3SEL45(0);

			PWM1->DTSRCSEL = PWM_DTSRCSEL_SM0SEL23(0)
					       | PWM_DTSRCSEL_SM0SEL45(0)
						   | PWM_DTSRCSEL_SM1SEL23(1)
					       | PWM_DTSRCSEL_SM1SEL45(0)
						   | PWM_DTSRCSEL_SM2SEL23(0)
						   | PWM_DTSRCSEL_SM2SEL45(0)
						   | PWM_DTSRCSEL_SM3SEL23(0)
						   | PWM_DTSRCSEL_SM3SEL45(0);

			PWM0->SM[0].CTRL2 |= PWM_CTRL2_FORCE_MASK;
			PWM1->SM[0].CTRL2 |= PWM_CTRL2_FORCE_MASK;


			ui16ChangeBipolar = 1;
		}


		if(ui16Control)
		{
			/* Full step control */
			if(f16Angle > ANGLE_0)
			{
				if(f16Angle > ANGLE_90)
				{
					f16Pha1 = FRAC16(0.5) + f16Amplitude;
					f16Pha2 = FRAC16(0.5) + f16Amplitude;
					f16Phb1 = FRAC16(0.5);
					f16Phb2 = FRAC16(0.5);
				}
				else
				{
					f16Phb1 = FRAC16(0.5) + f16Amplitude;
					f16Phb2 = FRAC16(0.5) + f16Amplitude;
					f16Pha1 = FRAC16(0.5);
					f16Pha2 = FRAC16(0.5);
				}
			}
			else
			{
				if(f16Angle > ANGLE_270)
				{
					f16Pha2 = FRAC16(0.5) - f16Amplitude;
					f16Pha1 = FRAC16(0.5) - f16Amplitude;
					f16Phb1 = FRAC16(0.5);
					f16Phb2 = FRAC16(0.5);
				}
				else
				{
					f16Phb2 = FRAC16(0.5) - f16Amplitude;
					f16Phb1 = FRAC16(0.5) - f16Amplitude;
					f16Pha1 = FRAC16(0.5);
					f16Pha2 = FRAC16(0.5);
				}
			}
		}
		else
		{
			/* micro */
			f16Pha1 = FRAC16(0.5) + sAngle.f16Sin;
			f16Pha2 = FRAC16(0.5) + sAngle.f16Sin;
			f16Phb1 = FRAC16(0.5) + sAngle.f16Cos;
			f16Phb2 = FRAC16(0.5) + sAngle.f16Cos;
		}
	}
	else
	{

		ui16ChangeBipolar = 0;
		/* unipolar, slow decay */
		if(!ui16ChangeUnipolar)
		{

			PWM0->DTSRCSEL = PWM_DTSRCSEL_SM0SEL23(0)
					       | PWM_DTSRCSEL_SM0SEL45(0)
						   | PWM_DTSRCSEL_SM1SEL23(0)
					       | PWM_DTSRCSEL_SM1SEL45(0)
						   | PWM_DTSRCSEL_SM2SEL23(0)
						   | PWM_DTSRCSEL_SM2SEL45(0)
						   | PWM_DTSRCSEL_SM3SEL23(0)
						   | PWM_DTSRCSEL_SM3SEL45(0);

			PWM1->DTSRCSEL = PWM_DTSRCSEL_SM0SEL23(0)
					       | PWM_DTSRCSEL_SM0SEL45(0)
						   | PWM_DTSRCSEL_SM1SEL23(0)
					       | PWM_DTSRCSEL_SM1SEL45(0)
						   | PWM_DTSRCSEL_SM2SEL23(0)
						   | PWM_DTSRCSEL_SM2SEL45(0)
						   | PWM_DTSRCSEL_SM3SEL23(0)
						   | PWM_DTSRCSEL_SM3SEL45(0);


			PWM0->SM[0].CTRL2 |= PWM_CTRL2_FORCE_MASK;
			PWM1->SM[0].CTRL2 |= PWM_CTRL2_FORCE_MASK;

			ui16ChangeUnipolar = 1;
		}

		if(ui16Control)
		{
			/* full */
			if(f16Angle >= 0)
			{
				if(f16Angle > ANGLE_90)
				{
					f16Pha1 = f16Amplitude;
					f16Pha2 = FRAC16(0.0);
					f16Phb1 = FRAC16(0.0);
					f16Phb2 = FRAC16(0.0);
				}
				else
				{
					f16Phb1 = f16Amplitude;
					f16Phb2 = FRAC16(0.0);
					f16Pha1 = FRAC16(0.0);
					f16Pha2 = FRAC16(0.0);
				}
			}
			else
			{
				if(f16Angle > ANGLE_270)
				{
					f16Pha2 = f16Amplitude;
					f16Pha1 = FRAC16(0.0);
					f16Phb1 = FRAC16(0.0);
					f16Phb2 = FRAC16(0.0);
				}
				else
				{
					f16Phb2 = f16Amplitude;
					f16Phb1 = FRAC16(0.0);
					f16Pha1 = FRAC16(0.0);
					f16Pha2 = FRAC16(0.0);
				}
			}
		}
		else
		{
			/* micro */
			if(sAngle.f16Sin >= 0)
			{
				f16Pha1 = sAngle.f16Sin;
				f16Pha2 = FRAC16(0.0);
			}
			else
			{
				f16Pha1 = FRAC16(0.0);
				f16Pha2 = MLIB_AbsSat_F16(sAngle.f16Sin);
			}

			if(sAngle.f16Cos >= 0)
			{
				f16Phb1 = sAngle.f16Cos;
				f16Phb2 = FRAC16(0.0);
			}
			else
			{
				f16Phb1 = FRAC16(0.0);
				f16Phb2 = MLIB_AbsSat_F16(sAngle.f16Cos);
			}
		}
	}

	FMSTR_Recorder(0);
	UpdatePWM0(f16Pha1, f16Pha2);
	UpdatePWM1(f16Phb1, f16Phb2);
}

static void	Pwm0Init(void)
{
	/* Enable FlexPWM submodule 0~3 function clock source in SYSCON */
	SYSCON->PWM0SUBCTL = SYSCON_PWM0SUBCTL_CLK0_EN_MASK
					   | SYSCON_PWM0SUBCTL_CLK1_EN_MASK
					   | SYSCON_PWM0SUBCTL_CLK2_EN_MASK
					   | SYSCON_PWM0SUBCTL_CLK3_EN_MASK;

	/* Enable interface clock of FlexPWM */
	SYSCON->AHBCLKCTRLSET[3] |= SYSCON_AHBCLKCTRL3_PWM0_MASK;

	/* Full cycle reload */
	PWM0->SM[0].CTRL |= PWM_CTRL_FULL_MASK;
	PWM0->SM[1].CTRL |= PWM_CTRL_FULL_MASK;

	/* Value register initial values, duty cycle 50% */
	PWM0->SM[0].INIT = (uint16_t)(-(PWM_MODULO/2));
	PWM0->SM[1].INIT = (uint16_t)(-(PWM_MODULO/2));

	PWM0->SM[0].VAL1 = (uint16_t)((PWM_MODULO/2)-1);

	PWM0->SM[0].VAL2 = (uint16_t)(-(PWM_MODULO/4));
	PWM0->SM[1].VAL2 = (uint16_t)(-(PWM_MODULO/4));

	PWM0->SM[0].VAL3 = (uint16_t)((PWM_MODULO/4));
	PWM0->SM[1].VAL3 = (uint16_t)((PWM_MODULO/4));

	/* set deadtime (number of Fast Peripheral Clocks) DTCNT0,1 = T_dead * f_fpc = 1.5us * 72MHz = 108 */
	PWM0->SM[0].DTCNT0 = 108;  //36
	PWM0->SM[1].DTCNT0 = 108;
	PWM0->SM[0].DTCNT1 = 108;
	PWM0->SM[1].DTCNT1 = 108;

	/* Modules one and two gets clock from module zero */
	PWM0->SM[1].CTRL2 = (PWM0->SM[1].CTRL2 & ~PWM_CTRL2_CLK_SEL_MASK) | PWM_CTRL2_CLK_SEL(0x2);
	PWM0->SM[1].CTRL2 = (PWM0->SM[1].CTRL2 & ~PWM_CTRL2_FORCE_SEL_MASK) | PWM_CTRL2_FORCE_SEL(0x1);
	/* Master reload active for modules one and two*/
	PWM0->SM[1].CTRL2 |= PWM_CTRL2_RELOAD_SEL_MASK;

	/* Master reload is generated every n-th opportunity */
	PWM0->SM[0].CTRL = (PWM0->SM[0].CTRL & ~PWM_CTRL_LDFQ_MASK) | PWM_CTRL_LDFQ(0);

	/* Master sync active for modules one and two*/
	PWM0->SM[1].CTRL2 = (PWM0->SM[1].CTRL2 & ~PWM_CTRL2_INIT_SEL_MASK) | PWM_CTRL2_INIT_SEL(0x2);

	/* Faults are disabled */
	PWM0->SM[0].DISMAP[0] = 0xF000U;
	PWM0->SM[0].DISMAP[1] = 0xF000U;
	PWM0->SM[1].DISMAP[0] = 0xF000U;
	PWM0->SM[1].DISMAP[1] = 0xF000U;
	PWM0->SM[2].DISMAP[0] = 0xF000U;
	PWM0->SM[2].DISMAP[1] = 0xF000U;
	PWM0->SM[3].DISMAP[0] = 0xF000U;
	PWM0->SM[3].DISMAP[1] = 0xF000U;

	/* Fault 0 active in logic level zero, manual clearing */
	PWM0->FCTRL |= PWM_FCTRL_FLVL(1);
	PWM0->FCTRL |= PWM_FCTRL_FAUTO(0);

	/* PWMs are re-enabled at PWM full cycle */
	PWM0->FSTS |= PWM_FSTS_FFULL(1);

	/* PWM fault filter - 5 Fast periph. clocks sample rate, 5 agreeing samples to activate */
	PWM0->FFILT |= PWM_FFILT_FILT_PER(5);
	PWM0->FFILT |= PWM_FFILT_FILT_CNT(5);

	/* Enable reload interrupt */
	PWM0->SM[0].INTEN |= PWM_INTEN_RIE_MASK;

	/* Start PWMs (set load OK flags and run) */
	PWM0->MCTRL |= PWM_MCTRL_CLDOK(15);
	PWM0->MCTRL |= PWM_MCTRL_LDOK(15);


	/* Enable A&B PWM outputs for submodules one, two and three */
	PWM0->OUTEN |= PWM_OUTEN_PWMA_EN(0xF) | PWM_OUTEN_PWMB_EN(0xF);

	NVIC_EnableIRQ(FLEXPWM0_RELOAD0_IRQn);
	NVIC_SetPriority(FLEXPWM0_RELOAD0_IRQn, 2);
}

static void	Pwm1Init(void)
{
	/* Enable FlexPWM submodule 0~3 function clock source in SYSCON */
	SYSCON->PWM1SUBCTL = SYSCON_PWM1SUBCTL_CLK0_EN_MASK
					   | SYSCON_PWM1SUBCTL_CLK1_EN_MASK
					   | SYSCON_PWM1SUBCTL_CLK2_EN_MASK
					   | SYSCON_PWM1SUBCTL_CLK3_EN_MASK;

	/* Enable interface clock of FlexPWM */
	SYSCON->AHBCLKCTRLSET[3] |= SYSCON_AHBCLKCTRL3_PWM1_MASK;

	/* Full cycle reload */
	PWM1->SM[0].CTRL |= PWM_CTRL_FULL_MASK;
	PWM1->SM[1].CTRL |= PWM_CTRL_FULL_MASK;

	/* Value register initial values, duty cycle 50% */
	PWM1->SM[0].INIT = (uint16_t)(-(PWM_MODULO/2));
	PWM1->SM[1].INIT = (uint16_t)(-(PWM_MODULO/2));

	PWM1->SM[0].VAL1 = (uint16_t)((PWM_MODULO/2)-1);

	PWM1->SM[0].VAL2 = (uint16_t)(-(PWM_MODULO/4));
	PWM1->SM[1].VAL2 = (uint16_t)(-(PWM_MODULO/4));

	PWM1->SM[0].VAL3 = (uint16_t)((PWM_MODULO/4));
	PWM1->SM[1].VAL3 = (uint16_t)((PWM_MODULO/4));

	/* Set deadtime (number of Fast Peripheral Clocks) DTCNT0,1 = T_dead * f_fpc = 1.5us * 72MHz = 108 */
	PWM1->SM[0].DTCNT0 = 108;
	PWM1->SM[1].DTCNT0 = 108;
	PWM1->SM[0].DTCNT1 = 108;
	PWM1->SM[1].DTCNT1 = 108;

	/* Modules one and two gets clock from module zero */
	PWM1->SM[1].CTRL2 = (PWM1->SM[1].CTRL2 & ~PWM_CTRL2_CLK_SEL_MASK) | PWM_CTRL2_CLK_SEL(0x2);
	PWM1->SM[1].CTRL2 = (PWM0->SM[1].CTRL2 & ~PWM_CTRL2_FORCE_SEL_MASK) | PWM_CTRL2_FORCE_SEL(0x1);
	/* Master reload active for modules one and two*/
	PWM1->SM[1].CTRL2 |= PWM_CTRL2_RELOAD_SEL_MASK;

	/* Master reload is generated every n-th opportunity */
	PWM1->SM[0].CTRL = (PWM1->SM[0].CTRL & ~PWM_CTRL_LDFQ_MASK) | PWM_CTRL_LDFQ(0);

	/* Master sync active for modules one and two*/
	PWM1->SM[1].CTRL2 = (PWM1->SM[1].CTRL2 & ~PWM_CTRL2_INIT_SEL_MASK) | PWM_CTRL2_INIT_SEL(0x2);

	/* Faults are disabled */
	PWM1->SM[0].DISMAP[0] = 0xF000U;
	PWM1->SM[0].DISMAP[1] = 0xF000U;
	PWM1->SM[1].DISMAP[0] = 0xF000U;
	PWM1->SM[1].DISMAP[1] = 0xF000U;
	PWM1->SM[2].DISMAP[0] = 0xF000U;
	PWM1->SM[2].DISMAP[1] = 0xF000U;
	PWM1->SM[3].DISMAP[0] = 0xF000U;
	PWM1->SM[3].DISMAP[1] = 0xF000U;

	/* fault 0 active in logic level zero, manual clearing */
	PWM1->FCTRL |= PWM_FCTRL_FLVL(1);
	PWM1->FCTRL |= PWM_FCTRL_FAUTO(0);

	/* PWMs are re-enabled at PWM full cycle */
	PWM1->FSTS |= PWM_FSTS_FFULL(1);

	/* PWM fault filter - 5 Fast periph. clocks sample rate, 5 agreeing samples to activate */
	PWM1->FFILT |= PWM_FFILT_FILT_PER(5);
	PWM1->FFILT |= PWM_FFILT_FILT_CNT(5);

	/* start PWMs (set load OK flags and run) */
	PWM1->MCTRL |= PWM_MCTRL_CLDOK(15);
	PWM1->MCTRL |= PWM_MCTRL_LDOK(15);

	/* enable A&B PWM outputs for submodules one, two and three */
	PWM1->OUTEN |= PWM_OUTEN_PWMA_EN(0xF) | PWM_OUTEN_PWMB_EN(0xF);

}

void UpdatePWM0(frac16_t phA, frac16_t phB)
{
    /* PWM duty cycles calculation */

    frac16_t f16DutyCycle;

    /* modulo read from pwma_sub0 val1 register */
    f16Modulo0 = PWM0->SM[0].VAL1 + 1;

    /* phase A */
    f16Duty0 = MLIB_Mul_F16(f16Modulo0,phA);
    PWM0->SM[0].VAL2 = (uint16_t)MLIB_Neg_F16(f16Duty0);
    PWM0->SM[0].VAL3 = (uint16_t)f16Duty0;

    f16Duty1 = MLIB_Mul_F16(f16Modulo0, phB);
    PWM0->SM[1].VAL2 = (uint16_t)MLIB_Neg_F16(f16Duty1);
    PWM0->SM[1].VAL3 = (uint16_t)f16Duty1;

    /* Set LDOK bit for sub0 sub1 sub2 */
    PWM0->MCTRL |= PWM_MCTRL_LDOK_MASK;
}


void UpdatePWM1(frac16_t phA, frac16_t phB)
{
    /* PWM duty cycles calculation */

    frac16_t f16DutyCycle;

    /* modulo read from pwma_sub0 val1 register */
    f16Modulo1 = PWM1->SM[0].VAL1 + 1;

    /* phase A */
    f16Duty2 = MLIB_Mul_F16(f16Modulo1,phA);
    PWM1->SM[0].VAL2 = (uint16_t)MLIB_Neg_F16(f16Duty2);
    PWM1->SM[0].VAL3 = (uint16_t)f16Duty2;

    /* phase B */
    f16Duty3 = MLIB_Mul_F16(f16Modulo1, phB);
    PWM1->SM[1].VAL2 = (uint16_t)MLIB_Neg_F16(f16Duty3);
    PWM1->SM[1].VAL3 = (uint16_t)f16Duty3;

    /* Set LDOK bit for sub0 sub1 sub2 */
    PWM1->MCTRL |= PWM_MCTRL_LDOK_MASK;
}

/*
 * USART Module initialization
 */
static void Usart0Init(void)
{
    /* attach main clock divide to FLEXCOMM0 (debug console) */
    CLOCK_AttachClk(BOARD_DEBUG_UART_CLK_ATTACH);

    usart_config_t config;
    /*
     *   usartConfig->baudRate_Bps = 115200U;
     *   usartConfig->parityMode = kUSART_ParityDisabled;
     *   usartConfig->stopBitCount = kUSART_OneStopBit;
     *   usartConfig->bitCountPerChar = kUSART_8BitsPerChar;
     *   usartConfig->loopback = false;
     *   usartConfig->enableTx = false;
     *   usartConfig->enableRx = false;
    */
    USART_GetDefaultConfig(&config);
    /* Override the Default configuration to satisfy FreeMASTER needs */
    config.baudRate_Bps = 115200U;
    config.enableTx = true;
    config.enableRx = true;

    USART_Init((USART_Type*)BOARD_DEBUG_UART_BASEADDR, &config, BOARD_DEBUG_UART_CLK_FREQ);

    /* Register communication module used by FreeMASTER driver. */
    FMSTR_SerialSetBaseAddress((USART_Type*)BOARD_DEBUG_UART_BASEADDR);

#if FMSTR_SHORT_INTR || FMSTR_LONG_INTR
    /* Enable UART interrupts. */
    EnableIRQ(BOARD_UART_IRQ);
    EnableGlobalIRQ(0);
#endif

}
