/**
 * @file JASourceChoiceGAM.cpp
 * @brief Source file for class JASourceChoiceGAM
 * @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 JASourceChoiceGAM (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 "JASourceChoiceGAM.h"

#include "AdvancedErrorManagement.h"

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

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

JASourceChoiceGAM::JASourceChoiceGAM() {
    // initialize member variables.
    numberOfPVs = 0;
}

JASourceChoiceGAM::~JASourceChoiceGAM() {
}

bool JASourceChoiceGAM::Initialise(MARTe::StructuredDataI & data) {
    //GAM parameters are initialized.
    using namespace MARTe;
    bool ok = GAM::Initialise(data);
    if (ok) {
        ok = data.Read("numberOfPVs", numberOfPVs);
        if (!ok) {
            REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "The numberOfPVs parameter shall be specified");
        }
    }
    return ok;
}

bool JASourceChoiceGAM::PrepareNextState(const MARTe::char8 * const currentStateName, const MARTe::char8 * const nextStateName) {
    //This method changes internal parameter based on next realtime state.
    return true;
}

bool JASourceChoiceGAM::Setup() {
    // Setup memory for input/output signals on the GAM.
    using namespace MARTe;
    bool ok = (numberOfInputSignals == numberOfPVs*3u);
    if (ok) {
        ok = (numberOfOutputSignals == numberOfPVs);
        if (!ok) {
            REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "%d *3 output signals shall be defined", numberOfPVs);
        }
    }
    else {
        REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "%d input signals shall be defined", numberOfPVs);
    }
    // Do type check for input signals.
    int int_num = 0;
    int float_num = 0;
    if (ok) {
        uint32 c;
        for (c = 0u; c < numberOfInputSignals; c++) {
            TypeDescriptor inputType = GetSignalType(InputSignals, c);
            if(inputType == UnsignedInteger32Bit){
                int_num++;
            } else if (inputType == Float32Bit) {
                float_num++;
            } else {
                ok = false;
            };
            if (!ok) {
                StreamString signalName;
                (void) GetSignalName(InputSignals, c, signalName);
                REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Signal %s shall be defined as uint32 or flaot32", signalName.Buffer());
            }
        }
    }
    // Do type check for output signals
    if (ok) {
        uint32 c;
        for (c = 0u; c < numberOfOutputSignals; c++) {
            TypeDescriptor outputType = GetSignalType(OutputSignals, c);
            ok = ((outputType == UnsignedInteger32Bit) || (outputType == Float32Bit));
            if (!ok) {
                StreamString signalName;
                (void) GetSignalName(InputSignals, c, signalName);
                REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Signal %s shall be defined as uint32 or float32", signalName.Buffer());
            }
        }
    }
    // Set memory
    inputUInt32.resize(numberOfPVs*2);
    inputFloat32.resize(numberOfPVs*2);
    choise.resize(numberOfPVs);
    outputUInt32.resize(numberOfPVs);
    outputFloat32.resize(numberOfPVs);

    prevUInt32.resize(numberOfPVs*2);
    prevFloat32.resize(numberOfPVs*2);

    if(ok){
        for(uint32 i=0; i<numberOfPVs; i++){ //Expected inp1, inp2, choise order in signal list.
            TypeDescriptor inputType = GetSignalType(InputSignals, i*3);
            if(inputType == UnsignedInteger32Bit){
                inputUInt32[2*i] = reinterpret_cast<uint32 *>(GetInputSignalMemory(3*i));
                inputUInt32[2*i+1] = reinterpret_cast<uint32 *>(GetInputSignalMemory(3*i+1));
                choise[i] = reinterpret_cast<uint32 *>(GetInputSignalMemory(3*i+2));
                outputUInt32[i] = reinterpret_cast<uint32 *>(GetOutputSignalMemory(i));
            } else if(inputType == Float32Bit){
                inputFloat32[2*i] = reinterpret_cast<float *>(GetInputSignalMemory(3*i));
                inputFloat32[2*i+1] = reinterpret_cast<float *>(GetInputSignalMemory(3*i+1));
                choise[i] = reinterpret_cast<uint32 *>(GetInputSignalMemory(3*i+2));
                outputFloat32[i] = reinterpret_cast<float32 *>(GetOutputSignalMemory(i));
            }
            prevUInt32[2*i] = 0;
            prevUInt32[2*i+1] = 0;
            prevFloat32[2*i] = 0;
            prevFloat32[2*i+1] = 0;
        }
    }
    return ok;
}

bool JASourceChoiceGAM::Execute() {
    // This method is called every realtime state thread cycle.
    using namespace MARTe;

    for (uint32 i=0; i < numberOfPVs; i++){
        if(*choise[i]==0){
            if(outputUInt32[i]){
                if(prevUInt32[i*2] != *inputUInt32[i*2]){
                    *outputUInt32[i] = *inputUInt32[i*2];
                    prevUInt32[i*2] = *inputUInt32[i*2];
                    prevUInt32[1+i*2] = *inputUInt32[1+i*2];
                }
            } else if(outputFloat32[i]){
                if(prevFloat32[i*2] != *inputFloat32[i*2]){
                    *outputFloat32[i] = *inputFloat32[i*2];
                    prevFloat32[i*2] = *inputFloat32[i*2];
                    prevFloat32[1+i*2] = *inputFloat32[1+i*2];
                }
            }
        } else {
            if(outputUInt32[i]){
                if(prevUInt32[1+i*2] != *inputUInt32[1+i*2]){
                    *outputUInt32[i] = *inputUInt32[1+i*2];
                    prevUInt32[i*2] = *inputUInt32[i*2];
                    prevUInt32[1+i*2] = *inputUInt32[1+i*2];
                }
            } else if (outputFloat32[i]){
                if(prevFloat32[1+i*2] != *inputFloat32[1+i*2]){
                    *outputFloat32[i] = *inputFloat32[1+i*2];
                    prevFloat32[i*2] = *inputFloat32[i*2];
                    prevFloat32[1+i*2] = *inputFloat32[1+i*2];
                }
            }
        }
    }
    return true;
}

CLASS_REGISTER(JASourceChoiceGAM, "1.0")