/* Bifur bot.  This bot turns away of walls by exploiting a bifurcation of 
    a dynamical system
    
    Created by JuanPi Carbajal
    carbajal@ifi.uzj.ch
    Wednesday, November 11 2009
*/

#include <math.h>
#include "xpilot_ai.h"
#include <stdio.h>

// AI parameters
#define MAXRANGE 200
#define GAIN 180
#define FEEL_ANG 30
#define ANG_THRS .55

/*Function Prototypes */
int feeler(int range, int degree);

/* Main */
int main (int argc, char **argv) {
	/*set max turn speed to 20 degrees per frame*/
	/*this value is related to the integration step*/
	/* Too big woudl create weird behavior*/
	AI_setmaxturn (20);

	/* launch xpilot with a ship named "Wallbot" */
	AI_xpilot_setargs("-join localhost -name Bifurbot");
	AI_xpilot_launch();

	return 1;
}	

/*feeler checks for walls at some degree and range from self ship */
int feeler(int range,int degree) {
    
    int x0=AIself_x();
    int y0=AIself_y();
    float dx=range * cos( rad (degree));
    float dy=range * sin( rad (degree));
    
    int res = AI_wallbetween(x0,y0,x0+dx,y0+dy);
	
	if (res == -1) 
		return range;
		
    return res;
}

void AImain (void) {
 int vel_angle=AIself_track(); //direction of motion
 int heading=AIself_heading(); // heading
 float feeler_ang[3],dist[3]; // angle of feelers and distances to walls
 float turn_angle=0.0; // Angle to turn
 float delta_ang=anglediff(heading,vel_angle)/180.0; //Normalized difference 
                                                     // between heading and 
                                                     // direction
 
// The ships turns according to the distance to the wall 
// Gets the distance from three feelers [-FEEL_ANG, 0, FEEL_ANG]
// and do something to fusion the info from feelers: minimum in this case

// Set morphology
 feeler_ang[0]=FEEL_ANG; feeler_ang[1]=0; feeler_ang[2]=-feeler_ang[0]; 

// Get the info 
 float obs_dist=1.5;
 for(int i=0; i<3;i++)
 {
    dist[i]=feeler(MAXRANGE,vel_angle+feeler_ang[i])/(float)MAXRANGE;
    if(dist[i]<obs_dist)
       obs_dist=dist[i];
  }
// Calculate the turn
 turn_angle=GAIN*(delta_ang+(1.0-pow(obs_dist,2)));

// Debug info 
// printf("dist: %g\n",obs_dist);
// printf("delta_ang: %g turn_angle:%g\n",delta_ang,turn_angle);

// Execute AI
 AIself_turn((int)turn_angle); //turn

 if (fabs(delta_ang) >= ANG_THRS) // If we are OK, trhust your way away from the wall
 {
     AIself_thrust(1);
 }   
 return;
}