#include "Planners/FF/FFPlanNode.h"
#include "Behaviors/StateMachine.h"
#include "Behaviors/Demos/Navigation/PilotDemo.h"

#define DOMAIN_FILE_NAME "/usr0/home/andrewba/Downloads/domain.pddl"
#define PROBLEM_FILE_NAME "/usr0/home/andrewba/Downloads/problem.pddl"
#define PLAN_FILE_NAME "/usr0/home/andrewba/Downloads/plan.txt"
#define PLAN_FILE_NAME_TEST "/usr0/home/andrewba/Downloads/planTest.txt"

$nodeclass ParentPlan : PilotDemo {

	// declare structure data types and global variables for generating problem file

	struct cyl {
		int id;
		int currLoc;
		int goalLoc;
	};
	
	struct loc {
		int id;
		int atLeft;
		int atRight;
		int atTop;
		int atBottom;
		int empty;
	};

	$provide vector<cyl> cylForProblem;
	$provide vector<loc> locForProblem;

	virtual void buildMap() {

		// set up locations (where the cylinders can potentially be located as goal states)

		NEW_SHAPE(location1, EllipseData,
			new EllipseData(worldShS, Point(-1000*0,1000*0,0,allocentric), 500, 500));
			location1->setColor("blue");
			location1->setObstacle(false);

		NEW_SHAPE(location2, EllipseData,
			new EllipseData(worldShS, Point(-1000*0,1000*1,0,allocentric), 500, 500));
			location2->setColor("blue");
			location2->setObstacle(false);
		
		NEW_SHAPE(location3, EllipseData,
			new EllipseData(worldShS, Point(-1000*0,1000*2,0,allocentric), 500, 500));
			location3->setColor("blue");
			location3->setObstacle(false);
		
		NEW_SHAPE(location4, EllipseData,
			new EllipseData(worldShS, Point(-1000*1,1000*0,0,allocentric), 500, 500));
			location4->setColor("blue");
			location4->setObstacle(false);
		
		NEW_SHAPE(location5, EllipseData,
			new EllipseData(worldShS, Point(-1000*1,1000*1,0,allocentric), 500, 500));
			location5->setColor("blue");
			location5->setObstacle(false);
		
		NEW_SHAPE(location6, EllipseData,
			new EllipseData(worldShS, Point(-1000*1,1000*2,0,allocentric), 500, 500));
			location6->setColor("blue");
			location6->setObstacle(false);
		
		NEW_SHAPE(location7, EllipseData,
			new EllipseData(worldShS, Point(-1000*2,1000*0,0,allocentric), 500, 500));
			location7->setColor("blue");
			location7->setObstacle(false);
		
		NEW_SHAPE(location8, EllipseData,
			new EllipseData(worldShS, Point(-1000*2,1000*1,0,allocentric), 500, 500));
			location8->setColor("blue");
			location8->setObstacle(false);
		
		NEW_SHAPE(location9, EllipseData,
			new EllipseData(worldShS, Point(-1000*2,1000*2,0,allocentric), 500, 500));
			location9->setColor("blue");
			location9->setObstacle(false);			
		
		// declare and set up initial cylinder locations 

		NEW_SHAPE(cyl1, EllipseData,
			new EllipseData(worldShS, Point(700*3,-700*2,66,allocentric), 100, 100));
			cyl1->setColor("red");
			cyl1->setObstacle(true);

		NEW_SHAPE(cyl2, EllipseData,
			new EllipseData(worldShS, Point(-1000*0,1000*1,66,allocentric), 100, 100));
			cyl2->setColor("blue");
			cyl2->setObstacle(true);
		
		NEW_SHAPE(cyl3, EllipseData,
			new EllipseData(worldShS, Point(700*1,-700*2,66,allocentric), 100, 100));
			cyl3->setColor("red");
			cyl3->setObstacle(true);
		
		NEW_SHAPE(cyl4, EllipseData,
			new EllipseData(worldShS, Point(700*0,-700*2,66,allocentric), 100, 100));
			cyl4->setColor("blue");
			cyl4->setObstacle(true);
		
		NEW_SHAPE(cyl5, EllipseData,
			new EllipseData(worldShS, Point(-700*1,-700*2,66,allocentric), 100, 100));
			cyl5->setColor("red");
			cyl5->setObstacle(true);
		
		NEW_SHAPE(cyl6, EllipseData,
			new EllipseData(worldShS, Point(-1000*1,1000*2,66,allocentric), 100, 100));
			cyl6->setColor("blue");
			cyl6->setObstacle(true);
		
		NEW_SHAPE(cyl7, EllipseData,
			new EllipseData(worldShS, Point(-700*3,-700*2,66,allocentric), 100, 100));
			cyl7->setColor("red");
			cyl7->setObstacle(true);
		
		NEW_SHAPE(cyl8, EllipseData,
			new EllipseData(worldShS, Point(-1000*2,1000*1,66,allocentric), 100, 100));
			cyl8->setColor("blue");
			cyl8->setObstacle(true);
		
		NEW_SHAPE(cyl9, EllipseData,
			new EllipseData(worldShS, Point(-700*5,-700*2,66,allocentric), 100, 100));
			cyl9->setColor("red");
			cyl9->setObstacle(true);

		// robot start and go-to point before starting any instance of PushIt

		//NEW_SHAPE(goToPoint, PointData,
		//	new PointData(worldShS, Point(-1000*1,-1000*5,0, allocentric)));
		//	goToPoint->setColor("green");
		//	goToPoint->setObstacle(false);
	}

	// declare global variables containing plan instruction variables (just two or three -- what to push, where to push it, from what angle to approach it), and global index counter 

	$provide vector<int> cylForPlan; 
	$provide vector<int> locForPlan;
	//$provide vector<int> approachAngle;
	$provide int indexCounter(0);

	$nodeclass Setup : StateNode : doStart {

		$reference ParentPlan::cylForProblem;
		$reference ParentPlan::locForProblem;
		
		cyl loadCyl1 = {1, 22, 1};
		cylForProblem.push_back(loadCyl1);
		
		cyl loadCyl2 = {2, 2, 2};
		cylForProblem.push_back(loadCyl2);

		cyl loadCyl3 = {3, 24, 3};
		cylForProblem.push_back(loadCyl3);

		cyl loadCyl4 = {4, 25, 4};
		cylForProblem.push_back(loadCyl4);

		cyl loadCyl5 = {5, 26, 5};
		cylForProblem.push_back(loadCyl5);

		cyl loadCyl6 = {6, 6, 6};
		cylForProblem.push_back(loadCyl6);

		cyl loadCyl7 = {7, 28, 7};
		cylForProblem.push_back(loadCyl7);

		cyl loadCyl8 = {8, 8, 8};
		cylForProblem.push_back(loadCyl8);

		cyl loadCyl9 = {9, 30, 9};
		cylForProblem.push_back(loadCyl9);

		loc loadLoc1 = {1, 2, 10, 19, 4, 1};
		locForProblem.push_back(loadLoc1);

		loc loadLoc2 = {2, 3, 1, 20, 5, 0};
		locForProblem.push_back(loadLoc2);

		loc loadLoc3 = {3, 18, 2, 21, 6, 1};
		locForProblem.push_back(loadLoc3);

		loc loadLoc4 = {4, 5, 11, 1, 7, 1};
		locForProblem.push_back(loadLoc4);

		loc loadLoc5 = {5, 6, 4, 2, 8, 1};
		locForProblem.push_back(loadLoc5);

		loc loadLoc6 = {6, 17, 5, 3, 9, 0};
		locForProblem.push_back(loadLoc6);

		loc loadLoc7 = {7, 8, 12, 4, 13, 1};
		locForProblem.push_back(loadLoc7);
	
		loc loadLoc8 = {8, 9, 7, 5, 14, 0};
		locForProblem.push_back(loadLoc8);

		loc loadLoc9 = {9, 16, 8, 6, 15, 1};
		locForProblem.push_back(loadLoc9);

		//after the 9th loc we don't care about the atLeft, atRight, atTop, atBottom fields, so "22" is arbitrary dummy for all after 9th loc, but we still care about the "id" and "empty" fields

		loc loadLoc10 = {10, 22, 22, 22, 22, 1}; 
		locForProblem.push_back(loadLoc10);

		loc loadLoc11 = {11, 22, 22, 22, 22, 1};
		locForProblem.push_back(loadLoc11);

		loc loadLoc12 = {12, 22, 22, 22, 22, 1};
		locForProblem.push_back(loadLoc12);

		loc loadLoc13 = {13, 22, 22, 22, 22, 1};
		locForProblem.push_back(loadLoc13);
		
		loc loadLoc14 = {14, 22, 22, 22, 22, 1};
		locForProblem.push_back(loadLoc14);

		loc loadLoc15 = {15, 22, 22, 22, 22, 1};
		locForProblem.push_back(loadLoc15);

		loc loadLoc16 = {16, 22, 22, 22, 22, 1};
		locForProblem.push_back(loadLoc16);

		loc loadLoc17 = {17, 22, 22, 22, 22, 1};
		locForProblem.push_back(loadLoc17);

		loc loadLoc18 = {18, 22, 22, 22, 22, 1};
		locForProblem.push_back(loadLoc18);

		loc loadLoc19 = {19, 22, 22, 22, 22, 1};
		locForProblem.push_back(loadLoc19);

		loc loadLoc20 = {20, 22, 22, 22, 22, 1};
		locForProblem.push_back(loadLoc20);

		loc loadLoc21 = {21, 22, 22, 22, 22, 1};
		locForProblem.push_back(loadLoc21);

		loc loadLoc22 = {22, 22, 22, 22, 22, 0};
		locForProblem.push_back(loadLoc22);

		loc loadLoc23 = {23, 22, 22, 22, 22, 1};
		locForProblem.push_back(loadLoc23);
		
		loc loadLoc24 = {24, 22, 22, 22, 22, 0};
		locForProblem.push_back(loadLoc24);

		loc loadLoc25 = {25, 22, 22, 22, 22, 0};
		locForProblem.push_back(loadLoc25);

		loc loadLoc26 = {26, 22, 22, 22, 22, 0};
		locForProblem.push_back(loadLoc26);

		loc loadLoc27 = {27, 22, 22, 22, 22, 1};
		locForProblem.push_back(loadLoc27);

		loc loadLoc28 = {28, 22, 22, 22, 22, 0};
		locForProblem.push_back(loadLoc28);

		loc loadLoc29 = {29, 22, 22, 22, 22, 1};
		locForProblem.push_back(loadLoc29);

		loc loadLoc30 = {30, 22, 22, 22, 22, 0};
		locForProblem.push_back(loadLoc30);

		postStateCompletion();
	}

	$nodeclass GenerateProblemFile : StateNode : doStart {

		$reference ParentPlan::cylForProblem;
		$reference ParentPlan::locForProblem;

		FILE *pfile = fopen(PROBLEM_FILE_NAME, "w");
		if (pfile == NULL) {
			cout << "Error writing problem file" << endl;
			postStateFailure();
		return;
		}
		fprintf(pfile, "(define (problem finishMosaic-problem)");
		fprintf(pfile, "\n");
		fprintf(pfile, "\t(:domain finishMosaic-domain)");
		fprintf(pfile, "\n");
		fprintf(pfile, "\t(:objects\n");
		fprintf(pfile, "\t\t");
		for (vector<cyl>::const_iterator it = cylForProblem.begin(); it != cylForProblem.end(); it++) {
			fprintf(pfile, "cyl%d ", it->id);
    		}
    		fprintf(pfile, "- cylinder\n");
		fprintf(pfile, "\t\t");
		for (vector<loc>::const_iterator it = locForProblem.begin(); it != locForProblem.end(); it++) {
			fprintf(pfile, "loc%d ", it->id);
    		}
		fprintf(pfile, "- location\n");
		fprintf(pfile, "\t)\n");
		fprintf(pfile, "\t(:init \n");
		for (vector<cyl>::const_iterator it = cylForProblem.begin(); it != cylForProblem.end(); it++) {
			fprintf(pfile, "\t\t(at cyl%d loc%d)\n", it->id, it->currLoc);
    		}
		for (vector<loc>::const_iterator it = locForProblem.begin(); it != locForProblem.end()-21; it++) {
			fprintf(pfile, "\t\t(atLeft loc%d loc%d)\n", it->atLeft, it->id);
    		}
		for (vector<loc>::const_iterator it = locForProblem.begin(); it != locForProblem.end()-21; it++) {
			fprintf(pfile, "\t\t(atRight loc%d loc%d)\n", it->atRight, it->id);
    		}
		for (vector<loc>::const_iterator it = locForProblem.begin(); it != locForProblem.end()-21; it++) {
			fprintf(pfile, "\t\t(atTop loc%d loc%d)\n", it->atTop, it->id);
    		}
		for (vector<loc>::const_iterator it = locForProblem.begin(); it != locForProblem.end()-21; it++) {
			fprintf(pfile, "\t\t(atBottom loc%d loc%d)\n", it->atBottom, it->id);
    		}
		for (vector<loc>::const_iterator it = locForProblem.begin(); it != locForProblem.end(); it++) {
			if (it->empty == 1) {
				fprintf(pfile, "\t\t(empty loc%d)\n", it->id);
			}
    		}
		fprintf(pfile, "\t)\n");
		fprintf(pfile, "\t(:goal (and \n");
		for (vector<cyl>::const_iterator it = cylForProblem.begin(); it != cylForProblem.end(); it++) {
			fprintf(pfile, "\t\t(at cyl%d loc%d)\n", it->id, it->goalLoc);
    		}
		fprintf(pfile, "\t\t)\n");
		fprintf(pfile, "\t)\n");
		fprintf(pfile, ")");
		fclose(pfile);

		cout << "At end of writing plan file node... is there a problem file now?" << endl;

		postStateCompletion();
	}

	$nodeclass Plan : FFPlanNode(DOMAIN_FILE_NAME) : doStart {

		problemFileName = PROBLEM_FILE_NAME;
		planFileName = PLAN_FILE_NAME;
	}

	$nodeclass ReadPlanFile : StateNode : doStart {

		$reference ParentPlan::cylForPlan;
		$reference ParentPlan::locForPlan;

		FILE *pfile = fopen(PLAN_FILE_NAME, "r");
		FILE *pfile2 = fopen(PLAN_FILE_NAME_TEST, "w");
		if (pfile == NULL) {
			cout << "Error reading plan file" << endl;
			postStateFailure();
			return;
		}

		char discard[5]; 
		char discard2[3];
		char direction[2]; 
		char cylinder[5]; 
		char discard3[6]; 
		char target[6]; 
		char discard4[6]; 
		char loadTarget[3];
		int cylReceive;
		int locReceive;

		fscanf(pfile, "%s", discard);
		while (fscanf(pfile, "%s %s %s %s %s %s", discard2, direction, cylinder, discard3, target, discard4) != EOF) {
			stringstream ss;
			ss << cylinder[3];
			ss >> cylReceive;
			cylForPlan.push_back(cylReceive);
			if (target[4] == '\0') {
				//cout << target[3] << endl;
				stringstream ss2;
				ss2 << target[3];		
				ss2 >> locReceive;
				locForPlan.push_back(locReceive);
			}
			else {
				loadTarget[0] = target[3];
				loadTarget[1] = target[4];
				loadTarget[2] = '\0';
				stringstream ss3;
				ss3 << loadTarget;		
				ss3 >> locReceive;
				locForPlan.push_back(locReceive);
			} 
  			fprintf(pfile2, "%s %s %s %s %s %s %d %d\n", discard2, direction, cylinder, discard3, target, discard4, cylReceive, locReceive);
		}		
		//$reference ParentPlan::approachAngle;
		fclose(pfile);
		fclose(pfile2);

		postStateCompletion();
	}

	$nodeclass PushIt : PilotNode(PilotTypes::pushObject) : doStart {

		$reference ParentPlan::cylForPlan;
		$reference ParentPlan::locForPlan;
		//$reference ParentPlan::approachAngle;
		$reference ParentPlan::indexCounter;

		string cylString;
		stringstream ss;
		string locationString;
		stringstream ss2;	

		ss << "cyl" << cylForPlan[indexCounter];
		ss >> cylString;

		ss2 << "location" << locForPlan[indexCounter];
		ss2 >> locationString;

		Shape<EllipseData> cyl = find_if<EllipseData>(worldShS, IsName(cylString));
		if ( !cyl.isValid()) return;
		Shape<EllipseData> location = find_if<EllipseData>(worldShS, IsName(locationString));
		if ( !location.isValid()) return; //instead do postStateFailure(); and then return, OR "cancel this request"
		pilotreq.objectShape = cyl; 
		pilotreq.targetShape = location;
		//pilotreq.displayObstacles = true;
    		//pilotreq.obstacleInflation = 30;
		//pilotreq.targetHeading = AngTwoPi(approachAngle[indexCounter]);  

		cyl->setObstacle(true);
		++indexCounter;
	}

	//$nodeclass GoToPoint : PilotNode(PilotTypes::goToShape) : doStart {
	//	$reference ParentPlan::cylinders;
	//	$reference ParentPlan::indexCounter;
	//
	//	string cylString;
	//	stringstream ss;
	//
	//	ss << "cyl" << cylinders[indexCounter];
	//	ss >> cylString;
	//
	//	NEW_SHAPE(cyl, EllipseData, find_if<EllipseData>(worldShS, IsName(cylString)));
	//
	//	cyl->setObstacle(true);
	//	GET_SHAPE(goToPoint, PointData, worldShS); 		
	//	pilotreq.targetShape = goToPoint;
	//}

	$setupmachine{
		
		startdemo: Setup =C=> GenerateProblemFile =C=> Plan =C=> ReadPlanFile =C=> PushIt =C=> PushIt =C=> PushIt =C=> PushIt =C=> PushIt =C=> PushIt =C=> SpeechNode("Done")

		//startdemo: GenerateProblemFile =C=> Plan =C=> ReadPlanFile =C=> executePlan
		//executePlan: PushIt =S(a)=> executePlan
		//executePlan: PushIt =S(b)=> done
		//done: SpeechNode("Done")
	}
}

REGISTER_BEHAVIOR(ParentPlan);