#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include "sensor_msgs/PointCloud.h"
#include "nav_msgs/Odometry.h"
#include <vector>
#include <cmath>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>
#include <list>
#include <tf/tf.h>
#include <boost/foreach.hpp>

#define foreach BOOST_FOREACH

struct POSE {
	float x, y, z, theta;
} myPose;

struct CELL {
	bool operator ==(const CELL& rhs) {
		return ((int) x == (int) rhs.x) && ((int) y == (int) rhs.y);
	}
	float x, y;
	int cost;
	int dir;
	bool block, open, closed;
} goalCell;

std::vector<CELL> path;
CELL current;
ros::Publisher velocity_publisher;
std::string FileName = "/home/robotics/catkin_ws/src/lab5/map2.txt";
double goalx = 4.5d;
double goaly = 9.0d;

CELL cellMake(float _x, float _y, bool _block) {
	CELL c;
	c.x = _x;
	c.y = _y;
	c.block = _block;
	return c;
}

bool cellEqual(CELL a, CELL b) {
	return a.x == b.x && a.y == b.y;
}

std::vector<CELL> cell_map;
CELL cCell;
std::list<CELL> openlist;

bool isCellSolid(float x, float y) {
	foreach(CELL c, cell_map)
	if(c.x == x && c.y == y)
	return c.block;
	return false;
}

CELL cellGet(float x, float y) {
	foreach(CELL c, cell_map)
	if(c.x == x && c.y == y)
	return c;
	return cellMake(0, 0, false);
}

int cellIndex(float x, float y) {
	for (int i = 0; i < cell_map.size(); ++i)
		if (cell_map[i].x == x && cell_map[i].y == y)
			return i;
	return -1;
}

