#include <QTime>
#include <QtMath>
#include <QRandomGenerator>

#include "StateController.h"
#include "ApplicationController.h"
#include "types.h"

StateController::StateController(QObject *parent) : QObject(parent) {}

void StateController::init()
{
    startTimer(QOBJECT_TIMER_TIMEOUT_MS);
    connect(&_ApplicationController, SIGNAL(didUnhandledMsgAppController(const QVariantList &)),
            this                   ,   SLOT(doStateControllerUnhandledMsgReceived(const QVariantList &)));

    _dryDemo.connectToState("Standby"            , this, &StateController::onStandbyStateChange           );
    _dryDemo.connectToState("Pre_Tx"             , this, &StateController::onPretreatmentStateChange      );
    _dryDemo.connectToState("Blood_Prime"        , this, &StateController::onBloodPrimeStateChange        );
    _dryDemo.connectToState("Treatment"          , this, &StateController::onTreatmentStateChange         );


    _transitionEventsFromStandby.clear();
    _transitionEventsFromStandby[CMD_PRE_TX        ] = {"T_SB_2_Pretx", static_cast<quint16>(MODE_PRET)};
    _transitionEventsFromStandby[CMD_BLOOD_PRIME   ] = {"T_SB_2_BP"   , static_cast<quint16>(MODE_TREA)};
    _transitionEventsFromStandby[CMD_MAIN_TX       ] = {"T_SB_2_Tx"   , static_cast<quint16>(MODE_TREA)};

    //connect(&_dryDemo, &QScxmlStateMachine::runningChanged, this, &StateController::onBackToStanbyEvent); // TODO remove
}

void StateController::quit()
{
    // TODo fill up
}

void StateController::doStateControllerUnhandledMsgReceived(const QVariantList &msg)
{
    MessageID receivedMsgID = static_cast<Can::MessageID>(msg[0].toUInt()); // TODO check the length before reading the index

    switch(receivedMsgID)
    {
        case ID_INITIATE_TREATMENT:
        case ID_UI_TREATMENT_PARAMS_TO_VALIDATE:
        case ID_UI_BLOOD_PRIME_CMD_RQST:
        case ID_UI_CONFIRMATION_RESULT_RESP:
        case ID_UI_TREATMENT_SET_POINT_BLOOD_FLOW_CHANGE_RQST:
            _treatmentRcvdMessages[receivedMsgID] = msg[1];
            break;

        //// These messages do not have a payload in from UI so a fake payload is added to be consumed
        //// and processed
        case ID_UI_ADJUST_DISPOSABLES_CONFIRM_RQST:
        case ID_UI_ADJUST_START_TREATMENT_RQST:
            _treatmentRcvdMessages[receivedMsgID] = 0;
            break;

        //case ID_UI_TX_END_RQST:
        //case ID_UI_CONSUMABLES_INSTALL:
        //case ID_UI_INTALLATION_CONFIRM:
        //case ID_UI_PATIENT_CONNECTION_RQST:
        //case ID_UI_PATIENT_CONNECTION_CONF_RQST:
        //case ID_UI_START_TX_RQST:
        //case ID_UI_SERVICE_MODE_RQST:
        //    _hasUserConfirmedToProceed = true;
        //    break;

        case ID_NONE:
        case ID_TD_OP_MODE_DATA:
        case ID_TD_TREATMENT_STATE_DATA:
        case ID_TD_RESP_INITIATE_TREATMENT:
        case ID_TD_ADJUST_DISPOSSABLES_CONFIRM_RESP:
        case ID_TD_RESP_TREATMENT_PARAMS_TO_VALIDATE:
        case ID_TD_BLOOD_PRIME_PROGRESS_DATA:
        case ID_TD_BLOOD_PRIME_CMD_RESP:
        case ID_TD_ADJUST_START_TREATMENT_RESP:
        case ID_TD_TREATMENT_SET_POINT_BLOOD_FLOW_CHANGE_RESP:
        case ID_TD_TREATMENT_SET_POINTS:
        case ID_TD_TREATMENT_TIME_DATA:
            // Do nothing
            break;
    }
}

void StateController::timerEvent(QTimerEvent *)
{
    if (!_dryDemo.isRunning()){
        _dryDemo.start();
    }

    _dryDemo.submitEvent("TimeoutEvent");

    if (_isBroadcastListReady){
        sendMessages(true);
    }

    if (_isSendListReady){
        sendMessages(false);
    }

    //if (! _treatmentRcvdMessages[ID_UI_CONFIRMATION_RESULT_RESP].isNull()){
    //    // Check for the user request message from the dry-deme menu
    //    handleDryDemoMenuRequestMessage();
    //}

    //qDebug() << "Active State" << _dryDemo.activeStateNames();
}

