/**
 * @file JARTStateMachineGAM.cpp
 * @brief Source file for class JARTStateMachineGAM
 * @date Nov 26, 2018
 * @author aneto
 *
 * @copyright Copyright 2015 F4E | European Joint Undertaking for ITER and
 * the Development of Fusion Energy ('Fusion for Energy').
 * Licensed under the EUPL, Version 1.1 or - as soon they will be approved
 * by the European Commission - subsequent versions of the EUPL (the "Licence")
 * You may not use this work except in compliance with the Licence.
 * You may obtain a copy of the Licence at: http://ec.europa.eu/idabc/eupl
 *
 * @warning Unless required by applicable law or agreed to in writing,
 * software distributed under the Licence is distributed on an "AS IS"
 * basis, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express
 * or implied. See the Licence permissions and limitations under the Licence.

 * @details This source file contains the definition of all the methods for
 * the class JARTStateMachineGAM (public, protected, and private). Be aware that some
 * methods, such as those inline could be defined on the header file, instead.
 */

/*---------------------------------------------------------------------------*/
/*                         Standard header includes                          */
/*---------------------------------------------------------------------------*/

/*---------------------------------------------------------------------------*/
/*                         Project header includes                           */
/*---------------------------------------------------------------------------*/

#include "JARTStateMachineGAM.h"

#include "AdvancedErrorManagement.h"

/*---------------------------------------------------------------------------*/
/*                           Static definitions                              */
/*---------------------------------------------------------------------------*/
static MARTe::uint64 getCurrentTimeUs() {
    using namespace MARTe;
    return static_cast<uint64>(HighResolutionTimer::Counter() * HighResolutionTimer::Period() * 1e6f + 0.5f);
}
/*---------------------------------------------------------------------------*/
/*                           Method definitions                              */
/*---------------------------------------------------------------------------*/

JARTStateMachineGAM::JARTStateMachineGAM() {
    currentState = WaitTrigger; // Set Entry state.
    plcOnTime = 0;              // Triggered time holder.

    //Output and condition in a given state.
    conditionTrigger = 1;
    aps_hvon = 0;
    aps_swon = 0;
    bps_hvon = 0;
    bps_swon = 0;
    mhvps_hvon = 0;

    // Parameters which get from Input signals.
    triggerSignal = NULL_PTR(MARTe::uint32 *);
    currentTime = NULL_PTR(MARTe::uint32 *);
    turn_off_delay = 2000; //us

    triggerDelay_mhvps_hvon = NULL_PTR(MARTe::uint32 *);
    triggerDelay_aps_hvon = NULL_PTR(MARTe::uint32 *);
    triggerDelay_aps_swon = NULL_PTR(MARTe::uint32 *);
    triggerDelay_bps_hvon = NULL_PTR(MARTe::uint32 *);
    triggerDelay_bps_swon = NULL_PTR(MARTe::uint32 *);
    triggerDelay_shotlen = NULL_PTR(MARTe::uint32 *);

    stopRequest = NULL_PTR(MARTe::uint32 *);
    modePulseLengthLimit = NULL_PTR(MARTe::uint32 *);
    short_pulse_mode = NULL_PTR(MARTe::uint32 *);

    modulation = NULL_PTR(MARTe::uint32 *);

    // write out target.
    outputSignal = NULL_PTR(MARTe::uint32 *);
    outputBeamON = NULL_PTR(MARTe::uint32 *);
    outputHVArmed = NULL_PTR(MARTe::uint32 *);
    outputHVInjection = NULL_PTR(MARTe::uint32 *);
    outputRFON = NULL_PTR(MARTe::uint32 *);
    outputBeamONTime = NULL_PTR(MARTe::uint32 *);
    outputRFONTime = NULL_PTR(MARTe::uint32 *);
    shotCounter = NULL_PTR(MARTe::uint32 *);

    mhvps_hvon_is_on = false;
    aps_hvon_is_on = false;
    aps_swon_is_on = false;
    bps_hvon_is_on = false;
    bps_swon_is_on = false;

    apsSwonHighResolutionTime = 0;

    aps_hvon_state=0;
    aps_swon_state=0;
    mhvps_hvon_state=0;
    bps_hvon_state=0;
    bps_swon_state=0;
}

JARTStateMachineGAM::~JARTStateMachineGAM() {
}

