#include<assert.h>
#include<iostream>
#include<vector>
using namespace std;
#include "selforg/sinecontroller.h"
#include "selforg/invertmotorspace.h"
const int MNumber = 2;
const int SNumber = 2;
void myrobot(sensor* sensors, int sensornumber, const motor* motors, int motornumber){
assert(sensornumber >= 2 && motornumber >= 2);
sensors[0]=motors[0]+(double(rand())/RAND_MAX-0.5)*0.3;
sensors[1]=motors[1]+(double(rand())/RAND_MAX-0.5)*0.3;
}
int main(){
AbstractController* controller = new InvertMotorSpace(10);
controller->init(2,2);
controller->setParam("epsA",0.01);
controller->print(stderr,0);
sensor sensors[SNumber];
motor motors[MNumber];
memset(motors,0,sizeof(motor)*MNumber);
for(int i=0; i < 1000; i++){
myrobot(sensors, SNumber, motors, MNumber);
cout << i << " S0: " << sensors[0] << ", " <<" S1: " << sensors[1];
list<Inspectable::iparamval> list_ = controller->getInternalParams();
vector<Inspectable::iparamval> v(list_.begin(), list_.end());
cout << i << " C00: " << v[4] << ", " <<" C01: " << v[5] << ", " <<
" C10: " << v[6] << ", " <<" C11: " << v[7] << endl;
controller->step(sensors, SNumber, motors, MNumber);
cout << i << " Motor values: " << motors[0] << ", " << motors[1] << endl;
}
delete controller;
return 0;
}