File content as of revision 52:54b3fe68a4f2:

#include "mbed.h"
#include "robot.h" // Initializes the robot. This include should be used in all main.cpp!
#include "math.h"

using namespace std;

//update angularLeft and angularRight
float get_error_angles(float x, float y,float theta);
void test_procedure_Lab_2();
void procedure_Lab_3();
void procedure_Lab_2();
void turn_to_target(float angleToTarget);
void initialise_parameters();
//fill initialLogValues with the values we already know (here the bordurs)
void fill_initial_log_values();
//generate a position randomly and makes the robot go there while updating the map
void randomize_and_map();
//make the robot do a pi/2 flip
void do_half_flip();
//go the the given position while updating the map
void go_to_point_with_angle(float target_x, float target_y, float target_angle);
void compute_angles_and_distance(float target_x, float target_y, float target_angle);
void compute_linear_angular_velocities();
//Updates sonar values 
void update_sonar_values(float leftMm, float frontMm, float rightMm);
//function that check if a cell A(x,y) is in the range of the front sonar S(xs,ys) (with an angle depending on the sonar used, front 0, left pi/3, right -pi/3) returns the probability it's occupied/empty [0;1]
float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected);
//print the map
void print_final_map();
//print the map with the robot marked on it
void print_final_map_with_robot_position();
//print the map with the robot and the target marked on it
void print_final_map_with_robot_position_and_target();
//go to a given line by updating angularLeft and angularRight
void go_to_line(float* angularLeft,float* angularRight,float line_a, float line_b, float line_c);
//calculate virtual force field and move
void vff(bool* reached);
void test_got_to_line(bool* reached);
//Go to a given X,Y position, regardless of the final angle
void go_to_point(float target_x, float target_y);
//go to line and follow it, from lab 1
void go_to_line_lab_1(float line_a, float line_b, float line_c);

//MATHS heavy functions
float dist(float robot_x, float robot_y, float target_x, float target_y);
float distFromLine(float robot_x, float robot_y, float line_a, float line_b, float line_c);
//returns the probability [0,1] that the cell is occupied from the log value lt
float log_to_proba(float lt);
//returns the log value that the cell is occupied from the probability value [0,1]
float proba_to_log(float p);
//returns the new log value
float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue );
//makes the angle inAngle between 0 and 2pi
float rad_angle_check(float inAngle);
//returns the angle between the vectors (x,y) and (xs,ys)
float compute_angle_between_vectors(float x, float y,float xs,float ys);
float x_robot_in_orthonormal_x(float x, float y);
float y_robot_in_orthonormal_y(float x, float y);
float robot_center_x_in_orthonormal_x();
float robot_center_y_in_orthonormal_y();
float robot_sonar_front_x_in_orthonormal_x();
float robot_sonar_front_y_in_orthonormal_y();
float robot_sonar_right_x_in_orthonormal_x();
float robot_sonar_right_y_in_orthonormal_y();
float robot_sonar_left_x_in_orthonormal_x();
float robot_sonar_left_y_in_orthonormal_y();
float estimated_width_indice_in_orthonormal_x(int i);
float estimated_height_indice_in_orthonormal_y(int j);
//update foceX and forceY if necessary
void update_force(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho );
//compute the X and Y force
void compute_forceX_and_forceY(float* forceX, float* forceY);
//robotX and robotY are from Odometria, calculate line_a, line_b and line_c
void calculate_line(float forceX, float forceY, float robotX, float robotY,float *line_a, float *line_b, float *line_c);
//return 1 if positiv, -1 if negativ
float sign1(float value);
//return 1 if positiv, 0 if negativ
int sign2(float value);
//set target in ortho space, in reference to the robot (so if the robot is in the middle, you want to him to go 10cm down and 15 right, set_target_to(15,-10)
void set_target_to(float x, float y);
void try_to_reach_target();
void test_map_sonar();

//those target are in comparaison to the robot (for exemple a robot in 50,50 with a taget of 0,0 would not need to move)
float targetX;//this is in the robot space top left
float targetY;//this is in the robot space top left
//Ortho but for the map (i.e x and Y are > 0)
float targetXOrhto;
float targetYOrhto;
float targetXOrhtoNotMap;
float targetYOrhtoNotMap;

const float pi=3.14159;

//spec of the sonar
//TODO MEASURE THE DISTANCE on X and Y of the robot space, between each sonar and the center of the robot and add it to calculus in updateSonarValues
const float RANGE_SONAR=50;//cm
const float RANGE_SONAR_MIN=10;//Rmin cm
const float INCERTITUDE_SONAR=10;//cm
const float ANGLE_SONAR=pi/3;//Omega rad

//those distance and angle are approximation in need of measurements, in the orthonormal space
const float ANGLE_FRONT_TO_LEFT=10*pi/36;//50 degrees
const float DISTANCE_SONAR_LEFT_X=-4;
const float DISTANCE_SONAR_LEFT_Y=4;