bool JARTStateMachineGAM::Initialise(MARTe::StructuredDataI & data) {
    using namespace MARTe;
    bool ok = GAM::Initialise(data);
    if (ok) {
        ok = data.Read("ConditionTrigger", conditionTrigger);
        if (!ok) {
            REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "The Condition1 shall be specified");
        }
    }
    if (ok) {
        ok = data.Read("mhvps_hvon", mhvps_hvon);
        if (!ok) {
            REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "The mhvps_hvon shall be specified");
        }
    }
    if (ok) {
        ok = data.Read("aps_hvon", aps_hvon);
        if (!ok) {
            REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "The aps_hvon shall be specified");
        }
    }
    if (ok) {
        ok = data.Read("aps_swon", aps_swon);
        if (!ok) {
            REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "The aps_swon shall be specified");
        }
    }
    if (ok) {
        ok = data.Read("bps_hvon", bps_hvon);
        if (!ok) {
            REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "The bps_hvon shall be specified");
        }
    }
    if (ok) {
        ok = data.Read("bps_swon", bps_swon);
        if (!ok) {
            REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "The bps_swon shall be specified");
        }
    }
    return ok;
}

bool JARTStateMachineGAM::PrepareNextState(const MARTe::char8 * const currentStateName, const MARTe::char8 * const nextStateName) {

    return true;
}

bool JARTStateMachineGAM::Setup() {
    using namespace MARTe;
    bool ok = (numberOfInputSignals == 12u);
    if (ok) {
        ok = (numberOfOutputSignals == 16u);
        if (!ok) {
            REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Seven output signals shall be defined %d",numberOfOutputSignals);
        }
    }
    else {
        REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Nine input signals shall be defined");
    }
    if (ok) {
        uint32 c;
        for (c = 0u; c < numberOfInputSignals; c++) {
            TypeDescriptor inputType = GetSignalType(InputSignals, c);
            ok = (inputType == UnsignedInteger32Bit);
            if (!ok) {
                StreamString signalName;
                (void) GetSignalName(InputSignals, c, signalName);
                REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Signal %s shall be defined as uint32", signalName.Buffer());
            }

        }
    }
    if (ok) {
        uint32 c;
        for (c = 0u; c < numberOfOutputSignals; c++) {
            TypeDescriptor outputType = GetSignalType(OutputSignals, c);
            ok = (outputType == UnsignedInteger32Bit || outputType == UnsignedInteger8Bit);
            if (!ok) {
                StreamString signalName;
                (void) GetSignalName(InputSignals, c, signalName);
                REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Signal %s shall be defined as uint32", signalName.Buffer());
            }
        }
    }
    if (ok) {
        currentTime = reinterpret_cast<uint32 *>(GetInputSignalMemory(0));
        triggerSignal = reinterpret_cast<uint32 *>(GetInputSignalMemory(1));
        triggerDelay_mhvps_hvon = reinterpret_cast<uint32 *>(GetInputSignalMemory(2));
        triggerDelay_aps_hvon = reinterpret_cast<uint32 *>(GetInputSignalMemory(3));
        triggerDelay_aps_swon = reinterpret_cast<uint32 *>(GetInputSignalMemory(4));
        triggerDelay_bps_hvon = reinterpret_cast<uint32 *>(GetInputSignalMemory(5));
        triggerDelay_bps_swon = reinterpret_cast<uint32 *>(GetInputSignalMemory(6));
        triggerDelay_shotlen = reinterpret_cast<uint32 *>(GetInputSignalMemory(7));
        stopRequest = reinterpret_cast<uint32 *>(GetInputSignalMemory(8));
        modePulseLengthLimit = reinterpret_cast<uint32 *>(GetInputSignalMemory(9));
        short_pulse_mode = reinterpret_cast<uint32 *>(GetInputSignalMemory(10));
        modulation = reinterpret_cast<uint32 *>(GetInputSignalMemory(11));


        outputSignal = reinterpret_cast<uint32 *>(GetOutputSignalMemory(0));
        outputBeamON = reinterpret_cast<uint32 *>(GetOutputSignalMemory(1));
        outputHVArmed = reinterpret_cast<uint32 *>(GetOutputSignalMemory(2));
        outputHVInjection = reinterpret_cast<uint32 *>(GetOutputSignalMemory(3));
        outputRFON = reinterpret_cast<uint32 *>(GetOutputSignalMemory(4));
        outputBeamONTime = reinterpret_cast<uint32 *>(GetOutputSignalMemory(5));
        outputRFONTime  = reinterpret_cast<uint32 *>(GetOutputSignalMemory(6));
        shotCounter = reinterpret_cast<uint32 *>(GetOutputSignalMemory(7));

        outputAPSHVON = reinterpret_cast<uint32 *>(GetOutputSignalMemory(8));
        outputAPSSWON = reinterpret_cast<uint32 *>(GetOutputSignalMemory(9));
        outputBPSHVON = reinterpret_cast<uint32 *>(GetOutputSignalMemory(10));
        outputBPSSWON = reinterpret_cast<uint32 *>(GetOutputSignalMemory(11));
        outputMHVPSON = reinterpret_cast<uint32 *>(GetOutputSignalMemory(12));

        outputSignalNI6259 = reinterpret_cast<uint32 *>(GetOutputSignalMemory(13));
        outputSignalNI6528P3 = reinterpret_cast<uint8 *>(GetOutputSignalMemory(14));
        outputSignalNI6528P4 = reinterpret_cast<uint8 *>(GetOutputSignalMemory(15));

        *shotCounter = 0;
    }
    return ok;
}

