#include "BlockChaser.h"

using namespace mathutils;

BlockChaser::BlockChaser(string name, unsigned int targetSID, string color)
: VisualRoutinesStateNode(name) //Pass the name to the parent calss

, headpointer_id(MotionManager::invalid_MC_ID) //Store the various motion manager ids
, armSequence_id(MotionManager::invalid_MC_ID)
, armMC_id(MotionManager::invalid_MC_ID)

, m_status(IDLE) //Set the current status to IDLE

, TargetEventSID( targetSID ) //Store the visual event id
, TargetColor( color ) //Store the target color

, shldrIdx(0) //Set the servo indexes to 0
, elbowIdx(0)
, wristIdx(0)
, leftGripperIdx(0)
, rightGripperIdx(0)
, panIdx(0)
, tiltIdx(0)

, alignedShldr(0.0) //Set the aligned servo values to 0.0
, alignedElbow(0.0)
, alignedWrist(0.0)

, lastHoriz(0.0) //Set the last camera space target location to 0.0, 0.0
, lastVert(0.0)

, yDist(0.0) //Set the last world location of the target to 0.0, 0.0
, xDist(0.0)
{
}

void BlockChaser::DoStart()
{
	//Start function for the behavior

	BehaviorBase::DoStart(); //Call base behavior

	//Add a persistent motion which will control where the camera looks
	SharedObject<HeadPointerMC> head_mc;
	headpointer_id = motman->addPersistentMotion(head_mc);

	//Set the status to AQUIRING_TARGET
	m_status = AQUIRING_TARGET;

	//Find the servo indexes for the camera, arm and gripper
	panIdx = capabilities.findOutputOffset(RobotInfo::outputNames[RobotInfo::HeadOffset+RobotInfo::PanOffset]);
	tiltIdx = capabilities.findOutputOffset(RobotInfo::outputNames[RobotInfo::HeadOffset+RobotInfo::TiltOffset]);
	shldrIdx = findArmIndex( RobotInfo::ArmShoulderOffset );
	elbowIdx = findArmIndex( RobotInfo::ArmElbowOffset );
	wristIdx = findArmIndex( RobotInfo::WristYawOffset );
	leftGripperIdx = findArmIndex( RobotInfo::LeftGripperOffset );
	rightGripperIdx = findArmIndex( RobotInfo::RightGripperOffset );

	//Add the default listeners needed in the program
	addDefaultListeners();

	//Set the camera to point to one edge of the table where the targets should be located
	MMAccessor<HeadPointerMC> headpointer(headpointer_id);
	headpointer->setJoints(-0.995503,0.673052,-0.995503);
}

void BlockChaser::DoStop()
{
	//Stop function for the behavior

	if( headpointer_id != MotionManager::invalid_MC_ID && motman->isOwner( headpointer_id ) > 0 )
	{
		//Remove the head motion
		motman->removeMotion( headpointer_id );
	}
	
	//Send the completion event in case this behavior is in a state machine
	postStateCompletion();

	//Base stop function
	BehaviorBase::DoStop();
}