const float ANGLE_FRONT_TO_RIGHT=-10*pi/36;//-50 degrees
const float DISTANCE_SONAR_RIGHT_X=4;
const float DISTANCE_SONAR_RIGHT_Y=4;

const float ANGLE_FRONT_TO_FRONT=0;
const float DISTANCE_SONAR_FRONT_X=0;
const float DISTANCE_SONAR_FRONT_Y=5;

//TODO adjust the size of the map for computation time (25*25?)
const float WIDTH_ARENA=200;//cm
const float HEIGHT_ARENA=200;//cm

//const int SIZE_MAP=25;
const int NB_CELL_WIDTH=20;
const int NB_CELL_HEIGHT=20;

//used to create the map 250 represent the 250cm of the square where the robot is tested
//float sizeCell=250/(float)SIZE_MAP;
float sizeCellWidth=WIDTH_ARENA/(float)NB_CELL_WIDTH;
float sizeCellHeight=HEIGHT_ARENA/(float)NB_CELL_HEIGHT;

float map[NB_CELL_WIDTH][NB_CELL_HEIGHT];//contains the log values for each cell
float initialLogValues[NB_CELL_WIDTH][NB_CELL_HEIGHT];

//Diameter of a wheel and distance between the 2
const float RADIUS_WHEELS=3.25;
const float DISTANCE_WHEELS=7.2;

//position and orientation of the robot when put on the map (ODOMETRY doesn't know those) it's in the robot space
//this configuration suppose that the robot is in the middle of the arena facing up (to be sure you can use print_final_map_with_robot_position
const float DEFAULT_X=20;
const float DEFAULT_Y=WIDTH_ARENA/2;
//const float DEFAULT_X=20;//lower right
//const float DEFAULT_Y=20;//lower right
const float DEFAULT_THETA=0;

const int MAX_SPEED=50;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP

const float KRHO=12, KA=30, KB=-13, KV=200, KH=200; //Kappa values

//CONSTANT FORCE FIELD
const float FORCE_CONSTANT_REPULSION=10000;//TODO tweak it
const float FORCE_CONSTANT_ATTRACTION=1;//TODO tweak it
const float RANGE_FORCE=30;//TODO tweak it

float alpha; //angle error
float rho; //distance from target
float beta;
float linear, angular, angular_left, angular_right;
float dt=0.5;
float temp;
float d2;
Timer t;

int main(){
    initialise_parameters();   
    procedure_Lab_3();                      //uses force fields to reach target with set_terget
    //procedure_Lab_2();                    //picks random targets and makes a map (SUCCESS - builds a more or less accurate map without colliding with obstacles)
    
    //theta=0;
    //X=0;
    //Y=0;
    
    //go_to_point(16.8, 78.6);              //(x,y) in the global coordinates x cm on the x direction, y cm in the y direction form the 0,0 of the table 
                                            //(SUCCESS - goes to the specified point)
    
    //go_to_line_lab_1(10, -10, 20);        //a,b,c of a line: ax+by+c=0, again on the global coordinates of the table (SUCCESS - goes along the line)
    
    //go_to_point_with_angle(46.8, 78.6, 0);//(x,y,theta) in the global coordinates (SUCCESS - goes to the point, the angle is almost right as well)
    
    //test_procedure_Lab_2();               //starts from the middle of the arena, goes to a point with set_terget
}

void procedure_Lab_3(){
    //try to reach the target is ortho space  
    set_target_to(0,80);//
    print_final_map_with_robot_position_and_target();
    try_to_reach_target();
    //set_target_to(0,20);//lower right
    //print_final_map_with_robot_position_and_target();
    //try_to_reach_target();
    //set_target_to(-20,-20);//up left
    //print_final_map_with_robot_position_and_target();
    //try_to_reach_target();
    //print the map forever
    while(1){
         print_final_map_with_robot_position_and_target();
    }
}

void test_map_sonar(){
    float leftMm;
    float frontMm;
    float rightMm;
    X=20;
    Y=WIDTH_ARENA/2;
    theta=0;
    while(1){
        leftMm = get_distance_left_sensor();
        frontMm = get_distance_front_sensor();
        rightMm = get_distance_right_sensor();
        pc.printf("%f, %f, %f",leftMm,frontMm,rightMm);
        //update the probabilities values 
        update_sonar_values(leftMm, frontMm, rightMm);
        print_final_map();
    }
}

void test_procedure_Lab_2(){
    X=HEIGHT_ARENA/2;
    Y=WIDTH_ARENA/2;
    set_target_to(-100,50);
    print_final_map_with_robot_position_and_target();
    go_to_point_with_angle(targetX, targetY, pi/2);
    print_final_map_with_robot_position_and_target();
    
}
void procedure_Lab_2(){
    for(int i=0;i<15;i++){
        randomize_and_map();
        print_final_map_with_robot_position();
        wait(2);
    }
    while(1){
        print_final_map_with_robot_position();
        wait(10);
    }
}


