#include "F28x_Project.h"
#include <stdio.h>
#include "keyFSM.h"
#include "childFSM.h"
#include "parentFSM.h"
#include "FSM_public.h"

#define  LED_BLINKY_GPIO    2
#define  INPUT_GPIO    15

uint32_t exec_time[50];
uint32_t stop_time;
int index = 0;

inline void tic(){
    CpuTimer1Regs.TCR.bit.TSS = 1;     // stop the timer
    CpuTimer1Regs.TCR.bit.TRB = 1;     // 1 = reload timer
    CpuTimer1Regs.TCR.bit.TSS = 0;     // start the timer
}


inline void toc(uint16_t index){
    stop_time = CpuTimer1.RegsAddr->TIM.all;
    CpuTimer1Regs.TCR.bit.TSS = 1;
    exec_time[index] = CpuTimer1.RegsAddr->PRD.all - stop_time;
}


int isDown = 0;



ChildFSM *cFSM;
ParentFSM *pFSM;
void childTest(){

    cFSM = createChildFSM();
    pFSM = createParentFSM();

    setChildNum((pFSM), 1);
    registerChildFSM((pFSM), (cFSM), 0);

    for (int i = 0; i < 15; i++)
    {
        ParentFSM_Input(pFSM)->arrindex = i;
        SetPreloaderReady(pFSM);

        ChildFSM_Input(cFSM)->arrindex = i;
        SetPreloaderReady(cFSM);

        ChildFSMData* data = getData((FSM *)(cFSM));
        printf(" %d ", data->arrindex);

        tic();
        _vptrFSM((FSM *)pFSM)->step((FSM *)pFSM);
        toc(index++);
        printf("\n");
    }
}





KeyFSM* keyFSM;
KeyFSMData* data;

void keyFSMTest(){
    keyFSM = createKeyFSM();
    data = getData((FSM *)keyFSM);

    printf("hello 28377\n");

    while(1){

        *KeyFSM_Input(keyFSM) = !GPIO_ReadPin(INPUT_GPIO);
        SetPreloaderReady(keyFSM);

        tic();
        Step(keyFSM);

        if(data->out != Idle){
            toc(++index);
            printf("%s\n", keyStr[data->out]);
        }
        else{
            toc(0);
        }

        DELAY_US(20*1000);
    }
}

typedef int (*fcn)(int, int);
int printtest(int a, int b){
    return a+b;
}

void funptrTest(){
    fcn fcnptr;
    fcnptr = printtest;

    int index = 0;
    while(1){
        tic();
        int c = fcnptr(10, 20);
        toc(index);
        index++;

        printf("%d\n", c);
        DELAY_US(20*1000);
    }
}

void main(void)
{
    InitSysCtrl();
    InitGpio();
    InitCpuTimers();
    ConfigCpuTimer(&CpuTimer1, 200, 100000);
    CpuTimer1Regs.TCR.all = 0x0000;

    GPIO_SetupPinOptions(LED_BLINKY_GPIO, GPIO_OUTPUT, GPIO_PUSHPULL);
    GPIO_SetupPinMux(LED_BLINKY_GPIO, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(INPUT_GPIO, GPIO_INPUT, GPIO_PULLUP);


    childTest();
//    keyFSMTest();
//    funptrTest();
}