void BlockChaser::processEvent(const EventBase& event)
{
	//Process the timer, motion and vision events

	if( event.getGeneratorID() == EventBase::motmanEGID )
	{
		//A motion sequence event will be triggered when various motions are completed
		if( m_status == ALIGNING_WITH_TARGET )
		{
			//If the arm is finished aligning then move to grabbing target
			m_status = GRABBING_TARGET;
		}
		else if( m_status == WAITING_FOR_GRAB )
		{
			//If the arm has moved to try to grab the target
			if( event.getTypeID() == EventBase::statusETID )
			{
				//If the move is complete, move to checking grab
				m_status = CHECKING_GRAB;
			}
		}
		else if( m_status == DONE_GRABBING && event.getTypeID() == EventBase::statusETID )
		{
			//If the arm has grabbed the target then remove the listeners and stop the behavior
			erouter->remove( this );
			DoStop();
		}
		return;
	}
	else if( event.getGeneratorID() == EventBase::timerEGID )
	{
		//Handle timer events, timers are used to make sure the program
		//continues should no motion or vision events be triggered

		if( m_status == WAITING_FOR_GRAB )
		{
			//If we are moving to the target see if we are ready to grab it
			if( findArm( false ) )
			{
				//If we are ready to grab then move to Checking Grab
				m_status = CHECKING_GRAB;
			}
			else
			{
				//If we are not ready to grab make sure the gripper is pointing at the target
                alignGripperWithTarget();
			}
		}
		else if( m_status == CHECKING_GRAB )
		{
			//If we are done moving to the target, check to see if we are aligned properly
			bool onTarget = findArm();

			if( onTarget )
			{
				//If we are then go to DONE_GRABBING and grab and move the target
				m_status = DONE_GRABBING;

				grabAndMoveTarget();
			}
			else
			{
				//If we missed then print an error message
				cout << "Missed, retrying" << endl;

				//Remove the motion and listener
				motman->removeMotion( armMC_id );
				erouter->remove( this );

				//Add the default listeners again
				addDefaultListeners();

				//Return to the alignment step
				returnToAlignment();
			}
		}
		return;
	}
	else if(event.getGeneratorID()==EventBase::visObjEGID)
	{
		//The following code is adapted from the track ball behavior
		//It stores the horizontal and vertical coordinates of the target in camera space
		if(event.getTypeID()==EventBase::statusETID)
		{
			const VisionObjectEvent& objev=static_cast<const VisionObjectEvent&>(event);
			lastHoriz=objev.getCenterX();
			lastVert=objev.getCenterY();
		}
		else
		{
			const VisionObjectEvent &visev = *reinterpret_cast<const VisionObjectEvent*>(&event);
			float const minBallArea = 0.01;  // one percent of the camera image
			if ( visev.getTypeID() == EventBase::deactivateETID || visev.getObjectArea() < minBallArea )
			{
				return;
			}
		}
	}

	//Re-Calculate the target's world space location
	updateTargetPosition();

	if( ( abs( lastHoriz ) >= 0.01 || abs( lastVert ) >= 0.01 ) && m_status == AQUIRING_TARGET )
	{
		//If the target is not in the center of the camera then return early
		return; //Wait while the camera aligns
	}
	else if( m_status == AQUIRING_TARGET )
	{
		//If the target is in the center of the camera viewport align the gripper
		m_status = ALIGNING_WITH_TARGET;

        alignWithTarget();
	}
	else if( m_status == GRABBING_TARGET )
	{
		//If we have finished aligning then move towards the target
		m_status = WAITING_FOR_GRAB;
		moveToTarget();
	}
}

void BlockChaser::addDefaultListeners()
{
	//Add a timer listener and visual event listener
    erouter->addTimer(this, 5, 0);
    erouter->addListener(this,EventBase::timerEGID,5);
    erouter->addListener(this,EventBase::visObjEGID,TargetEventSID);
}

void BlockChaser::updateTargetPosition()
{
	// This code is adapted from the tracking behavior
	// move the camera a small distance in the direction of the target
	// This is "proportional" control, because we move the head proportionally further when the error (horiz and vert) is larger
	float tilt=state->outputs[tiltIdx] - ( lastVert*CameraFOV/7.0f );
	float pan=state->outputs[panIdx] + ( lastHoriz*CameraFOV/6.0f );

	// now request access to the headpointer we added in DoStart and set the joint angles
	MMAccessor<HeadPointerMC> headpointer(headpointer_id);

	// Set the camera to look at the target
	if(NumHeadJoints>2)
	tilt/=2; // we're going to replicate the tilt parameter in the next call, so divide by 2
	headpointer->setJoints(tilt,pan,tilt);

	float newTilt = deg2rad( 90.0 ) - abs( tilt ); //Subtract the tilt from 90 degrees to make it useable
	float newPan = -1 * pan; //Take the negative of the pan (assuming the target is on one side of the table)

	float distance = ( 20.875 * ( tan( newTilt ) ) ) + 3; //Distance from base to target directly

    //Rotate the distance by the pan
    //A2:=Cos (45)*A - Sin (45)*B
    //B2:=Sin (45)*A + Cos (45)*B
    xDist = ( -1 * distance ) * sin( newPan );
    yDist = distance * cos( newPan );
}

