#include "eyebot.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "mydef.h"
#include "PathSearch.h"
#include "npara.h"
#include "weights.h"
#define THRESH 250
#define MAXSTEPS 10000
PSDHandle psd[4];
int stop=0;

/*required by the neural net*/
double sigmoid(double x)
{ return 1.0 / (1.0 + (double)exp(-x));
}

/*neural net (multi layer perceptron with 1 hidden layer)*/
void feedforward(double N_in[NIN], double N_hid[NHID], double N_out[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]);
  }
}

/*displayes the maze with path on display*/
void ShowMaze(int m[X_MAZE][Y_MAZE]){
  int i,j;
  LCDSetPos(0,0);
  for(i=0;i<6;i++){
    for(j=0;j<12;j++){
      if(m[i][j]<=0) {
        switch(m[i][j]){
          case 0:
            LCDPrintf(">");
            break;
          case -1:
            LCDPrintf("^");
            break;
          case -2:
            LCDPrintf("<");
            break;
          case -3:
            LCDPrintf(",");
            break;
        }

      } else LCDPrintf("%c",0x23);

    }
    LCDPrintf("\n");
  }
}



/*callback that checks for abortion KEY*/
void KeyCheck(){
  if(KEYRead() == KEY4) stop=1;
}

/*gets the goal koordinates interactivly*/
void GetGoal(MazePositionType *g){
  int key;
  g->x = 1;
  g->y = 1;
  LCDClear();
  LCDMenu("+","-","ENT","END");
  LCDPrintf("Goal Koordinates\n");
  key = -1;
  while(key!=KEY3 && !stop){
    key = KEYRead();
    LCDSetPos(1,0);
    LCDPrintf("X: %2.2d",g->x);
    if(key==KEY1){ if((g->x)<4){ (g->x)++;} else {(g->x)=1;}}
    if(key==KEY2){ if((g->x)>1){ (g->x)--;} else {(g->x)=4;}}
    if(key==KEY4) stop = 1;
  }
  key = -1;
  while(key!=KEY3 && !stop){
    key = KEYRead();
    LCDSetPos(2,0);
    LCDPrintf("Y: %2.2d",g->y);
    if(key==KEY1){ if((g->y)<8){ (g->y)++;} else{ (g->y)=1;}}
    if(key==KEY2){ if((g->y)>1){ (g->y)--;} else{ (g->y)=8;}}
    if(key==KEY4) stop = 1;
  }
  LCDSetPos(4,3);
  LCDPrintf("Ready");
  OSWait(100);
  LCDClear();
}

