/**
 * @file JARampupGAM.cpp
 * @brief Source file for class JARampupGAM
 * @date Jan, 2019
 * @author rhari
 *
 * @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 JARampupGAM (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 "AdvancedErrorManagement.h"
#include "JARampupGAM.h"

/*---------------------------------------------------------------------------*/
/*                           Static definitions                              */
/*---------------------------------------------------------------------------*/

/*---------------------------------------------------------------------------*/
/*                           Method definitions                              */
/*---------------------------------------------------------------------------*/

JARampupGAM::JARampupGAM() : GAM() {
    current_setpoint = NULL_PTR(MARTe::float32 *);
    target_value = NULL_PTR(MARTe::float32 *);
    rampup_time = NULL_PTR(MARTe::float32 *);
    start = NULL_PTR(MARTe::uint32 *);
    standby = NULL_PTR(MARTe::uint32 *);
    isAuto = NULL_PTR(MARTe::uint32 *);
    FHPS_PrePro = NULL_PTR(MARTe::float32 *);

    output = NULL_PTR(MARTe::float32 *);
    state = NULL_PTR(MARTe::uint32 *);

    rampup_rate = 0.0f;
    inRampup = false;
    resetFlag = true;
    inWaitHVON = false;
    inWaitStandby = false;
}

JARampupGAM::~JARampupGAM() {
}

bool JARampupGAM::Initialise(MARTe::StructuredDataI & data) {
    using namespace MARTe;
    return GAM::Initialise(data);
}

bool JARampupGAM::Setup() {
    using namespace MARTe;
    bool ok = (numberOfInputSignals == 7u);
    if (ok) {
        ok = (numberOfOutputSignals == 2u);
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "Two output signals shall be defined.");
        }
    }
    else {
        REPORT_ERROR(ErrorManagement::ParametersError, "Six input signals shall be defined.");
    }
    uint32 currentspvIndex;
    uint32 targetvIndex;
    uint32 timeIndex;
    uint32 startIndex;
    uint32 standbyIndex;
    uint32 isAutoIndex;
    uint32 fhpsPreProIndex;

    if (ok) {
        StreamString signalName = "Currspv";
        ok = GetSignalIndex(InputSignals, currentspvIndex, signalName.Buffer());
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "Currspv input signal shall be defined.");
        }
        else {
            TypeDescriptor inputType = GetSignalType(InputSignals, currentspvIndex);
            ok = (inputType == Float32Bit);
            if (!ok) {
                REPORT_ERROR(ErrorManagement::ParametersError, "Signal Currspv shall be defined as float32.");
            }
        }
    }
    if (ok) {
        StreamString signalName = "Targetv";
        ok = GetSignalIndex(InputSignals, targetvIndex, signalName.Buffer());
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "Targetv input signal shall be defined.");
        }
        else {
            TypeDescriptor inputType = GetSignalType(InputSignals, targetvIndex);
            ok = (inputType == Float32Bit);
            if (!ok) {
                REPORT_ERROR(ErrorManagement::ParametersError, "Signal Targetv shall be defined as float32.");
            }
        }
    }
    if (ok) {
        StreamString signalName = "Time";
        ok = GetSignalIndex(InputSignals, timeIndex, signalName.Buffer());
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "Time input signal shall be defined.");
        }
        else {
            TypeDescriptor inputType = GetSignalType(InputSignals, timeIndex);
            ok = (inputType == Float32Bit);
            if (!ok) {
                REPORT_ERROR(ErrorManagement::ParametersError, "Signal Time shall be defined as float32.");
            }
        }
    }
    if (ok) {
        StreamString signalName = "Start";
        ok = GetSignalIndex(InputSignals, startIndex, signalName.Buffer());
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "Start input signal shall be defined.");
        }
        else {
            TypeDescriptor inputType = GetSignalType(InputSignals, startIndex);
            ok = (inputType == UnsignedInteger32Bit);
            if (!ok) {
                REPORT_ERROR(ErrorManagement::ParametersError, "Start shall be defined as uint32.");
            }
        }
    }
    if (ok) {
        StreamString signalName = "PLC_STANDBY";
        ok = GetSignalIndex(InputSignals, standbyIndex, signalName.Buffer());
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "PLC_STANDBY input signal shall be defined.");
        }
        else {
            TypeDescriptor inputType = GetSignalType(InputSignals, standbyIndex);
            ok = (inputType == UnsignedInteger32Bit);
            if (!ok) {
                REPORT_ERROR(ErrorManagement::ParametersError, "PLC_STANDBY shall be defined as uint32.");
            }
        }
    }
    if (ok) {
        StreamString signalName = "MANUAL_AUTO";
        ok = GetSignalIndex(InputSignals, isAutoIndex, signalName.Buffer());
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "MANUAL_AUTO input signal shall be defined.");
        }
        else {
            TypeDescriptor inputType = GetSignalType(InputSignals, isAutoIndex);
            ok = (inputType == UnsignedInteger32Bit);
            if (!ok) {
                REPORT_ERROR(ErrorManagement::ParametersError, "MANUAL_AUTO shall be defined as uint32.");
            }
        }
    }
    if (ok) {
        StreamString signalName = "FHPS_PrePro";
        ok = GetSignalIndex(InputSignals, fhpsPreProIndex, signalName.Buffer());
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "FHPS_PrePro input signal shall be defined.");
        }
        else {
            TypeDescriptor inputType = GetSignalType(InputSignals, fhpsPreProIndex);
            ok = (inputType == Float32Bit);
            if (!ok) {
                REPORT_ERROR(ErrorManagement::ParametersError, "Signal FHPS_PrePro shall be defined as float32.");
            }
        }
    }

    if (ok) {
        TypeDescriptor inputType = GetSignalType(OutputSignals, 0);
        ok = (inputType == Float32Bit);
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "Signal Output shall be defined as float32.");
        }
    }
    if (ok) {
        TypeDescriptor inputType = GetSignalType(OutputSignals, 1);
        ok = (inputType == UnsignedInteger32Bit);
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "Signal state shall be defined as float32.");
        }
    }

    if (ok) {
        current_setpoint = reinterpret_cast<float32 *>(GetInputSignalMemory(currentspvIndex));
        target_value = reinterpret_cast<float32 *>(GetInputSignalMemory(targetvIndex));
        rampup_time = reinterpret_cast<float32 *>(GetInputSignalMemory(timeIndex));
        start = reinterpret_cast<uint32 *>(GetInputSignalMemory(startIndex));
        standby = reinterpret_cast<uint32 *>(GetInputSignalMemory(standbyIndex));
        isAuto = reinterpret_cast<uint32 *>(GetInputSignalMemory(isAutoIndex));
        FHPS_PrePro = reinterpret_cast<float32 *>(GetInputSignalMemory(fhpsPreProIndex));
        
        output = reinterpret_cast<float32 *>(GetOutputSignalMemory(0));
        state = reinterpret_cast<uint32 *>(GetOutputSignalMemory(1));
    }
    return ok;
}

