// Source Code for GA Based Navigation

/* 
Navigational Planning   using Genetic Algorithms 
Author: Srikanta Patnaik and Team 
Machine Intelligence Laboratory, University College of Engineering, Burla, India
Date of Writing: April, 2003; Version: 1.0
Book: Robot Cognition and Navigation: An Experiment with Mobile Robots
Publisher: Springer
ISBN: 978-3-540-23446-3
URL: http://www.springer.com/3-540-23446-2
 
*/


#include<iostream.h>
#include<graphics.h>
#include<stdlib.h>
#include<conio.h>
#include<math.h>
#include <dos.h>
#include<time.h>

# define BNDX1 50
# define BNDY1 50
# define BNDX2 450
# define BNDY2 450


struct gene
{
int x;
int y;
double fitness;
int valid;
gene* next;
};

// tested data x=70,y=350,step=50;
//  x=440,y=440,step=50;
//  x=70,y=70,step=50;

int goal_x=440;
int goal_y=440;
int curr_x;
int curr_y;
int step;
int sensing_step;
int cross_step;
int best_x;
int best_y;
float best_fitness;

gene* initpop;
gene* crosspop;
gene* path_list;


double distance( int x1,int y1,int x2,int y2)
{
double dist= sqrt((double)(x2-x1)*(double)(x2-x1)+(double)(y2-y1)*(double)(y2-y1));
return dist;
}

class setup
{
int bx1;
int bx2;
int by1;
int by2;
int obs1[10];
int obs2[10];
int obs3[10];
int obs4[10];
int obs5[10];
int obs6[10];
int obs7[10];
int obs8[10];

public:
setup(void);
void draw_border(void);
void draw_obstacles(void);
void cleanup(void);
};

void setup::setup(void)
{
bx1=50;
bx2=450;
by1=50;
by2=450;
}

void setup::draw_border(void)
{
setcolor(WHITE);
setlinestyle(SOLID_LINE,0,0);
rectangle(bx1,by1,bx2,by2);
setcolor(LIGHTRED);
rectangle(bx1-1,by1-1,bx2+1,by2+1);
rectangle(bx1-10,by1-10,bx2+10,by2+10);
setfillstyle(SOLID_FILL,LIGHTRED);
floodfill(bx1-5,by1-5,LIGHTRED);
setcolor(WHITE);
setfillstyle(SOLID_FILL,WHITE);
floodfill(350,350,WHITE);
setcolor(LIGHTRED);
}

void draw_obstacles()
{
setcolor(LIGHTRED);
setlinestyle(SOLID_LINE,0,0);
setfillstyle(SOLID_FILL,LIGHTRED);

int obs1[10]={140,120,170,100,185,120,175,140};
int obs2[10]={240,120,270,140,225,164,210,135};
int obs3[10]={178,160,200,180,185,200,170,180};
int obs4[10]={245,175,285,200,258,204,230,190};
int obs5[10]={310,215,360,240,330,270,298,250};
int obs6[10]={110,245,130,225,180,240,130,280};
int obs7[10]={230,258,270,250,250,280,220,280};
int obs8[10]={220,320,230,300,250,330,230,340};

fillpoly(4,obs1);
fillpoly(4,obs2);
fillpoly(4,obs3);
fillpoly(4,obs4);
fillpoly(4,obs5);
fillpoly(4,obs6);
fillpoly(4,obs7);
fillpoly(4,obs8);
}

void setup::draw_obstacles(void)
{
setcolor(LIGHTRED);
setlinestyle(SOLID_LINE,0,0);
setfillstyle(SOLID_FILL,LIGHTRED);

int obs1[10]={140,120,170,100,185,120,175,140};
int obs2[10]={240,120,270,140,225,164,210,135};
int obs3[10]={178,160,200,180,185,200,170,180};
int obs4[10]={245,175,285,200,258,204,230,190};
int obs5[10]={310,215,360,240,330,270,298,250};
int obs6[10]={110,245,130,225,180,240,130,280};
int obs7[10]={230,258,270,250,250,280,220,280};
int obs8[10]={220,320,230,300,250,330,230,340};
int obs9[10]={190,330,210,350,180,370,170,350};
int obs10[10]={450,400,450,410,350,410,350,400};
int obs11[10]={450,100,450,110,350,110,350,100};
int obs12[10]={300,450,310,450,310,360,300,360};

fillpoly(4,obs1);
fillpoly(4,obs2);
fillpoly(4,obs3);
fillpoly(4,obs4);
fillpoly(4,obs5);
fillpoly(4,obs6);
fillpoly(4,obs7);
fillpoly(4,obs8);
fillpoly(4,obs9);
fillpoly(4,obs10);
fillpoly(4,obs11);
fillpoly(4,obs12);
}

