/* 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.
  ========================================================================= */

/**
 * BWBallChallenge.cc
 * 
 * This file contains the behavior for scoring
 * a goal with the black and white ball for
 * the 2003 challenge.
 *
 * Written by: Juan Fasola
 */

#include <math.h>

#include "Behaviors.h"
#include "BeLowLevel.h"
#include "BehaviorPacketEncoder.h"
#include "FeatureSet.h"
#include "state_machine.h"
#include "../Main/RobotMain.h"
#include "../Vision/BWBallDetector.h"

const double ball_alpha = 0.04;

#define SEC		*1000000 + FS->current_time
#define IS_NOT_UP	> FS->current_time

/**
 * BeScoreBW : score a goal with the black and white ball
 */

const int num_search_points = 9;
vector2d search_points[] = { vector2d(0.0, 0.0),
			     vector2d(1000.0, -1000.0),
			     vector2d(750.0, 750.0),
			     vector2d(-1000.0, -1000.0),
			     vector2d(-750.0, 750.0),
			     vector2d(-750.0, -750.0),
			     vector2d(-1000.0, 1000.0),
			     vector2d(750.0, -750.0),
			     vector2d(1000.0, 1000.0) };

class BeScoreBW {
  static const int beh_id;

  static char const * const beh_name;
  
  static const int NumStates=7;
  enum State { GOTO_BALL, KICK_BALL, BACK_UP, LOST_BALL, 
	       LOCALIZE, TURN_TO_ANGLE, BALL_SEARCH };

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;

  double head_scan_theta;
  double head_scan_increment;
  double mean_ball_conf;
  vector3d last_saw_ball_rel;
  bool saw_ball_last_frame;
  int goto_search_point;

  ulong Time;
  ulong Time2;
  ulong Time3;
  int stare_cont;
  double turning;
  double kick_angle;
  double theta;
  double alpha;
  double si;
  double wall_turn;
  bool angle_set;
  bool wall_bool;
  bool localized;
  vector2d point;
  BeGetBall *get_ball;
  BeFindBall *find_ball;
  BeTurn *turn;
  FeatureSet* FS;



public:
  BeScoreBW();
  ~BeScoreBW();
  bool atPoint();
  bool atWallOrCorner();
  double operator()(FeatureSet *FS_p,MotionCommand *command, vector2d p);

  uchar *encodeAllNames(ulong time,uchar *buf,int buf_size);
  uchar *encodeTrace(uchar *buf,int buf_size);

};

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

char const * const BeScoreBW::state_names[BeScoreBW::NumStates] = {
  "GOTO_BALL",
  "KICK_BALL",
  "BACK_UP",
  "LOST_BALL",
  "LOCALIZE",
  "TURN_TO_ANGLE",
  "BALL_SEARCH" };

BeScoreBW::BeScoreBW()
{
  fsm.init(state_names,NumStates,BALL_SEARCH,16,16);

  point = vector2d(0,0);
  localized = false;
  get_ball = new BeGetBall();
  find_ball = new BeFindBall();
  turn = new BeTurn();
  Time = Time2 = Time3 = stare_cont = 0;
  turning = theta = alpha = si = wall_turn = 0.0;
  kick_angle = 1.57;
  angle_set = wall_bool = false;

  head_scan_theta = M_PI/3;
  head_scan_increment = -0.0872;
  mean_ball_conf = 0;
  last_saw_ball_rel.set(0, 0, 0);
  saw_ball_last_frame = false;
  goto_search_point = 0;
}

BeScoreBW::~BeScoreBW()
{
  if(get_ball!=NULL){
    delete get_ball;
    get_ball = NULL;
  }

  if(find_ball!=NULL){
    delete find_ball;
    find_ball = NULL;
  }

  if(turn!=NULL){
    delete turn;
    turn = NULL;
  }
}

