/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include "../Motion/MotionInterface.h"
#include "../Vision/Vision.h"


#include "../Motion/Kinematics.h"
#include "../headers/Util.h"
#include "../Main/RobotMain.h"

#include <list>

#include "ScoreGoal.h"

using namespace Motion;
using namespace std;

char const * const ScoreGoal::beh_name = "ScoreGoal";

char const * const ScoreGoal::state_names[ScoreGoal::NumStates] = {
  "CORRECTING",
  "SATISFIED",
  "WANDERING",
  "KICK"
};


ScoreGoal::ScoreGoal()
{
  command = new MotionCommand;
  mzero(*command);

  fsm.init(state_names,NumStates,CORRECTING,16,16);
  kin = Kinematics(Kinematics::ERS7);
  //lastpixelcnts = new list<float>();
}

ScoreGoal::~ScoreGoal()
{
}

void
ScoreGoal::reset(ulong time)
{
  fsm.setState(CORRECTING,0,"Reset",time);
}

bool ScoreGoal::initConnections()
{
  EventManager *event_mgr;
  event_mgr = EventManager::getManager();

  vis_id = event_mgr->getEventProcessorId(Vision::name);
  vision = dynamic_cast<Vision *>(event_mgr->listenEventProcessor(beh_name,vis_id));

  return (vision!=NULL);
}

bool ScoreGoal::setupEventMgr(){
  return true;
};

bool ScoreGoal::update(ulong time,const EventList *events)
{
  cache_tracker.updateAvailable(time);

  return true;
}

