/**
 * @file ElectricProbesGAM.cpp
 * @brief Source file for class ElectricProbesGAM
 * @date 06/04/2018
 * @author Andre Neto
 *
 * @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 ElectricProbesGAM (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 "CLASSMETHODREGISTER.h"

#include "ElectricProbesGAM.h"
#include "RegisteredMethodsMessageFilter.h"

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

//namespace MARTeIsttok {
namespace MARTe {
/**
 * The number of signals
 */
    const  uint32 EP_NUM_INPUTS  = 4u; 
    const  uint32 EP_NUM_OUTPUTS = 2u; 

/*---------------------------------------------------------------------------*/
/*                           Method definitions                              */
/*---------------------------------------------------------------------------*/
//namespace MARTeIsttok {
    ElectricProbesGAM::ElectricProbesGAM() : 
                GAM(),
                MessageI() {
        gain = 0u;
        numberOfSamplesAvg = 1u;

        //outputSignals = NULL_PTR(MARTe::float32 **);

        inputElectricTop    = NULL_PTR(MARTe::float32 *);
        inputElectricInner  = NULL_PTR(MARTe::float32 *);
        inputElectricOuter  = NULL_PTR(MARTe::float32 *);
        inputElectricBottom = NULL_PTR(MARTe::float32 *);

        lastInputs = NULL_PTR(MARTe::float32**);


        outputEpR = NULL_PTR(MARTe::float32 *);
        outputEpZ = NULL_PTR(MARTe::float32 *);
        resetInEachState = false;
    }

    ElectricProbesGAM::~ElectricProbesGAM() {
        //if (inputSignals != NULL_PTR(MARTe::float32 **)) {
        //    delete[] inputSignals;
        //}
        /*if (outputSignals != NULL_PTR(MARTe::float32 **)) {
            delete[] outputSignals;
        }
        */
        if (lastInputs != NULL_PTR(MARTe::float32**)) {
            MARTe::uint32 k;
            for (k=0u; k < EP_NUM_INPUTS; k++) {
                if (lastInputs[k] != NULL_PTR(MARTe::float32*)) {
                    delete[] lastInputs[k];
                }
            }
            delete[] lastInputs;
        }
/*
        for (k=0u; k < EP_NUM_INPUTS; k++) {
            if(lastInputs[k] != NULL_PTR(MARTe::float32 *)) {
                delete [] lastInputs[k];
            }
        } 
*/
    }

    bool ElectricProbesGAM::Initialise(MARTe::StructuredDataI & data) {
        using namespace MARTe;
        bool ok = GAM::Initialise(data);
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "Could not Initialise the GAM");
        }
        if (ok) {
            ok = data.Read("Gain", gain);
            if (!ok) {
                REPORT_ERROR(ErrorManagement::ParametersError, "The parameter Gain shall be set");
            }
        }
        if (ok) {
            REPORT_ERROR(ErrorManagement::Information, "Parameter Gain set to %d", gain);
        }
        if (ok) {
            ok = data.Read("NumberOfSamplesAvg", numberOfSamplesAvg);
            if (!ok) {
                REPORT_ERROR(ErrorManagement::ParametersError, 
                        "The parameter NumberOfSamplesAvg shall be set");
            }
        }
        if (ok) {
            REPORT_ERROR(ErrorManagement::Information, "Parameter NumberOfSamplesAvg set to %d",
                    numberOfSamplesAvg);
        }
        if (ok) {
            uint32 auxResetInEachState = 0u;
            ok = data.Read("ResetInEachState", auxResetInEachState);
            if (!ok) {
                REPORT_ERROR(ErrorManagement::InitialisationError, "Error reading ResetInEachState");
            }
            else {
                if (auxResetInEachState == 1u) {
                    resetInEachState = true;
                }
                else if (auxResetInEachState == 0u) {
                    resetInEachState = false;
                }
                else {
                    ok = false;
                    REPORT_ERROR(ErrorManagement::InitialisationError, "Wrong value for ResetInEachState. Possible values 0 (false) or 1 (true)");
                }
            }
        }

        return ok;
    }

    bool ElectricProbesGAM::Setup() {
        using namespace MARTe;
        uint32 numberOfInputSignals = GetNumberOfInputSignals();
        bool ok = (numberOfInputSignals == 4u);
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "The number of input signals shall be equal to 4. numberOfInputSignals = %d ", numberOfInputSignals);
        }

        if (ok) {
            lastInputs = new float32*[numberOfInputSignals];
            uint32 n;
            if (lastInputs != NULL_PTR(MARTe::float32**)) {
                for (n = 0u; n < numberOfInputSignals ; n++) {
                    if (numberOfSamplesAvg > 1u) {
                        lastInputs[n] = new float32[numberOfSamplesAvg - 1u];
                        if (lastInputs[n] != NULL_PTR(MARTe::float32*)) {
                            uint32 i;
                            for (i = 0u; i < (numberOfSamplesAvg - 1u); i++) {
                                lastInputs[n][i] = 0.0F;
                            }
                        }
                    }
                }
            }

            for (n = 0u; (n < numberOfInputSignals) && (ok); n++) {
                StreamString inputSignalName;
                ok = GetSignalName(InputSignals, n, inputSignalName);
                TypeDescriptor inputSignalType = GetSignalType(InputSignals, n);
                ok = (inputSignalType == Float32Bit);
                if (!ok) {
                    const char8 * const inputSignalTypeStr = TypeDescriptor::GetTypeNameFromTypeDescriptor(inputSignalType);
                    REPORT_ERROR(ErrorManagement::ParametersError,
                            "The type of the input signals shall be float32. inputSignalType = %s", inputSignalTypeStr);
                }
                uint32 numberOfInputSamples = 0u;
                if (ok) {
                    ok = GetSignalNumberOfSamples(InputSignals, n, numberOfInputSamples);
                }

                if (ok) {
                    ok = (numberOfInputSamples == 1u);
                }
                if (!ok) {
                    REPORT_ERROR(ErrorManagement::ParametersError,
                            "The number of input signals samples shall be equal to 1. numberOfInputSamples = %d", numberOfInputSamples);
                }
                uint32 numberOfInputDimensions = 0u;
                if (ok) {
                    ok = GetSignalNumberOfDimensions(InputSignals, n, numberOfInputDimensions);
                }

                if (ok) {
                    ok = (numberOfInputDimensions == 0u);
                    if (!ok) {
                        REPORT_ERROR(
                                ErrorManagement::ParametersError,
                                "The number of input signals dimensions shall be equal to 0. numberOfInputDimensions(%s) = %d", inputSignalName.Buffer(), numberOfInputDimensions);
                    }
                }
                uint32 numberOfInputElements = 0u;
                if (ok) {
                    ok = GetSignalNumberOfElements(InputSignals, n, numberOfInputElements);
                }
                if (ok) {
                    ok = (numberOfInputElements == 1u);
                }
                if (!ok) {
                    REPORT_ERROR(ErrorManagement::ParametersError,
                            "The number of input signal elements shall be equal to 1. numberOfInputElements(%s) = %d", inputSignalName.Buffer(), numberOfInputElements);
                }

//                if (ok) {
//                     inputSignals[n] = reinterpret_cast<float32 *>(GetInputSignalMemory(n));
//                }


            }
            if (ok) {
                inputElectricTop    = reinterpret_cast<float32 *>(GetInputSignalMemory(0u));
                inputElectricInner  = reinterpret_cast<float32 *>(GetInputSignalMemory(1u));
                inputElectricOuter  = reinterpret_cast<float32 *>(GetInputSignalMemory(2u));
                inputElectricBottom = reinterpret_cast<float32 *>(GetInputSignalMemory(3u));

                REPORT_ERROR(ErrorManagement::Information, "InputSignals reinterpret_cast OK");
            }

        }

        // OutputSignals
        uint32 numberOfOutputSignals = GetNumberOfOutputSignals();
        ok = (numberOfOutputSignals == 2u);
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "The number of output signals shall be equal to 2. numberOfOutputSignals = %d ", numberOfOutputSignals);
        }
        if (ok) {
            uint32 n;
            for (n = 0u; (n < numberOfOutputSignals) && (ok); n++) {
                StreamString outputSignalName;
                ok = GetSignalName(OutputSignals, n, outputSignalName);
                TypeDescriptor outputSignalType = GetSignalType(OutputSignals, n);
                ok = (outputSignalType == Float32Bit);
                if (!ok) {
                    const char8 * const outputSignalTypeStr = TypeDescriptor::GetTypeNameFromTypeDescriptor(outputSignalType);
                    REPORT_ERROR(ErrorManagement::ParametersError,
                            "The type of the output signals shall be float32. outputSignalType = %s", outputSignalTypeStr);
                }
                uint32 numberOfOutputSamples = 0u;
                if (ok) {
                    ok = GetSignalNumberOfSamples(OutputSignals, n, numberOfOutputSamples);
                }

                if (ok) {
                    ok = (numberOfOutputSamples == 1u);
                }
                if (!ok) {
                    REPORT_ERROR(ErrorManagement::ParametersError,
                            "The number of output signals samples shall be equal to 1. numberOfOutputSamples = %d", numberOfOutputSamples);
                }
                uint32 numberOfOutputDimensions = 0u;
                if (ok) {
                    ok = GetSignalNumberOfDimensions(OutputSignals, n, numberOfOutputDimensions);
                }

                if (ok) {
                    ok = (numberOfOutputDimensions == 0u);
                    if (!ok) {
                        REPORT_ERROR(ErrorManagement::ParametersError,
                                "The number of output signals dimensions shall be equal to 0.  numberOfOutputDimensions (%s) = %d", outputSignalName.Buffer(), numberOfOutputDimensions);
                    }
                }
                uint32 numberOfOutputElements = 0u;
                if (ok) {
                    ok = GetSignalNumberOfElements(OutputSignals, n, numberOfOutputElements);
                }
                if (ok) {
                    ok = (numberOfOutputElements == 1u);
                }
                if (!ok) {
                    REPORT_ERROR(ErrorManagement::ParametersError,
                                 "The number of output signals elements shall be equal to 1. (%s) numberOfOutputElements = %d", outputSignalName.Buffer(), numberOfOutputElements);
                }

            }
            if (ok) {
                outputEpR = reinterpret_cast<float32 *>(GetOutputSignalMemory(0u));
                outputEpZ = reinterpret_cast<float32 *>(GetOutputSignalMemory(1u));
            }
        }

        // Install message filter
        ReferenceT<RegisteredMethodsMessageFilter> registeredMethodsMessageFilter("RegisteredMethodsMessageFilter");

        if (ok) {
            ok = registeredMethodsMessageFilter.IsValid();
        }

        if (ok) {
            registeredMethodsMessageFilter->SetDestination(this);
            ok = InstallMessageFilter(registeredMethodsMessageFilter);
        }

        return ok;

    }

    bool ElectricProbesGAM::Execute() {
        //*outputSignal = *inputSignal;
        *outputEpR = 4.3;
//        *outputEpZ = 3.4;
        *outputEpZ = *inputElectricTop - *inputElectricOuter;
        //*outputSignal1 = *inputSignals[0]; // - *inputSignals[1];
        //*outputSignal1 = *inputSignals[0] - *inputSignals[1];

        //update the last values
        for (MARTe::uint32 i = 0u; i < EP_NUM_INPUTS; i++) {

            if (numberOfSamplesAvg > 2u) {
                /*lint -e{9117} implicit conversion is safe*/
                for (MARTe::uint32 k = (numberOfSamplesAvg - 1u); k > 0u; k--) {
                    lastInputs[i][k] = lastInputs[i][k - 1];
                }
            }
        }
        if (numberOfSamplesAvg > 1u) {
            lastInputs[0][0] = *inputElectricTop;
            lastInputs[1][0] = *inputElectricInner;
            lastInputs[2][0] = *inputElectricOuter;
            lastInputs[3][0] = *inputElectricBottom;
                                                  //lastInputs[i][0] = input[i][numberOfSamples - 1u];
        }

         return true;
    }

    bool ElectricProbesGAM::PrepareNextState(const char8 * const currentStateName,
            const char8 * const nextStateName) {
        bool ret = true;

        if (resetInEachState) {
            lastStateExecuted = nextStateName;
        }
/*
            bool cond1 = (stateVector.GetDataPointer() != NULL_PTR(float64 **));
            bool cond2 = (derivativeStateVector.GetDataPointer() != NULL_PTR(float64 **));
            if (cond1 && cond2) {
                for (uint32 i = 0u; i < sizeStateVector; i++) {
                    stateVector(i, 0u) = 0.0;
                    derivativeStateVector(i, 0u) = 0.0;
                }
            }
            else {
                REPORT_ERROR(ErrorManagement::ParametersError, "stateVector or derivativeStateVector = NULL ");
                ret = false;
            }

        }
        else {
            //If the currentStateName and lastStateExecuted are different-> rest values
            if (lastStateExecuted != currentStateName) {
                bool cond1 = (stateVector.GetDataPointer() != NULL_PTR(float64 **));
                bool cond2 = (derivativeStateVector.GetDataPointer() != NULL_PTR(float64 **));
                if (cond1 && cond2) {
                    for (uint32 i = 0u; i < sizeStateVector; i++) {
                        stateVector(i, 0u) = 0.0;
                        derivativeStateVector(i, 0u) = 0.0;
                    }
                }
                else {
                    REPORT_ERROR(ErrorManagement::ParametersError, "stateVector or derivativeStateVector = NULL ");
                    ret = false;
                }
            }
            lastStateExecuted = nextStateName;
        }
*/
        return ret;
    }


    bool ElectricProbesGAM::ExportData(MARTe::StructuredDataI & data) {
        using namespace MARTe;
        bool ok = GAM::ExportData(data);
        if (ok) {
            ok = data.CreateRelative("Parameters");
        }
        if (ok) {
            ok = data.Write("Gain", gain);
        }
        if (ok) {
            ok = data.MoveToAncestor(1u);
        }
        return ok;
    }

    ErrorManagement::ErrorType ElectricProbesGAM::CalcOffSets() {

        ErrorManagement::ErrorType ret = MARTe::ErrorManagement::NoError;
        REPORT_ERROR(ErrorManagement::Information, 
                        "CalcOffSets. numberOfSamplesAvg: %d!", numberOfSamplesAvg);
        if (numberOfSamplesAvg > 1u) {
            for (uint32 i = 0u; i < EP_NUM_INPUTS; i++) {
                inputOffsets[i] = 0.0f;
                for (uint32 k =  0 ; k < numberOfSamplesAvg; k++) {
                    inputOffsets[i] += lastInputs[i][k];
                }
                inputOffsets[i] /= numberOfSamplesAvg;
                REPORT_ERROR(ErrorManagement::Information, 
                        "CalcOffSets. Offset:%d= %f!", i, inputOffsets[i]);
            }
        }

        return ret;
    }


    CLASS_REGISTER(ElectricProbesGAM, "1.0")

//    CLASS_METHOD_REGISTER(ElectricProbesGAM, ElectricProbesGAM::CalcOffSets)
    CLASS_METHOD_REGISTER(ElectricProbesGAM, CalcOffSets)

} /* namespace MARTeIsttok */

//  vim: syntax=cpp ts=4 sw=4 sts=4 sr et