#ifndef RRT

#define RRT

#include <stdio.h>

#include "iostream.h"

#include <stdlib.h>

#include <math.h>

#include "collide.h"

#define pi 3.1416

#define Null -1

#define Reach 0

#define Advance 1

#define Trapped 3

#define free 1

#define collide 0

#define pass 1

#define fail 0

#define forward 1

#define sharepath 1

#define freepath 0

#define newRobot 3

struct Nodes{

float x, y, theta;

int parent;

int level;

int status;

int shareRegion;

};

Nodes path[2][1000];

Nodes node[2][20000];

Nodes dualnode[2][20000];

float goal[2][3];

int halfWidth,halfLength,startpt,endpt;

int reach[2],finalStatus[2],numRegion;

Collision obstacle;

int finalLevel[2],numNodes[2],zone[2];

class Build{

public:

float rand[3], phi,alpha;

float minDistance[2];

int numNode[2], currentNode[2], nearestNode[2];

int state,direction;

float minGoalDistance[2];

void build(){}

int build(float, float) ;

init();

rrt();

extend( float, float, float,int);

pathsearch();

sharezone();

private:

float value[3];

};

Build::build(float x, float y){

value[0]=x;

value[1]=y;

}

Build::init(){

node[0][0].x=10.0;

node[0][0].y=50.0;

node[0][0].theta=0;

node[0][0].parent=Null;

node[0][0].level=0;

goal[0][0]=410;

goal[0][1]=50.0;

goal[0][2]=1.57;

node[1][0].x=50.0;

node[1][0].y=450.0;

node[1][0].theta=0;

node[1][0].parent=Null;

node[1][0].level=0;

goal[1][0]=430.0;

goal[1][1]=50.0;

goal[1][2]=4.71;

halfWidth=15;

halfLength=8;

minGoalDistance[2]=1000;

phi=10;

numNode[0]=1;

currentNode[0]=1;

nearestNode[0]=0;

obstacle.polygon();

}

Build::rrt(){

int loopNum=10;

int signs;

for (int i=0;i<loopNum;i++){

rand[0]=random()%470;

rand[1]=random()%570;

rand[2]=random()%30;

signs=random()%2;

direction=random()%2;

if(signs==0) rand[2]=-1*rand[2];

rand[2]=rand[2]*pi/180;

state=extend(rand[0], rand[1], rand[2],direction);

if(state==Reach) {

loopNum=i;

printf("Reach\n");

}

else loopNum++;

}

return 0;

}