void poseCallback(const nav_msgs::Odometry::ConstPtr &msg) {
	myPose.x = msg->pose.pose.position.x;
	myPose.y = msg->pose.pose.position.y;
	myPose.z = msg->pose.pose.position.z;
	tf::Quaternion q(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y,
			msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
	myPose.theta = tf::getYaw(q);
}

float getDistance(float cX, float cY, float dX, float dY) {
	return sqrt(pow((dX - cX), 2) + pow((dY - cY), 2));
}

std::vector<std::string> Tokenize(std::string Message) {
	std::vector < std::string > Tokens = std::vector<std::string>();
	std::string Directory(Message);
	size_t PathDepth = std::count(Directory.begin(), Directory.end(), ',') + 1;
	for (size_t n = 0; n < PathDepth; n++) {
		Tokens.push_back(
				std::string(Directory.substr(0, Directory.find(',')).c_str()));
		Directory.erase(0, Directory.find(',') + 1);
	}
	return Tokens;
}

void euclidean(CELL n, int d) {
	int heuristic = getDistance(cCell.x, cCell.y, n.x, n.y);
	int nextStepCost = cCell.cost + heuristic;
	int i = cellIndex(n.x, n.y);

	if (nextStepCost < cell_map[i].cost) {
		cell_map[i].open = false;
		cell_map[i].closed = false;
		openlist.remove(cell_map[i]);
	}

	if (!cell_map[i].open && !cell_map[i].closed) {
		cell_map[i].cost = nextStepCost;
		cell_map[i].dir = d;
		cell_map[i].open = true;
		openlist.push_back(cell_map[i]);
	}
}

//Cells in this context are defined at midpoints,
//therefore it's not explicit what their bounds are
bool isRobotInCell(CELL c)
{
	return getDistance(myPose.x, myPose.y, c.x, c.y) < 0.5;
}

CELL getNextCellInPath()
{
	bool inCell = false;
	foreach(CELL c, path) {
		if(inCell) 
			return c;
		else if(isRobotInCell(c))
			inCell = true;
	}
	return current; //fail safe
}

int generatePath() {
	
	cell_map = std::vector<CELL>();

	//First I load and generate all the cells
	std::ifstream file(FileName.c_str());
	if (!file.good()) {
		std::cout << "Could not find map.txt, ensure file exists in working dir." << std::endl;
		return 1;
	}

	std::string line = "";
	int y = 10;
	while (std::getline(file, line)) {
		std::vector < std::string > raw = Tokenize(line);
		for (int x = 0; x < raw.size(); ++x)
			if (raw[x].length() > 0) { //removes the end line space 
				bool block = atoi(raw[x].c_str());
				float _x = x - 9;
				cell_map.push_back(cellMake(_x, y, block));				
			}
		--y;
	}
	file.close();

	//Now I have to plot the path using the A* algorithm
	//Ensure that we aren't starting in partial cells
	int __x = (int) myPose.x;
	int __y = (int) myPose.y;
	std::cout << "Starting at: " << __x << ", " << __y << std::endl;
	int __gx = (int) std::floor(goalx);
	int __gy = (int) std::floor(goaly);	
	goalCell = cellMake(__gx, __gy, false);
	std::cout << "Ending at: " << __gx << ", " << __gy << std::endl;
	int __cx = __gx;
	int __cy = __gy;

	bool foundPath = false;
	CELL dCell = cellMake(__gx, __gy, false);
	cCell = cellMake(__x, __y, false);
	openlist = std::list<CELL>();
	openlist.push_back(cCell);

	while (!openlist.empty()) {
		cCell = openlist.front();

		if (cellEqual(cCell, dCell)) {
			foundPath = true;
			__cx = cCell.x;
			__cy = cCell.y;
			break;
		}

		cCell.open = false;
		cCell.closed = true;
		openlist.remove(cCell);
		
		CELL west = cellGet(cCell.x - 1, cCell.y);
		CELL east = cellGet(cCell.x + 1, cCell.y);
		CELL south = cellGet(cCell.x, cCell.y - 1);
		CELL north = cellGet(cCell.x, cCell.y + 1);
		CELL nw = cellGet(cCell.x - 1, cCell.y + 1);
		CELL ne = cellGet(cCell.x + 1, cCell.y + 1);
		CELL sw = cellGet(cCell.x - 1, cCell.y - 1);
		CELL se = cellGet(cCell.x + 1, cCell.y - 1);		

		if (cCell.x - 1 > -9) 		
			if (!west.block)
				euclidean(west, 2);		

		if (cCell.x + 1 < 8) 			
			if (!east.block)
				euclidean(east, 8);		

		if (cCell.y - 1 > -9) 		
			if (!south.block)
				euclidean(south, 1);		

		if (cCell.y + 1 < 10) 		
			if (!north.block)
				euclidean(north, 4);
		
		
		if (cCell.y + 1 < 10 && cCell.x - 1 > -9) 		
			if (!nw.block && !north.block && !west.block)
				euclidean(nw, 6);		

		if (cCell.y + 1 < 10 && cCell.x + 1 < 8) 		
			if (!ne.block && !north.block && !east.block)
				euclidean(ne, 12);		

		if (cCell.y - 1 > -9 && cCell.x - 1 > -9) 		
			if (!sw.block && !south.block && !west.block)
				euclidean(sw, 3);
		

		if (cCell.y - 1 > -9 && cCell.x + 1 < 8) 		
			if (!se.block && !south.block && !east.block)
				euclidean(se, 9);		
		
	}

	path = std::vector<CELL>();
	if (foundPath) {
		CELL a = cellGet(__cx, __cy);
		while (__cx != __x || __cy != __y) {
			a = cellGet(__cx, __cy);
			path.push_back(a);

			if (a.dir & 2)
				++__cx;
			else if (a.dir & 8)
				--__cx;

			if (a.dir & 1)
				++__cy;
			else if (a.dir & 4)
				--__cy;
		}
	} else {
		std::cout << "We couldn't find a path, ending!" << std::endl;
		return 2;
	}

	path.push_back(cellMake(__x, __y, false)); 
	std::reverse(path.begin(), path.end());	

	std::cout << "Here is the A* path we found: " << std::endl;
	for (int i = 0; i < path.size(); ++i)
		std::cout << path[i].x << ", " << path[i].y << ", " << path[i].block << std::endl;
	std::cout << "-----------------------------" << std::endl;	
	
	return 0;
}

int main(int argc, char** argv) {
	
	std::cout << "Setting the initial path" << std::endl;
	myPose.x = -8.0f;
	myPose.y = -2.0f;
	
	int initialSetup = generatePath();
	
	if(initialSetup > 0)
		return initialSetup;

	
	ros::init(argc, argv, "astar");

	ros::NodeHandle n;
	velocity_publisher = n.advertise < geometry_msgs::Twist > ("cmd_vel", 1000);

	ros::NodeHandle n3;
	ros::Subscriber pose_subscriber = n3.subscribe("base_pose_ground_truth", 1000, poseCallback);

	ros::Rate r(30);
	
	bool reachedDestination = false;
	std::string paramx, paramy;

	while (ros::ok()) {			
		
		if (n.searchParam("goalx", paramx) && n.searchParam("goaly", paramy)) {		
			double newx, newy;
			n.getParam(paramx, newx);
			n.getParam(paramy, newy);
			if(goalx != newx || newy != goaly) {				
				goalx = newx;
				goaly = newy;
				std::cout << "Creating new goal at " << newx << ", " << newy << std::endl;
				if(generatePath() > 0) {
					std::cout << "Failed to generate the new path!" << std::endl;
					return 3;
				}		
			}
		}		
	
		if((getDistance(myPose.x, myPose.y, goalx, goaly) > 0.8)) {
			geometry_msgs::Twist msg;
			current = getNextCellInPath();
			msg.linear.x = 2 * sqrt(pow((current.x - myPose.x), 2) + pow((current.y - myPose.y), 2));
			float angleR = atan2(current.y - myPose.y, current.x - myPose.x);
			angleR = atan2(sin(angleR), cos(angleR)); //normlization
			msg.angular.z = angleR - myPose.theta;
			msg.angular.z *= 4;
			velocity_publisher.publish(msg);
		} else if(!reachedDestination) {
			reachedDestination = true;
			std::cout << "Arrived at destination" << std::endl;
		}

		r.sleep();
		ros::spinOnce();
	}	 

	return 0;

}