/**
 * @author Rémi Heredero
 * @version 1.0.0
 * @date August 2023
 * @file drive.c
 */

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

void DRIVE_init(DRIVE* me){
    me->state = STDR_INIT;
    ALIVE_init(&me->myChecker, 2);
    ALIVE_onSetup(&me->myChecker, CM_DRIVE_SETUP, &ALWAYSTRUE);
    ALIVE_onWait(&me->myChecker, DRIVE_emitStart, me);
    ALIVE_onDead(&me->myChecker, DRIVE_emitStop, me);
    ALIVE_onBorn(&me->myChecker, DRIVE_emitResurrect, me);
    me->wait.f = NULL;
    me->run.f = NULL;
    me->dead.f = NULL;
}

void DRIVE_startBehaviour(DRIVE* me){
    POST(me, &DRIVE_processEvent, evDRinit, 3000, 0);
}

bool DRIVE_processEvent(Event* ev) {
    bool processed = false;
    DRIVE* me = (DRIVE*)Event_getTarget(ev);
    DRIVE_STATES oldState = me->state;
    evIDT evid = Event_getId(ev);
    uint64_t data = Event_getData(ev);
//    STEERING_STATES steeringState = steering()->state;

    
    switch (me->state) {        // onState
        case STDR_INIT:
            if (ev->id == evDRinit) {
                me->state = STDR_WAIT;
                ALIVE_startBehaviourChecker(&me->myChecker); // Start alive checker
            }
            break;
        
        case STDR_WAIT:
            if (ev->id == evDRstart) {
                me->state = STDR_RUN;
            }
            ALIVE_setAliveTime(&me->myChecker, KART_CST.DRIVE_ALIVE_TIME);
            ALIVE_emitBorn(&me->myChecker, 100, 0);      // Born after 100 ms
            ALIVE_emitReady(&me->myChecker, 200, 0);     // Ready after 200 ms
            break;
        
        case STDR_RUN:
            if (ev->id == evDRstop) {
                me->state = STDR_DEAD;
            }
            
            if (ev->id == evDRpollTorque) {
                //if(steeringState == STST_RUN) {
                    CM_DRIVE_POWER(&eKart.torque);
                //}
            }
            
            if (ev->id == evDRpollSpeed) {
                CM_DISPLAY_SPEED(&eKart.speed);
            }
            
            DRIVE_emitPollTorque(me, KART_CST.DRIVE_STOP_TIME*9, 0);
            break;
        
        case STDR_DEAD:
            if (ev->id == evDRresurrect) {
                me->state = STDR_WAIT;
            }
            break;
    }

    if(oldState != me->state){
        switch (oldState) {     // onExit
            case STDR_INIT:
                break;
            
            case STDR_WAIT:
                break;
            
            case STDR_RUN:
                break;
            
            case STDR_DEAD:
                break;
        }

        switch (me->state) {    // onEntry
            case STDR_INIT:
                break;
            
            case STDR_WAIT:
                if (me->wait.f != NULL) {
                    me->wait.f(me->wait.p);
                }
                break;
            
            case STDR_RUN:
                if (me->run.f != NULL) {
                    me->run.f(me->run.p);
                }
                break;
            
            case STDR_DEAD:
                if (me->dead.f != NULL) {
                    me->dead.f(me->dead.p);
                }
                break;
        }

        processed = true;
    }
    return processed;
}

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

void DRIVE_onWait(DRIVE* me, DRIVE_CALLBACK_FUNCTION f, void* p) {
    me->wait.f = f;
    me->wait.p = p;
}

void DRIVE_onRun(DRIVE* me, DRIVE_CALLBACK_FUNCTION f, void* p) {
    me->run.f = f;
    me->run.p = p;
}

void DRIVE_onDead(DRIVE* me, DRIVE_CALLBACK_FUNCTION f, void* p) {
    me->dead.f = f;
    me->dead.p = p;
}

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

void DRIVE_emitStart(void* p) {
    DRIVE* me = (DRIVE*) p;
    POST(me, &DRIVE_processEvent, evDRstart, 0, 0);
}

void DRIVE_emitStop(void* p) {
    DRIVE* me = (DRIVE*) p;
    POST(me, &DRIVE_processEvent, evDRstop, 0, 0);
}

void DRIVE_emitResurrect(void* p) {
    DRIVE* me = (DRIVE*) p;
    POST(me, &DRIVE_processEvent, evDRresurrect, 0, 0);
}

void DRIVE_emitPollSpeed(void* p) {
    DRIVE* me = (DRIVE*) p;
    POST(me, &DRIVE_processEvent, evDRpollSpeed, 0, 0);
}

void DRIVE_emitPollTorque(DRIVE* me, uint16_t t, int64_t data) {
    POST(me, &DRIVE_processEvent, evDRpollTorque, t, data);
}

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

void DRIVE_setMyChecker(DRIVE* me, ALIVE v) {
    me->myChecker = v;
}