///////////////////////////////////////////////////////////////////////////////
// 
//  PLEASE DO NOT MODIFY
// 
///////////////////////////////////////////////////////////////////////////////

#include "fmi2Template.h"

#ifdef __cplusplus
extern "C" {
#endif

#define MODEL_IDENTIFIER {{identifier}}
#define MODEL_GUID "{{guid}}"

#define NR {{ model.nr }}
#define NX {{ model.nx }}
#define NU {{ model.nu }}
#define NY {{ model.ny }}

// macro to be used to log messages. The macro check if current 
// log category is valid and, if true, call the logger provided by simulator.
#define FILTERED_LOG(instance, status, categoryIndex, message, ...) if (status == fmi2Error || status == fmi2Fatal || isCategoryLogged(instance, categoryIndex)) \
        instance->functions->logger(instance->functions->componentEnvironment, instance->instanceName, status, \
        logCategoriesNames[categoryIndex], message, ##__VA_ARGS__);

typedef struct {
    ModelState state;
    fmi2Real r[{{ model.nr }}];
    fmi2Real time;
    fmi2Char instanceName[256]; // TODO: change 256 by a max_str_len parameter from user argv
    fmi2Type type;
    fmi2String GUID;
    const fmi2CallbackFunctions *functions;
    fmi2Boolean loggingOn;
    fmi2Boolean logCategories[NUMBER_OF_CATEGORIES];
    fmi2ComponentEnvironment componentEnvironment;
    fmi2Boolean isDirtyValues;
} ModelInstance;

static const fmi2String logCategoriesNames[] = {"logAll", "logError", "logFmiCall", "logEvent"};
static ModelInstance instance;

{% if model.has_states() %}
#define _X   (comp->r + {{model.vr0.x}})
#define _DER (comp->r + {{model.vr0.der}})
#define _X0  (comp->r + {{model.vr0.x0}})
{% endif %}
{% if model.has_inputs() %}
#define _U   (comp->r + {{model.vr0.u}})
#define _U0  (comp->r + {{model.vr0.u0}})
{% endif %}
{% if model.has_outputs() %}
#define _Y   (comp->r + {{model.vr0.y}})
{% endif %}

{% if model.has_states() %}
static const fmi2ValueReference vrs_x[{{model.nx}}] = {{ model.vr.x | array2cstr }};
static const fmi2ValueReference vrs_der[{{model.nx}}] = {{model.vr.der | array2cstr}};
{% endif %}
{% if model.has_outputs() %}
static const fmi2ValueReference vrs_y[{{model.ny}}] = {{model.vr.y | array2cstr}};
{% endif %}

{% if model.has_states() %}
static const fmi2Real A[{{model.nx}}][{{model.nx}}] = {{model.A | array2cstr}};
{% endif %}
{% if model.has_states() and model.has_inputs() %}
static const fmi2Real B[{{model.nx}}][{{model.nu}}] = {{model.B | array2cstr}};
{% endif %}
{% if model.has_states() and model.has_outputs() %}
static const fmi2Real C[{{model.ny}}][{{model.nx}}] = {{model.C | array2cstr}};
{% endif %}
{% if model.has_inputs() and model.has_outputs() %}
static const fmi2Real D[{{model.ny}}][{{model.nu}}] = {{model.D | array2cstr}};
{% endif %}
{% if model.has_states() %}
static const fmi2Real x0_reset[{{model.nx}}] = {{model.x0 | array2cstr}};
{% endif %}
{% if model.has_inputs() %}
static const fmi2Real u0_reset[{{model.nu}}] = {{model.u0 | array2cstr}};
{% endif %}

#ifndef max
#define max(a,b) ((a)>(b) ? (a) : (b))
#endif

#ifndef min
#define min(a,b) ((a)>(b) ? (b) : (a))
#endif

///////////////////////////////////////////////////////////////////////////////
// Private functions
///////////////////////////////////////////////////////////////////////////////
static fmi2Boolean isCategoryLogged(ModelInstance *comp, int categoryIndex) {
    if (categoryIndex < NUMBER_OF_CATEGORIES
        && (comp->logCategories[categoryIndex] || comp->logCategories[LOG_ALL])) {
        return fmi2True;
    }
    return fmi2False;
}

static fmi2Boolean isInvalidState(ModelInstance *comp, const char *f, int statesExpected) {
    if (!comp)
        return fmi2True;
    if (!(comp->state & statesExpected)) {
        comp->state = modelError;
        FILTERED_LOG(comp, fmi2Error, LOG_ERROR, "%s: Illegal call sequence.", f)
        return fmi2True;
    }
    return fmi2False;
}

static fmi2Boolean isNullPtr(ModelInstance* comp, const char *f, const char *arg, const void *p) {
    if (!p) {
        comp->state = modelError;
        FILTERED_LOG(comp, fmi2Error, LOG_ERROR, "%s: Invalid argument %s = NULL.", f, arg)
        return fmi2True;
    }
    return fmi2False;
}

static fmi2Boolean isVROutOfRange(ModelInstance *comp, const char *f, fmi2ValueReference vr, int end) {
    if (vr >= end) {
        FILTERED_LOG(comp, fmi2Error, LOG_ERROR, "%s: Illegal value reference %u.", f, vr)
        comp->state = modelError;
        return fmi2True;
    }
    return fmi2False;
}

static fmi2Boolean isInvalidNumber(ModelInstance *comp, const char *f, const char *arg, int n, int nExpected) {
    if (n != nExpected) {
        comp->state = modelError;
        FILTERED_LOG(comp, fmi2Error, LOG_ERROR, "%s: Invalid argument %s = %d. Expected %d.", f, arg, n, nExpected)
        return fmi2True;
    }
    return fmi2False;
}

static fmi2Status unsupportedFunction(fmi2Component c, const char *fName, int statesExpected) {
    ModelInstance *comp = (ModelInstance *)c;
    //fmi2CallbackLogger log = comp->functions->logger;`
    if (isInvalidState(comp, fName, statesExpected))
        return fmi2Error;
    FILTERED_LOG(comp, fmi2OK, LOG_FMI_CALL, fName);
    FILTERED_LOG(comp, fmi2Error, LOG_ERROR, "%s: Function not implemented.", fName)
    return fmi2Error;
}

/**
 * \brief Vector inner product 
 */
static fmi2Real innerProduct(const fmi2Real *v1, const fmi2Real *v2, const size_t n) {
    size_t i = 0.0;
    fmi2Real ret = 0.0;
    for (i = 0; i < n; ++i){
        ret += v1[i]*v2[i];
    }
    return ret;
}

{% if model.has_states() %}
/**
 *  \brief Update derivative values
 */
static void updateDerivatives(ModelInstance* comp){
    size_t i = 0;
    for (i = 0; i < NX; i++) {
        fmi2ValueReference der_i = vrs_der[i];
        comp->r[der_i] = innerProduct(A[i], _X, NX);
{% if model.has_inputs() %}
        comp->r[der_i] += innerProduct(B[i], _U, NU);
{% endif %}
    }
}

/**
 *  \brief Update states values using numerical integration
 *  \todo TODO: Implement other integration methods
 */
static void updateStates(ModelInstance* comp, fmi2Real h){
    const fmi2Real dt = min({{ dt }}, h);
    size_t i = 0;
    for (i = 0; i < NX; i++) {
        fmi2Real hc = h;
        fmi2ValueReference x_i = vrs_x[i];
        fmi2ValueReference der_i = vrs_der[i];
        // Update states using forward Euler
        while(hc > 0) {
            comp->r[x_i] += min(dt, hc) * comp->r[der_i];
            hc -= dt;
        }
    }
}
{% endif %}

{% if model.has_outputs() %}
/**
 * \brief Update output values based on current state
 */
static void updateOutputs(ModelInstance* comp) {
    size_t i = 0;
    for (i = 0; i < NY; i++){
        fmi2ValueReference y_i = vrs_y[i];
        r(y_i) = 0;
{% if model.has_states() %}
        r(y_i) += innerProduct(C[i], _X, NX);
{% endif %}
{% if model.has_inputs() %}
        r(y_i) += innerProduct(D[i], _U, NU);
{% endif %}
    }
}
{% endif %}

{% if model.has_states() %}
static void copyX0toX(ModelInstance* comp){
    memcpy(_X, _X0, NX*sizeof(fmi2Real));
}

/**
 * \brief ReSet state initial conditions to original values
 */
static void resetX(ModelInstance* comp){
    memcpy(_X0, x0_reset, NX*sizeof(fmi2Real));
    copyX0toX(comp);
}
{% endif %}

{% if model.has_inputs() %}
static void copyU0toU(ModelInstance* comp) {
    memcpy(_U, _U0, NU*sizeof(fmi2Real));
}

/**
 * \brief ReSet input initial conditions to original values
 */
static void resetU(ModelInstance* comp) {
    memcpy(_U0, u0_reset, NU*sizeof(fmi2Real));
    copyU0toU(comp);
}
{% endif %}

static void evaluate(ModelInstance* comp){
{% if model.has_states() %}
    updateDerivatives(comp);
{% endif %}
{% if model.has_outputs() %}
    updateOutputs(comp);
{% endif %}
}

#include "fmi2Template.c"

#ifdef __cplusplus
}
#endif