void BlockChaser::alignWithTarget()
{
	//Align the gripper with the target vertically
	if( xDist > 0 ) //Make sure the target is on the right side of the table
	{
	    cout << "Y Dist: " << yDist << endl;
		if( xDist > 20.25 )
		{
			//Make sure we can reach the target
			//20.25 = 8.5 + 7.5 + 4.25 = shoulder-elbow + elbow-wrist + wrist-gripper
			cout << "Out of range" << endl;
			return;
		}

		//First turn the shoulder away from the arm slightly
		//Shoulder to elbow: 8.5 inches
		//Elbow to wrist: 7.5 inches

		cout << "Y Dist: " << yDist << " X Dist " << xDist << endl;

        float newShldr = 0.0;
        if( xDist < 7.5 )
        {
			//If the distance in the x axis is less then the length of the elbow to wrist then
			//set the shoulder to 90 degrees
            newShldr = deg2rad( 90.0 );
            newShldr += asin( ( 7.5 - xDist ) / ( 8.5 ) );
        }
        else if( xDist < 16 )
        {
			//Find the angle which will allow the elbow-wrist portion to be parallel to the x-axis
            newShldr = asin( ( xDist - 7.5 ) / ( 8.5 ) );
        }
        else
        {
			//If we need to extend the arm directly towards the target then set the shoulder to 
			//a convenient value
            newShldr = 0.734472;
        }

        cout << "New Shoulder: " << rad2deg( newShldr ) << endl;

		//Find the elbow value based on the shoulder value
		float newElbow = deg2rad( 90.0 ) - abs( newShldr );
		newElbow = deg2rad( 90.0 ) - newElbow;
		newElbow *= -1;

		//Set the wrist to be pointing towards the target
		float newWrist = deg2rad( -90.0 );

		//Create a motion sequence to move the arm to the new location
		SharedObject<MediumMotionSequenceMC> mseq_mc;
		mseq_mc->advanceTime(750);    // 0.75 secs to load position
		mseq_mc->loadFile("SafePos.pos");

		mseq_mc->advanceTime(1500);    // 0.75 sec to align vertically
		mseq_mc->setOutputCmd(shldrIdx, newShldr);
		mseq_mc->setOutputCmd(elbowIdx, newElbow);
		mseq_mc->setOutputCmd(wristIdx, newWrist);
		mseq_mc->setOutputCmd(leftGripperIdx, 0.5);
		mseq_mc->setOutputCmd(rightGripperIdx, 0.5);

		//Store the calculated values for the aligned arm
		alignedShldr = newShldr;
		alignedElbow = newElbow;
		alignedWrist = newWrist;

		//Remove the arm motion if it exists
		if( armSequence_id != MotionManager::invalid_MC_ID && motman->isOwner( armSequence_id ) > 0 )
		{
			motman->removeMotion( armSequence_id );
		}

		//Add the motion sequence
		armSequence_id = motman->addPrunableMotion(mseq_mc);

		//Listen for the motion to complete
		erouter->addListener( this, EventBase::motmanEGID, armSequence_id );
	}
}

void BlockChaser::moveToTarget()
{
	//Use moveToPoint to move directly towards the target

	//Create a motion sequence
	SharedObject<ArmMC> arm_mc;
	armMC_id = motman->addPersistentMotion(arm_mc);

    //one inch = 25.4 millimeters, use this to convert the inches to millimeters
	float yMilli = yDist * 25.4;
	float xMilli = xDist * -25.4; //The x location is negated since my coordinate system is
							      //different then Tekkotsu's

	//Make sure the arm does not move too fast and use moveToPoint to move to xMilli, yMilli
	for( int i = 0; i < 5; i++ )
	{
		MMAccessor<ArmMC>(armMC_id)->setMaxSpeed(i, 0.3);
	}
	MMAccessor<ArmMC>(armMC_id)->moveToPoint(yMilli, xMilli, 0);

    cout << "Moving to: " << xMilli << "," << yMilli << " Target: " << xDist << ", " << yDist << endl;

	//Listen for when the motion is complete
	erouter->addListener( this, EventBase::motmanEGID, armMC_id );
}