void get_initial_population(void)
{
// int tempstep=step;
int xo,yo;
int counter=0;
gene* ptr2;
gene* ptr1=new gene;
ptr1->x=0;
ptr1->y=0;
ptr1->valid=0;
//	   if(getpixel(ptr1->x,ptr1->y)==WHITE)ptr1->valid=1;
//	   else ptr1->valid=0;
ptr1->next=NULL;

////////////////////////
counter=0;
xo=curr_x;yo=curr_y;
ptr2=new gene;

//  tempstep=random(step)+1;
while(counter<sensing_step)
{
counter++;
yo--;
if(getpixel(xo,yo)==LIGHTRED){yo+=2;break;}
}
if(counter>step)
{ptr2->x=curr_x;
ptr2->y=curr_y-step;}
else
{
ptr2->x=xo;
ptr2->y=yo;
}
if(getpixel(ptr2->x,ptr2->y)==WHITE)ptr2->valid=1;
else ptr2->valid=0;
ptr2->next=ptr1;
ptr1=ptr2;
////////////////////////
counter=0;
xo=curr_x;yo=curr_y;
ptr2=new gene;
//   tempstep=random(step)+1;
while(counter<sensing_step)
{
counter++;
yo--;
xo++;
if(getpixel(xo,yo)==LIGHTRED){yo+=2;xo-=2;break;}
}
if(counter>step)
{ptr2->x=curr_x+cross_step;
ptr2->y=curr_y-cross_step;}
else
{
ptr2->x=xo;
ptr2->y=yo;
}
if(getpixel(ptr2->x,ptr2->y)==WHITE)ptr2->valid=1;
else ptr2->valid=0;
ptr2->next=ptr1;
ptr1=ptr2;
////////////////////////
counter=0;
xo=curr_x;yo=curr_y;
ptr2=new gene;
//   tempstep=random(step)+1;
while(counter<sensing_step)
{
counter++;
xo++;
if(getpixel(xo,yo)==LIGHTRED){xo-=2;break;}
}
if(counter>step)
{ptr2->x=curr_x+step;
ptr2->y=curr_y;}
else
{
ptr2->x=xo;
ptr2->y=yo;
}
if(getpixel(ptr2->x,ptr2->y)==WHITE)ptr2->valid=1;
else ptr2->valid=0;
ptr2->next=ptr1;
ptr1=ptr2;
//////////////////////////
counter=0;
xo=curr_x;yo=curr_y;
ptr2=new gene;
//   tempstep=random(step)+1;
while(counter<sensing_step)
{
counter++;
yo++;
xo++;
if(getpixel(xo,yo)==LIGHTRED){yo-=2;xo-=2;break;}
}
if(counter>cross_step)
{ptr2->x=curr_x+cross_step;
ptr2->y=curr_y+cross_step;}
else
{
ptr2->x=xo;
ptr2->y=yo;
}
if(getpixel(ptr2->x,ptr2->y)==WHITE)ptr2->valid=1;
else ptr2->valid=0;
ptr2->next=ptr1;
ptr1=ptr2;
////////////////////////////////////////
counter=0;
xo=curr_x;yo=curr_y;
ptr2=new gene;
//	   tempstep=random(step)+1;
while(counter<sensing_step)
{
counter++;
yo++;
if(getpixel(xo,yo)==LIGHTRED){yo-=2;break;}
}
if(counter>step)
{ptr2->x=curr_x;
ptr2->y=curr_y+step;}
else
{
ptr2->x=xo;
ptr2->y=yo;
}
if(getpixel(ptr2->x,ptr2->y)==WHITE)ptr2->valid=1;
else ptr2->valid=0;
ptr2->next=ptr1;
ptr1=ptr2;
////////////////////////////////////////
counter=0;
xo=curr_x;yo=curr_y;
ptr2=new gene;
//   tempstep=random(step)+1;
while(counter<sensing_step)
{
counter++;
xo--;
yo++;
if(getpixel(xo,yo)==LIGHTRED){yo-=2;xo+=2;break;}
}
if(counter>cross_step)
{ptr2->x=curr_x-cross_step;
ptr2->y=curr_y+cross_step;}
else
{
ptr2->x=xo;
ptr2->y=yo;
}
if(getpixel(ptr2->x,ptr2->y)==WHITE)ptr2->valid=1;
else ptr2->valid=0;
ptr2->next=ptr1;
ptr1=ptr2;
////////////////////////////////
counter=0;
xo=curr_x;yo=curr_y;
ptr2=new gene;
//   tempstep=random(step)+1;
while(counter<sensing_step)
{
counter++;
xo--;
if(getpixel(xo,yo)==LIGHTRED){xo+=2;break;}
}
if(counter>step)
{ptr2->x=curr_x-step;
ptr2->y=curr_y;}
else
{
ptr2->x=xo;
ptr2->y=yo;
}
if(getpixel(ptr2->x,ptr2->y)==WHITE)ptr2->valid=1;
else ptr2->valid=0;
ptr2->next=ptr1;
ptr1=ptr2;
//////////////////////////////////////////
counter=0;
xo=curr_x;yo=curr_y;
ptr2=new gene;
//	   tempstep=random(step)+1;
while(counter<sensing_step)
{
counter++;
xo--;
yo--;
if(getpixel(xo,yo)==LIGHTRED){yo+=2;xo+=2;break;}
}
if(counter>cross_step)
{ptr2->x=curr_x-cross_step;
ptr2->y=curr_y-cross_step;}
else
{
ptr2->x=xo;
ptr2->y=yo;
}
if(getpixel(ptr2->x,ptr2->y)==WHITE)ptr2->valid=1;
else ptr2->valid=0;
ptr2->next=ptr1;
ptr1=ptr2;

///////////////////////////////////

initpop=ptr1;
}