// ---------------------- Private methods ------------------ //
void StateController::sendMessages(bool isBroadcast)
{
    QList msgList = _broadcastMessages;

    if (isBroadcast){
        _broadcastMessages.clear();
    }
    else{
        msgList = _sendMessages;
        _sendMessages.clear();
    }

    for (auto item: msgList){
        emit _ApplicationController.didActionTransmit(item);
    }
}

void StateController::handleDryDemoMenuRequestMessage()
{
    quint32 msgPayload = extractU32DataFromReceivedMessage(ID_UI_CONFIRMATION_RESULT_RESP, 0);
    if (msgPayload != UI_MSG_BB_PAYLOAD_ID) { return; }

    msgPayload = extractU32DataFromReceivedMessage(ID_UI_CONFIRMATION_RESULT_RESP, UI_MSG_BB_STATUS_INDEX);
    _treatmentStatus.userCmd = static_cast<User_Command_ID>(msgPayload);
    _treatmentRcvdMessages[ID_UI_CONFIRMATION_RESULT_RESP].clear();
}

void StateController::handleBackToStandbyEvent(State_Status &status)
{
    //_dryDemo.connectToEvent(_back2StandbyEvent, [&status](const QScxmlEvent &event) {
    //    qDebug() << "Event invoked:" << event.name();
    //    status = STATE_ON_ENTRY;
    //}, Qt::UniqueConnection);

    if (! _treatmentRcvdMessages[ID_UI_CONFIRMATION_RESULT_RESP].isNull()){
        // Check for the user request message from the dry-demo menu
        handleDryDemoMenuRequestMessage();
        _treatmentStatus.stateTransitionEvent = "T_Back_2_SB";
        status                                = STATE_ON_EXIT;
    }
}

void StateController::onBackToStanbyEvent()
{
    // TODO remove
    qDebug() << "Active State" << _dryDemo.activeStateNames();
}

// ----------- State transition methods ---------------- //
void StateController::onStandbyStateChange(bool active)
{
    static State_Status status = STATE_ON_ENTRY;

    auto onEntry = [=](){
        handleTDOpModeTransitionBroadcastData(MODE_STAN, 0, BC_IMMEDIATELY);
        setTreatmentParametersToDefault();
        resetDryDemoVariables();

        status = STATE_ON_ACTION;
        qDebug() << "Standby on entry";
    };

    auto onAction = [=](){
        status = STATE_ON_ACTION;

        if (! _treatmentRcvdMessages[ID_INITIATE_TREATMENT].isNull()){
            _treatmentRcvdMessages[ID_INITIATE_TREATMENT].clear();

            _treatmentStatus.stateTransitionEvent = _transitionEventsFromStandby[CMD_PRE_TX][0].toString();

            handleUIResponseMessage(ID_TD_RESP_INITIATE_TREATMENT, ACCEPT_VALUE);
            status = STATE_ON_EXIT;
        }

        if (_treatmentStatus.userCmd != CMD_NONE){
            if ((_treatmentStatus.userCmd > CMD_STAND_BY) && (_treatmentStatus.userCmd <= CMD_DISINFECTION)) {
                _treatmentStatus.stateTransitionEvent = _transitionEventsFromStandby[_treatmentStatus.userCmd][0].toString();
                TD_OP_MODE opMode = static_cast<TD_OP_MODE>(_transitionEventsFromStandby[_treatmentStatus.userCmd][1].toUInt());
                qDebug() << "Submit event" << _treatmentStatus.stateTransitionEvent << opMode;
                handleTDOpModeTransitionBroadcastData(opMode, 0, BC_IMMEDIATELY);
                _treatmentStatus.userCmd = CMD_NONE;
                status = STATE_ON_EXIT;
            }
        }
        handleTDOpModeTransitionBroadcastData(MODE_STAN, 0, BC_DELAYED);
        handleBackToStandbyEvent(status);
    };

    auto onExit = [=](){
        qDebug() << "Standby in exit";
        _dryDemo.submitEvent(_treatmentStatus.stateTransitionEvent);
        status = STATE_ON_ENTRY;
    };

    switch (status) {  // TODO macro it later
    case STATE_ON_ENTRY :   if (active) onEntry ();  break;
    case STATE_ON_ACTION:   if (active) onAction();  break;
    case STATE_ON_EXIT  :   if (active) onExit  ();  break;
    }
}