void BlockChaser::grabAndMoveTarget()
{
	//Close the gripper, move the target away and release it

	//Create a new motion sequence
	SharedObject<MediumMotionSequenceMC> mseq_mc;
	mseq_mc->advanceTime(750);    // 0.75 secs to grab
	mseq_mc->setOutputCmd(shldrIdx, state->outputs[shldrIdx]);
	mseq_mc->setOutputCmd(elbowIdx, state->outputs[elbowIdx]);
	mseq_mc->setOutputCmd(wristIdx, state->outputs[wristIdx]);
	mseq_mc->loadFile("GrabPos.pos"); //Close the gripper, keep the arm stationary

	mseq_mc->advanceTime(1500);    // 0.75 secs to move target
	mseq_mc->loadFile("FinalPos.pos");

	mseq_mc->advanceTime(2250);    // 0.75 secs to open gripper
	mseq_mc->loadFile("OpenGripper.pos");

	mseq_mc->advanceTime(3000);    // 0.75 secs to move away from target
	mseq_mc->loadFile("SafeFinal.pos");

	//Remove arm motion
	if( armSequence_id != MotionManager::invalid_MC_ID && motman->isOwner( armSequence_id ) > 0 )
	{
		motman->removeMotion( armSequence_id );
	}

	motman->removeMotion( armMC_id );

	//Add motion to motion manager
	armSequence_id = motman->addPrunableMotion(mseq_mc);

	erouter->remove( this );

	//Listen for when the motion is complete
	erouter->addListener( this, EventBase::motmanEGID, armSequence_id );
}