void get_crossed_population(void)
{
int midx,midy,midx1,midy1,midx2,midy2,midx3,midy3,midx4,midy4;
int midx5,midy5,midx6,midy6,midx7,midy7,midx8,midy8;
gene* cur=initpop;
gene* ptr1=new gene;
ptr1->x=cur->x;
ptr1->y=cur->y;
ptr1->next=NULL;

gene* mate=initpop;
gene* ptr2;
gene* ptr3;
while(cur->next)
{
while(mate->next)
{
if(mate==cur) mate=mate->next;
ptr2=new gene;
ptr2->x=cur->x;
ptr2->y=mate->y;
if(getpixel(ptr2->x,ptr2->y)==WHITE)ptr2->valid=1;
else ptr2->valid=0;

////////////////////////////////////////

midx=(curr_x+ptr2->x)/2;
midy=(curr_y+ptr2->y)/2;
if((midx>BNDX1)&&(midx<BNDX2))
{  if(getpixel(midx,midy)==LIGHTRED)ptr2->valid=0;}

midx1=(curr_x+midx)/2;
midy1=(curr_y+midy)/2;
if((midx1>BNDX1)&&(midx1<BNDX2))
{  if(getpixel(midx1,midy1)==LIGHTRED)ptr2->valid=0;}

midx2=(midx+ptr2->x)/2;
midy2=(midy+ptr2->y)/2;
if((midx2>BNDX1)&&(midx2<BNDX2))
	{  if(getpixel(midx2,midy2)==LIGHTRED)ptr2->valid=0;}

midx3=(curr_x+midx1)/2;
midy3=(curr_y+midy1)/2;
if((midx3>BNDX1)&&(midx3<BNDX2))
{  if(getpixel(midx3,midy3)==LIGHTRED)ptr2->valid=0;}

midx4=(midx1+midx)/2;
midy4=(midy1+midy)/2;
if((midx4>BNDX1)&&(midx4<BNDX2))
	{  if(getpixel(midx4,midy4)==LIGHTRED)ptr2->valid=0;}

midx5=(midx+midx2)/2;
midy5=(midy+midy2)/2;
if((midx5>BNDX1)&&(midx5<BNDX2))
	{  if(getpixel(midx5,midy5)==LIGHTRED)ptr2->valid=0;}

midx6=(midx2+ptr2->x)/2;
midy6=(midy2+ptr2->y)/2;
if((midx6>BNDX1)&&(midx6<BNDX2))
	{  if(getpixel(midx6,midy6)==LIGHTRED)ptr2->valid=0;}


/////////////////////////////////////////

ptr3=new gene;
ptr3->x=mate->x;
ptr3->y=cur->y;
if(getpixel(ptr2->x,ptr2->y)==WHITE)ptr3->valid=1;
	else ptr2->valid=0;

////////////////////////////////////////
midx=(curr_x+ptr3->x)/2;
midy=(curr_y+ptr3->y)/2;
if((midx>BNDX1)&&(midx<BNDX2))
	{  if(getpixel(midx,midy)==LIGHTRED)ptr3->valid=0;}

midx1=(curr_x+midx)/2;
midy1=(curr_y+midy)/2;
if((midx1>BNDX1)&&(midx1<BNDX2))
	{  if(getpixel(midx1,midy1)==LIGHTRED)ptr3->valid=0;}

midx2=(midx+ptr3->x)/2;
midy2=(midy+ptr3->y)/2;
if((midx2>BNDX1)&&(midx2<BNDX2))
	{  if(getpixel(midx2,midy2)==LIGHTRED)ptr3->valid=0;}

midx3=(curr_x+midx1)/2;
midy3=(curr_y+midy1)/2;
if((midx3>BNDX1)&&(midx3<BNDX2))
	{  if(getpixel(midx3,midy3)==LIGHTRED)ptr3->valid=0;}

midx4=(midx1+midx)/2;
midy4=(midy1+midy)/2;
if((midx4>BNDX1)&&(midx4<BNDX2))
	{  if(getpixel(midx4,midy4)==LIGHTRED)ptr3->valid=0;}

midx5=(midx+midx2)/2;
midy5=(midy+midy2)/2;
if((midx5>BNDX1)&&(midx5<BNDX2))
	{  if(getpixel(midx5,midy5)==LIGHTRED)ptr3->valid=0;}

midx6=(midx2+ptr3->x)/2;
midy6=(midy2+ptr3->y)/2;
if((midx6>BNDX1)&&(midx6<BNDX2))
	{  if(getpixel(midx6,midy6)==LIGHTRED)ptr3->valid=0;}


/////////////////////////////////////////
ptr2->next=ptr1;
ptr3->next=ptr2;
ptr1=ptr3;

mate=mate->next;
}
mate=initpop;
cur=cur->next;
}

crosspop=ptr1;
}