void try_to_reach_target(){
    //atan2 gives the angle beetween pi and -pi
    float angleToTarget=atan2(targetYOrhtoNotMap,targetXOrhtoNotMap);
    turn_to_target(angleToTarget);
    bool reached=false;
    int print=0;
    while (!reached) {
        vff(&reached);
        //test_got_to_line(&reached);
        if(print==10){
            leftMotor(1,0);
            rightMotor(1,0);
            /*
            pc.printf("\r\n theta=%f", theta);
            pc.printf("\r\n IN ORTHO:");
            pc.printf("\r\n X Robot=%f", robot_center_x_in_orthonormal_x());
            pc.printf("\r\n Y Robot=%f", robot_center_y_in_orthonormal_y());
            pc.printf("\r\n X Target=%f", targetXOrhto);
            pc.printf("\r\n Y Target=%f", targetYOrhto);
            */
            print_final_map_with_robot_position_and_target();
            print=0;
        }else
            print+=1;
    }
    //Stop at the end
    leftMotor(1,0);
    rightMotor(1,0);
    pc.printf("\r\n target reached");
    wait(3);//
}

//target in ortho space
void set_target_to(float x, float y){
    targetX=y;
    targetY=-x;
    targetXOrhto=x_robot_in_orthonormal_x(targetX,targetY);
    targetYOrhto=y_robot_in_orthonormal_y(targetX,targetY);
    targetXOrhtoNotMap=x;
    targetYOrhtoNotMap=y;
}

void initialise_parameters(){
    i2c1.frequency(100000);
    initRobot(); //Initializing the robot
    pc.baud(9600); // baud for the pc communication

    measure_always_on();//TODO check if needed
    wait(0.5);
    //fill the map with the initial log values
    fill_initial_log_values();

    theta=DEFAULT_THETA;
    X=DEFAULT_X;
    Y=DEFAULT_Y;
}

//fill initialLogValues with the values we already know (here the bordurs)
void fill_initial_log_values(){
    //Fill map, we know the border are occupied
    for (int i = 0; i<NB_CELL_WIDTH; i++) {
        for (int j = 0; j<NB_CELL_HEIGHT; j++) {
            if(j==0 || j==NB_CELL_HEIGHT-1 || i==0 || i==NB_CELL_WIDTH-1)
                initialLogValues[i][j] = proba_to_log(1);
            else
                initialLogValues[i][j] = proba_to_log(0.5);
        }
    }
}

//generate a position randomly and makes the robot go there while updating the map
void randomize_and_map() {
    //TODO check that it's aurelien's work
    float movementOnX=rand()%(int)(WIDTH_ARENA/2);
    float movementOnY=rand()%(int)(HEIGHT_ARENA/2);
    
    float signOfX=rand()%2;
    if(signOfX < 1)
        signOfX=-1;
    float signOfY=rand()%2;
    if(signOfY  < 1)
        signOfY=-1;
        
    float x = movementOnX*signOfX;
    float y = movementOnY*signOfY;
    float target_angle = 2*((float)(rand()%31416)-15708)/10000.0;
    set_target_to(x,y);
    go_to_point_with_angle(targetX, targetY, target_angle);
}


void do_half_flip(){
    Odometria();
    float theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi
    if(theta_plus_h_pi > pi)
        theta_plus_h_pi=-(2*pi-theta_plus_h_pi);
    leftMotor(0,100);
    rightMotor(1,100);
    while(abs(theta_plus_h_pi-theta)>0.05){
        Odometria();
       // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
    }
    leftMotor(1,0);
    rightMotor(1,0);    
}

//ODOMETRIA MUST HAVE BEEN CALLED
//function that check if a cell A(x,y) is in the range of the front sonar S(xs,ys) (with an angle depending on the sonar used, front 0, left pi/3, right -pi/3) returns the probability it's occupied/empty [0;1]
float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected){

    float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2));
    if( distancePointToSonar < RANGE_SONAR){
        float anglePointToSonar=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam
        float alphaBeforeAdjustment=anglePointToSonar-theta-angleFromSonarPosition;
        anglePointToSonar=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
        
        if(alphaBeforeAdjustment>pi)
            alphaBeforeAdjustment=alphaBeforeAdjustment-2*pi;
        if(alphaBeforeAdjustment<-pi)
            alphaBeforeAdjustment=alphaBeforeAdjustment+2*pi;
            
        //float anglePointToSonar2=atan2(y-ys,x-xs)-theta;
        
        //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
        //check if absolute difference between the angles is no more than Omega/2
        if(anglePointToSonar <= ANGLE_SONAR/2 || anglePointToSonar >= rad_angle_check(-ANGLE_SONAR/2)){
            if( distancePointToSonar < (distanceObstacleDetected - INCERTITUDE_SONAR)){
            //point before obstacle, probably empty
            /*****************************************************************************/
                float Ea=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
                float Er;
                if(distancePointToSonar < RANGE_SONAR_MIN){
                    //point before minimum sonar range
                    Er=0.f;
                }else{
                    //point after minimum sonar range
                    Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2);
                }
             /*****************************************************************************/
                //if((1.f-Er*Ea)/2.f >1 || (1.f-Er*Ea)/2.f < 0)
                //    pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1.f-Er*Ea)/2.f,Er,Ea,alphaBeforeAdjustment);
                return (1.f-Er*Ea)/2.f;
            }else{
                //probably occupied
            /*****************************************************************************/
                float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
                float Or;
                if( distancePointToSonar <= (distanceObstacleDetected + INCERTITUDE_SONAR)){
                    //point between distanceObstacleDetected +- INCERTITUDE_SONAR
                    Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(INCERTITUDE_SONAR),2);
                }else{
                    //point after in range of the sonar but after the zone detected
                    Or=0;
                }
            /*****************************************************************************/
                //if((1+Or*Oa)/2 >1 || (1+Or*Oa)/2 < 0)
                //    pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1+Or*Oa)/2,Or,Oa,alphaBeforeAdjustment);
                return (1+Or*Oa)/2;
            }
        }   
    }
    //not checked by the sonar
    return 0.5;
}