void StateController::onPretreatmentStateChange(bool active)
{
    static State_Status status = STATE_ON_ENTRY;

    auto onEntry = [=](){
        handleTDOpModeTransitionBroadcastData(MODE_PRET, 0, BC_IMMEDIATELY);
        status = STATE_ON_ACTION;
        qDebug() << "PreTx on entry";
    };

    auto onAction = [=](){
        status = STATE_ON_ACTION;
        if (! _treatmentRcvdMessages[ID_UI_ADJUST_DISPOSABLES_CONFIRM_RQST].isNull()){
            _treatmentRcvdMessages[ID_UI_ADJUST_DISPOSABLES_CONFIRM_RQST].clear();
            handleUIResponseMessage(ID_TD_ADJUST_DISPOSSABLES_CONFIRM_RESP, ACCEPT_VALUE);
        }

        if (! _treatmentRcvdMessages[ID_UI_TREATMENT_PARAMS_TO_VALIDATE].isNull()){
            setTreatmentParametersFromUI(_treatmentRcvdMessages[ID_UI_TREATMENT_PARAMS_TO_VALIDATE]);
            _treatmentRcvdMessages[ID_UI_TREATMENT_PARAMS_TO_VALIDATE].clear();
        }

        if (! _treatmentRcvdMessages[ID_UI_ADJUST_START_TREATMENT_RQST].isNull()){
            _treatmentRcvdMessages[ID_UI_ADJUST_START_TREATMENT_RQST].clear();
            handleUIResponseMessage(ID_TD_ADJUST_START_TREATMENT_RESP, ACCEPT_VALUE);
            status                                = STATE_ON_EXIT;
            _treatmentStatus.stateTransitionEvent = "T_Pretx_2_BP";
        }
        handleBackToStandbyEvent(status);
    };

    auto onExit = [=](){
        qDebug() << "PreTx in exit";
        _dryDemo.submitEvent(_treatmentStatus.stateTransitionEvent);
        status = STATE_ON_ENTRY;
    };

    switch (status) {  // TODO macro it later
    case STATE_ON_ENTRY :   if (active) onEntry ();  break;
    case STATE_ON_ACTION:   if (active) onAction();  break;
    case STATE_ON_EXIT  :   if (active) onExit  ();  break;
    }
}

