#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        );
    //_dryDemo.connectToState("Disposables"      , this, &StateController::onDisposablesStateChange        );
    //_dryDemo.connectToState("System_Prime"     , this, &StateController::onSystemPrimeStateChange        );
    //_dryDemo.connectToState("BP_HR"            , this, &StateController::onBPHRStateChange               );
    //_dryDemo.connectToState("Ultrafiltraion"   , this, &StateController::onUltrafiltrationStateChange    );
    //_dryDemo.connectToState("Connection"       , this, &StateController::onConnectionStateChange         );

    //_dryDemo.connectToState("Start_Tx"         , this, &StateController::onStartTreatmentStateChange     );
    //_dryDemo.connectToState("Blood_Prime"      , this, &StateController::onTreatmentBloodPrimeStateChange);
    //_dryDemo.connectToState("Treatment"        , this, &StateController::onTreatmentTreatmentStateChange );

    //_dryDemo.connectToState("End_Tx"           , this, &StateController::onEndTreatmentStateChange       );
    //_dryDemo.connectToState("Post_Tx"          , this, &StateController::onPostTreatmentStateChange      );
    //_dryDemo.connectToState("Disinfect"        , this, &StateController::onDisinfectStateChange          );

    _transitionEventsFromStandby.clear();
    _transitionEventsFromStandby[CMD_PRE_TX      ] = "T_SB_2_Pretx";
    _transitionEventsFromStandby[CMD_BLOOD_PRIME   ] = "T_SB_2_BP";
    _transitionEventsFromStandby[CMD_MAIN_TX   ] = "T_SB_2_Tx";
    //_transitionEventsFromStandby[CMD_CONSUMABLES    ] = "Transition_2_Consumables";
    //_transitionEventsFromStandby[CMD_DISPOSABLES    ] = "Transition_2_Disposables";
    //_transitionEventsFromStandby[CMD_SYSTEM_PRIME   ] = "Transition_2_Sys_Prime";
    //_transitionEventsFromStandby[CMD_BP_HR          ] = "Transition_2_BP_HR";
    //_transitionEventsFromStandby[CMD_ULTRAFILTRATION] = "Transition_2_Ultrafiltraion";
    //_transitionEventsFromStandby[CMD_CONNECTION     ] = "Transition_2_Patient_Connection";
    //_transitionEventsFromStandby[CMD_START_TX       ] = "Transition_2_Start_Tx";
    //_transitionEventsFromStandby[CMD_END_TX         ] = "Transition_2_End_Tx";
    //_transitionEventsFromStandby[CMD_DISINFECTION   ] = "Transition_2_Disinfect";

}

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

void StateController::doStateControllerUnhandledMsgReceived(const QVariantList &msg)
{
    MessageID receivedMsgID = static_cast<Can::MessageID>(msg[0].toUInt());

    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:
        //case ID_UI_TX_PARAMS_RQST:
        //case ID_UI_CONFIRM_RESP:
        //case ID_UI_RINSEBACK_CMD_RQST:
        //case ID_SAMPLE_WATER_RESULT:
        //case ID_UI_SET_UF_VOLUME_RQST:
        //case ID_USER_TX_TIME_CHANGES_RQST:
        //case ID_UI_CONFIRM_TX_PARAMS:
        //case ID_UI_SALINE_BOLUS_RQST:
        //case ID_UI_PRESSURE_LIMITS_CHANGE_RQST:
        //case ID_UI_ALARM_USER_ACTION_RQST:
        //case ID_USER_BLOOD_DIAL_RATE_CHANGE_RQST:
        //case ID_UI_HEPARIN_PAUSE_RESUME_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:
        //case ID_UI_TREATMENT_LOG_RQST:
        //case ID_UI_POST_TREATMNET_NEXT_RQST:
        //case ID_UI_DISPOSABLE_REMOVAL_CONFIRM_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();
        _dryDemo.submitEvent(_back2StandbyEvent);
    }
}