Build::extend( float x, float y, float theta, int direct){

float distance, goalDistance[2],dx,dy,dtheta,temptheta,orienttheta;

float length,width,zero,rangle;

int status,constraintStatus,angle,heading,goalBias;

for(int j=startpt;j<endpt;j++){

minDistance[j]=1000;

finalStatus[j]=1;

goalBias=random()%100;

for (int i=0;i<numNode[j];i++){

distance=sqrt((x-node[j][i].x)*(x-node[j][i].x)+(y-node[j][i].y)*(y- node[j][i].y));

if(distance<minDistance[j]){

nearestNode[j]=i;

minDistance[j]=distance;

}

}

goalDistance[j]=sqrt((goal[j][0]-node[j][nearestNode[j]].x)*(goal[j][0]- node[j][nearestNode[j]].x)+ goal[j][1]- node[j][nearestNode[j]].y)*(goal[j][1]-node[j][nearestNode[j]].y));

if(goalDistance[j]<50) zone[j]=1;

if(zone[j]==1 && goalDistance[j]>50) return Trapped;

if(goalDistance[j]<phi+50) heading=direct;

else heading=forward;

dtheta=(phi*tan(theta))/(2*halfLength);

temptheta=node[j][nearestNode[j]].theta+dtheta;

if (temptheta>2*pi) temptheta-=2*pi;

if(temptheta<0) temptheta=2*pi+temptheta;

if(heading==forward){

dx=phi*cos(temptheta);

dy=phi*sin(temptheta);

}

else {

dx=phi*cos(temptheta+pi);

dy=phi*sin(temptheta+pi);

}

zero=-dx*sin(temptheta)+dy*cos(temptheta);

if(fabs(zero)<.05){

constraintStatus=pass;

}

//-- upper right corner coordinate

car.x[0]=node[j][nearestNode[j]].x+dx+(halfWidth*cos(temptheta)+halfLength*sin(temptheta));

car.y[0]=node[j][nearestNode[j]].y+dy+(-halfWidth*sin(temptheta)+halfLength*cos(temptheta));

//-- lower right corner

car.x[1]=node[j][nearestNode[j]].x+dx+(halfWidth*cos(temptheta)-halfLength*sin(temptheta));

car.y[1]=node[j][nearestNode[j]].y+dy+(-halfWidth*sin(temptheta)-halfLength*cos(temptheta));

//-- lower left corner

car.x[2]=node[j][nearestNode[j]].x+dx+(-halfWidth*cos(temptheta)-halfLength*sin(temptheta));

car.y[2]=node[j][nearestNode[j]].y+dy+(halfWidth*sin(temptheta)-halfLength*cos(temptheta));

//-- upper left corner

car.x[3]=node[j][nearestNode[j]].x+dx+(-halfWidth*cos(temptheta)+halfLength*sin(temptheta));

car.y[3]=node[j][nearestNode[j]].y+dy+(halfWidth*sin(temptheta)+halfLength*cos(temptheta));

for (i=0;i<4;i++){

// Checking for collision

status=obstacle.coll(car.x[i],car.y[i]);

finalStatus[j]*=status;

}

// Limit the world coordinates

if( node[j][nearestNode[j]].x<470 && node[j][nearestNode[j]].y<570 &&

node[j][nearestNode[j]].x>0 && node[j][nearestNode[j]].y>0)

{

finalStatus[j]*=free;}

else finalStatus[j]*=collide;

if (finalStatus[j]==free){

numNodes[j]++;

node[j][currentNode[j]].x=node[j][nearestNode[j]].x+dx;

node[j][currentNode[j]].y=node[j][nearestNode[j]].y+dy;

node[j][currentNode[j]].theta=temptheta;

node[j][currentNode[j]].parent=nearestNode[j];

node[j][currentNode[j]].level=node[j][nearestNode[j]].level+1;

goalDistance[j]=sqrt((goal[j][0]-node[j][currentNode[j]].x)*(goal[j][0]-node[j][currentNode[j]].x)+

(goal[j][1]-node[j][currentNode[j]].y)*(goal[j][1]-node[j][currentNode[j]].y));

// orientation is not a concern in this case

//if(goalDistance[j]<=phi ){

// orientation takes into account

if(goalDistance[j]<=phi &&( temptheta>goal[j][2]-.1&& temptheta<goal[j][2]+.1)){

reach[j]=1;

if(reach[0]==1) startpt=1,endpt=2;

else if(reach[1]==1) startpt=0,endpt=1;

else startpt=0,endpt=2;

node[j][currentNode[j]].x=goal[j][0];

node[j][currentNode[j]].y=goal[j][1];

node[j][currentNode[j]].theta=temptheta;

node[j][currentNode[j]].parent=nearestNode[j];

//node[j][currentNode[j]].level=node[j][nearestNode[j]].level+1;

}

else{

reach[j]=0;

currentNode[j]++;

numNode[j]++;

}

}

}

if(reach[1]==1&&reach[0]==1){

pathsearch();

return Reach;

}

else

return Trapped;

}

Build::pathsearch(){ // Search for the final paths

int control[2];

int count[2];

control[0]=1;

control[1]=1;

count[0]=node[0][currentNode[0]].level;

count[1]=node[1][currentNode[1]].level;

printf("count[0]=%d/n",count[0]);

for(int i=0;i<2;i++){

while(control[i]==1){

path[i][count[i]].x=node[i][currentNode[i]].x;

path[i][count[i]].y=node[i][currentNode[i]].y;

path[i][count[i]].theta=node[i][currentNode[i]].theta*180/pi;

currentNode[i]=node[i][currentNode[i]].parent;

printf("path[%d][%d]=%f\n",i,count[i],path[i][count[i]].theta);

if(count[i]==0) control[i]=0;

else{ control[i]=1;

count[i]--;

finalLevel[i]++;

}

}

}

}

Build::sharezone(){ // Defining sharezones

float distance;

for (int i =1;i<finalLevel[0];i++){

for(int j=1;j<finalLevel[1];j++){

distance=sqrt((path[0][i].x-path[1][j].x)*(path[0][i].x-path[1][j].x)+

(path[0][i].y-path[1][j].y)*(path[0][i].y-path[1][j].y));

if(distance<2*halfLength){

path[0][i].status=sharepath;

path[1][j].status=sharepath;

if(path[0][i].status==sharepath && path[0][i-1].status!=sharepath){

numRegion++;

path[0][i].shareRegion=numRegion;

path[1][j].shareRegion=numRegion;

}

if(path[0][i].status==sharepath && path[0][i-1].status==sharepath){

path[0][i].shareRegion=numRegion;

path[1][j].shareRegion=numRegion;

}

}

}

}

printf("numShareRegion=%d\n",numRegion);

}

#endif