#include <iostream>

#include <fstream>
#include <queue>
#include <cstdlib>
#include "vectorlist.h"
using namespace std;
#define SCALE_MAP 2
#define GRID_ROWS 1000
#define GRID_COLS 1000
float gridMap[GRID_ROWS][GRID_COLS];
int waypointx[10000];
int waypointy[10000];
int valid[10000];
const double startpx = -10.1; //Robot's starting pose
const double startpy = 3.2;
const double startpa = 0;
int iwidth = 1086; //map image width in pixels
int iheight = 443; //map image height in pixels
int wwidth = 44; //map world width in meters
int wheight = 18; //map world height in meters

////////////////////////////////////////////////////////////////////
// Function Declarations
////////////////////////////////////////////////////////////////////
void inputMap();
void outputMap();
void computeWH(double x, double y, int& w, int& h);
void computeXY(int w, int h, double& x, double& y);
void grow();
void propagate(double start, double finish, int starty, int startx, int& done);
void nextWaypoint (int count);
void relaxWaypoints(int& count);
int clear(int x1, int y1, int x2, int y2);

////////////////////////////////////////////////////////////////////
// The wavefront path planner accepts as input a user's goal point
// and generate the waypoints of a path to the user's goal point by
// using the wavefront path planing approach
////////////////////////////////////////////////////////////////////
void wavefrontPlaner(double gx, double gy, queue<Vector>& waypoints)
{
double px, py;
Vector waypoint;
int startx,starty;
int goalx,goaly;
int i,j;
int count = 0;
int done = 0;
// read the input map into the grid map
inputMap();
//Convert starting and goal points from the world into the image
computeWH(startpx,startpy, startx, starty);
computeWH(gx, gy, goalx, goaly);
// check if the goal point is in the wall
if (gridMap[goaly][goalx] == 1.0)
{
cerr << "Goal point is embedded in wall and, thus, unreachable" << endl;
exit (1);
}
// label the goal point
gridMap[goaly][goalx] = 2.0;
// grow the grid map 5 times
grow(); grow(); grow(); grow(); grow();
// propagate the wavefront
i = 2;
while (done == 0){
propagate((double)i,(double)(i+1), starty, startx, done);
i++;
}
// check for unreachable place
if (done == 2){
cerr << "Goal point is enclosed and, thus, unreachable" << endl;
exit (1);
}
// compute waypoints
waypointx[0] = startx;
waypointy[0] = starty;
valid[count] = 1;
while (gridMap[waypointy[count]][waypointx[count]] != 3.0)
{
nextWaypoint(count);
count++;
valid[count] = 1;
}
// relax waypoints
relaxWaypoints(count);
// restore the grid map
for (i = 0; i < iheight/SCALE_MAP; i++)
for (j = 0; j < iwidth/SCALE_MAP; j++)
if (gridMap[i][j] > 1.0) gridMap[i][j] = 0.0;
// store waypoints in grid map and in waypoints
for (i = 0; i < (count+1); i++)
{
gridMap[waypointy[i]][waypointx[i]] = 1.0;
computeXY(waypointx[i], waypointy[i], px, py);
waypoint.setVector(px-startpx, py-startpy);
waypoints.push(waypoint);
}
 // prints the grid map into scaled map with waypoints
outputMap();
}

//////////////////////////////////////////////////////////////////
// Function inputMap reads in the input map information into
// an initial occupancy grid, which is stored in gridMap.
//////////////////////////////////////////////////////////////////
void inputMap()
{
int i, j, m, n;
int maxVal;
char inputLine1[80], nextChar;
ifstream inFile("hospital_section.pnm");
/* Initialize map to 0's, meaning all free space */
for (m=0; m<GRID_ROWS; m++)
for (n=0; n<GRID_COLS; n++)
gridMap[m][n] = 0.0;
/* Read past first line */
inFile.getline(inputLine1,80);
/* Read in width, height, maxVal */
inFile >> iwidth >> iheight >> maxVal;
cout << "Width = " << iwidth << ", Height = " << iheight << endl;
/* Read in map; */
for (i=0; i<iheight; i++)
for (j=0; j<iwidth; j++) {
inFile >> nextChar;
if (!nextChar)
gridMap[i/SCALE_MAP][j/SCALE_MAP] = 1.0;
}
cout << "Map input complete.\n";
}