void print_final_map() {
    float currProba;
    pc.printf("\n\r");
    for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
        for (int x= 0; x<NB_CELL_WIDTH; x++) {
                currProba=log_to_proba(map[x][y]);
            if ( currProba < 0.5) {
                pc.printf("   ");
            } else {
                if(currProba==0.5)
                    pc.printf(" . ");
                else
                    pc.printf(" X ");
            }
        }
        pc.printf("\n\r");
    }
}

void print_final_map_with_robot_position() {
    float currProba;
    Odometria();
    float Xrobot=robot_center_x_in_orthonormal_x();
    float Yrobot=robot_center_y_in_orthonormal_y();
    
    float heightIndiceInOrthonormal;
    float widthIndiceInOrthonormal;
    
    float widthMalus=-(3*sizeCellWidth/2);
    float widthBonus=sizeCellWidth/2;
    
    float heightMalus=-(3*sizeCellHeight/2);
    float heightBonus=sizeCellHeight/2;

    pc.printf("\n\r");
    for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
        for (int x= 0; x<NB_CELL_WIDTH; x++) {
            heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y);
            widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x);
            if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))                    
                pc.printf(" R ");
            else{
                currProba=log_to_proba(map[x][y]);
                if ( currProba < 0.5)
                    pc.printf("   ");
                else{
                    if(currProba==0.5)
                        pc.printf(" . ");
                    else
                        pc.printf(" X ");
                } 
            }
        }
        pc.printf("\n\r");
    }
}

void print_final_map_with_robot_position_and_target() {
    float currProba;
    Odometria();
    float Xrobot=robot_center_x_in_orthonormal_x();
    float Yrobot=robot_center_y_in_orthonormal_y();
    
    float heightIndiceInOrthonormal;
    float widthIndiceInOrthonormal;
    
    float widthMalus=-(3*sizeCellWidth/2);
    float widthBonus=sizeCellWidth/2;
    
    float heightMalus=-(3*sizeCellHeight/2);
    float heightBonus=sizeCellHeight/2;

    pc.printf("\n\r");
    for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
        for (int x= 0; x<NB_CELL_WIDTH; x++) {
            heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y);
            widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x);
            if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))
                pc.printf(" R ");
            else{
                if(targetYOrhto >= (heightIndiceInOrthonormal+heightMalus) && targetYOrhto <= (heightIndiceInOrthonormal+heightBonus) && targetXOrhto >= (widthIndiceInOrthonormal+widthMalus) && targetXOrhto <= (widthIndiceInOrthonormal+widthBonus))                    
                    pc.printf(" T ");
                else{
                    currProba=log_to_proba(map[x][y]);
                    if ( currProba < 0.5)
                        pc.printf("   ");
                    else{
                        if(currProba==0.5)
                            pc.printf(" . ");
                        else
                            pc.printf(" X ");
                    } 
                }
            }
        }
        pc.printf("\n\r");
    }
}

//MATHS heavy functions
/**********************************************************************/
//Distance computation function
float dist(float robot_x, float robot_y, float target_x, float target_y){
    //pc.printf("%f, %f, %f, %f",robot_x,robot_y,target_x,target_y);
    return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
}

//returns the probability [0,1] that the cell is occupied from the log valAue lt
float log_to_proba(float lt){
    return 1-1/(1+exp(lt));
}

//returns the log value that the cell is occupied from the probability value [0,1]
float proba_to_log(float p){
    return log(p/(1-p));
}

//returns the new log value
float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue ){
    return previousLogValue+proba_to_log(currentProbability)-originalLogvalue;
}

//makes the angle inAngle between 0 and 2pi
float rad_angle_check(float inAngle){
    //cout<<"before :"<<inAngle;
    if(inAngle > 0){
        while(inAngle > (2*pi))
            inAngle-=2*pi;
    }else{
        while(inAngle < 0)
            inAngle+=2*pi;
    }
    //cout<<" after :"<<inAngle<<endl;
    return inAngle;
}

