#include "Clock.h"
#include "IntcInterrupts.h"
#include "DigitalOut.h"
#include "PinAssignment.h"
#include "MPC5744P.h"

/********************
* Define
********************/
#define PIT_INTERRUPT_VECTOR_CH0 226

#define PIT_NUM_CH 4

/******************************
* Private function declaration
*******************************/

static void task_run_500(void);
static void task_run_100(void);
static int PIT_Init();
static int PIT_add(void (*fnc)(), uint32_t delta, uint8_t priority);
static void INTC_deInit(void);
static int PIT_deInit();
/****************************
* Main
*****************************/
int main(void)
{
    int counter = 0;

    Clock_sysclck_init(16); // setup the clock
    PinAssignment_init();
    DigitalOut_init();
    PIT_Init();
    PIT_add(task_run_100, FREQUENCY_1MS_TASK, PRIORITY_1MS_TASK);
    INTC_InitINTCInterrupts();

    /* Loop forever */
    while (1)
    {
        PIT_deInit();
        INTC_deInit();
        Clock_waitms(2);
        PIT_Init();
        PIT_add(task_run_500, FREQUENCY_1MS_TASK, PRIORITY_1MS_TASK);
        INTC_InitINTCInterrupts();
        Clock_waitms(5000);

        PIT_deInit();
        INTC_deInit();
        Clock_waitms(2);
        PIT_Init();
        PIT_add(task_run_100, FREQUENCY_1MS_TASK, PRIORITY_1MS_TASK);
        INTC_InitINTCInterrupts();
        Clock_waitms(5000);

        counter++;
    }
    return 0;
}

/*************************
* Private Functions
**************************/
static int PIT_Init()
{
    PIT_0.MCR.B.MDIS = 0; // stop PIT when in debug mode  , Enable PIT module
    PIT_0.MCR.B.FRZ = 1;
    // Reset all register
    PIT_0.TIMER[0].LDVAL.R = 0U;
    PIT_0.TIMER[0].TCTRL.R = 0U;
    PIT_0.TIMER[0].TFLG.R = 0xFFFFFFFFU;

    return 0;
}

 static int PIT_deInit()
 {
     PIT_0.TIMER[0].TFLG.B.TIF = 1;
     PIT_0.TIMER[0].LDVAL.R = 0U;
     PIT_0.TIMER[0].TCTRL.R = 0U;
     PIT_0.TIMER[0].TFLG.R = 0U;
     return 0;
 }

 static void INTC_deInit(void)
 {
    __asm__("mtmsr 0\n    ");
 }

static void task_run_500(void)
{
    static uint32_t timestamp = 0U;
    if (timestamp >= 500)
    {
        DigitalOut_toggleDigitalOutput(DIGITAL_OUT_CH_LED1);
        timestamp = 0U;
    }
    else
    {
        timestamp ++;
    }
    PIT_0.TIMER[0].TFLG.B.TIF = 1;
}

static void task_run_100(void)
{
    static uint32_t timestamp = 0U;
    if (timestamp >= 100)
    {
        DigitalOut_toggleDigitalOutput(DIGITAL_OUT_CH_LED1);
        timestamp = 0U;
    }
    else
    {
        timestamp ++;
    }
    PIT_0.TIMER[0].TFLG.B.TIF = 1;
}

static int PIT_add(void (*fnc)(), uint32_t delta, uint8_t priority)
{
    // if there are less than 4 channels
    PIT_0.TIMER[0].LDVAL.B.TSV = (uint32_t)PERIPHERAL_CLOCK_SPEED_HZ / (uint32_t)delta;
    PIT_0.TIMER[0].TCTRL.B.TEN = 1;
    PIT_0.TIMER[0].TCTRL.B.TIE = 1;

    INTC_InstallINTCInterruptHandler(fnc, (uint8_t)(PIT_INTERRUPT_VECTOR_CH0), (uint8_t)priority);

    return -1; // no channel available
}