// ---------------------- 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()
{
    qint32 IDIndex = 0;
    Types::U32 param;
    GetValue(_treatmentRcvdMessages[ID_UI_CONFIRMATION_RESULT_RESP].toByteArray(), IDIndex, param);
    if (param.value != UI_MSG_BB_PAYLOAD_ID) { return; }

    IDIndex = UI_MSG_BB_STATUS_INDEX;
    GetValue(_treatmentRcvdMessages[ID_UI_CONFIRMATION_RESULT_RESP].toByteArray(), IDIndex, param);
    _treatmentStatus.userCmd = static_cast<User_Command_ID>(param.value);
    _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_EXIT;
    });


}



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

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

        status = STATE_ON_ACTION;

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

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

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

            QVariantList resp;
            resp.append(static_cast<quint16>(ID_TD_RESP_INITIATE_TREATMENT));
            resp.append(Can_Id::eChlid_TD_UI);
            resp.append(1);
            resp.append(0);
            _isSendListReady = false;
            _sendMessages.append(resp);
            _isSendListReady = true;
            _dryDemo.submitEvent(_transitionEventsFromStandby[CMD_PRE_TX]);
            status = STATE_ON_EXIT;
        }

        if (_treatmentStatus.userCmd != CMD_NONE){
            if ((_treatmentStatus.userCmd > CMD_STAND_BY) && (_treatmentStatus.userCmd <= CMD_DISINFECTION)) {
                qDebug() << "Submit event" << _transitionEventsFromStandby[_treatmentStatus.userCmd];
                _dryDemo.submitEvent(_transitionEventsFromStandby[_treatmentStatus.userCmd]);
                _treatmentStatus.userCmd = CMD_NONE;
                status = STATE_ON_EXIT;
            }
        }
    };

    auto inExit = [=](){
        qDebug() << "Standby in exit";
        status                     = STATE_ON_ENTRY;
    };

    switch (status) {  // TODO macro it later
    case STATE_ON_ENTRY :   if (active) inEntry ();  break;
    case STATE_ON_ACTION:   if (active) inAction();  break;
    case STATE_ON_EXIT  :   if (active) inExit  ();  break;
    }
}

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

    auto inEntry = [=](){
        handleTDOpModeTransitionBroadcastData(MODE_PRET, 0);

        status = STATE_ON_ACTION;

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

    auto inAction = [=](){
        if (! _treatmentRcvdMessages[ID_UI_ADJUST_DISPOSABLES_CONFIRM_RQST].isNull()){
            _treatmentRcvdMessages[ID_UI_ADJUST_DISPOSABLES_CONFIRM_RQST].clear();

            QVariantList resp;
            resp.append(static_cast<quint16>(ID_TD_ADJUST_DISPOSSABLES_CONFIRM_RESP));
            resp.append(Can_Id::eChlid_TD_UI);
            resp.append(1);
            resp.append(0);
            _isSendListReady = false;
            _sendMessages.append(resp);
            _isSendListReady = true;
        }

        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();
        }

        status = STATE_ON_ACTION;

        if (! _treatmentRcvdMessages[ID_UI_ADJUST_START_TREATMENT_RQST].isNull()){
            _treatmentRcvdMessages[ID_UI_ADJUST_START_TREATMENT_RQST].clear();
            sendStartTxResponse();
            status = STATE_ON_EXIT;
            _dryDemo.submitEvent("T_Pretx_2_BP");
        }
    };

    auto inExit = [=](){
        qDebug() << "PreTx in exit";
        status = STATE_ON_ENTRY;
    };

    switch (status) {  // TODO macro it later
    case STATE_ON_ENTRY :   if (active) inEntry ();  break;
    case STATE_ON_ACTION:   if (active) inAction();  break;
    case STATE_ON_EXIT  :   if (active) inExit  ();  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;

    auto inEntry = [=](){
        handleTDOpModeTransitionBroadcastData(MODE_TREA, 0);
        handleBloodPrimeBroadcastData(0.0, 0, 0, true);
        accumBloodVolML   = 0.0;
        currBloodFlowMLPM = BLOOD_PRIME_START_FLOW_MLPM;
        resume            = true;

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

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

    auto inAction = [=](){
        if (! _treatmentRcvdMessages[ID_UI_BLOOD_PRIME_CMD_RQST].isNull()){
            QVariantList resp;
            qint32 index = 0;
            Types::U32 u32Var;
            resp.append(static_cast<quint16>(ID_TD_BLOOD_PRIME_CMD_RESP));
            resp.append(Can_Id::eChlid_TD_UI);
            resp.append(ACCEPT_VALUE);
            resp.append(0);
            _isSendListReady = false;
            _sendMessages.append(resp);
            _isSendListReady = true;
            GetValue(_treatmentRcvdMessages[ID_UI_BLOOD_PRIME_CMD_RQST].toByteArray(), index, u32Var);
            resume = (u32Var.value == 1 ? true : false);
            quint32 state = (u32Var.value == 1 ? 1 : 2);

            QVariantList msg;
            msg.append(static_cast<quint16>(ID_TD_TREATMENT_STATE_DATA));
            msg.append(Can_Id::eChlid_TD_Sync);
            msg.append(1); msg.append(state);  msg.append(0);  msg.append(0); msg.append(0); msg.append(0); msg.append(0); msg.append(0);
            msg.append(0); msg.append(0);
            _isBroadcastListReady = false;
            _broadcastMessages.append(msg);
            _isBroadcastListReady = true;

            _treatmentRcvdMessages[ID_UI_BLOOD_PRIME_CMD_RQST].clear();
        }

        if (! _treatmentRcvdMessages[ID_UI_TREATMENT_SET_POINT_BLOOD_FLOW_CHANGE_RQST].isNull()){
            qint32 index = 0;
            Types::U32 u32Var;
            GetValue(_treatmentRcvdMessages[ID_UI_TREATMENT_SET_POINT_BLOOD_FLOW_CHANGE_RQST].toByteArray(), index, u32Var);
            quint32 var = u32Var.value;
            handleBloodPrimeFlowChangeRequest(var, currBloodFlowMLPM);
            //qDebug() << "FLow" << currBloodFlowMLPM;

            _treatmentRcvdMessages[ID_UI_TREATMENT_SET_POINT_BLOOD_FLOW_CHANGE_RQST].clear();
        }

        //_dryDemo.connectToEvent("T_Back_2_SB", [](const QScxmlEvent &event) {
        //    qDebug() << "Event invoked:" << event.name();
        //    status = STATE_ON_EXIT;
        //});
        handleBackToStandbyEvent(status);

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

        status = STATE_ON_ACTION;


        if (accumBloodVolML > BLOOD_PRIME_VOLUME_ML) {
            status = STATE_ON_EXIT;
            qDebug() << "VOL" << accumBloodVolML;
            _dryDemo.submitEvent("T_BP_2_Tx");
        }
    };

    auto inExit = [=](){
        //qDebug() << "Blood prime on exit";
        status = STATE_ON_ENTRY;
    };

    switch (status) {  // TODO macro it later
    case STATE_ON_ENTRY :   if (active) inEntry ();  break;
    case STATE_ON_ACTION:   if (active) inAction();  break;
    case STATE_ON_EXIT  :   if (active) inExit  ();  break;
    }
}

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

    auto inEntry = [=](){
        handleTDOpModeTransitionBroadcastData(MODE_TREA, 0);
        QVariantList msg;
        msg.append(static_cast<quint16>(ID_TD_TREATMENT_STATE_DATA));
        msg.append(Can_Id::eChlid_TD_Sync);
        msg.append(2); msg.append(0);  msg.append(0);  msg.append(0); msg.append(0); msg.append(0); msg.append(0); msg.append(0);
        msg.append(0); msg.append(0);
        _isBroadcastListReady = false;
        _broadcastMessages.append(msg);
        _isBroadcastListReady = true;


        status = STATE_ON_ACTION;

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

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


        handleBackToStandbyEvent(status);

    };

    auto inExit = [=](){
        qDebug() << "Tx  in exit";
        status                     = STATE_ON_ENTRY;
    };

    switch (status) {  // TODO macro it later
    case STATE_ON_ENTRY :   if (active) inEntry ();  break;
    case STATE_ON_ACTION:   if (active) inAction();  break;
    case STATE_ON_EXIT  :   if (active) inExit  ();  break;
    }
}



void StateController::handleTDOpModeTransitionBroadcastData(Can::TD_OP_MODE mode, quint32 subMode)
{
    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::sendStartTxResponse()
{
    QVariantList resp;
    resp.append(static_cast<quint16>(ID_TD_ADJUST_START_TREATMENT_RESP));
    resp.append(Can_Id::eChlid_TD_UI);
    resp.append(ACCEPT_VALUE);
    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;
}