//returns the angle between the vectors (x,y) and (xs,ys)
float compute_angle_between_vectors(float x, float y,float xs,float ys){
    //alpha angle between ->x and ->SA
    //vector S to A ->SA
    float vSAx=x-xs;
    float vSAy=y-ys;
    //norme SA
    float normeSA=sqrt(pow(vSAx,2)+pow(vSAy,2));
    //vector ->x (1,0)
    float cosAlpha=1*vSAy/*+0*vSAx*//normeSA;;
    //vector ->y (0,1)
    float sinAlpha=/*0*vSAy+*/1*vSAx/normeSA;//+0*vSAx;
    if (sinAlpha < 0)
        return -acos(cosAlpha);
    else
        return acos(cosAlpha);
}

//return 1 if positiv, -1 if negativ
float sign1(float value){
    if(value>=0) 
        return 1;
    else 
        return -1;
}

//return 1 if positiv, 0 if negativ
int sign2(float value){
    if(value>=0) 
        return 1;
    else 
        return 0;
}

/*
Robot space:      orthonormal space:
      ^                 ^
      |x                |y
   <- R                 O ->
    y                     x
*/
//Odometria must bu up to date
float x_robot_in_orthonormal_x(float x, float y){
    return robot_center_x_in_orthonormal_x()-y;
}

//Odometria must bu up to date
float y_robot_in_orthonormal_y(float x, float y){
    return robot_center_y_in_orthonormal_y()+x;
}

float robot_center_x_in_orthonormal_x(){
    return NB_CELL_WIDTH*sizeCellWidth-Y;
}

float robot_center_y_in_orthonormal_y(){
    return X;
}

float robot_sonar_front_x_in_orthonormal_x(){
    return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_FRONT_X;
}
float robot_sonar_front_y_in_orthonormal_y(){
    return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_FRONT_Y;
}

float robot_sonar_right_x_in_orthonormal_x(){
    return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_RIGHT_X;
}
float robot_sonar_right_y_in_orthonormal_y(){
    return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_RIGHT_Y;
}

float robot_sonar_left_x_in_orthonormal_x(){
    return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_LEFT_X;
}
float robot_sonar_left_y_in_orthonormal_y(){
    return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_LEFT_Y;
}

float estimated_width_indice_in_orthonormal_x(int i){
    return sizeCellWidth/2+i*sizeCellWidth;
}
float estimated_height_indice_in_orthonormal_y(int j){
    return sizeCellHeight/2+j*sizeCellHeight;
}

void update_force(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ){
    //get the coordonate of the map and the robot in the ortonormal space
    float xCenterCell=estimated_width_indice_in_orthonormal_x(widthIndice);
    float yCenterCell=estimated_height_indice_in_orthonormal_y(heightIndice);
    //compute the distance beetween the cell and the robot
    float distanceCellToRobot=sqrt(pow(xCenterCell-xRobotOrtho,2)+pow(yCenterCell-yRobotOrtho,2));
    //check if the cell is in range
    if(distanceCellToRobot <= range) {
        /*float anglePointToRobot=compute_angle_between_vectors(xCenterCell,yCenterCell,xRobotOrtho,yRobotOrtho);//angle beetween the point and the sonar
        float alphaBeforeAdjustment=anglePointToRobot-theta;
        anglePointToRobot=rad_angle_check(alphaBeforeAdjustment);
        if(anglePointToRobot <= pi/2 || anglePointToRobot >= rad_angle_check(-pi/2)){
        */
            float probaCell=log_to_proba(map[widthIndice][heightIndice]);
            float xForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(xCenterCell-xRobotOrtho)/pow(distanceCellToRobot,3);
            float yForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(yCenterCell-yRobotOrtho)/pow(distanceCellToRobot,3);
            *forceX+=xForceComputed;
            *forceY+=yForceComputed;
        //}
    }
}

void test_got_to_line(bool* reached){
    float line_a=1;
    float line_b=2;
    float line_c=-140;
    //we update the odometrie
    Odometria();
    float angularRight=0;
    float angularLeft=0;

    go_to_line(&angularLeft,&angularRight,line_a,line_b,line_c);
    //pc.printf("\r\n line: %f x + %f y + %f =0", line_a, line_b, line_c);

    leftMotor(sign2(angularLeft),abs(angularLeft));
    rightMotor(sign2(angularRight),abs(angularRight));
    
    pc.printf("\r\n dist=%f", dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto));

    //wait(0.1);
    Odometria();
    if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)<10)
        *reached=true;
}
void vff(bool* reached){
    float line_a;
    float line_b;
    float line_c;
    //Updating X,Y and theta with the odometry values
    float forceX=0;
    float forceY=0;
    //we update the odometrie
    Odometria();
    //we check the sensors
    float leftMm = get_distance_left_sensor();
    float frontMm = get_distance_front_sensor();
    float rightMm = get_distance_right_sensor();
    float angularRight=0;
    float angularLeft=0;
    //update the probabilities values 
    update_sonar_values(leftMm, frontMm, rightMm);
    //we compute the force on X and Y
    compute_forceX_and_forceY(&forceX, &forceY);
    //we compute a new ine
    calculate_line(forceX, forceY, X, Y,&line_a,&line_b,&line_c);
    go_to_line(&angularLeft,&angularRight,line_a,line_b,line_c);

    //Updating motor velocities
    
    leftMotor(sign2(angularLeft),abs(angularLeft));
    rightMotor(sign2(angularRight),abs(angularRight));

    //wait(0.1);
    Odometria();
    if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)<10)
        *reached=true;
}