void delete_list(gene* p)
{  gene* temp;
while(p->next)
{
temp=p->next;
delete p;
p=temp;

}
}

void find_fitness(gene* p)
{
while(p)
{
if((p->x==curr_x)&&(p->y==curr_y))p->valid=0;
if(p->valid)
p->fitness=distance(p->x,p->y,goal_x,goal_y)+distance(curr_x,curr_y,p->x,p->y);

else p->fitness=1000.0;
p=p->next;
}
}

void find_best_gene(void)
{
best_x=initpop->x;
best_y=initpop->y;
best_fitness=1000.0;
gene* temp=initpop;
while(temp->next)
{  
if(getpixel(temp->x,temp->y)!=WHITE) temp->valid=0;
if(temp->valid==1)
{
if(temp->fitness<best_fitness)
{
best_x=temp->x;
best_y=temp->y;
best_fitness=temp->fitness;
}   
}
temp=temp->next;
}
temp=crosspop;
while(temp->next)
{   
if(getpixel(temp->x,temp->y)!=WHITE) temp->valid=0;
if(temp->valid==1)
{
if(temp->fitness<best_fitness)
{
best_x=temp->x;
best_y=temp->y;
best_fitness=temp->fitness;
}    
}
temp=temp->next;
}
}

void adjust_step()
{
double dstep= (double)step;
if(distance(best_x,best_y,goal_x,goal_y)<dstep) step=5;
if(step==0)step=5;
}

