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

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

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

MARTe::float32 absFloat(MARTe::float32 x) {
    if (x < 0.0f) {
        return -x;
    }
    return x;
}

JATriangleWaveGAM::JATriangleWaveGAM() {
    frequency = NULL_PTR(MARTe::float32 *);
    amplitude = NULL_PTR(MARTe::float32 *);
    offset = NULL_PTR(MARTe::float32 *);
    plcStandby = NULL_PTR(MARTe::uint32 *);
    waveOutput = NULL_PTR(MARTe::float32 *);
    time = 0.0f;
}

JATriangleWaveGAM::~JATriangleWaveGAM() {
}

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

bool JATriangleWaveGAM::Setup() {
    using namespace MARTe;
    bool ok = (numberOfInputSignals == 4u);
    if (ok) {
        ok = (numberOfOutputSignals == 1u);
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "One output signal shall be defined.");
        }
    }
    else {
        REPORT_ERROR(ErrorManagement::ParametersError, "Four input signals shall be defined.");
    }
    uint32 freqIndex;
    uint32 ampIndex;
    uint32 offsetIndex;
    uint32 plcStandbyIndex;
    if (ok) {
        StreamString signalName = "Frequency";
        ok = GetSignalIndex(InputSignals, freqIndex, signalName.Buffer());
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "Frequency input signal shall be defined.");
        }
        else {
            TypeDescriptor inputType = GetSignalType(InputSignals, freqIndex);
            ok = (inputType == Float32Bit);
            if (!ok) {
                REPORT_ERROR(ErrorManagement::ParametersError, "Signal Frequency shall be defined as float32.");
            }
        }
    }
    if (ok) {
        StreamString signalName = "Amplitude";
        ok = GetSignalIndex(InputSignals, ampIndex, signalName.Buffer());
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "Amplitude input signal shall be defined.");
        }
        else {
            TypeDescriptor inputType = GetSignalType(InputSignals, ampIndex);
            ok = (inputType == Float32Bit);
            if (!ok) {
                REPORT_ERROR(ErrorManagement::ParametersError, "Signal Amplitude shall be defined as float32.");
            }
        }
    }
    if (ok) {
        StreamString signalName = "Offset";
        ok = GetSignalIndex(InputSignals, offsetIndex, signalName.Buffer());
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "Offset input signal shall be defined.");
        }
        else {
            TypeDescriptor inputType = GetSignalType(InputSignals, offsetIndex);
            ok = (inputType == Float32Bit);
            if (!ok) {
                REPORT_ERROR(ErrorManagement::ParametersError, "Signal Offset shall be defined as float32.");
            }
        }
    }
    if (ok) {
        StreamString signalName = "PLCSTANDBY";
        ok = GetSignalIndex(InputSignals, plcStandbyIndex, signalName.Buffer());
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "PLCSTANDBY input signal shall be defined.");
        }
        else {
            TypeDescriptor inputType = GetSignalType(InputSignals, plcStandbyIndex);
            ok = (inputType == UnsignedInteger32Bit);
            if (!ok) {
                REPORT_ERROR(ErrorManagement::ParametersError, "PLCSTANDBY shall be defined as uint32.");
            }
        }
    }
    if (ok) {
        TypeDescriptor inputType = GetSignalType(OutputSignals, 0);
        ok = (inputType == Float32Bit);
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError, "Signal Amplitude shall be defined as float32.");
        }
    }
    if (ok) {
        frequency = reinterpret_cast<float32 *>(GetInputSignalMemory(freqIndex));
        amplitude = reinterpret_cast<float32 *>(GetInputSignalMemory(ampIndex));
        offset = reinterpret_cast<float32 *>(GetInputSignalMemory(offsetIndex));
        plcStandby = reinterpret_cast<uint32 *>(GetInputSignalMemory(plcStandbyIndex));
        waveOutput = reinterpret_cast<float32 *>(GetOutputSignalMemory(0));
    }
    return ok;
}

bool JATriangleWaveGAM::Execute() {
    using namespace MARTe;
    // If frequency is not set, output 0.
    if (*frequency <= 0.0f || *plcStandby == 0u) {
        *waveOutput = 0.0f;
        return true;
    }
    // Increase the current time.
    ++time;
    // Calculate the period in milliseconds
    float32 periodMs = 1000.0 / *frequency;
    // Make sure the time is on [0, periodMs] interval.
    while (time > periodMs) {
        time -= periodMs;
    }
    // Formula:
    // f(x) = |x - 0.5| * 2 * amplitude
    // where x is between 0 and 1
    *waveOutput = absFloat((time / periodMs) - 0.5f) * 2.0f * (*amplitude) + *offset;
    return true;
}

CLASS_REGISTER(JATriangleWaveGAM, "1.0")