/*
 * Include Files
 *
 */
#if defined(MATLAB_MEX_FILE)
#include "tmwtypes.h"
#include "simstruc_types.h"
#else
#include "rtwtypes.h"
#endif

/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */
#include <math.h>
#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sstream>
#include <iostream>

#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string.hpp>


/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */
#define u_width 1
#define y_width 1
/*
 * Create external references here.
 *
 */
/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */
/* extern double func(double a); */

int inline openOutputPipe(const std::string& outputName)
{
    const int readFlags = O_WRONLY;

    std::cout << "Waiting for a client to write..." << std::endl;
    int outputPipeFD = open(outputName.c_str(), readFlags);
    if (outputPipeFD < 0)
    {
        throw new std::runtime_error("Error: open( ): for Input Pipe ");
    }



    return outputPipeFD;
}

class GlobalWrapper
{
public:
    bool initialized;
    int inputPipe;
    int outputPipe;
    char inputBuffer[1024];
    GlobalWrapper() :
            initialized(false)
    {
    }
};

GlobalWrapper gVars;

#undef MDL_START  /* Change to #undef to remove function */

inline void init(const int processId)
{
    if(!gVars.initialized)
    {

        const int readFlags = O_RDONLY;

        std::stringstream inputPipeNameSS;
        inputPipeNameSS << std::string("/home/bewo/Hiwi/geneial_strategy/build/output.") << processId << std::string(".fifo");
        const std::string inputPipeName = inputPipeNameSS.str();

        std::cout << "Opening Input Pipe..." << std::endl;
        gVars.inputPipe = open(inputPipeName.c_str(),readFlags);

        if(gVars.inputPipe < 0)
        {
            std::cout << "Error Creating Input Pipe." << std::endl;
        }


        std::stringstream outputPipeNameSS;
        outputPipeNameSS << std::string("/home/bewo/Hiwi/geneial_strategy/build/input.") << processId << std::string(".fifo");
        const std::string outputPipeName = outputPipeNameSS.str();

        gVars.outputPipe= openOutputPipe(outputPipeName);


        //Initialization

        gVars.initialized = true;


    }

}



/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */

/*
 * Output functions
 *
 */
void ga_strategy_01_Outputs_wrapper(const real_T *t_req_act,
			const real_T *wheel_ang_vel,
			const real_T *gear_ratio,
			const real_T *velocity,
			const real_T *fuel_cons_abs,
			const real_T *soc,
			const real_T *fuel_consumption,
			const real_T *time,
			real_T *split)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */

    const int processId = 1;

    init(processId); //TODO (bewo): Get ProcessId from Matlab input?
    assert(gVars.initialized);

    std::stringstream ss;
    const std::string delimiter(";");

    ss << *t_req_act << delimiter
       << *wheel_ang_vel << delimiter
       << *gear_ratio << delimiter
       << *velocity << delimiter
       << *fuel_cons_abs << delimiter
       << *soc << delimiter
       << *fuel_consumption << delimiter
       << *time;
    const auto output = ss.str();
    const char* rawStr =output.c_str();


    std::cout <<"Open Pipe " << std::endl;
    write(gVars.outputPipe, rawStr, strlen(rawStr));

    std::cout <<"inputPipe" << gVars.inputPipe << std::endl;

    int bytes = read(gVars.inputPipe, gVars.inputBuffer, 1023);
    std::cout << "Read " << bytes << std::endl;
    gVars.inputBuffer[bytes] = '\0';

    std::cout <<"Read:" << gVars.inputBuffer <<std::endl;
    std::vector<std::string> numbers;
    std::vector<double> outputs;



    boost::split(numbers, gVars.inputBuffer, boost::is_any_of(";"));

    for (const auto & number : numbers) {
        //std::cout << "Converting " << number << std::endl;
        try
        {
            outputs.push_back(boost::lexical_cast<double>(number));
        }
        catch(const boost::bad_lexical_cast &)
        {
            std::cout << "Cannot Convert " << number << std::endl;
        }
    }

    if(outputs.size() != 1)
    {
        std::cout << "Was expecting 1 values, got " << outputs.size() << std::endl;
    }

    *split = outputs[0];

/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}