/**
 * @author Rémi Heredero
 * @version 1.0.0
 * @date September 2023
 * @file steering.c
 */

#include "steering.h"
#include "can_message.h"
#include "car.h"
//#include "drive.h"

void alive_born(void* p){
    STEERING* me = (STEERING*) p;
    STEERING_emitResurrect(me);
    //CM_STEERING_SETUP(&ALWAYS3);
}

void STEERING_init(STEERING* me){
    me->state = STST_INIT;
    ALIVE_init(&me->myChecker, 3);
    ALIVE_onSetup(&me->myChecker, CM_STEERING_SETUP, &ALWAYS2);
    ALIVE_onWait(&me->myChecker, STEERING_emitStart, me);
    ALIVE_onDead(&me->myChecker, STEERING_emitStop, me);
    ALIVE_onBorn(&me->myChecker, alive_born, me);
    me->wait.f = NULL;
    me->run.f = NULL;
    me->dead.f = NULL;
}

void STEERING_startBehaviour(STEERING* me){
    POST(me, &STEERING_processEvent, evSTinit, 3000, 0);
}

bool STEERING_processEvent(Event* ev) {
    bool processed = false;
    STEERING* me = (STEERING*)Event_getTarget(ev);
    STEERING_STATES oldState = me->state;
    evIDT evid = Event_getId(ev);
    uint64_t data = Event_getData(ev);
    
    switch (me->state) {        // onState
        case STST_INIT:
            if (ev->id == evSTinit) {
                me->state = STST_WAIT;
                ALIVE_startBehaviourChecker(&me->myChecker); // Start alive checker
            }
            break;
        
        case STST_WAIT:
            if (ev->id == evSTstart) {
                me->state = STST_RUN;
            }
//            ALIVE_setAliveTime(&me->myChecker, KART_CST.STEERING_ALIVE_TIME);
//            ALIVE_emitBorn(&me->myChecker, 500, 0);
//            ALIVE_emitReady(&me->myChecker, 1000, 0);
            break;
        
        case STST_RUN:
            if (ev->id == evSTstop) {
                me->state = STST_DEAD;
            }
            
            if (ev->id == evSTpollDir) {
                //if(drive()->state == STDR_RUN) {
                    CM_STEERING_SET(&eKart.position);
                //}
            }
            break;
        
        case STST_DEAD:
            if (ev->id == evSTresurrect) {
                me->state = STST_WAIT;
            }
            break;
    }

    if(oldState != me->state){
        switch (oldState) {     // onExit
            case STST_INIT:
                break;
            
            case STST_WAIT:
                break;
            
            case STST_RUN:
                break;
            
            case STST_DEAD:
                break;
        }

        switch (me->state) {    // onEntry
            case STST_INIT:
                break;
            
            case STST_WAIT:
                if (me->wait.f != NULL) {
                    me->wait.f(me->wait.p);
                }
                ALIVE_setAliveTime(&me->myChecker, KART_CST.STEERING_ALIVE_TIME);
                ALIVE_emitBorn(&me->myChecker, 500, 0);
                break;
            
            case STST_RUN:
                if (me->run.f != NULL) {
                    me->run.f(me->run.p);
                }
                break;
            
            case STST_DEAD:
                if (me->dead.f != NULL) {
                    me->dead.f(me->dead.p);
                }
                break;
        }

        processed = true;
    }
    return processed;
}

/*************
 * Callbacks *
 *************/

void STEERING_onWait(STEERING* me, STEERING_CALLBACK_FUNCTION f, void* p) {
    me->wait.f = f;
    me->wait.p = p;
}

void STEERING_onRun(STEERING* me, STEERING_CALLBACK_FUNCTION f, void* p) {
    me->run.f = f;
    me->run.p = p;
}

void STEERING_onDead(STEERING* me, STEERING_CALLBACK_FUNCTION f, void* p) {
    me->dead.f = f;
    me->dead.p = p;
}

/************
 * EMITTERS *
 ************/

void STEERING_emitStart(void* p) {
    STEERING* me = (STEERING*) p;
    POST(me, &STEERING_processEvent, evSTstart, 0, 0);
}

void STEERING_emitStop(void* p) {
    STEERING* me = (STEERING*) p;
    POST(me, &STEERING_processEvent, evSTstop, 0, 0);
}

void STEERING_emitResurrect(void* p) {
    STEERING* me = (STEERING*) p;
    POST(me, &STEERING_processEvent, evSTresurrect, 0, 0);
}

void STEERING_emitPollDir(void* p) {
    STEERING* me = (STEERING*) p;
    POST(me, &STEERING_processEvent, evSTpollDir, 0, 0);
}

/***********
 * SETTERS *
 ***********/

void STEERING_setMyChecker(STEERING* me, ALIVE v) {
    me->myChecker = v;
}