void mutation(void)
{
int x,y;
gene* tem;
for(int i=0;i<10;i++)
{
x= curr_x+random(4)-2;
y=curr_y+random(4)-2;
if(getpixel(x,y)==WHITE)

{
tem=new gene;
tem->x=x;
tem->y=y;
tem->valid=1;
tem->next=crosspop;
crosspop=tem;
}
/*
x= curr_x-random(10);
y=curr_y+random(10);
if(getpixel(x,y)==WHITE)

{
tem=new gene;
tem->x=x;
tem->y=y;
tem->valid=1;
tem->next=crosspop;
crosspop=tem;
}
x= curr_x+random(10);
y=curr_y-random(10);
if(getpixel(x,y)==WHITE)

{
tem=new gene;
tem->x=x;
tem->y=y;
tem->valid=1;
tem->next=crosspop;
crosspop=tem;
}
x= curr_x-random(10);
y=curr_x-random(10);
if(getpixel(x,y)==WHITE)

{
tem=new gene;
tem->x=x;
tem->y=y;
tem->valid=1;
tem->next=crosspop;
crosspop=tem;
}   */
}
}


void main()
{
clrscr();
double tim;
int generation=0;
cout<<"enter starting x(50-450):"; cin>>curr_x;
cout<<"enter starting y(50-450):"; cin>>curr_y;
cout<<"enter sensing range:";cin>>sensing_step;
cout<<"enter robot step size(5 to "<<sensing_step<<"):";cin>>step;
int bndm=(BNDY1+BNDY2)/2;
if(curr_y<bndm)curr_y=bndm+(bndm-curr_y);
else
if(curr_y>bndm)curr_y=bndm-(curr_y-bndm);
double d=(double) step;
cross_step=(int) (d/sqrt(2));

int driver,mode;
driver=DETECT;
initgraph(&driver,&mode,"\\tcc\\bgi" );
setup s;
s.draw_border();
s.draw_obstacles();

if(getpixel(curr_x,curr_y)==BLACK)
{ 
cout<<" we are in the out of work space."<<endl;
cout<<" press any key to terminate.";
getch();
closegraph();
exit(0);
}
if(getpixel(curr_x,curr_y)==LIGHTRED)
{ 
cout<<" starting point on the obstacle"<<endl;
cout<<"press any key to terminate.";
getch();
closegraph();
exit(0);
}

setcolor(BLUE);
circle(goal_x,goal_y,5);
circle(curr_x,curr_y,5);

gene* temp=new gene;
temp->x=curr_x;
temp->y=curr_y;
temp->next=NULL;
path_list=temp;

int tempx,tempy;
clock_t  star,end;
star = clock();

do
{
generation++;
get_initial_population();
get_crossed_population();
//		mutation();
find_fitness(initpop);
find_fitness(crosspop);
find_best_gene();
//		adjust_step();

setcolor(GREEN);
line(curr_x,curr_y,best_x,best_y);
circle(best_x,best_y,2);
curr_x=best_x;
curr_y=best_y;

temp=new gene;
temp->x=best_x;
temp->y=best_y;
temp->next=path_list;
path_list=temp;

delete_list(initpop);
delete_list(crosspop);
initpop=NULL;
crosspop=NULL;

}
while(!((curr_x>=goal_x-5)&&(curr_x<=goal_x+5)&&
			(curr_y>=goal_y-5)&&(curr_y<=goal_y+5)));
end = clock();
tim=( (end - star) / CLK_TCK);

setcolor(GREEN);
getch();
closegraph();
delete_list(initpop);
delete_list(crosspop);

cout<<"time taken to search path="<<tim;
double dist=0.0;
temp=path_list;
while(temp->next)
{
dist+=distance(temp->x,temp->y,temp->next->x,temp->next->y);
temp=temp->next;
}
cout<<endl<<"distance covered="<<dist<<" units";
getch();
delete_list(path_list);
}