void StateController::onBloodPrimeStateChange(bool active)
{
    static State_Status status        = STATE_ON_ENTRY;
    static float accumBloodVolML      = 0.0;
    static quint32 currBloodFlowMLPM  = BLOOD_PRIME_START_FLOW_MLPM;
    static bool resume                = true;
    static quint32 bloodPrimeResume   = 1;
    static quint32 bloodPrimePause    = 2;
    static float rampStepML           = 0.0;
    static bool hasFlowChngeRequested = false;

    auto onEntry = [=](){
        handleTDOpModeTransitionBroadcastData(MODE_TREA, 0, BC_IMMEDIATELY);
        // Clear the blood prime data immediately to prevent residual accumulated volume from a previous run
        handleBloodPrimeBroadcastData(0.0, 0, 0, BC_IMMEDIATELY);
        accumBloodVolML       = 0.0;
        currBloodFlowMLPM     = BLOOD_PRIME_START_FLOW_MLPM;
        resume                = true;
        hasFlowChngeRequested = false;

        quint32 bloodPrimeStateRamp = 1;
        QList<quint32> txStates     = {TREAT_BLOOD_PRIME_STATE, bloodPrimeStateRamp, 0, 0, 0, 0, 0, 0, 0, 0};
        handleTreatmentStatesBroadcastData(txStates, BC_IMMEDIATELY);

        rampStepML = getBloodPrimeRampStepML();
        qDebug() << "Rmpa" << rampStepML;

        status = STATE_ON_ACTION;

        qDebug() << "Blood prime on entry";
    };

    auto onAction = [=](){
        status = STATE_ON_ACTION;
        if (! _treatmentRcvdMessages[ID_UI_BLOOD_PRIME_CMD_RQST].isNull()){
            handleUIResponseMessage(ID_TD_BLOOD_PRIME_CMD_RESP, ACCEPT_VALUE);

            quint32 resumeStatus = extractU32DataFromReceivedMessage(ID_UI_BLOOD_PRIME_CMD_RQST, 0);

            resume = (resumeStatus == bloodPrimeResume ? true : false);
            quint32 state = (resumeStatus == bloodPrimeResume ? bloodPrimeResume : bloodPrimePause);

            QList<quint32> txStates = {TREAT_BLOOD_PRIME_STATE, state, 0, 0, 0, 0, 0, 0, 0, 0};
            handleTreatmentStatesBroadcastData(txStates, BC_IMMEDIATELY);
            _treatmentRcvdMessages[ID_UI_BLOOD_PRIME_CMD_RQST].clear();
        }

        if (! _treatmentRcvdMessages[ID_UI_TREATMENT_SET_POINT_BLOOD_FLOW_CHANGE_RQST].isNull()){
            // TODO do we need to check the range
            currBloodFlowMLPM = extractU32DataFromReceivedMessage(ID_UI_TREATMENT_SET_POINT_BLOOD_FLOW_CHANGE_RQST, 0);
            handleUIResponseMessage(ID_TD_TREATMENT_SET_POINT_BLOOD_FLOW_CHANGE_RESP, ACCEPT_VALUE);
            hasFlowChngeRequested = true;
            qDebug() << "FLow" << currBloodFlowMLPM;

            _treatmentRcvdMessages[ID_UI_TREATMENT_SET_POINT_BLOOD_FLOW_CHANGE_RQST].clear();
        }

        handleBloodPrimeVolML(resume, rampStepML, hasFlowChngeRequested, currBloodFlowMLPM, accumBloodVolML);
        handleTreatmentSetpointsBroadcastData(currBloodFlowMLPM, _treatmentParams.dialysateFlowRateMLPM,
                                              _treatmentParams.dialysateTemperatureC);

        if (accumBloodVolML > BLOOD_PRIME_VOLUME_ML) {
            _treatmentStatus.stateTransitionEvent = "T_BP_2_Tx";
            status                                = STATE_ON_EXIT;
            // Send the final volume value prior to trnasiotining to treatment
            handleBloodPrimeBroadcastData(accumBloodVolML, 0, 0, BC_IMMEDIATELY);
            qDebug() << "VOL" << accumBloodVolML;
        }

        handleBackToStandbyEvent(status);

        //qDebug() << "Blood state" << status;
    };

    auto onExit = [=](){
        //qDebug() << "Blood prime on exit";
        _dryDemo.submitEvent(_treatmentStatus.stateTransitionEvent);
        status = STATE_ON_ENTRY;
    };

    switch (status) {  // TODO macro it later
    case STATE_ON_ENTRY :   if (active) onEntry ();  break;
    case STATE_ON_ACTION:   if (active) onAction();  break;
    case STATE_ON_EXIT  :   if (active) onExit  ();  break;
    }
}

void StateController::onTreatmentStateChange(bool active)
{
    static State_Status status = STATE_ON_ENTRY;


    auto onEntry = [=](){
        handleTDOpModeTransitionBroadcastData(MODE_TREA, 0, BC_IMMEDIATELY);
        QList<quint32> txStates = {TREAT_START_STATE, 0, 0, 0, 0, 0, 0, 0, 0, 0};
        handleTreatmentStatesBroadcastData(txStates, BC_IMMEDIATELY);
        txStates = {TREAT_DIALYSIS_STATE, 0, 0, 0, 0, 0, 0, 0, 0, 0};
        handleTreatmentStatesBroadcastData(txStates, BC_IMMEDIATELY);

        status = STATE_ON_ACTION;

        qDebug() << "Tx on entry";
    };

    auto onAction = [=](){
        status = STATE_ON_ACTION;
        handleTDOpModeTransitionBroadcastData(MODE_TREA, 0, BC_DELAYED);
        QList<quint32> txStates = {TREAT_DIALYSIS_STATE, 0, 0, 0, 0, 0, 0, 0, 0, 0};
        handleTreatmentStatesBroadcastData(txStates, BC_DELAYED);
        handleBackToStandbyEvent(status);
    };

    auto onExit = [=](){
        qDebug() << "Tx  in exit";
        _dryDemo.submitEvent(_treatmentStatus.stateTransitionEvent);
        status = STATE_ON_ENTRY;
    };

    switch (status) {  // TODO macro it later
    case STATE_ON_ENTRY :   if (active) onEntry ();  break;
    case STATE_ON_ACTION:   if (active) onAction();  break;
    case STATE_ON_EXIT  :   if (active) onExit  ();  break;
    }
}

// ************* Hepler functions ************* //

