#include "eyebot.h"
#include <math.h>
#include "npara.h"/*neural net papameters*/
#include "weights.h"/*weights of the neural net*/



double sigmoid(double x){ return 1.0 / (1.0 + (double)exp(-x));}

void feedforward(double N_in[NIN], double N_hid[NHID], double N_out[NOUT], double w_in[NIN][NHID], double w_out[NHID][NOUT])
{ int i,j;

  /* calculate activation of hidden neurons*/
  N_in[NIN-1] = 1.0; /* set bias input neuron*/
  for (i=0; i<NHID-1; i++){
    N_hid[i] = 0.0;
    for (j=0; j<NIN; j++)
      N_hid[i] += N_in[j] * w_in[j][i];
    N_hid[i] = sigmoid(N_hid[i]);
  }
  N_hid[NHID-1] = 1.0; /* set bias hidden neuron*/
  /* calculate activation and output of output neurons*/
  for (i=0; i<NOUT; i++)
  { N_out[i] = 0.0;
    for (j=0; j<NHID; j++)
      N_out[i] += N_hid[j] * w_out[j][i];
    N_out[i] = sigmoid(N_out[i]);
  }
}



int main()
{
  PSDHandle f_psd,l_psd,r_psd;
  VWHandle vw;
  SpeedType speed;

  int key;
  int front,left,right,time;
  double in[NIN],out[NOUT], hid[NHID];/*neural net*/
  key = KEY1;

  /*initialisation of PSDs and V-Omega driving interface*/
  l_psd = PSDInit(PSD_LEFT);
  f_psd = PSDInit(PSD_FRONT);
  r_psd = PSDInit(PSD_RIGHT);
  PSDStart(l_psd|f_psd|r_psd, TRUE);
  vw = VWInit(VW_DRIVE ,1);
  VWStartControl(vw,7,0.3,7,0.1);
  VWSetSpeed(vw,0.0,0.0);
  OSWait(100);

  /*main loop*/
  LCDMenu(" "," "," ","END");
  while (key!=KEY4){

    /*get PSD readings*/
    left  = PSDGet(l_psd);
    front = PSDGet(f_psd);
    right = PSDGet(r_psd);

    /*scale and clip from [90mm..410mm] to [0..1]*/
    if(left  > 410) left=410;
    if(front > 410) front=410;
    if(right > 410) right=410;
    (left  < 90)?(in[0]=0.0):(in[0]=((double)left  - 90.0)/410.0);
    (front < 90)?(in[1]=0.0):(in[1]=((double)front - 90.0)/410.0);
    (right < 90)?(in[2]=0.0):(in[2]=((double)right - 90.0)/410.0);

    /*compute net output*/
    time = OSGetCount();
    feedforward(in, hid, out, w_in, w_out);
    time = OSGetCount() - time;

    /*set speeds*/
    speed.v = out[0]*0.4-0.2;
    speed.w = out[1]*4.0-2.0;
    VWSetSpeed(vw,speed.v,speed.w);

    /*print some information on the LCD screen*/
    LCDSetPos (0, 0);
    VWGetSpeed(vw,&speed);
    LCDPrintf("%2.2f  %2.2f\n%4d  %4d  %4d\n ", speed.v,speed.w ,left,front,right);
    LCDPrintf("NNTime: %d\n",time);
    key=KEYRead();
  }

  /*stop and release V-Omega*/
  VWSetSpeed(vw,0.0,0.0);
  VWStopControl(vw);
  VWRelease(vw);
  return 0;
}