bool JARampupGAM::PrepareNextState(const MARTe::char8 * const currentStateName, const MARTe::char8 * const nextStateName){
    if(strcmp(nextStateName, "WaitHVON_PREP")==0 || strcmp(nextStateName, "WaitHVON_SDN_PREP")==0 || 
    strcmp(nextStateName, "WaitHVON")==0 || strcmp(nextStateName, "WaitHVON_SDN")==0){
        inWaitHVON = true;
        inWaitStandby = false;
    } else{
        inWaitHVON = false;
        if(strcmp(nextStateName,"WaitStandby")==0 ){
            inWaitStandby = true;
        } else {
            inWaitStandby = false;
        }
    }
    return true;
}

bool JARampupGAM::Execute() {
    using namespace MARTe;
    if(!inWaitHVON){
        if (*target_value <= 0.0f || *standby == 0u) {
            *output = 0.0f;
            rampup_rate = 0.0f;
            if(*target_value <= 0.0f){
                *state = 3u;
            } else {
                *state = 0u;
            }
            return true;
        }

        if(*start == 1u && *isAuto==0u){ //isAuto = 1.Manual, 0.auto-rampup.
            inRampup = true;
            resetFlag = true;
            *output = 0.0f; //Enable if it should start always zero.
        }

        // Calcrate new rampup rate.
        if(*rampup_time != 0 && resetFlag == true){
            rampup_rate = (*target_value - *current_setpoint) / *rampup_time/1000.0f; // Volt/msec
            resetFlag = false;
        }

        // Update Parameter
        if(*standby == 1u ){
            if(*isAuto == 1u){
                if (inWaitStandby){
                    *output = *target_value;
                } else{
                    *output = *FHPS_PrePro;
                }
                //*output = *target_value;
                *state = 0u;
                return true;
            }
            else if (inRampup){
                if (*output + rampup_rate < *target_value  && *rampup_time != 0){
                    *output = *output + rampup_rate;
                    *state = 1u;
                } else {
                    *output = *target_value;
                    *state = 2u;
                    inRampup = false;
                }
            }
        } 
        return true;
    } else {
        if(*isAuto == 0){
            *output = *FHPS_PrePro;
        } else{
            *output = *target_value;
        }
        return true;
    }
}

CLASS_REGISTER(JARampupGAM, "1.0")