bool BlockChaser::findArm(bool printResults /*= true*/)
{
	//Check to see if the gripper is surrounding the target

	//Clear the shape and sketch spaces
    camShS.clear();
    camSkS.clear();

	//Find the blue lines and ellipses
    NEW_SKETCH(camFrame, uchar, sketchFromSeg());
    NEW_SKETCH(blue_stuff, bool, visops::colormask(camFrame,"blue"));
    NEW_SHAPEVEC(lines, LineData, LineData::extractLines(blue_stuff));
    NEW_SHAPEVEC(blueEllipses, EllipseData, EllipseData::extractEllipses(blue_stuff));

	//If no ellipses were found then print an error message
    if( blueEllipses.size() == 0 )
    {
        if( printResults )
        {
            cout << "ERROR: Can't see gripper!" << endl;
        }
        return false;
    }

	//Find an ellipse of the target color
    NEW_SKETCH(target_stuff, bool, visops::colormask(camFrame,TargetColor));
    NEW_SHAPEVEC(ellipses, EllipseData, EllipseData::extractEllipses(target_stuff));

	//If no targets were found print an error message
    if( ellipses.size() == 0 )
    {
        if( printResults )
        {
            cout << "ERROR: Can't see target, assuming I didn't get it!" << endl;
        }
        return false;
    }

	//We need to find the blue lines on the gripper

	//---------------------------------------
	//The following block of code will find all the lines and store their
	//distances from the blue ellipse in a vector
    vector< int > distances;

    SHAPEVEC_ITERATE(lines, LineData, line)
		EndPoint point = line->end1Pt();
		EndPoint point2 = line->end2Pt();

		float xCoord = ( point.coordX() + point2.coordX() ) / 2.0;
		float yCoord = ( point.coordY() + point2.coordY() ) / 2.0;

		float actX = blueEllipses[ 0 ]->getCentroid().coordX();
		float actY = blueEllipses[ 0 ]->getCentroid().coordY();

		float dist = sqrt( pow( ( xCoord - actX ), 2 ) + pow( ( yCoord - actY ), 2 ) );

		distances.push_back( dist );
    END_ITERATE;
	//---------------------------------------

	//This block of code will find the line with the minimum distance from the ellipse
	//---------------------------------------
    float minDist = -1;
    unsigned int minIndex = -1;

    for( unsigned int idx = 0; idx < distances.size(); idx++ )
    {
        if( minDist == -1 || distances[ idx ] < minDist )
        {
            minDist = distances[ idx ];
            minIndex = idx;
        }
    }
	//---------------------------------------

	//This block of code will find the line with the second
	//shortest distance from the ellipse
	//---------------------------------------
    float minDist1 = -1;
    float minDist2 = -1;
    unsigned int minIndex1 = -1;
    unsigned int minIndex2 = -1;

    for( unsigned int idx = 0; idx < distances.size(); idx++ )
    {
        if( idx != minIndex )
        {
            if( minDist1 == -1 || distances[ idx ] < minDist1 )
            {
                minDist2 = minDist1;
                minIndex2 = minIndex1;

                minDist1 = distances[ idx ];
                minIndex1 = idx;
            }
            else if( minDist2 == -1 || distances[ idx ] < minDist2 )
            {
                minDist2 = distances[ idx ];
                minIndex2 = idx;
            }
        }

        if( minDist == -1 || distances[ idx ] < minDist )
        {
            minDist = distances[ idx ];
            minIndex = idx;
        }
    }
	//---------------------------------------

	//If two lines were not found print an error message
    if( minDist1 == -1 || minDist2 == -1 )
    {
        if( printResults )
        {
            cout << "ERROR: Can not see gripper..." << endl;
        }
        return false;
    }

	//Create a sketch which will store the outline of the grab area
    Sketch<bool> grabSOutline(camSkS, "GrabOutline");
    grabSOutline = false; //Clear the sketch

	//Add all permutations of the endpoints to the sketch as lines to ensure the outline is added
    addLineToSketch( grabSOutline, lines[ minIndex1 ]->end1Pt(), lines[ minIndex2 ]->end1Pt() ); //1-1 to 2-1
    addLineToSketch( grabSOutline, lines[ minIndex1 ]->end1Pt(), lines[ minIndex2 ]->end2Pt() ); //1-1 to 2-2

    addLineToSketch( grabSOutline, lines[ minIndex1 ]->end2Pt(), lines[ minIndex2 ]->end1Pt() ); //1-2 to 2-1
    addLineToSketch( grabSOutline, lines[ minIndex1 ]->end2Pt(), lines[ minIndex2 ]->end2Pt() ); //1-2 to 2-2

    addLineToSketch( grabSOutline, lines[ minIndex1 ]->end1Pt(), lines[ minIndex1 ]->end2Pt() ); //1-1 to 1-2
    addLineToSketch( grabSOutline, lines[ minIndex2 ]->end1Pt(), lines[ minIndex2 ]->end2Pt() ); //2-1 to 2-2

	//Set the outline to green and draw it
    grabSOutline->setColor("green");
    grabSOutline->V();

	//Create a final sketch of the target area by filling in the previous sketch and adding back in
	//the lines
    NEW_SKETCH(b_filled, bool, visops::fillInterior( grabSOutline ));
    addLineToSketch( b_filled, lines[ minIndex1 ]->end1Pt(), lines[ minIndex2 ]->end1Pt() ); //1-1 to 2-1
    addLineToSketch( b_filled, lines[ minIndex1 ]->end1Pt(), lines[ minIndex2 ]->end2Pt() ); //1-1 to 2-2
    addLineToSketch( b_filled, lines[ minIndex1 ]->end2Pt(), lines[ minIndex2 ]->end1Pt() ); //1-2 to 2-1
    addLineToSketch( b_filled, lines[ minIndex1 ]->end2Pt(), lines[ minIndex2 ]->end2Pt() ); //1-2 to 2-2
    addLineToSketch( b_filled, lines[ minIndex1 ]->end1Pt(), lines[ minIndex1 ]->end2Pt() ); //1-1 to 1-2
    addLineToSketch( b_filled, lines[ minIndex2 ]->end1Pt(), lines[ minIndex2 ]->end2Pt() ); //2-1 to 2-2

	//Find the center of the ellipse
    int centerX = ellipses[ 0 ]->getCentroid().coordX();
    int centerY = ellipses[ 0 ]->getCentroid().coordY();

	//Return true if the center exists in the grab zone false otherwise
    return b_filled( centerX, centerY );
}