double BeScoreBW::operator()(FeatureSet *FS_p,MotionCommand *command, vector2d p)
{
  FS = FS_p;
  point = p;
  vector2d walk_dir; 

  double a_vel = 0, turning = 0.0;

//   vector2d move_vel = get_ball->getMove(FS,use_verbatim);
//   x_vel = move_vel.x;
//   y_vel = move_vel.y;

  if(FS->ball_vector.length()>0)
    a_vel = FS->ball_vector.angle();

  vector2d my_pos(FS->my_position.position.mean.x,
		  FS->my_position.position.mean.y);

  if(my_pos.y < 0)
    si = 1.0;
  else
    si = -1.0;
  
  if(FS->saw_ball_confidence > .5) {
    saw_ball_last_frame = true;
    last_saw_ball_rel = FS->ball_vector3d;
    mean_ball_conf += ball_alpha; 
  } else {
    mean_ball_conf *= (1.0 - ball_alpha);
    saw_ball_last_frame = false;
  }

  double scan_x = 0, scan_y = 0;

  bool done=false;
  fsm.startLoop(FS->current_time);
  while(!done && fsm.error==0) {
    switch(fsm.getState())
      {
      case BALL_SEARCH:
	
	scan_x = 500*cos(head_scan_theta);
	scan_y = 500*sin(head_scan_theta);
	
	if(head_scan_theta > M_PI/3 && head_scan_increment > 0)
	  head_scan_increment = -head_scan_increment;
	
	if(head_scan_theta < -M_PI/3 && head_scan_increment < 0)
	  head_scan_increment = -head_scan_increment;
	
	walk_dir = FS->getRelativePosn(search_points[goto_search_point]);
	
	// if we're lost, head scan markers
	if(FS->my_position.position.sMaj > 400.0) {
	  command->head_cmd = HEAD_SCAN_MARKERS;
	} else {
	  command->head_cmd = HEAD_LOOKAT;
	  command->head_lookat = vector3d(scan_x, scan_y, 94);
	  head_scan_theta += head_scan_increment;
	}
	
	if(FS->saw_ball_confidence > .5)
	  TRANS_CONT(fsm, LOST_BALL, 1, "fixate on ball");

	if(walk_dir.length() < 100)
	  goto_search_point = (goto_search_point + 1) % num_search_points;

	command->motion_cmd = MOTION_WALK_TROT;
	command->vx = walk_dir.x;
	command->vy = walk_dir.y;
	command->va = M_PI/6;

	done = true;
	break;
	
	// We must see the ball to enter this state.
      case GOTO_BALL:

	/**
	 * Approaches the ball at full speed if
	 * far away from it,and progressively slows
	 * down as it gets closer. Aims to get the
	 * ball between the dog's legs.
	 */
	
	//if(mean_ball_conf < .6)
	//  TRANS_CONT(fsm, LOST_BALL, 5, "Need to find ball again");

	if(mean_ball_conf < .6)
	  TRANS_CONT(fsm, LOCALIZE, 6, "Hope we have it");
	
	command->head_cmd = HEAD_LOOKAT;
	command->head_lookat.set(FS->ball_vector.x, FS->ball_vector.y, 94.0/2.0);
	
	command->motion_cmd = MOTION_WALK_TROT;
	command->vx = FS->ball_vector.x;
	command->vy = FS->ball_vector.y;
	command->va = FS->ball_vector.angle()/2; 
	
	done = true;
	break;
	
      case LOST_BALL:
	/**
	 * We have no idea where the ball is,
	 * lets look around, shall we?
	 */

	/* We see the ball, go get it */
	if(mean_ball_conf > .9)
	  TRANS_CONT(fsm,GOTO_BALL,5,"SeeBall");
	
	scan_x = 500*cos(head_scan_theta);
	scan_y = 500*sin(head_scan_theta);
	
	if(head_scan_theta > M_PI/3 && head_scan_increment > 0)
	  head_scan_increment = -head_scan_increment;
	
	if(head_scan_theta < -M_PI/3 && head_scan_increment < 0)
	  head_scan_increment = -head_scan_increment;

	command->motion_cmd = MOTION_WALK_TROT;
	command->vx = 0;
	command->vy = 0;

	if(FS->saw_ball_confidence < .5 &&
	   !saw_ball_last_frame)
	  command->va = M_PI/6;
	else
	  command->va = 0;
	
	command->head_cmd = HEAD_LOOKAT;

	if(FS->saw_ball_confidence > .5 ||
	   saw_ball_last_frame) {
	  command->head_lookat = FS->ball_vector3d;
	} else {
	  command->head_lookat = vector3d(scan_x, scan_y, 94);
	  head_scan_theta += head_scan_increment;
	}

	if(fsm.timeInState() > SecToTime(6) &&
	   FS->saw_ball_confidence < .5)
	  TRANS_CONT(fsm, BALL_SEARCH, 6, "timeout");

	done = true;
	break;

      case LOCALIZE:
	/**
	 * This is the localize state, it holds the ball
	 * for a bit, then scans markers for 2 seconds.
	 */
	if(fsm.isNewState()){
	  Time = 1 SEC;
	  Time2 = 0;
	}
	
	/* Look at ball first to get it to stay still */
	if(Time IS_NOT_UP)
	  {
	    command->head_cmd = HEAD_LOOKAT;		
	    command->head_lookat.set(115.0,0.0,0.0);
	  }
	
	/* Scan markers and localize */
	else
	  {
	    if(Time2 == 0)
	      Time2 = 2 SEC;
	    
	    if(Time2 IS_NOT_UP)
	      {
		command->head_cmd = HEAD_SCAN_MARKERS;
	      }
	    
	    /* Time is up, so lets turn to a good shooting angle */
	    else
	      {
		Time = Time2 = 0;
		done = true;
		turn->reset(FS->current_time);
		TRANS_CONT(fsm,TURN_TO_ANGLE,5,"DoneLocalizing");
	      }
	  }
	
	done = true;
	break;
	
      case KICK_BALL:
	/**
	 * The heart of the behavior, actually kicks
	 * the ball in the right direction (hopefully)
	 * towards the goal.
	 */
	
	/* Choose appropriate kick to use */
	if(!localized && FS->world_model->score_goal().angle() > 0)
	  command->motion_cmd = MOTION_KICK_BW_L;
	else if(!localized)
	  command->motion_cmd = MOTION_KICK_BW_R;

	localized = true;
	
	/* Wait till kick is done, then find the ball */
	if(fsm.timeInState() > 2400000)
	  {
	    Time = 0;
	    done = true;
	    wall_bool = false;
	    localized = false;
	    TRANS_CONT(fsm,BALL_SEARCH,5,"KickCompletedFindBall");
	  }
	
	done = true;
	break;
	
      case TURN_TO_ANGLE:
	/**
	 * Turn to an appropriate angle in order 
	 * to shoot the ball on goal, then go to 
	 * the state that actually kicks. If we
	 * are at a wall,then back up first.
	 */
	if(fsm.isNewState()){
	  Time = 2 SEC;
	  angle_set = false;
	  turn->reset(FS->current_time);
	}
	
	/**
	 * Angle to turn to gets calculated only once.
	 */
	if(!angle_set)
	  {
	    /* Main center region */
	    if(fabs(my_pos.y) < 370)
	      {
		theta = FS->world_model->score_goal().rotate(1.52*si).angle();
		alpha = FS->world_model->score_goal().rotate(-1.47*si).angle();
	      }
	    
	    /* At wall or corner */
	    else if(!wall_bool && atWallOrCorner())
	      {
		walk_dir = FS->getRelativePosn(vector2d(-halfLength+400,0));
		wall_turn = walk_dir.angle() + FS->my_angle;
		wall_bool = true;
		done = true;
		turn->reset(FS->current_time);
		TRANS_CONT(fsm,BACK_UP,5,"AtWall");
	      }
	    
	    /* Top corner regions */
	    else if(my_pos.x < -halfLength + 600)
	      {
		if(my_pos.y > 0)
		  theta = FS->world_model->score_goal().rotate(1.3).angle();
		else
		  theta = FS->world_model->score_goal().rotate(-1.3).angle();
		
		if(theta < 0.0)
		  alpha = theta - 1.0;
		else
		  alpha = theta + 1.0;
	      }
	    
	    /* Lower side regions */
	    else
	      {
		/* Other half of field */
		if(my_pos.x > 300)
		  {
		    theta = FS->world_model->score_goal().rotate(1.57*si).angle();
		    alpha = FS->world_model->score_goal().rotate(-1.57*si).angle();
		  }
		
		/* Lower right side */
		else if(my_pos.y > 0)
		  {
		    theta = FS->world_model->score_goal().rotate(1.5*si).angle();
			alpha = FS->world_model->score_goal().rotate(-1.5*si).angle();
		  }
		
		/* Lower left side */
		else
		  {
			theta = FS->world_model->score_goal().rotate(1.65*si).angle();
			alpha = FS->world_model->score_goal().rotate(-1.57*si).angle();
		  }
	      }
	    
	    /* Get appropriate angle to turn to */
	    kick_angle = min(fabs(theta), fabs(alpha));
	    
	    if(fabs(kick_angle) == fabs(alpha))
	      kick_angle = alpha;
	    else
	      kick_angle = theta;
	    
	    /* Set the angle */
	    kick_angle += FS->my_angle;
	    angle_set = true;
	  }
	
	/* Turn to the angle that is set */
	turning = turn->circleBallToAngle(FS,command,kick_angle,RAD(3.0));
	
	/* Done turning, kick the ball */
	if(turning == 0.0)
	  {
	    done = true;
	    angle_set = false;
	    TRANS_CONT(fsm,KICK_BALL,6,"DoneCirclingBall");
	      }
	
	    done = true;
	    break;
	    
      case BACK_UP:
	/**
	 * This state is just to give the dog
	 * time to back up from the wall with
	 * the ball in his possession. Then 
	 * switch to localize.
	 */
	if(fsm.timeInState() < 7600000)
	  {
	    if(!localized)
	      {
		command->motion_cmd = MOTION_BACKUP_BW;
		  }
	    else
	      {
		command->head_cmd = HEAD_LOOKAT;		
		command->head_lookat.set(115.0,0.0,0.0);	
	      }
	    localized = true;
	  }
	else
	  {
	    localized = false;
	    TRANS_CONT(fsm,LOCALIZE,5,"BackedUpLocalize");
	  }
	
	done = true;
	break;
	
      default:
	TRANS_CONT(fsm,GOTO_BALL,5,"Ball is loose");
	break;
      };
  }
  
  if(fsm.error!=0){
    fsm.dumpLoopEvents();
    //command->motion_cmd = MOTION_DANCE;
    fsm.error = 0;
  }
  fsm.endLoop();
  
  return 0.0;
}


static BeScoreBW *Behavior = NULL;

void BehaviorSystem::bwball_challenge(FeatureSet *features)
{
  logCallChain(CALL_test);

  if(Behavior == NULL) {
    Behavior = new BeScoreBW();
  }

  (*Behavior)(features,command, vector2d(-halfLength,0));
}

bool BeScoreBW::atPoint()
{
  vector2d thepoint = FS->getRelativePosn(point);
  return (thepoint.x < 250 && 
         thepoint.y < 250 && 
         thepoint.x > -250 && 
	  thepoint.y > -250);
}

bool BeScoreBW::atWallOrCorner()
{
  vector2d my_pos(FS->my_position.position.mean.x,
		  FS->my_position.position.mean.y);

  double yp = fabs(my_pos.y);
  double xp = fabs(my_pos.x);

  return (yp > halfWidth - 250 || xp > halfLength - 250 ||
	  (yp > halfWidth - 350 && xp > halfLength - 350));
}