bool JARTStateMachineGAM::Execute() {
    using namespace MARTe;

    if (currentState == WaitTrigger) {

        //State Transition condition
        if ((*triggerSignal == conditionTrigger)) {
            //REPORT_ERROR(ErrorManagement::Debug, "Start beam-on sequence at %d.", *currentTime);
            plcOnTime = *currentTime; //Save pulse start time.
            //*outputBeamON = 0;
            //State transition.
            currentState = SwitchingHVPS;
        }
    }
    else if (currentState == SwitchingHVPS) {

        //Actions in this state.
    	if (*stopRequest != 0 || *triggerSignal != conditionTrigger) {
    		*outputSignal = 0;
    		currentState = HVTerminate;
    	}
        if (*currentTime >= (plcOnTime + *triggerDelay_bps_hvon) && bps_hvon_is_on == false){
            //Do action
            *outputSignal += bps_hvon;
            bps_hvon_is_on = true; bps_hvon_state=1;
            //REPORT_ERROR(ErrorManagement::Debug, "bps_hvon was set to outputSignal at %d.", *currentTime);
            *outputBPSHVON=1;
        }
        if (*currentTime >= (plcOnTime + *triggerDelay_aps_hvon) && aps_hvon_is_on == false) {
            //Do action
            *outputSignal += aps_hvon;
            aps_hvon_is_on = true; aps_hvon_state=1;
            //REPORT_ERROR(ErrorManagement::Debug, "aps_hvon was set to outputSignal at %d.", *currentTime);
            *outputAPSHVON=1;
        }
        if (*currentTime >= (plcOnTime + *triggerDelay_bps_swon) && bps_swon_is_on==false){
            //Do action
            *outputSignal += bps_swon;
            bps_swon_is_on = true; bps_swon_state=1;
            //REPORT_ERROR(ErrorManagement::Debug, "bps_swon was set to outputSignal at %d.", *currentTime);
            *outputBPSSWON=1;
        }
        if (*currentTime >= (plcOnTime + *triggerDelay_mhvps_hvon) && mhvps_hvon_is_on==false) {
            //Do action
            *outputSignal += mhvps_hvon;
            mhvps_hvon_is_on = true; mhvps_hvon_state=1;
            //REPORT_ERROR(ErrorManagement::Debug, "mhvps_hvon was set to outputSignal at %d.", *currentTime);
            *outputMHVPSON=1;
        }
        if (bps_swon_is_on && mhvps_hvon_is_on && *currentTime >= (plcOnTime + *triggerDelay_aps_swon)){
            //Do action
            *outputSignal += aps_swon;
            aps_swon_is_on = true; aps_swon_state=1;
            apsSwonHighResolutionTime = getCurrentTimeUs();
            apsSwonTime = *currentTime;
            //REPORT_ERROR(ErrorManagement::Debug, "aps_swon was set to outputSignal at %d.", *currentTime);
            *outputAPSSWON=1;
        }
        *outputBeamONTime = *currentTime - plcOnTime; //Save RFON start time.
        if (bps_hvon_is_on && aps_hvon_is_on){
            *outputHVArmed = 1;
        }
        if (bps_swon_is_on || mhvps_hvon_is_on){
            *outputHVInjection = 1;
        }

        //State transition condition
        if (aps_swon_is_on){
            currentState = RFON;
            *outputRFON = 0;
            *outputBeamON = 1;
            *shotCounter += 1;
            //REPORT_ERROR(ErrorManagement::Debug, "state was changed to RFON");
        }
    }
    else if (currentState == RFON) {

        //Action in this state.
        if (*stopRequest != 0 || *triggerSignal != conditionTrigger) {
            //debug
            //if((*stopRequest != 0)){
                //REPORT_ERROR(ErrorManagement::Debug, "Stop request was called.!!!");
            //} else {
                //REPORT_ERROR(ErrorManagement::Debug, "PLC_ON was reset.!!!");
            //}
            *outputSignal -= aps_swon;
            mhvps_hvon_is_on = false; mhvps_hvon_state=0;
            aps_hvon_is_on = false; aps_hvon_state=0;
            aps_swon_is_on = false; aps_swon_state=0;
            bps_hvon_is_on = false; bps_hvon_state=0;
            bps_swon_is_on = false; bps_swon_state=0;
            currentState = HVTerminate;
            *outputAPSHVON=0;
            *outputAPSSWON=0;
            *outputBPSHVON=0;
            *outputBPSSWON=0;
            *outputMHVPSON=0;
        }
        uint32 updatePeriod = 100; // in microsecnds (get this from Timer)
        if ((*modePulseLengthLimit == 1u) || (getCurrentTimeUs() + updatePeriod >= (apsSwonHighResolutionTime + *triggerDelay_shotlen))) {
            
            // Now we do busy wait
            while (getCurrentTimeUs() < (apsSwonHighResolutionTime + *triggerDelay_shotlen)) {
                //REPORT_ERROR(ErrorManagement::Debug, "!");
            }
            // We stop busy waiting #executionOverhead before expected pulse off time 
            //debug
            //if((*modePulseLengthLimit == 1u)){
            //    REPORT_ERROR(ErrorManagement::Debug, "Mode limit detected.!!!");
            //} else {
            //    REPORT_ERROR(ErrorManagement::Debug, "Shot length reached to the setpoint.!!!");
            //}
            //debug end.
            //Do action
            *outputSignal -= aps_swon; //Turn off only APS_SWON first.
            mhvps_hvon_is_on = false; 
            aps_hvon_is_on = false; 
            aps_swon_is_on = false; aps_swon_state=0;
            bps_hvon_is_on = false; 
            bps_swon_is_on = false; 
            *outputAPSHVON=0;
            *outputAPSSWON=0;
            *outputBPSHVON=0;
            *outputBPSSWON=0;
            *outputMHVPSON=0;
            //REPORT_ERROR(ErrorManagement::Debug, "0 was set to outputSignal at %d.", *currentTime);
        }
        *outputRFON = 1;
        *outputBeamONTime = *currentTime - plcOnTime;
        *outputRFONTime = *currentTime - apsSwonTime;


        //State transition condition
        if (!aps_swon_is_on && !bps_swon_is_on && !mhvps_hvon_is_on) {
            currentState = HVTerminate;
            apsSwoffTime = *currentTime;
            //REPORT_ERROR(ErrorManagement::Debug, "state was changed to HVTerminate");
        }
    }
    else if (currentState == HVTerminate) {
        //In the HVTerminate state, turn APS_SWON off first, and wait 1ms. Finally turn other PS off.
        //Action in this state.
        *outputBeamON = 0;
        *outputHVArmed = 0;
        *outputHVInjection = 0;
        *outputRFON = 0;

        // State transition condition.
        if (*currentTime - apsSwoffTime >= turn_off_delay){
            *outputSignal = 0;
            mhvps_hvon_state=0;
            aps_hvon_state=0;
            bps_hvon_state=0;
            bps_swon_state=0;
        }
        if (*triggerSignal == false){ 
            //Check PLC_ON is reset
            currentState = WaitTrigger;
            *outputSignal = 0;
            //REPORT_ERROR(ErrorManagement::Debug, "PLC_ON was reset. The State was changed to WaitTrigger at %d.", *currentTime);
        }
    }
    if(*short_pulse_mode == 1){
        p3Value = 1*aps_hvon_state + 8*bps_hvon_state +16*bps_swon_state + 64*(*outputBeamON);
        *outputSignalNI6259 = 1*aps_swon_state;
        *outputSignalNI6528P3 = ~p3Value;
        //REPORT_ERROR(ErrorManagement::Debug, "short pulse mode with p3: %d.", p3Value);
    } else {
        p3Value = 1*aps_hvon_state +2*aps_swon_state + 8*bps_hvon_state +16*bps_swon_state + 64*(*outputBeamON);
        *outputSignalNI6528P3 = ~p3Value;
        //REPORT_ERROR(ErrorManagement::Debug, "long pulse mode with p3: %d.", p3Value);
    }
    if (modulation) {
        p4Value = 8*mhvps_hvon_state + 32;
    }
    else {
        p4Value = 8*mhvps_hvon_state;
    }
    //*outputSignalNI6528P4 = ~(*ni6528p4Value | p4Value);
    *outputSignalNI6528P4 = ~p4Value;
    return true;
}

CLASS_REGISTER(JARTStateMachineGAM, "1.0")