/*erases the maze*/
void EraseMaze(int m[X_MAZE][Y_MAZE]){
  int i,j;
  /*because sensing the maze is not working properly the maze is given*/
  int empty_maze[X_MAZE][Y_MAZE]={
  { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
  { 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1},
  { 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 1},
  { 1, 0, 1, 0, 1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1},
  { 1, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1},
  { 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1},
  { 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1},
  { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
  { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}
  };
  for(i=0;i<X_MAZE;i++)
    for(j=0;j<Y_MAZE;j++)
      m[i][j]=empty_maze[i][j];
}


/*gets the position and orientation in the maze*/
void GetPos(VWHandle h,MazePositionType *p){
  /*converts VWPositionType into MazePositionType (to do)*/
  float value;
  PositionType position;
  VWGetPosition(h,&position);


  p->x = (int)((position.y-0.54)/-0.36);
  p->y = (int)((position.x+0.54)/0.36);

  value = (position.phi/3.1415926535);
  if(value<0.25 && value>-0.25) p->ori = 0;
  else if(value>0.75 || value <-0.75) p->ori = 2;
  else if(value<0.0) p->ori = 3;
  else p->ori = 1;
}


/*
2 versions of sensing the layout of the maze
both not working as they should
*/
/*
int Sense(VWHandle h,int m[X_MAZE][Y_MAZE],MazePositionType p){

  //updates the maze after robot has moved and differs between
  //no changes 0, new wall 1

  int f,l,b,r;
  int orientation[4];
  int i,j;
  int mm[X_MAZE][Y_MAZE];
  OSWait(2);
  // VWGetPosition(h,&pos);
  //switch(p.ori){
  //   case 0: VWSetPosition(h,pos.x,pos.y, 0.0);
  //   case 1: VWSetPosition(h,pos.x,pos.y, 3.1415926535/2.0);
  //   case 2: VWSetPosition(h,pos.x,pos.y, 3.1415926535);
  //   case 3: VWSetPosition(h,pos.x,pos.y, 3.1415926535/(-2.0));
  //}
  //VWDriveWait(h);

  f = PSDGet(psd[0]);
  l = PSDGet(psd[1]);
  b = 500;//back PSD not active
  r = PSDGet(psd[3]);



  orientation[p.ori] = f;
  orientation[(p.ori+1)%4] = l;
  orientation[(p.ori+2)%4] = b;
  orientation[(p.ori+3)%4] = r;



  for(i = 0;i<X_MAZE;i++)
    for(j = 0;j<Y_MAZE;j++)
      if(m[i][j]<0) m[i][j]=0;
      mm[i][j]=m[i][j];


  if(orientation[0]<THRESH) for(i = -1;i<=1;i++) m[p.x+i][p.y+1]=1;

  if(orientation[1]<THRESH) for(i = -1;i<=1;i++) m[p.x-1][p.y+i]=1;

  if(orientation[2]<THRESH) for(i = -1;i<=1;i++) m[p.x+i][p.y-1]=1;

  if(orientation[3]<THRESH) for(i = -1;i<=1;i++) m[p.x+1][p.y+i]=1;

  for(i = 0;i<X_MAZE;i++)
    for(j = 0;j<Y_MAZE;j++)
      if(m[i][j]-mm[i][j]){ return 1;};


  return 0;

}

int Sense2(){

  int f,l,b,r;
  float x,y;
  int i,j;
  PositionType position;
  f = PSDGet(psd[0]);
  l = PSDGet(psd[1]);
  b = 500;//back PSD not active
  r = PSDGet(psd[3]);
  VWGetPosition(vw,&position);
  if(f<THRESH){
    x = ((-1)*position.y) + (sin((-1.0)*position.phi)*0.001*(float)f);
    y = (position.x) + (cos((-1.0)*position.phi)*0.001*(float)f);
    i = (int)((x+0.27)/0.18);
    j = (int)((y+0.27)/0.18);
    m[i][j] = 1;
  }
  if(l<THRESH){
    position.phi = position.phi + 1.57079633;
    x = ((-1)*position.y) + (sin((-1.0)*position.phi)*0.001*(float)l);
    y = (position.x) + (cos((-1.0)*position.phi)*0.001*(float)l);
    i = (int)((x+0.27)/0.18);
    j = (int)((y+0.27)/0.18);
    m[i][j] = 1;
  }
  if(r<THRESH){
    position.phi = position.phi - 3.14159265;
    x = ((-1)*position.y) + (sin((-1.0)*position.phi)*0.001*(float)r);
    y = (position.x) + (cos((-1.0)*position.phi)*0.001*(float)r);
    i = (int)((x+0.27)/0.18);
    j = (int)((y+0.27)/0.18);
    m[i][j] = 1;
  }
  return 0;
}
*/


/*switches between agents and makes the robot drive as required by the aktual maze position*/
int TakeAction(VWHandle h,int m[X_MAZE][Y_MAZE],MazePositionType p){

  int left,front,right;
  double in[NIN],hid[NHID],out[NOUT];
  double v,w;
  LCDSetPos(0,0);
  if(VWStalled(h)) return 0;
  if(p.ori+m[p.x][p.y]==0){
     /*
     agent for driving straight
     this is done bby a neural net
     */
     left  = PSDGet(psd[1]);
     front = PSDGet(psd[0]);
     right = PSDGet(psd[3]);
     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);
     feedforward(in, hid, out);
     v = out[0]*0.4-0.2;
     w = out[1]*4.0-2.0;
     VWSetSpeed(h,v,w);
  }else{
    /*
    agent for turning
    simple routine that turns the robot roughly into the required direction
    */
    if(p.ori+m[p.x][p.y]==(-1) || p.ori+m[p.x][p.y]==3) VWSetSpeed(h,0.0,1.0);
    else VWSetSpeed(h,0.0,-1.0);
  }
  return 1;

}



int main(void){

  /*definitions*/
  MazePositionType goal,mgoal;
  MazePositionType oldpos;
  int out;/*return value of integer functions*/
  int steps;/*steps made to reach Goal*/
  int move,err;
  int maze[X_MAZE][Y_MAZE];
  MazePositionType pos,mpos;
  VWHandle vw;
  TimerHandle t1;


  /*init*/
  t1 = OSAttachTimer(20,KeyCheck);
  EraseMaze(maze);
  vw = VWInit(VW_DRIVE,1);
  VWStartControl(vw,7,0.3,7,0.1);
  VWSetSpeed(vw,0.0,0.0);
  psd[0] = PSDInit(PSD_FRONT);
  psd[1] = PSDInit(PSD_LEFT);
  psd[2] = PSDInit(PSD_FRONT2);
  psd[3] = PSDInit(PSD_RIGHT);
  err = PSDStart(psd[0]|psd[1]|psd[2]|psd[3], TRUE);
  if (err != 0) { printf("Error starting PSDs\n"); return 1; }
  OSWait(10);


  /*get goal coordinates*/
  GetGoal(&goal);
  mgoal.x = goal.x*2-1;
  mgoal.y = goal.y*2-1;
  mgoal.ori = goal.ori;
  GetPos(vw,&pos);
  mpos.x = pos.x*2-1;
  mpos.y = pos.y*2-1;
  mpos.ori = pos.ori;
  /*while not ended*/
  while(!stop){
    steps = 0;
    while(!stop && !((pos.x == goal.x) && (pos.y == goal.y)) ){

      /* Here should be the step where the layout of the maze sensed
      ....
      */
      GetPos(vw,&pos);
      oldpos.x=pos.x;
      oldpos.y=pos.y;
      oldpos.ori=pos.ori;
      mpos.x = pos.x*2-1;
      mpos.y = pos.y*2-1;
      mpos.ori = pos.ori;
      /*
      the representation of the maze used for planning has twice the resolution. that is why
      position and goal must be converted before planning the path.
      */
      out = PathSearch(maze,mpos,mgoal);
      if(out==0) stop = 1;

      /*display the maze and actual position*/
      ShowMaze(maze);
      LCDPrintf("x:%d y:%d ori:%d",pos.x,pos.y,pos.ori);

      /*
      drive as long as next maze position is not reached.
      old action is executed 12 more times to move completly into next square
      */
      move = 12;
      while(!stop && move){
        TakeAction(vw,maze,mpos);
        GetPos(vw,&pos);
        LCDSetPos(7,0);
        LCDPrintf("x:%2d y:%2d o:%2d",pos.x,pos.y,pos.ori);
        mpos.ori=pos.ori;
        /*if next position reached*/
        if(((pos.x-oldpos.x)!=0 || (pos.y-oldpos.y)!=0)) {
          move --;/*count down*/
          LCDSetPos(7,0);
          LCDPrintf("position reached\n");
          OSWait(1);
        }
      }
      VWSetSpeed(vw,0.0,0.0);

    }
    stop = 1;
  }
  LCDClear();
  LCDSetPos(3,0);
  if(out == 0) LCDPrintf("  there is no\n  path to goal\n    position");else LCDPrintf("  goal reached");
  /*release*/
  VWSetSpeed(vw,0.0,0.0);
  VWStopControl(vw);
  VWRelease(vw);
  PSDRelease();
  OSDetachTimer(t1);
  return 1;
}