double ScoreGoal::operator()(ulong time,Motion::MotionCommand *command)
{
	bool done=false;
	static bool up = false; // Direction of head motion when wandering
	static float timeRotate = -1.0;

	float pixelThreshold = 850.0;
	float goalThreshold = 20.0; 
	
        float servo_error_x = vision->findVisualServoingErrorX(COLOR_PINK);
	float servo_error_y = vision->findVisualServoingErrorY(COLOR_PINK);
	float pixel_cnt = vision->findPixelCnt(COLOR_PINK);
	float goal_error_x = vision->findVisualServoingErrorX(COLOR_YELLOW);
	float goal_pixel_cnt = vision->findPixelCnt(COLOR_YELLOW);

	double rangeX = kin.robot.head_pan_max - kin.robot.head_pan_min;
	double rangeY = kin.robot.head_tilt2_max - kin.robot.head_tilt2_min;
	double centerMinX = kin.robot.head_pan_max + kin.robot.head_pan_min - (.10 * rangeX);
	double centerMaxX = kin.robot.head_pan_max + kin.robot.head_pan_min + (.10 * rangeX);
	double turnMinX = kin.robot.head_pan_max + kin.robot.head_pan_min - (.20 * rangeX);
	double turnMaxX = kin.robot.head_pan_max + kin.robot.head_pan_min + (.20 * rangeX);
	
	float lastBallCentroid = -2.0;
	float lastGoalCentroid = -4.0;

	if(goal_pixel_cnt > goalThreshold) 
		lastgoalerrorx.push_front(goal_error_x);
	else
		lastgoalerrorx.push_front(-1);

	cout << "goal pixel count is " << goal_pixel_cnt << endl;
	cout << "ball pixel count is " << pixel_cnt << endl;

	
	if(pixel_cnt != -1)
        	lastballerrorx.push_front(servo_error_x);
	else
		lastballerrorx.push_front(-1);
	if(lastgoalerrorx.size() > 30)
		lastgoalerrorx.pop_back();

	if(lastballerrorx.size() > 30)
		lastballerrorx.pop_back();

	for(list<float>::iterator i = lastgoalerrorx.begin(); i != lastgoalerrorx.end(); i++) {
		if(*i != -1) {
			lastGoalCentroid = *i;
			break;
		}
	}

	for(list<float>::iterator i = lastballerrorx.begin(); i != lastballerrorx.end(); i++)
	{
		if(*i != -1) {
			lastBallCentroid = *i;
			break;
		}
	}

	cout << "fabs(lastBallCentroid - lastGoalCentroid) is " << fabs(lastBallCentroid - lastGoalCentroid) << endl;
	
    	float sum = 0.0;
	if(pixel_cnt != -1.0)
		lastpixelcnts.push_front(pixel_cnt);
	
	if(lastpixelcnts.size() > 30)
		lastpixelcnts.pop_back();

	for(list<float>::iterator i = lastpixelcnts.begin(); i != lastpixelcnts.end(); i++)
		sum += *i;
  
	pixel_cnt = sum /= lastpixelcnts.size();
  
	//cout << "avgpixelcnt over last 30 iterations = " << avgpixelcnt << endl;
	//cout << "Servo error is " << servo_error_x << " on X, " << servo_error_y << " on Y" << endl;
	//cout << "Pixel count is " << pixel_cnt << endl;

	if(goal_pixel_cnt > goalThreshold)
		command->setFaceLEDs(LED_E, LED_BRIGHT, MODE_A);

	fsm.startLoop(time);
	while(!done && fsm.error==0){
		switch(fsm.getState())	{
			case CORRECTING:
			{
	//cout << "	
	if(fabs(servo_error_x) < .15 && fabs(servo_error_y) < .30 
		&& pixel_cnt >= pixelThreshold  && 
		command->head_pan > centerMinX && command->head_pan < centerMaxX)
            TRANS_CONT(fsm,SATISFIED,5,"ErrorOk");
	  
	  if(servo_error_x == -1.0 && servo_error_y == -1.0)
	    TRANS_CONT(fsm, WANDERING, 6, "LostBall");
	  
	//cout << "x: " << servo_error_x << " y: " << servo_error_y << endl;
	  
          float vx = 0.0, va = 0.0, vy = 0.0;
	  
	  double displacementX = rangeX * .03 * fabs(servo_error_x * servo_error_x);
	  displacementX *= servo_error_x > 0 ? 1 : -1;
	  double displacementY = rangeY * .07 * fabs(servo_error_y * servo_error_y);
	  displacementY *= servo_error_y > 0 ? 1 : -1;
	  
	  command->head_cmd = HEAD_ANGLES;
	  command->head_pan += displacementX;
	  if(command->head_pan < kin.robot.head_pan_min)
	    command->head_pan = kin.robot.head_pan_min;
	  if(command->head_pan > kin.robot.head_pan_max)
	    command->head_pan = kin.robot.head_pan_max;
	  
	  command->head_tilt2 += displacementY; 
	  if(command->head_tilt2 < kin.robot.head_tilt2_min)
	    command->head_tilt2 = kin.robot.head_tilt2_min;
	  if(command->head_tilt2 > kin.robot.head_tilt2_max)
	    command->head_tilt2 = kin.robot.head_tilt2_max;
	  
	if(pixel_cnt < pixelThreshold * 1.2)
		vx = (fabs(pixelThreshold - pixel_cnt) / pixelThreshold) * MaxDX * .75;

	if(command->head_pan < turnMinX && displacementX < (rangeX * .1))
        	va = -.4 * MaxDA;
        if(command->head_pan > turnMaxX && displacementX > -(rangeX * .1))
        	va = .4 * MaxDA;
 
	if(command->head_pan < centerMinX && displacementX < (rangeX * .1))
        	vy = -.4 * MaxDY;
        if(command->head_pan > centerMaxX && displacementX > -(rangeX * .1))
        	vy = .4 * MaxDY;
	
	if(va != 0.0 || vx != 0.0 || vy != 0.0)
		command->motion_cmd = MOTION_WALK_TROT;
	else
		command->motion_cmd = MOTION_STAND_NEUTRAL;

	command->vx = vx;
	command->vy = vy;
	command->va = va;

	command->head_roll = 0.0;
	command->head_tilt = (kin.robot.head_tilt_max + kin.robot.head_tilt_min) * .25; // Don't lose the ball when directly in front of dog.
	
	done = true;
	  
        }
        break;
      case KICK:
        {
	cout << "In kicking state..." << endl;

	timeRotate = -1.0;
	command->setFaceLEDs(LED_C, LED_BRIGHT, MODE_A);
        if(fsm.timeInState() < SecToTime(1.25)){
        	command->motion_cmd = MOTION_WALK_TROT;
	        command->vy = 0.0;
	        command->vx = .4 * MaxDX;
	        command->va = 0.0;
	 }
	else if(fsm.timeInState() < SecToTime(1.4)){
		command->motion_cmd = MOTION_KICK_DIVE;
		command->vy = 0.0;
		command->vx = 0.0;
		command->va = 0.0;
	}
	else
	{
	TRANS_CONT(fsm, WANDERING, 5, "FinishedKick");
	} 
          done = true;
        }
        break;
	case SATISFIED:
	{
		command->setFaceLEDs(LED_A, LED_BRIGHT, MODE_A);
		if(lastBallCentroid == -2.0) {
			timeRotate = -1.0;
			TRANS_CONT(fsm, WANDERING, 5, "FoundBall");
		}

		if(fabs(lastBallCentroid - lastGoalCentroid) < .15)
			timeRotate = fsm.timeInState();			

		if(timeRotate != -1.0 && fsm.timeInState() - timeRotate > SecToTime(.65)) 
			TRANS_CONT(fsm, KICK, 6, "KICK");
    
		//cout << "Wandering" << endl;
		float vx = 0.0;
		if(pixel_cnt < pixelThreshold)
			vx = ((pixelThreshold - pixel_cnt) / pixelThreshold) * MaxDX * .75;
		command->motion_cmd = MOTION_WALK_TROT;
		command->vx = vx;
		command->vy = 0.5 * MaxDY;
		command->va = fabs(lastBallCentroid) * .5 * MaxDA * (lastBallCentroid > 0 ? 1 : -1);
		command->head_cmd = HEAD_ANGLES;
		command->head_tilt2 += (up ? .1 *  rangeY : -.1 * rangeY); 
		if(command->head_tilt2 < kin.robot.head_tilt2_min){
			command->head_tilt2 = kin.robot.head_tilt2_min;
			up = true;
		}
		if(command->head_tilt2 > .85 * kin.robot.head_tilt2_max) {
			command->head_tilt2 = .5 * kin.robot.head_tilt2_max;
			up = false;
		}
		
		command->head_pan = (kin.robot.head_pan_max + kin.robot.head_pan_min) / 2;
		
		done = true;
	}
	break;
	case WANDERING:
	{
	if(servo_error_x != -1.0)
		TRANS_CONT(fsm, CORRECTING, 5, "FoundBall");
    
	//cout << "Wandering" << endl;
    command->motion_cmd = MOTION_WALK_TROT;
    command->vy = 0.0;
    command->vx = 0.0;
    command->va = 0.4 * MaxDA;

	  command->head_cmd = HEAD_ANGLES;
  command->head_tilt2 += (up ? .05 *  rangeY : -.05 * rangeY); 
	  if(command->head_tilt2 < kin.robot.head_tilt2_min){
	    command->head_tilt2 = kin.robot.head_tilt2_min;
		up = true;
		}
	  if(command->head_tilt2 > .5 * kin.robot.head_tilt2_max) {
	    command->head_tilt2 = .5 * kin.robot.head_tilt2_max;
		up = false;
	}
	command->head_tilt = (kin.robot.head_tilt_max + kin.robot.head_tilt_min) * .25; // Don't lose the ball when directly in front of dog.

    done = true;
  }
  break;
    }
  }
  if(fsm.error!=0){
    fsm.handleErr(command);
  }
  fsm.endLoop();
  return 1.0;
}

const Motion::MotionCommand *ScoreGoal::get(ulong time)
{
mzero(command->led);

  if(cache_tracker.shouldUpdate(time)){
    // update info
    (*this)(time,command);

    cache_tracker.cacheUpdated(time);
  }

  return command;
}

REGISTER_EVENT_PROCESSOR(ScoreGoal,ScoreGoal::beh_name,ScoreGoal::create);