//robotX and robotY are from Odometria, calculate line_a, line_b and line_c
void calculate_line(float forceX, float forceY, float robotX, float robotY,float *line_a, float *line_b, float *line_c){
    /*
    in the teachers maths it is 
    
    *line_a=forceY;
    *line_b=-forceX;
    
    because a*x+b*y+c=0
    a impact the vertical and b the horizontal
    and he has to put them like this because
    Robot space:      orthonormal space:
      ^                 ^
      |x                |y
   <- R                 O ->
    y                     x
    but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to 
    */
    *line_a=forceX;
    *line_b=forceY;
    //TODO check that
    //because the line computed always pass by the robot center we dont need lince_c
    //float xRobotOrtho=robot_center_x_in_orthonormal_x();
    //float yRobotOrtho=robot_center_y_in_orthonormal_y();
    //*line_c=forceX*yRobotOrtho+forceY*xRobotOrtho;    
    *line_c=0;
}

/*angleToTarget is obtained through atan2 so it s:
< 0 if the angle is bettween pi and 2pi on a trigo circle
> 0 if it is between 0 and pi
*/
void turn_to_target(float angleToTarget){
    Odometria();
    float theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi
    if(theta_plus_h_pi > pi)
        theta_plus_h_pi=-(2*pi-theta_plus_h_pi);
     if(angleToTarget>0){   
        leftMotor(0,1);
        rightMotor(1,1);
    }else{
        leftMotor(1,1);
        rightMotor(0,1);
    }
    while(abs(angleToTarget-theta_plus_h_pi)>0.05){
        Odometria();
        theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi
         if(theta_plus_h_pi > pi)
            theta_plus_h_pi=-(2*pi-theta_plus_h_pi);
        //pc.printf("\n\r diff=%f", abs(angleToTarget-theta_plus_h_pi));
    }
    leftMotor(1,0);
    rightMotor(1,0);    
}

//currently line_c is not used
void go_to_line(float* angularLeft,float* angularRight,float line_a, float line_b, float line_c){
    float lineAngle;
    float angleError;
    float linear;
    float angular; 
    
    bool direction=true;
    
    float angleToTarget=atan2(targetYOrhtoNotMap,targetXOrhtoNotMap);
    bool inFront=true;
    if(angleToTarget < 0)//the target is in front
        inFront=false;

    if(theta > 0){
        //the robot is oriented to the left
        if(theta > pi/2){
            //the robot is oriented lower left
            if(inFront)
                direction=false;//robot and target not oriented the same way
        }else{
            //the robot is oriented upper left
            if(!inFront)
                direction=false;//robot and target not oriented the same way
        }
    }else{
        //the robot is oriented to the right
        if(theta < -pi/2){
            //the robot is oriented lower right
            if(inFront)
                direction=false;//robot and target not oriented the same way
        }else{ 
            //the robot is oriented upper right
            if(!inFront)
                direction=false;//robot and target not oriented the same way
        }
    }
    //pc.printf("\r\n Target is in front");
    
     if(line_b!=0){
        if(!direction)
            lineAngle=atan(-line_a/line_b);
        else
            lineAngle=atan(line_a/-line_b);
    }
    else{
        lineAngle=1.5708;
    }
    
    //Computing angle error
    angleError = lineAngle-theta;
    angleError = atan(sin(angleError)/cos(angleError));

    //Calculating velocities
    linear=KV*(3.1416);
    angular=KH*angleError;

    *angularLeft=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
    *angularRight=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
    
    float aL=*angularLeft;
    float aR=*angularRight;
    //Normalize speed for motors
    if(abs(*angularLeft)>abs(*angularRight)) {  
        *angularRight=MAX_SPEED*abs(aR/aL)*sign1(aR);
        *angularLeft=MAX_SPEED*sign1(aL);
    }
    else {
        *angularLeft=MAX_SPEED*abs(aL/aR)*sign1(aL);
        *angularRight=MAX_SPEED*sign1(aR);
    }
    pc.printf("\r\n X=%f; Y=%f", robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y());
}

