/**
 * @file JAPreProgrammedGAM.cpp
 * @brief Source file for class JAPreProgrammedGAM
 * @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 JAPreProgrammedGAM (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 "File.h"
#include "JAPreProgrammedGAM.h"
#include "MessageI.h"
#include "RegisteredMethodsMessageFilter.h"

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

// How often output signals are updated.
const MARTe::uint32 cycleMs = 10u;

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

JAPreProgrammedGAM::JAPreProgrammedGAM() :
        GAM() {
    using namespace MARTe;
    filenameSignalIndex = 0u;
    timeSignal = NULL_PTR(MARTe::int32 *);
    loadTriggerSignal = NULL_PTR(MARTe::uint32 *);
    fhpsrefSignal = NULL_PTR(MARTe::float32 *);
    rfonStateSignal = NULL_PTR(MARTe::uint32 *);

    valueSignals = NULL_PTR(MARTe::float32 **);
    preProgrammedValues = NULL_PTR(MARTe::float32 **);
    preProgrammedTime = NULL_PTR(MARTe::int32 *);
    fileLoadedSignal = NULL_PTR(MARTe::uint32 *);
    fileLoadErrorOutput = NULL_PTR(MARTe::uint32 *);

    startTime = 0u;
    numberOfPreProgrammedValues = 0u;
    numberOfPreProgrammedTimeRows = 0u;
    currentRow = 0u;
    mode = None;
    preProgrammedExecutaionPeriodMs = 0u;
    msCounter = 0u;
    preProgrammedRow = 0u;
    resetOutputSignals = false;
    readOnce = true;
}

JAPreProgrammedGAM::~JAPreProgrammedGAM() {
    DeleteArrays();
    if (valueSignals != NULL_PTR(MARTe::float32 **)) {
        delete[] valueSignals;
    }
}

bool JAPreProgrammedGAM::Initialise(MARTe::StructuredDataI & data) {
    using namespace MARTe;
    bool ok = GAM::Initialise(data);
    if (ok) {
        ok = data.Read("Directory", directory);
        if (!ok) {
            REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "The Directory shall be specified");
        }
    }
    if (ok) {
        ok = data.Read("PreProgrammedPeriodMs", preProgrammedExecutaionPeriodMs);
        if (!ok) {
            REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "The Directory shall be specified");
        }
    }
    return ok;
}

bool JAPreProgrammedGAM::Setup() {
    using namespace MARTe;
    //Signal number check.
    bool ok = (numberOfInputSignals == 4u);
    if (ok) {
        ok = (numberOfOutputSignals > 2u);
        if (!ok) {
            REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "At least two output signal shall be defined");
        }
    }
    else {
        REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Four input signals with the filename and file load trigger shall be defined");
    }
    //Input signals type consistency check.
    if (ok) {
        StreamString signalName = "Filename";
        ok = GetSignalIndex(InputSignals, filenameSignalIndex, signalName.Buffer());
        if (!ok) {
            REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Filename input signal shall be defined");
        }
        else {
            TypeDescriptor inputType = GetSignalType(InputSignals, filenameSignalIndex);
            ok = (inputType == CharString);
            if (!ok) {
                ok = (inputType == Character8Bit);
            }
            if (!ok) {
                StreamString signalName;
                (void) GetSignalName(InputSignals, filenameSignalIndex, signalName);
                REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Signal %s shall be defined as string", signalName.Buffer());
            }
        }
    }
    if (ok) {
        StreamString signalName = "CSV_LOAD";
        uint32 loadSignalIndex;
        ok = GetSignalIndex(InputSignals, loadSignalIndex, signalName.Buffer());
        if (!ok) {
            REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "CSV_LOAD input signal shall be defined");
        }
        else {
            TypeDescriptor inputType = GetSignalType(InputSignals, loadSignalIndex);
            ok = (inputType == UnsignedInteger32Bit);
            if (!ok) {
                StreamString signalName;
                (void) GetSignalName(InputSignals, loadSignalIndex, signalName);
                REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Signal %s shall be defined as uint32", signalName.Buffer());
            } else {
                loadTriggerSignal = reinterpret_cast<uint32 *>(GetInputSignalMemory(loadSignalIndex));
            }
        }
    }
    if (ok) {
        StreamString signalName = "FHPS_REF";
        uint32 fhpsrefIndex;
        ok = GetSignalIndex(InputSignals, fhpsrefIndex, signalName.Buffer());
        if (!ok) {
            REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "FHPS_REF input signal shall be defined");
        }
        else {
            TypeDescriptor inputType = GetSignalType(InputSignals, fhpsrefIndex);
            ok = (inputType == Float32Bit);
            if (!ok) {
                StreamString signalName;
                (void) GetSignalName(InputSignals, fhpsrefIndex, signalName);
                REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Signal %s shall be defined as float32", signalName.Buffer());
            } else {
                fhpsrefSignal = reinterpret_cast<float32 *>(GetInputSignalMemory(fhpsrefIndex));
            }
        }
    }
    if (ok) {
        StreamString signalName = "RFON";
        uint32 rfonIndex;
        ok = GetSignalIndex(InputSignals, rfonIndex, signalName.Buffer());
        if (!ok) {
            REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "RFON input signal shall be defined");
        }
        else {
            TypeDescriptor inputType = GetSignalType(InputSignals, rfonIndex);
            ok = (inputType == UnsignedInteger32Bit);
            if (!ok) {
                StreamString signalName;
                (void) GetSignalName(InputSignals, rfonIndex, signalName);
                REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Signal %s shall be defined as float32", signalName.Buffer());
            } else {
                rfonStateSignal = reinterpret_cast<uint32 *>(GetInputSignalMemory(rfonIndex));
            }
        }
    }

    //Output signals type check.
    if (ok) {
        TypeDescriptor timeType = GetSignalType(OutputSignals, 0);
        ok = (timeType == SignedInteger32Bit);
        if (!ok) {
            StreamString signalName;
            (void) GetSignalName(InputSignals, 0, signalName);
            REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Signal %s shall be defined as int32", signalName.Buffer());
        }

        uint32 i;
        for (i = 1u; (i <= 6) && (ok); i++) {
            TypeDescriptor outputType = GetSignalType(OutputSignals, i);
            ok = (outputType == Float32Bit);
            if (!ok) {
                StreamString signalName;
                (void) GetSignalName(OutputSignals, i, signalName);
                REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Signal %s shall be defined as float32", signalName.Buffer());
            }
        }

        if (ok) {
            TypeDescriptor fileLoadedType = GetSignalType(OutputSignals, 7u);
            ok = (fileLoadedType == UnsignedInteger32Bit);
            if (!ok) {
                StreamString signalName;
                (void) GetSignalName(OutputSignals, 6u, signalName);
                REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Signal %s shall be defined as uint32", signalName.Buffer());
            }
        }
        if (ok) {
            TypeDescriptor fileLoadErrorOutputType = GetSignalType(OutputSignals, 8u);
            ok = (fileLoadErrorOutputType == UnsignedInteger32Bit);
            if (!ok) {
                StreamString signalName;
                (void) GetSignalName(OutputSignals, 7u, signalName);
                REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Signal %s shall be defined as uint32", signalName.Buffer());
            }
        }
    }

    //Register signal memory
    if (ok) {
        timeSignal = reinterpret_cast<int32 *>(GetOutputSignalMemory(0));
        valueSignals = new float32*[6u];
        uint32 i;
        for (i = 1u; i <= 6u; i++) {
            valueSignals[i - 1] = reinterpret_cast<float32 *>(GetOutputSignalMemory(i));
        }
        fileLoadedSignal = reinterpret_cast<uint32 *>(GetOutputSignalMemory(7u));
        fileLoadErrorOutput = reinterpret_cast<uint32 *>(GetOutputSignalMemory(8u));
    }
    return ok;
}

bool JAPreProgrammedGAM::PrepareNextState(const MARTe::char8 * const currentStateName, const MARTe::char8 * const nextStateName) {
    bool ok = true;
    if (strcmp(nextStateName, "WaitReady") == 0) {
        mode = LoadFileMode;
        resetOutputSignals = true;
        // Reset read once flag when reentering WaitReady state.
        readOnce = true;
    } else if (strcmp(nextStateName, "WaitPermit") == 0) {
        mode = LoadFileMode;
        resetOutputSignals = true;
        currentRow = 0u;
    } else if (strcmp(nextStateName, "WaitHVON_PREP") == 0 || strcmp(nextStateName, "WaitHVON_SDN_PREP") == 0) {
        ok = numberOfPreProgrammedTimeRows > 0;
        if (!ok) {
            REPORT_ERROR(MARTe::ErrorManagement::IllegalOperation, "Entering PreProgrammed mode without any waveform data.");
        }
        mode = PreProgrammedMode;
        msCounter = cycleMs;
        currentRow = 0u;
    } else {
        mode = None;
    }
    return ok;
}

bool JAPreProgrammedGAM::LoadFile() {
    using namespace MARTe;

    DeleteArrays();

    bool ok = true;

    const MARTe::char8 * const filenameSignal = reinterpret_cast<const char8 * const >(GetInputSignalMemory(filenameSignalIndex));
    //Prepare full path to the configuration file.
    StreamString filename = directory;
    filename += DIRECTORY_SEPARATOR;
    filename += filenameSignal;
    REPORT_ERROR(MARTe::ErrorManagement::Debug, "Opening file %s", filename.Buffer());

    //parse prepro configuration file into two arrays(time, values).
    File f;
    ok = f.Open(filename.Buffer(), BasicFile::ACCESS_MODE_R);
    if (ok) {
        //Count up number of file Rows.
        numberOfPreProgrammedTimeRows = 0u;
        StreamString tokenLine;
        while (f.GetLine(tokenLine)) {
            if (tokenLine[0] != '#' && tokenLine[0] != '-') {
                numberOfPreProgrammedTimeRows++;
            }
            tokenLine = "";
        }
        //Count up number of file lines
        numberOfPreProgrammedValues = 0u;
        f.Seek(0);
        uint32 t = 0u;
        tokenLine = "";
        while ((ok) && (f.GetLine(tokenLine))) {
            // Skip comment line and minus time
            if (tokenLine[0] == '#' || tokenLine[0] == '-') {
                tokenLine = "";
                continue;
            }
            // Prepare two arrays at first.
            if (numberOfPreProgrammedValues == 0) {
                StreamString token;
                char8 ignored;
                tokenLine.Seek(0);
                while (tokenLine.GetToken(token, ",", ignored)) {
                    numberOfPreProgrammedValues++;
                    token = "";
                }
REPORT_ERROR(MARTe::ErrorManagement::ParametersError,"numberOfPreProgrammedVal %d",  numberOfPreProgrammedValues);
                ok = (numberOfPreProgrammedValues == 7u);//From time row to FHPS row.
                numberOfPreProgrammedValues -= 1u; //From MHVPS row to FHPS row.
                if (ok) {
                    preProgrammedTime = new int32[numberOfPreProgrammedTimeRows];
                    preProgrammedValues = new float32*[numberOfPreProgrammedTimeRows];
                    uint32 j;
                    for (j = 0u; j < numberOfPreProgrammedTimeRows; j++) {
                        preProgrammedValues[j] = new float32[numberOfPreProgrammedValues];
                    }
                }
                else {
                    REPORT_ERROR(MARTe::ErrorManagement::ParametersError, 
                    "Number of columns in csv file (%d) is not consistent with the number of output signals (%d)",
                     numberOfPreProgrammedValues, 6u);
                    *fileLoadErrorOutput = 1;
                }
            }
            // Add loaded data into two arrays.
            if (ok) {
                StreamString token;
                char8 ignored;
                tokenLine.Seek(0);
                uint32 idx = 0u;
                while (tokenLine.GetToken(token, ",", ignored)) {
                    if (idx == 0u) {
                        preProgrammedTime[t] = atoi(token.Buffer());
                        if (t == 0 && preProgrammedTime[0] != 0) {
                            REPORT_ERROR(MARTe::ErrorManagement::Debug, "Prepro start from none zero time.");
                        }
                    }
                    else {
                        ok = ((idx - 1) < numberOfPreProgrammedValues);
                        if (ok) {
                            preProgrammedValues[t][idx - 1] = static_cast<float32>(atof(token.Buffer()));
                        }
                        else {
                            REPORT_ERROR(MARTe::ErrorManagement::FatalError, "Number of columns in csv file is not consistent with the number of output signals in line %d", t);
                            *fileLoadErrorOutput = 1;
                        }
                    }
                    token = "";
                    idx++;
                }
                t++;
            }
            tokenLine = "";
        }
        f.Close();
    }
    if (ok) {
        currentRow = 0u;
    }
    else {
        numberOfPreProgrammedTimeRows = 0u;
        REPORT_ERROR(ErrorManagement::Warning, "Failed to read waveform data from file.");
        *fileLoadErrorOutput = 2;
    }
    return ok;
}

bool JAPreProgrammedGAM::Execute() {
    using namespace MARTe;
    bool ok = true;

    switch (mode) {
        case LoadFileMode: {
            if (*loadTriggerSignal == 1u) {
                if (readOnce) {
                    *fileLoadErrorOutput = 0;
                    *fileLoadedSignal = LoadFile() ? 1 : 0;
                    readOnce = false;
                }
            } else {
                // Reset read once flag when loadTriggerSignal is reset.
                readOnce = true;
            }

            if (resetOutputSignals) {
                // Write default values to output signals
                *timeSignal = 0u;
                *valueSignals[FHPS-1] = *fhpsrefSignal;
                resetOutputSignals = false;
            } else {
                *valueSignals[FHPS-1] = *fhpsrefSignal; //copy input(2) to val_arr(4)
            }
        } break;

        case PreProgrammedMode: {
            ok = numberOfPreProgrammedTimeRows > 0;

            //add 20210105. Before RFON, prepro setpoints should be same as the values listed at csv first line.
            if (ok && *rfonStateSignal == 0) {
                uint32 j;
                for (j = 0u; j < FHPS; j++) {
                    *valueSignals[j] = preProgrammedValues[0][j];
                }
            }
            //end 20210105

            if (ok && currentRow < numberOfPreProgrammedTimeRows && *rfonStateSignal == 1) {
                if (msCounter >= cycleMs) {
                    msCounter -= cycleMs;
                    int32 currentTime = preProgrammedTime[currentRow];
                    //REPORT_ERROR(MARTe::ErrorManagement::Debug, "Write Time at %d",currentRow);
                    // Write to output signals
                    *timeSignal = currentTime;
                    uint32 j;
                    for (j = 0u; j < FHPS; j++) {
                        *valueSignals[j] = preProgrammedValues[currentRow][j];
                        //REPORT_ERROR(MARTe::ErrorManagement::Debug, "Write Value %f at row %d",preProgrammedValues[currentRow][j], currentRow);
                    }
                    //REPORT_ERROR(MARTe::ErrorManagement::Debug, "Writing pre programmed data for time %d", currentTime);
                    
                    // Update row
                    ++currentRow;
                }
                msCounter += preProgrammedExecutaionPeriodMs;
            }
        } break;
        default:
            // Nothing to do.
            REPORT_ERROR(MARTe::ErrorManagement::Warning, "Unhandled mode.");
            break;
    }
    return ok;
}

void JAPreProgrammedGAM::DeleteArrays() {
    if (preProgrammedValues != NULL_PTR(MARTe::float32 **)) {
        MARTe::uint32 i;
        for (i = 0u; i < numberOfPreProgrammedValues; i++) {
            delete preProgrammedValues[i];
        }
        delete[] preProgrammedValues;
    }
    if (preProgrammedTime != NULL_PTR(MARTe::int32 *)) {
        delete[] preProgrammedTime;
    }
}

CLASS_REGISTER(JAPreProgrammedGAM, "1.0")