/////////////////////////////////////////////////////////////////
// Function outputMap writes out the occupancy grid information
// into a scaled map
/////////////////////////////////////////////////////////////////
void outputMap()
{
int i, j;
int maxVal;
char inputLine1[80];
ifstream inFile("hospital_section.pnm");
ofstream outFile("scaled_hospital_section.pnm");
/* Read past first line */
inFile.getline(inputLine1,80);
/* Read in width, height, maxVal */
inFile >> iwidth >> iheight >> maxVal;
 outFile << inputLine1 << endl;
outFile << iwidth/SCALE_MAP << " " << iheight/SCALE_MAP << endl
<< maxVal << endl;
for (i=0; i<iheight/SCALE_MAP; i++)
for (j=0; j<iwidth/SCALE_MAP; j++) {
if (gridMap[i][j] == 1.0)
outFile << (char) 0;
else
outFile << (char) -1;
}
cout << "Scaled map output to file." << endl;
}
//////////////////////////////////////////////////////////////////
// Transform from the world coordinates to the image coordinates
//////////////////////////////////////////////////////////////////
void computeWH(double x, double y, int& w, int& h)
{
w = (int)((double)iwidth*((wwidth/2.+x)/(wwidth*2.)));
h = (int)((double)iheight*((wheight/2.-y)/(wheight*2.)));
}
//////////////////////////////////////////////////////////////////
// Transform from the image coordinates to the world coordinates
//////////////////////////////////////////////////////////////////
void computeXY(int w, int h, double& x, double& y)
{
x = ((double)w*wwidth*2./(double)iwidth)-wwidth/2.;
y = wheight/2.-((double)h*wheight*2./(double)iheight);
}
////////////////////////////////////////////////////////////////
// grow the occupancy grid, gripMap, for a single step,
// i.e., one grid cell increase
///////////////////////////////////////////////////////////////
void grow()
{
int row,col;
for (row= 0; row < iheight; row++)
{
for (col= 0; col< iwidth; col++)
{
if (gridMap[row][col] == 1.0)
{
if (row > 0)
{
if (gridMap[row-1][col] == 0.0) gridMap[row-1][col] = -1.0;
}
if (col> 0)
{
if (gridMap[row][col-1] == 0.0) gridMap[row][col-1] = -1.0;
}
if (row < (iheight-1))
{
if (gridMap[row+1][col] == 0.0) gridMap[row+1][col] = -1.0;
}
if (col < (iwidth-1))
 {
if (gridMap[row][col+1] == 0.0) gridMap[row][col+1] = -1.0;
}
if ((col< (iwidth-1)) && (i < (iheight-1)))
{
if (gridMap[row+1][col+1] == 0.0) gridMap[row+1][col+1] = -1.0;
}
if ((col < (iwidth-1)) && (i > 0))
{
if (gridMap[row-1][col+1] == 0.0) gridMap[row-1][col+1] = -1.0;
}
if ((col> 0) && (row > 0))
{
if (gridMap[row-1][col-1] == 0.0) gridMap[row-1][col-1] = -1.0;
}
if ((col> 0) && (row < (iheight-1)))
{
if (gridMap[row+1][col-1] == 0.0) gridMap[row+1][col-1] = -1.0;
}
}
}
}

for (row = 0; row < iheight; row++)

{
for (col= 0; col< iwidth; col++)
{
if (gridMap[row][col] == -1.0) gridMap[row][col] = 1.0;
}
}
}// end of function grow
///////////////////////////////////////////////////////////////////
// propagate wavefront for single step
///////////////////////////////////////////////////////////////////
void propagate(double start, double finish, int starty, int startx, int& done)
{
int i,j,k=0;
for (i = 0; i < iheight; i++)
{
for (j = 0; j < iwidth; j++)
{
if (gridMap[i][j] == start)
{
if ((i == starty) && (j == startx))
{
done = 1;
} else
{
if (i > 0)
{
if (gridMap[i-1][j] == 0.0) {gridMap[i-1][j] = finish; k=1;
}
}
if (j > 0)
{
if (gridMap[i][j-1] == 0.0) {gridMap[i][j-1] = finish; k=1;
}
 }
if (i < (iheight-1))
{
if (gridMap[i+1][j] == 0.0) {gridMap[i+1][j] = finish; k=1;
}
}
if (j < (iwidth-1))
{
if (gridMap[i][j+1] == 0.0) {gridMap[i][j+1] = finish; k=1;
}
}
if ((j < (iwidth-1)) && (i < (iheight-1)))
{
if (gridMap[i+1][j+1] == 0.0) {gridMap[i+1][j+1] = finish; k=1;
}
}
if ((j < (iwidth-1)) && (i > 0))
{
if (gridMap[i-1][j+1] == 0.0) {gridMap[i-1][j+1] = finish; k=1;
}
}
if ((j > 0) && (i > 0))
{
if (gridMap[i-1][j-1] == 0.0) {gridMap[i-1][j-1] = finish; k=1;
}
}
if ((j > 0) && (i < (iheight-1)))
{
if (gridMap[i+1][j-1] == 0.0) {gridMap[i+1][j-1] = finish; k=1;
}
}
}
}
}
}
if (k == 0) done = 2;
}// end of function propagate
////////////////////////////////////////////////////////////////
// compute next waypoint
///////////////////////////////////////////////////////////////
void nextWaypoint(int count)
{
if (gridMap[waypointy[count]+1][waypointx[count]+1] == (gridMap[waypointy[coun
t]][waypointx[count]]-1.0))
{
waypointy[count+1] = waypointy[count]+1;
waypointx[count+1] = waypointx[count]+1;
}
if (gridMap[waypointy[count]-1][waypointx[count]+1] == (gridMap[waypointy[coun
t]][waypointx[count]]-1.0))
{
waypointy[count+1] = waypointy[count]-1;
waypointx[count+1] = waypointx[count]+1;
}
if (gridMap[waypointy[count]-1][waypointx[count]-1] == (gridMap[waypointy[coun
t]][waypointx[count]]-1.0))
{
waypointy[count+1] = waypointy[count]-1;
waypointx[count+1] = waypointx[count]-1;
 }
if (gridMap[waypointy[count]+1][waypointx[count]-1] == (gridMap[waypointy[coun
t]][waypointx[count]]-1.0))
{
waypointy[count+1] = waypointy[count]+1;
waypointx[count+1] = waypointx[count]-1;
}
if (gridMap[waypointy[count]-1][waypointx[count]] == (gridMap[waypointy[count]
][waypointx[count]]-1.0))
{
waypointy[count+1] = waypointy[count]-1;
waypointx[count+1] = waypointx[count];
}
if (gridMap[waypointy[count]][waypointx[count]-1] == (gridMap[waypointy[count]
][waypointx[count]]-1.0))
{
waypointy[count+1] = waypointy[count];
waypointx[count+1] = waypointx[count]-1;
}
if (gridMap[waypointy[count]+1][waypointx[count]] == (gridMap[waypointy[count]
][waypointx[count]]-1.0))
{
waypointy[count+1] = waypointy[count]+1;
waypointx[count+1] = waypointx[count];
}
if (gridMap[waypointy[count]][waypointx[count]+1] == (gridMap[waypointy[count]
][waypointx[count]]-1.0))
{
waypointy[count+1] = waypointy[count];
waypointx[count+1] = waypointx[count]+1;
}
}// end of function nextWaypoint
////////////////////////////////////////////////////////////
// relax waypoints
////////////////////////////////////////////////////////////
void relaxWaypoints(int& count)
{
int i, pos, oldcount;
int tmpx[10000];
int tmpy[10000];
oldcount = 10000;
while (count < oldcount)
{
pos = 0;
oldcount = count;
cout << "count=" << count << endl;
while (pos < (count-1))
{
if (clear(waypointx[pos],waypointy[pos],
waypointx[pos+2],waypointy[pos+2]) == 1)
valid[pos+1] = 0;
pos = pos + 2;
}
pos = 0;
for (i = 0; i < (count+1); i++)
 {
if (valid[i] == 1)
{
tmpx[pos] = waypointx[i];
tmpy[pos] = waypointy[i];
pos++;
}
}
count = pos - 1;
for (i = 0; i < (count+1); i++)
{
waypointx[i] = tmpx[i];
waypointy[i] = tmpy[i];
valid[i] = 1;
}
}
}

////////////////////////////////////////////////////////
// clear wavefront from occupancy grid
////////////////////////////////////////////////////////
int clear(int x1, int y1, int x2, int y2)
{
int i,j,imin,imax,jmin,jmax,c=1;
imin = x1;
imax = x2;
if (x1 > x2){imax = x1; imin = x2;}
jmin = y1;
jmax = y2;
if (y1 > y2){jmax = y1; jmin = y2;}
double s = (double)(y2-y1)/(double)(x2-x1);
for (i = imin; i < (imax + 1); i++){
for (j = jmin; j < (jmax + 1); j++){
if (fabs(fabs((double)(j-y1))-fabs((double)(i-x1)*s)) < 2.0){
if (gridMap[j][i] == 1.0) c = 0;
}
}
}
return c;
}