#include <stdio.h>
#include "controlWare.h"
#include "component.h"
#include "moduleMan.h"

double u[2] = {1.0, 1.0};  /* input */
double v[2] = {0, 0};      /* output */

double a[2] = {0.23, -.5};
double b[2] = {-0.3, 0.8};

int updated = 0;
double input;

typedef struct {
    COMPONENT c;
} SENSOR;

int sensorProc(COMPONENT *pc)
{
    writeOutput(pc, 0, v[1]);
    return 0;
}

MODULE_DESC s_mdSensor = {
    sizeof(SENSOR),
    0, 1 /* output */, 0,
    NULL,
    NULL,
    sensorProc,
    NULL,
    NULL
};

typedef struct {
    COMPONENT c;
} ACTUATOR;

int actuatorProc(COMPONENT *pc)
{   double d;
    
    if (readInput(pc, 0, &d) < 0){
        fprintf(stderr, "component %s readinput error\n", pc->szName);
        return -1;
    }
    input = d;
    updated = 1;
    return 0;
}

MODULE_DESC s_mdActuator = {
    sizeof(ACTUATOR),
    1 /* input */, 0, 0,
    NULL,
    NULL,
    actuatorProc,
    NULL,
    NULL
};

int main(int argc, char *argv[])
{   double out;
   
    if (argc != 2){
        fprintf(stderr, "usage: %s configuration\n", argv[0]);
        return -1;
    }

    if (initControlWare() < 0){
        fprintf(stderr, "initControlWare() error\n");
        return -1;
    }
    
    if (registerModule("dummySensor", &s_mdSensor) < 0 ||
        registerModule("dummyActuator", &s_mdActuator) < 0){
        fprintf(stderr, "registerModule() error\n");
        return -1;
    }
    
    if (startControlWare(argv[1]) < 0){
        fprintf(stderr, "startControlWare() error\n");
        return -1;
    }
    
    while (1){
        if (updated){
            u[0] = u[1]; u[1] = input;
            updated = 0;
        }
        out = a[0] *v[0] + a[1] * v[1] + b[0] * u[0] + b[1] * u[1];
                
        v[0] = v[1]; v[1] = out;
        sleep(1);
    }
}