#ifndef INCLUDED_BallChaser_h_
#define INCLUDED_BallChaser_h_

#include "Behaviors/BehaviorBase.h"
#include "Events/EventRouter.h"
#include "Events/VisionObjectEvent.h"
#include "Motion/HeadPointerMC.h"
#include "Motion/MMAccessor.h"
#include "Motion/MotionManager.h"
#include "Motion/ArmMC.h"
#include "Motion/MotionSequenceMC.h"
#include "Shared/ProjectInterface.h"
#include "Shared/WorldState.h"
#include "Shared/mathutils.h"
#include "DualCoding/VisualRoutinesBehavior.h"
#include "DualCoding/VisualRoutinesStateNode.h"
#include "DualCoding/Sketch.h"
#include "DualCoding/visops.h"
#include "DualCoding/EllipseData.h"
#include "DualCoding/ShapeEllipse.h"

#include <math.h>

using namespace mathutils;
using namespace std;
using namespace DualCoding;

//---------------------------------------------
//BlockChaser class
//
//This class contains the core logic of this program.
//It is also a VisualRoutinesStateNode so it can be put into a state machine
//It will send a completion signal when the behavior is complete
//---------------------------------------------
class BlockChaser : public VisualRoutinesStateNode {
    public:
        BlockChaser(string name, unsigned int targetSID, string color);

        void DoStart(); //Start function for the behavior

        void DoStop(); //End function for the behavior

        void processEvent(const EventBase& event); //Handles visual object events, timers and motion manager events

        bool findArm(bool printResults = true); //Checks to see if the gripper has surrounded the target

        static void addLineToSketch( Sketch<bool>& sketch, EndPoint point1, EndPoint point2 ); //Adds a line defined by the endpoints
																							   //to the sketch

        static inline unsigned int findArmIndex( unsigned int offset )
        {
			//This function will return the output index for the offset passed to the function
			//It is used to find references to the correct servos
            return capabilities.findOutputOffset(RobotInfo::outputNames[RobotInfo::ArmOffset+offset]);
        }

        static inline std::string getClassDescription()
        {
			//Called from getDescription, returns this class's description
            return "Grabs the block of the specified color";
        }

        virtual inline std::string getDescription() const
        {
			//Returns this class's description
            return getClassDescription();
        }

    protected:
        void addDefaultListeners(); //Initlialization function, adds listeners for visual object events and timers
        void updateTargetPosition(); //Re-calculate where in space the target is located
        void alignWithTarget(); //Align the gripper with the target along the y-axis
        void moveToTarget(); //Use moveToPoint to move the gripper to the target
        void grabAndMoveTarget(); //Close the gripper, move it to a new location and open the gripper
        void alignGripperWithTarget(); //Make sure the gripper is pointing at the target
        void returnToAlignment(); //Should the gripper miss then this will set the program back to the alignment step

        MotionManager::MC_ID headpointer_id; //Motion Manager id for the webcam servo motion
        MotionManager::MC_ID armSequence_id; //Motion Manager id for the arm servo motion
        MotionManager::MC_ID armMC_id; //Motion manager id for the Arm Motion Controller

		//Enumerates the various steps involved in this program
        enum status
        {
            IDLE, //Waiting for the target to be detected
            AQUIRING_TARGET, //Focusing the camera on the target
            ALIGNING_WITH_TARGET, //Aligning the gripper with the target vertically
            GRABBING_TARGET, //Moving to the target
            WAITING_FOR_GRAB, //Waiting for the moving to finish
            CHECKING_GRAB, //Checking to see if the gripper will be able to grab the target
            DONE_GRABBING //Final step, grab and move the target
        };

		//Stores the current status of the program
        status m_status;

        unsigned int TargetEventSID; //Vision object event ID for the target color
        string TargetColor; //Target color description (i.e. "pink", "red", etc.)

        unsigned int shldrIdx; //Servo index for the shoulder of the arm
        unsigned int elbowIdx; //Servo index for the elbow of the arm
        unsigned int wristIdx; //Servo index for the wrist of the arm
        unsigned int leftGripperIdx; //Servo index for the left half of the gripper
        unsigned int rightGripperIdx; //Servo index for the right half of the gripper
        unsigned int panIdx; //Servo index for the pan camera servo
        unsigned int tiltIdx; //Servo index for the tilt camera servo

        float alignedShldr; //Servo value for the shoulder when the arm is aligned vertically
        float alignedElbow; //Servo value for the elbow when the arm is aligned vertically
        float alignedWrist; //Servo value for the wrist when the arm is aligned vertically

        float lastHoriz; //Last x-coordinate in camera space of the target 
        float lastVert; //Last y-coordinate in camera space of the target 

        float yDist; //Last measurment in inches for how far the target is from the base in the y-axis
        float xDist; //Last measurment in inches for how far the target is from the base in the x-axis
};

//Child class of BlockChaser, it searches for Pink Targets
class PinkBlockChaser : public BlockChaser
{
    public:
        PinkBlockChaser()
        : BlockChaser("PinkBlockChaser", ProjectInterface::visPinkBallSID, "pink")
        {}
};

//Child class of BlockChaser, it searches for Red Targets
class RedBlockChaser : public BlockChaser
{
    public:
        RedBlockChaser()
        : BlockChaser("RedBlockChaser", ProjectInterface::visRedBallSID, "red")
        {}
};

//Child class of BlockChaser, it searches for Orange Targets
class OrangeBlockChaser : public BlockChaser
{
    public:
        OrangeBlockChaser()
        : BlockChaser("OrangeBlockChaser", ProjectInterface::visOrangeSID, "orange")
        {}
};

//Child class of BlockChaser, it searches for Green Targets
class GreenBlockChaser : public BlockChaser
{
    public:
        GreenBlockChaser()
        : BlockChaser("GreenBlockChaser", ProjectInterface::visGreenBallSID, "green")
        {}
};

//Child class of BlockChaser, it searches for Yellow Targets
class YellowBlockChaser : public BlockChaser
{
    public:
        YellowBlockChaser()
        : BlockChaser("YellowBlockChaser", ProjectInterface::visYellowBallSID, "yellow")
        {}
};

#endif