void BlockChaser::addLineToSketch( Sketch<bool>& sketch, EndPoint point1, EndPoint point2 )
{
	//This code is based off of the Bresenham Line algorithm and will draw a line
	//from point1 to point2 in the sketch

    int x0 = point1.coordX();
    int y0 = point1.coordY();
    int x1 = point2.coordX();
    int y1 = point2.coordY();

    //Bresenham's line algorithm
    bool steep = abs( y1 - y0 ) > abs( x1 - x0 );
    if( steep )
    {
        swap( x0, y0 );
        swap( x1, y1 );
    }
    if( x0 > x1 )
    {
        swap( x0, x1 );
        swap( y0, y1 );
    }

    int deltaX = x1 - x0;
    int deltaY = abs( y1 - y0 );
    int error = deltaX / 2;

    int ystep;
    int y = y0;
    if( y0 < y1 )
    {
        ystep = 1;
    }
    else
    {
        ystep = -1;
    }

    for( int x = x0; x <= x1; x++ )
    {
        if( steep )
        {
            sketch( y, x ) = true;
        }
        else
        {
            sketch( x, y ) = true;
        }
        error -= deltaY;
        if( error < 0 )
        {
            y += ystep;
            error += deltaX;
        }
    }
}

void BlockChaser::alignGripperWithTarget()
{
    //Let's find where the wrist is!

    float shldr= state->outputs[shldrIdx];
    float elbow= state->outputs[elbowIdx];

    //First use the shoulder rotation
    //A2:=Cos (45)*A - Sin (45)*B
    //B2:=Sin (45)*A + Cos (45)*B
    float xWrist = -8.5 * sin( shldr );
    float yWrist = 8.5 * cos( shldr );

    float xGripper = xWrist + ( -12.0 * sin( shldr + elbow ) );
    float yGripper = yWrist + ( 12.0 * cos( shldr + elbow ) );

    //Now add in the elbow rotation
    xWrist += ( -7.5 * sin( shldr + elbow ) );
    yWrist += ( 7.5 * cos( shldr + elbow ) );

    //Lets find the angle between the lines defined by the following points:
    //Line1: ( xWrist, yWrist ) to ( xGripper, yGripper )
    //Line2: ( xWrist, yWrist ) to ( xDist, yDist )
    //tan( angle ) = ( slope2 - slope1 ) / ( 1 + ( slope1 * slope2 ) )
    //angle = arctan of that

    float slope1 = ( yGripper - yWrist ) / ( xGripper - xWrist );
    float slope2 = ( yDist - yWrist ) / ( xDist - xWrist );
    float wrist = atan( ( slope2 - slope1 ) / ( 1 + ( slope1 * slope2 ) ) );
    wrist = -1 * abs( wrist );

	//Wrist now stores the value for the wrist to have to point at the target

	//Set the wrist servo to use the new value
    MMAccessor<ArmMC>(armMC_id)->setJointValue(2, wrist);
}

void BlockChaser::returnToAlignment()
{
	//If we missed the gripper then return to the alignment step

	//Set the status to the ALIGNING_WITH_TARGET
    m_status = ALIGNING_WITH_TARGET;

	//Create a motion sequence
    SharedObject<MediumMotionSequenceMC> mseq_mc;

	//Move back to the aligned position
    mseq_mc->advanceTime(1000);    // 1 sec to align vertically
    mseq_mc->setOutputCmd(shldrIdx, alignedShldr);
    mseq_mc->setOutputCmd(elbowIdx, alignedElbow);
    mseq_mc->setOutputCmd(wristIdx, alignedWrist);
    mseq_mc->setOutputCmd(leftGripperIdx, 0.5); //Open the gripper
    mseq_mc->setOutputCmd(rightGripperIdx, 0.5);

	//Remove the arm motion sequence
    if( armSequence_id != MotionManager::invalid_MC_ID && motman->isOwner( armSequence_id ) > 0 )
    {
        motman->removeMotion( armSequence_id );
    }

	//Add the motion sequence
    armSequence_id = motman->addPrunableMotion(mseq_mc);

	//Listen for when the motion sequence is complete
    erouter->addListener( this, EventBase::motmanEGID, armSequence_id );
}