//compute the force on X and Y
void compute_forceX_and_forceY(float* forceX, float* forceY){
     //we put the position of the robot in an orthonormal space
     float xRobotOrtho=robot_center_x_in_orthonormal_x();
     float yRobotOrtho=robot_center_y_in_orthonormal_y();

     float forceRepulsionComputedX=0;
     float forceRepulsionComputedY=0;
     //for each cell of the map we compute a force of repulsion
     for(int i=0;i<NB_CELL_WIDTH;i++){
        for(int j=0;j<NB_CELL_HEIGHT;j++){
            update_force(i,j,RANGE_FORCE,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotOrtho,yRobotOrtho);
        }
    }
    //update with attraction force
    *forceX=+forceRepulsionComputedX;
    *forceY=+forceRepulsionComputedY;
    float distanceTargetRobot=sqrt(pow(targetXOrhto-xRobotOrtho,2)+pow(targetYOrhto-yRobotOrtho,2));
    if(distanceTargetRobot != 0){
        *forceX-=FORCE_CONSTANT_ATTRACTION*(targetXOrhto-xRobotOrtho)/distanceTargetRobot;
        *forceY-=FORCE_CONSTANT_ATTRACTION*(targetYOrhto-yRobotOrtho)/distanceTargetRobot;
    }
    float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2));
    if(amplitude!=0){//avoid division by 0 if forceX and forceY  == 0
        *forceX=*forceX/amplitude;
        *forceY=*forceY/amplitude;
    }
}

//x and y passed are TargetNotMap
float get_error_angles(float x, float y,float theta){
    float angleBeetweenRobotAndTarget=atan2(y,x);
    if(y > 0){
        if(angleBeetweenRobotAndTarget < pi/2)//up right
            angleBeetweenRobotAndTarget=-pi/2+angleBeetweenRobotAndTarget;
        else//up right
            angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-pi/2;
    }else{
        if(angleBeetweenRobotAndTarget > -pi/2)//lower right
            angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-pi/2;
        else//lower left
            angleBeetweenRobotAndTarget=2*pi+angleBeetweenRobotAndTarget-pi/2;
    }
    return angleBeetweenRobotAndTarget-theta;
}

void compute_angles_and_distance(float target_x, float target_y, float target_angle){
    alpha = atan2((target_y-Y),(target_x-X))-theta;
    alpha = atan(sin(alpha)/cos(alpha));
    rho = dist(X, Y, target_x, target_y);
    d2 = rho;
    beta = -alpha-theta+target_angle;        
    
    //Computing angle error and distance towards the target value
    rho += dt*(-KRHO*cos(alpha)*rho);
    temp = alpha;
    alpha += dt*(KRHO*sin(alpha)-KA*alpha-KB*beta);
    beta += dt*(-KRHO*sin(temp));
   //pc.printf("\n\r d2=%f", d2);
    //pc.printf("\n\r dt=%f", dt);
}

void compute_linear_angular_velocities(){
    //Computing linear and angular velocities
    if(alpha>=-1.5708 && alpha<=1.5708){
        linear=KRHO*rho;
        angular=KA*alpha+KB*beta;
    }
    else{
        linear=-KRHO*rho;
        angular=-KA*alpha-KB*beta;
    }
    angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
    angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
    
    //Normalize speed for motors
    if(angular_left>angular_right) {
        angular_right=MAX_SPEED*angular_right/angular_left;
        angular_left=MAX_SPEED;
    } else {
        angular_left=MAX_SPEED*angular_left/angular_right;
        angular_right=MAX_SPEED;
    }
}

//go the the given position while updating the map
void go_to_point_with_angle(float target_x, float target_y, float target_angle) {
    Odometria();
    alpha = atan2((target_y-Y),(target_x-X))-theta;
    alpha = atan(sin(alpha)/cos(alpha));
    rho = dist(X, Y, target_x, target_y);
    beta = -alpha-theta+target_angle;
    //beta = atan(sin(beta)/cos(beta));
    bool keep_going=true;
    float leftMm;
    float frontMm; 
    float rightMm;
    do {
        //Timer stuff
        dt = t.read();
        t.reset();
        t.start();
        
        //Updating X,Y and theta with the odometry values
        Odometria();
        leftMm = get_distance_left_sensor();
        frontMm = get_distance_front_sensor();
        rightMm = get_distance_right_sensor();

        //pc.printf("\n\r leftMm=%f", leftMm);
        //pc.printf("\n\r frontMm=%f", frontMm);
        //pc.printf("\n\r rightMm=%f", rightMm);
    
        //if in dangerzone 
        if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){
            leftMotor(1,0);
            rightMotor(1,0);
            update_sonar_values(leftMm, frontMm, rightMm);
            //TODO Giorgos maybe you can also test the do_half_flip() function
            Odometria();
            //do a flip TODO
            keep_going=false;
            do_half_flip();   
        }else{
            //if not in danger zone continue as usual
            update_sonar_values(leftMm, frontMm, rightMm);
            compute_angles_and_distance(target_x, target_y, target_angle); //Compute the angles and the distance from target
            compute_linear_angular_velocities(); //Using the angles and distance, compute the velocities needed (linear & angular)
    
            //pc.printf("\n\r X=%f", X);
            //pc.printf("\n\r Y=%f", Y);
    
            //pc.printf("\n\r a_r=%f", angular_right);
            //pc.printf("\n\r a_l=%f", angular_left);
    
            //Updating motor velocities
            leftMotor(1,angular_left);
            rightMotor(1,angular_right);
    
            wait(0.2);
            //Timer stuff
            t.stop();
            pc.printf("\n\r dist to target= %f",d2);
        }
    } while(d2>1 && (abs(target_angle-theta)>0.01) && keep_going);

    //Stop at the end
    leftMotor(1,0);
    rightMotor(1,0);
}

