//-*-c++-*-
#ifndef INCLUDED_Soccer_h_
#define INCLUDED_Soccer_h_


// ssh student@chiara1e 'killall -9 tekkotsu-CHIARA'

#include <iostream>
using namespace std;

#include "Behaviors/Leapers.h"

Shape<BlobData> ballMarker;
Shape<BlobData> chiaraMarker;
Point chiaraCenter;
bool decidelr;
bool IsLeft;
bool IsRight;

float relativeAngle;

#nodeclass Soccer: LeapMachine

    #shortnodeclass FindBlobs : MapBuilderNode($,MapBuilderRequest::localMap) : constructor
	std::vector<Point> pts;
	pts.push_back(Point(9000,0,-2000));
	pts.push_back(Point(500,0,0));
	pts.push_back(Point(2000,5000,-500));
	pts.push_back(Point(2000,5000,-5000));
	pts.push_back(Point(2000,-5000,-500));
	pts.push_back(Point(2000,-5000,-5000));
	pts.push_back(Point(2000,0,-4000));
	NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, pts, false));
	mapreq.searchArea = gazePoly;
        mapreq.motionSettleTime = 2000;
        mapreq.maxDist = 1e6;
        mapreq.clearShapes = true;
        mapreq.pursueShapes = false;
        mapreq.rawY = true;
        mapreq.addObjectColor(blobDataType, "pink");
        mapreq.addObjectColor(blobDataType, "red");
	mapreq.minBlobAreas[ProjectInterface::getColorIndex("pink")] = 25;
	mapreq.minBlobAreas[ProjectInterface::getColorIndex("red")] = 25;

    #shortnodeclass GetPinkBall : VisualRoutinesStateNode : DoStart
	NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
	cout<<"I saw in total "<<blobs.size()<<" blobs."<<endl;
	NEW_SHAPEVEC(colored, BlobData, subset(blobs, IsColor("pink")));
	cout<<"Of those, "<<colored.size()<<" were pink"<<endl;
	if(colored.empty()) {
		postStateFailure();
		return;
	}
	cout<<"I have a biggest pink"<<endl;
	ballMarker = max_element(colored, BlobData::AreaLessThan());
	postStateCompletion();
	
    #shortnodeclass GetChiara : VisualRoutinesStateNode : DoStart
	NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
	NEW_SHAPEVEC(colored, BlobData, subset(blobs, IsColor("red")));
	cout<<"Of those, "<<colored.size()<<" were red"<<endl;
	if(colored.empty()) {
		postStateFailure();
		return;
	}
	chiaraCenter.setCoords(0,0,0);
	SHAPEVEC_ITERATE(colored, BlobData, blob)
                chiaraCenter += blob->getCentroid();
        END_ITERATE;
        chiaraCenter.setCoords(chiaraCenter.coordX() / colored.size(), chiaraCenter.coordY() / colored.size(), chiaraCenter.coordZ() / colored.size());
	chiaraMarker = max_element(colored, BlobData::AreaLessThan());
	postStateCompletion();

    #shortnodeclass OrientToBlob : StateNode : DoStart
	Point cent = ballMarker->getCentroid();
	float ang = atan2(cent.coordY(), cent.coordX());
	if(abs(ang) < M_PI/18)
		postStateCompletion();
	else
		postStateSignal<float>(ang);

    #shortnodeclass WalkToBlob : StateNode : DoStart
	Point cent = ballMarker->getCentroid();
	float dist = (cent.coordX());
	cout<<"I WANT TO MOVE "<<dist -350<<endl<<endl<<endl;
	if(335 < abs(dist) && abs(dist) < 365)
		postStateCompletion();
	else
		postStateSignal<float>(dist -350);

    #shortnodeclass FindAngle : StateNode : DoStart
	Point a = ballMarker->getCentroid();
	Point b = chiaraCenter;//Point(chiaraMarker->getBoundingBox().xmin, (chiaraMarker->getBoundingBox().ymin + chiaraMarker->getBoundingBox().ymax)/2, 0);
	cout	<<"Ball is at "<<a.coordX()<<", "<<a.coordY()<<", "<<a.coordZ()<<endl
		<<"Chiara is at "<<b.coordX()<<", "<<b.coordY()<<", "<<b.coordZ()<<endl;
	NEW_SHAPE(ballcenter, PointData, PointData(localShS, a));
	NEW_SHAPE(chiarabottom, PointData, PointData(localShS, b));
	Point c = b - a;
	float aLen = a.xyNorm();
	float bLen = b.xyNorm();
	float cLen = c.xyNorm();
	relativeAngle = acos((aLen*aLen + bLen*bLen - cLen*cLen) / (2*aLen*bLen));
	cout<<"THE ANGLE IS "<<relativeAngle<<endl<<endl<<endl;
	if(b.coordY() > a.coordY()) {
		decidelr = true;
		postStateCompletion();	//chiara is to the left of ball
	} 
	else {
		decidelr = false;
		postStateFailure();	//chiara is to the right of ball
	}
		
    #shortnodeclass WhackDecide : StateNode : DoStart
    	if(decidelr == true)
    		postStateCompletion();
    	else
    		postStateFailure();

    #shortnodeclass TestAngle : StateNode : DoStart
	if(relativeAngle > M_PI * (3/8.0) && relativeAngle < M_PI * (5/8.0))
		postStateCompletion();
	else
		postStateFailure();

    #shortnodeclass RandomLeft : StateNode : DoStart
	IsLeft = true;
	postStateCompletion();
	
    #shortnodeclass RandomRight : StateNode : DoStart
	IsRight = true;
	postStateCompletion();

    #shortnodeclass RandomDecision : StateNode : DoStart
    	if(IsLeft == true)
    		postStateCompletion();
    	else
    		postStateFailure();

    #shortnodemethod setup
	#statemachine

		//Declarations
		startnode : StateNode
		legdownstart : LegsDown
		legsupstart : LegsUp
		randomt : StateNode
		map : FindBlobs
		mapcomplete : FindBlobs
		maptest : FindBlobs
		pinkball : GetPinkBall
		chiara : GetChiara
		pinkballtest : GetPinkBall
		chiaratest : GetChiara
		findangle : FindAngle
		findangletest : FindAngle
		testangle : TestAngle
		searchturn : Turn($, M_PI/2)
		searchturntest : Turn($, M_PI/2)
		orientblob : OrientToBlob
		orientblobtest : OrientToBlob
		orientblobcomplete : OrientToBlob
		rotateblobo : OrientToBlob
		walkblobcomplete : WalkToBlob
		rotateblobw : WalkToBlob
		whackdecide : WhackDecide
		sideways : WhackDecide
		shiftl : Sideways ($, 100)
		shiftr : Sideways ($, -100)
		swingr : SwingArmRight($, 100)
		swingl : SwingArmLeft($, 100)
		swingrfail : SwingArmRight($, 100)
		swinglfail : SwingArmLeft($, 100)
		whackr : SwingArmRight($, 100)
		whackl : SwingArmLeft($, 100)
		whackrfail : SwingArmRight($, 100)
		whacklfail : SwingArmLeft($, 100)
		legdown : LegsDown
		legup : LegsUp
		legdowntest : LegsDown
		leguptest : LegsUp
		swingfail : SwingArmLeft($, 100)
		whackfail : SwingArmRight($, 100)
		orientblobfail : OrientToBlob
		walkblobfail : WalkToBlob
		randomleft : RandomLeft
		randomright : RandomRight
		randomdecision : RandomDecision
		legdownfinall : LegsDown
		legdownfinalr : LegsDown
		legupfinal : LegsUp
		endrotatel : Turn($, M_PI/2)
		endrotater : Turn($, -M_PI/2)

		//Maps and info getting
		startnode =T(5000)=> legsupstart
		legsupstart =B(ChiaraInfo::RedButOffset)=> legdownstart
		legsupstart =B(ChiaraInfo::YellowButOffset)=> legdownstart
		legsupstart =B(ChiaraInfo::GreenButOffset)=> legdownstart
		legdownstart =T(3000)=> map
		map =MAP=> pinkball
		pinkball =C=> orientblob
		pinkball =F=> searchturn
		searchturn =C=> map
		orientblob =S<float>=> Turn =C=> map
		orientblob =C=> chiara
		
		//Failure to find Chiara
		chiara =F=> randomt
		randomt =RND=> {randomleft, randomright}
		randomleft =C=> swinglfail
		randomright =C=> swingrfail
		swinglfail =C=> orientblobfail
		swingrfail =C=> orientblobfail
		orientblobfail =S<float>=> Turn =C=> map
		orientblobfail =C=> walkblobfail
		walkblobfail =S<float>=> Forward =C=> map
		walkblobfail =C=> legup
		legup =T(3000)=> randomdecision
		randomdecision =C=> whackrfail
		randomdecision =F=> whacklfail
		whacklfail =C=> startnode
		whackrfail =C=> startnode
		
		//Completion to find Chiara
		chiara =C=> findangle
		findangle =C=> swingr
		findangle =F=> swingl
		swingr =C=> orientblobcomplete
		swingl =C=> orientblobcomplete
		orientblobcomplete =S<float>=> Turn =C=> map
		orientblobcomplete =C=> walkblobcomplete
		walkblobcomplete =S<float>=> Forward =C=> map
		walkblobcomplete =C=> maptest
		
		//Test and Complete
		maptest =MAP=> pinkballtest
		pinkballtest =C=> orientblobtest
		orientblobtest =S<float>=> Turn =C=> maptest
		orientblobtest =C=> chiaratest
		pinkballtest =F=> searchturntest
		searchturntest =C=> maptest
		chiaratest =C=> findangletest
		chiaratest =F=> randomt
		findangletest =C=> testangle
		findangletest =F=> testangle
		testangle =C=> rotateblobw
		testangle =F=> sideways
		sideways =C=> shiftl
		sideways =F=> shiftr
		shiftl =C=> rotateblobo
		shiftr =C=> rotateblobo
		rotateblobo =S<float>=> Turn =C=> maptest
		rotateblobo =C=> maptest
		rotateblobw =S<float>=> Forward =C=> maptest
		rotateblobw =C=> leguptest
		leguptest =T(3000)=> whackdecide
		whackdecide =C=> whackl
		whackdecide =F=> whackr
		whackl =C=> legdownfinall
		whackr =C=> legdownfinalr
		legdownfinall =T(4000)=> endrotatel
		legdownfinalr =T(4000)=> endrotater
		endrotatel =C=> legupfinal
		endrotater =C=> legupfinal
		legupfinal =T(4000)=> startnode
		
        #endstatemachine

#endnodeclass

#endif