void StateController::handleTDOpModeTransitionBroadcastData(const TD_OP_MODE mode, const quint32 subMode, const Broadcast_Type BCType)
{
    static quint32 broadcastCounter = 0;

    if ((++broadcastCounter % NUM_OF_COUNTS_TIMER_BC_EMIT != 0) && (BCType == BC_DELAYED)) { return; }

    QVariantList msg;
    msg.append(static_cast<quint16>(ID_TD_OP_MODE_DATA));
    msg.append(Can_Id::eChlid_TD_Sync);
    msg.append(mode);
    msg.append(subMode);
    _isBroadcastListReady = false;
    _broadcastMessages.append(msg);
    _isBroadcastListReady = true;
}

void StateController::setTreatmentParametersFromUI(const QVariant &payload)
{
    QVariantList resp;
    resp.append(static_cast<quint16>(ID_TD_RESP_TREATMENT_PARAMS_TO_VALIDATE));
    resp.append(Can_Id::eChlid_TD_UI);
    resp.append(ACCEPT_VALUE);

    qint32 index = 0;
    Types::U32 u32Var;
    Types::F32 f32Var;

    GetValue(payload.toByteArray(), index, u32Var);
    _treatmentParams.finalConfirmation            = u32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, u32Var);
    _treatmentParams.treatmentModality         = u32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, u32Var);
    _treatmentParams.hdfDilution                = u32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, u32Var);
    _treatmentParams.bloodFlowRateMLPM            = u32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, u32Var);
    _treatmentParams.dialysateFlowRateMLPM        = u32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, u32Var);
    _treatmentParams.treatmentDurationMin         = u32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, u32Var);
    _treatmentParams.heparinDeliveryDurationMin   = u32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, u32Var);
    _treatmentParams.heparinType                  = u32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, u32Var);
    _treatmentParams.dryBicarbCartSize           = u32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, u32Var);
    _treatmentParams.sodiumMEQL    = u32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, u32Var);
    _treatmentParams.bicarbobateMEQL         = u32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, u32Var);
    _treatmentParams.dialyzerType  = u32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, u32Var);
    _treatmentParams.fluidBlousVolumeML     = u32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, u32Var);
    _treatmentParams.bloodPressureIntervalMin = u32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, u32Var);
    _treatmentParams.rinsebackVolumeML       = u32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, u32Var);
    _treatmentParams.hepatitisBStatus             = u32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, u32Var);
    _treatmentParams.acidConcentrate         = u32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, f32Var);
    _treatmentParams.substitutionFluidVolumeL             = f32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, f32Var);
    _treatmentParams.heparinBolusVolumeML             = f32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, f32Var);
    _treatmentParams.heparinDeliveryRateMLHR             = f32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, f32Var);
    _treatmentParams.dialysateTemperatureC             = f32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, f32Var);
    _treatmentParams.acidKConcentrateConversionFactor             = f32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, f32Var);
    _treatmentParams.ufPreWeightKG             = f32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, f32Var);
    _treatmentParams.ufEstimatedTargetWeightKG             = f32Var.value;
    resp.append(0);

    GetValue(payload.toByteArray(), index, f32Var);
    _treatmentParams.ufVolumeL             = f32Var.value;
    resp.append(0);

    _treatmentStatus.hasTxParamsBeenInitialized = true;

    _isSendListReady = false;
    _sendMessages.append(resp);
    _isSendListReady = true;
}

void StateController::handleUIResponseMessage(const Can::Message_ID_Enum msgID, const quint32 acceptReject)
{
    QVariantList resp;
    resp.append(static_cast<quint16>(msgID));
    resp.append(Can_Id::eChlid_TD_UI);
    resp.append(acceptReject);
    resp.append(0);
    _isSendListReady = false;
    _sendMessages.append(resp);
    _isSendListReady = true;
}

void StateController::resetDryDemoVariables()
{
    _treatmentStatus.hasTxParamsBeenInitialized = false;
    _treatmentStatus.treatmentElapsedTimeS      = DEF_TX_ELAPSED_TIME_S;
    _treatmentStatus.remainingTreatmentTimeS    = DEF_TX_PARAM_PRESCRIBED_DUR_S - _treatmentStatus.treatmentElapsedTimeS;
    _treatmentStatus.stateTransitionEvent       = "";
}

quint32 StateController::extractU32DataFromReceivedMessage(const Message_ID_Enum msgID, qint32 index)
{
    Types::U32 u32Var;
    GetValue(_treatmentRcvdMessages[msgID].toByteArray(), index, u32Var);

    return u32Var.value;
}