//Updates sonar values
void update_sonar_values(float leftMm, float frontMm, float rightMm){
    if(leftMm==0)
        pc.printf("\n\r leftMm==0");
    if(frontMm==0)
        pc.printf("\n\r frontMm==0");
    if(rightMm==0)
        pc.printf("\n\r rightMm==0");
    float currProba;
    float i_in_orthonormal;
    float j_in_orthonormal;
    for(int i=0;i<NB_CELL_WIDTH;i++){
        for(int j=0;j<NB_CELL_HEIGHT;j++){
                //check if the point A(x,y) in the world space is within the range of the sonar (which has the coordinates xs, ys in the world space)
            //check that again
            //compute for front sonar
            i_in_orthonormal=estimated_width_indice_in_orthonormal_x(i);
            j_in_orthonormal=estimated_height_indice_in_orthonormal_y(j);

            currProba=compute_probability_t(i_in_orthonormal,j_in_orthonormal,robot_sonar_front_x_in_orthonormal_x(),robot_sonar_front_y_in_orthonormal_y(),ANGLE_FRONT_TO_FRONT,frontMm/10);
            map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];//map is filled as map[0][0] get the data for the point closest to the origin
            //compute for right sonar
            currProba=compute_probability_t(i_in_orthonormal,j_in_orthonormal,robot_sonar_right_x_in_orthonormal_x(),robot_sonar_right_y_in_orthonormal_y(),ANGLE_FRONT_TO_RIGHT,rightMm/10);
            map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
             //compute for left sonar
            currProba=compute_probability_t(i_in_orthonormal,j_in_orthonormal,robot_sonar_left_x_in_orthonormal_x(),robot_sonar_left_y_in_orthonormal_y(),ANGLE_FRONT_TO_LEFT,leftMm/10);
            map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
        }
    }
}


//Go to a given X,Y position, regardless of the final angle
void go_to_point(float target_x, float target_y){
    float angle_error; //angle error
    float d; //distance from target
    float k_linear=10, k_angular=200;

    do {        
        //Updating X,Y and theta with the odometry values
        Odometria();

        //Computing angle error and distance towards the target value
        angle_error = atan2((target_y-Y),(target_x-X))-theta;
        angle_error = atan(sin(angle_error)/cos(angle_error));
        d=dist(X, Y, target_x, target_y);

        //Computing linear and angular velocities
        linear=k_linear*d;
        angular=k_angular*angle_error;
        angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
        angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;

        
        //Normalize speed for motors
        if(angular_left>angular_right) {
            angular_right=MAX_SPEED*angular_right/angular_left;
            angular_left=MAX_SPEED;
        } else {
            angular_left=MAX_SPEED*angular_left/angular_right;
            angular_right=MAX_SPEED;
        }

        pc.printf("\n\r X=%f", X);
        pc.printf("\n\r Y=%f", Y);

        //Updating motor velocities
        leftMotor(1,angular_left);
        rightMotor(1,angular_right);

        wait(0.5);
    } while(d>1);

    //Stop at the end
    leftMotor(1,0);
    rightMotor(1,0);
}

//go to line and follow it, from lab 1
void go_to_line_lab_1(float line_a, float line_b, float line_c){

    float angle_error; //angle error
    float d; //distance from line
    float kd=5, kh=200, kv=200;
    float linear, angular, angular_left, angular_right;
    float line_angle;
     
    //Check if b=0, if yes line_angle=90
    if(line_b!=0){
        line_angle=atan(-line_a/line_b);
    }
    else{
        line_angle=1.5708;
    }

    do {
        pc.printf("\n\n\r entered while");

        //Updating X,Y and theta with the odometry values
        Odometria();

        //Computing angle error and distance from the target line
        angle_error = line_angle-theta;
        angle_error = atan(sin(angle_error)/cos(angle_error));
        d=distFromLine(X, Y, line_a, line_b, line_c);
        pc.printf("\n\r d=%f", d);

        //Calculating velocities
        linear=kv*(3.1416-angle_error);
        angular=-kd*d+kh*angle_error;
        angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
        angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;

        //Normalize MAX_SPEED for motors
        if(angular_left>angular_right) {
            angular_right=MAX_SPEED*angular_right/angular_left;
            angular_left=MAX_SPEED;
        }
        else {
            angular_left=MAX_SPEED*angular_left/angular_right;
            angular_right=MAX_SPEED;
        }
        
        //Updating motor velocities
        if(angular_left>0){
            leftMotor(1,angular_left);
        }
        else{
            leftMotor(0,-angular_left);
        }
        
        if(angular_right>0){
            rightMotor(1,angular_right);
        }
        else{
            rightMotor(0,-angular_right);
        }

        wait(0.5);
    } while(1);       
}

float distFromLine(float robot_x, float robot_y, float line_a, float line_b, float line_c){
    return abs((line_a*robot_x+line_b*robot_y+line_c)/sqrt(line_a*line_a+line_b*line_b));
}