/* 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 "PotentialField.h"
#include "../Main/TeamMsgMgr.h"
#include "../WorldModel/WorldModel.h"
#include "../headers/field.h"
#include "constants.h"
#include "FeatureSet.h"

const double PotentialField::ball_pf_max_value;
const double PotentialField::corridor_pf_max_value;
const double PotentialField::block_pf_max_value;

PotentialField::PotentialField() {
  last_min_point.set(0.0, 0.0);
  scan_x = -halfLength;
}

vector2d PotentialField::getMinPoint(FeatureSet *features) {

  return incrementalGetMinPt(features);
}

vector2d PotentialField::gridGetMinPoint(FeatureSet *features) {
  const double step_size = 10;

  double min = 1000000;
  vector2d retval = vector2d(0.0, 0.0);

  for(double x = -halfLength; x<=halfLength; x += step_size) {
    for(double y = -halfWidth; y<=halfWidth; y += step_size) {
      vector2d pt = vector2d(x, y);
      double temp = calcPoint(pt, features);
      
      if(temp < min) {
	retval = pt;
	min = temp;
      }
    }
  }

  return retval;
}

vector2d PotentialField::incrementalGetMinPt(FeatureSet *features) {

  const double step_size = 50;

  double min_p = calcPoint(last_min_point, features);

  /* First we do a fine grained search around the current point. */
  double start_x, start_y, end_x, end_y;

  // Scan a 500 mm block around the current point.
  // Clip it to the size of the field.

  start_x = max(-halfLength, last_min_point.x - 250);
  end_x = min(halfLength, last_min_point.x + 250);
  start_y = max(-halfWidth, last_min_point.y - 250);
  end_y = min(halfWidth, last_min_point.y + 250);
  
  for(double x=start_x; x<end_x; x += step_size) {
    for(double y=start_y; y<end_y; y += step_size) {
      vector2d pt = vector2d(x, y);
      double temp = calcPoint(pt, features);
      
      if(temp < min_p) {
	last_min_point = pt;
	min_p = temp;
      }
    }
  }
  
  // Now that we've examined the neighborhood of our current min,
  // let's examine a single row of points on the field and update
  // scan_x so we look at a different row next frame.
  for(double y = -halfWidth; y <= halfWidth; y += step_size) {
    vector2d pt = vector2d(scan_x, y);
    double temp = calcPoint(pt, features);
    
    if(temp < min_p) {
      last_min_point = pt;
      min_p = temp;
    }
  }
  
  scan_x += step_size;
  if(scan_x > halfLength)
    scan_x = -halfLength;

  return last_min_point;
}

vector2d PotentialField::getSupporterPath(FeatureSet *features) {
  
  vector2d retval;
  vector2d grad = getGrad(features).norm();
  vector2d min_dir = 
    (getMinPoint(features) - features->my_position_body);
  
  double mag = min_dir.length();

  min_dir.normalize();
  
  double dot_prod = grad.dot(min_dir);
  
  if(dot_prod > 0) {
    retval = min_dir;
  } else {
    // Okay, we're heading up the gradient. That's bad.
    // See which way we can go perpendicular to the gradient
    // that moves us closer to the min.
    retval = grad.perp();
    dot_prod = retval.dot(min_dir);
    if(dot_prod < 0)
      retval = retval*(-1);

    // retval now points perpendicular to the gradient in the
    // direction we want to move. But if we're too close to
    // an obstacle, this doesn't make us move away from it.
    // So we'll take the weighted sum of the perp vector with
    // the way we want to move down the gradient.

    retval = retval + grad*7;
  }

  retval.normalize();
  return retval*mag;
}

vector2d PotentialField::getGrad(FeatureSet *features) {
  return getGrad(features->world_model->my_position(), features);
}

vector2d PotentialField::getGrad(vector2d use_point, FeatureSet *features) {
  
  double dx = 0;
  double dy = 0;
  double value1 = 0;
  double value2 = 0;
  double value3 = 0;
  
  vector2d my_loc = use_point;
  
  // Make sure the point we test is on the field.
  if(my_loc.x > 0)
    dx = -1.0;
  else
    dx = 1.0;
  
  if(my_loc.y > 0)
    dy = -1.0;
  else
    dy = 1.0;
  
  value1 = calcPoint(my_loc, features);
  
  vector2d test_pt_x;
  test_pt_x.set(my_loc.x + dx, my_loc.y);
  value2 = calcPoint(test_pt_x, features);
  
  
  vector2d test_pt_y;
  test_pt_y.set(my_loc.x, my_loc.y + dy);
  value3 = calcPoint(test_pt_y, features);
  
  
  return vector2d((value2 - value1)*-dx, (value3 - value1)*-dy);
}

void PotentialField::printField(FeatureSet *features,
				double resolution_in_mm) {
 
  double min_x = 10000;
  double min_y = 10000;
  double min_val = 10000;
  
  char pbuf[128];
  char big_buf[4096];
  
  big_buf[0] = 0;
  pbuf[0] = 0;
  
//   pprintf(TextOutputStream,
// 	  "me: %f %f\nball %f %f\ntm1 %f %f\ntm2 %f %f\ntm3 %f %f\n",
// 	  features->my_position.position.mean.x,
// 	  features->my_position.position.mean.y,
// 	  features->getBestBallPosn().x,
// 	  features->getBestBallPosn().y,
// 	  features->world_model->teammate[0].mean.x,
// 	  features->world_model->teammate[0].mean.y,
// 	  features->world_model->teammate[1].mean.x,
// 	  features->world_model->teammate[1].mean.y,
// 	  features->world_model->teammate[2].mean.x,
// 	  features->world_model->teammate[2].mean.y);
  
  for(double y=-halfWidth; y<halfWidth; y+=resolution_in_mm) {
    
    sprintf(pbuf, "ROW (%lf) ", y);
    strcat(big_buf, pbuf);
    
    for(double x=-halfLength; x<halfLength; x+=resolution_in_mm) {
      
      sprintf(pbuf,
	      "%f ", calcPoint(vector2d(x, y), features));
      strcat(big_buf, pbuf);
      
      double temp = calcPoint(vector2d(x, y), features);
      if(temp < min_val) {
	min_x = x;
	min_y = y;
	min_val = temp;
      }
    }
    
    pprintf(TextOutputStream, "%s\n", big_buf);    
    big_buf[0] = 0;
  }
  
  pprintf(TextOutputStream, "Grid min: (%lf, %lf) %lf\n",
	  min_x, min_y, min_val);
}

double PotentialField::calcPoint(vector2d point,
				 FeatureSet *features) {

  double retval = 0;

  
  vector2d ball;
  features->getBestBallPosn(ball); 
  vector2d score_goal = vector2d(halfLength, 0);
  vector2d defend_goal = vector2d(-halfLength, 0);

  if(features->world_model->goalColor==DEFEND_CYAN_GOAL) {
    score_goal.set(-halfLength, 0);
    defend_goal.set(halfLength, 0);
  }

  int supporter_role = features->team_msg_mgr->getSupporterRole();
  
  retval += getWallPF(point);
  
  retval += getDefenseZonePF(point, features->world_model->teamColor);

  retval += getBallPF(point, ball, 
		      supporter_role);

  // Avoid our teammates
  for(int i=0; i<3; i++) {
    vector2d tm(0,0);
    if (!features->world_model->teammate_pos_mean(tm, i)) {
      pprintf(TextOutputStream,"BUG: no teammate pos found for teammate %d\n", i);
    }
    
    // We need to check and make sure it's valid data - we may
    // not be getting network messages from this teammate.
    if(tm.x!=0 && tm.y!=0)
      retval += getTeammatePF(point, tm);
  }

  retval += getXBiasPF(point, ball,
  		       features->world_model->teamColor, 
  		       supporter_role,
		       features->team_msg_mgr->isLeaderGoalie());
  
  
  retval += getCorridorPF(point, score_goal, ball,
			  supporter_role,
			  features->team_msg_mgr->isLeaderGoalie());
  
  retval += getBlockOwnGoalPF(point, defend_goal, ball, supporter_role);
  
  retval += getYBiasPF(point, ball, supporter_role);
  
  return retval;
}

/* Return the contribution from the four walls to the
   supporting attacker's potential field. We also include
   a bias for the defense zone as if the line marking it
   was a wall. */
double PotentialField::getWallPF(vector2d point) {
  double retval = 0;
  double tmp;
  
  /* -X wall */
  tmp = max(0.0, wall_repulsion + 
	    wall_falloff*(halfLength + point.x));
  
  retval += tmp;
  
  /* +X wall */
  tmp = max(0.0, wall_repulsion + 
	    wall_falloff*(halfLength - point.x));
  retval += tmp;

  /* -Y wall */
  tmp = max(0.0, wall_repulsion + 
	    wall_falloff*(halfWidth + point.y));
  retval += tmp;
  
  /* +Y wall */
  tmp = max(0.0, wall_repulsion + 
	    wall_falloff*(halfWidth - point.y));
  retval += tmp;
  
  return retval;
}

double PotentialField::getDefenseZonePF(vector2d point, int uniform_color) {
  
  vector2d goal_center;

  if(uniform_color==TEAM_COLOR_BLUE) {
    goal_center.set(halfLength, 0);
  } else {
    goal_center.set(-halfLength, 0);
  }
  
  double dist = GVector::distance(point, goal_center);

  return max(0.0,
	     defense_zone_repulsion + 
	     dist*defense_zone_fall_off);
}

/* The contribution from the ball to the potential field. */
double PotentialField::getBallPF(vector2d point, vector2d ball,
				 int supporter_role) {
  
  double dist = GVector::distance(point, ball);
  
  switch(supporter_role) {
    case RoleCommand::COM_OFF_SUPP:
    case RoleCommand::COM_OFF_SUPP_LONG:
      return min(fabs(ball_equilibrium - dist)*ball_slope, ball_pf_max_value);

    case RoleCommand::COM_DEF_SUPP:
    default:
      
      // If we're the defensive supporter, the ball only
      // repels us - i.e. get out of the primary attacker's
      // way. We don't want it to have an attractive component
      // so we play further back on the field for defense.
      return max((ball_equilibrium - dist)*ball_slope, 0.0);
  }
}

/* Contribution from a teammate to potential field */
double PotentialField::getTeammatePF(vector2d point, vector2d tm) {
  
  double dist = GVector::distance(point, tm);
  
  return max(0.0, teammate_repulsion + dist*teammate_falloff);
}


double PotentialField::getXBiasPF(vector2d point,
				  vector2d primary,
				  int uniform_color,
				  int supporter_role,
				  bool leader_is_goalie) {
  

  // If the goalie is the primary attacker, we do not use
  // the x-bias for a defensive supporter because we want all
  // robots to be in front of the ball 'cuz it's [hopefully]
  // going to be moving upfield. This also helps keep the
  // puppies out of the goalie's way.
  if(leader_is_goalie &&
     supporter_role==RoleCommand::COM_DEF_SUPP)
    supporter_role = RoleCommand::COM_OFF_SUPP;
  
  switch(supporter_role) {
    // If the goalie has the ball, it tell one uf our supporters to go to the
    // far end of the field to receive a clear.
    case RoleCommand::COM_OFF_SUPP_LONG:
      if(uniform_color==TEAM_COLOR_BLUE) {
	return max(0.0, 
		   (point.x - 0)*x_bias_gain);
      } else {
	return max(0.0, 
		   (0 - point.x)*x_bias_gain);
      }

    case RoleCommand::COM_OFF_SUPP:
      
      if(uniform_color==TEAM_COLOR_BLUE) {
	return max(0.0, 
		   (point.x - primary.x)*x_bias_gain);
      } else { 
	return max(0.0, 
		   (primary.x - point.x)*x_bias_gain);
      }
      
    case RoleCommand::COM_DEF_SUPP:
    default:
      
      if(uniform_color==TEAM_COLOR_BLUE) {
	return max(0.0, 
		   (primary.x - point.x + x_bias_def_offset)*x_bias_gain);
      } else {
	return max(0.0, 
		   (point.x - primary.x + x_bias_def_offset)*x_bias_gain);
      }
  }
}


/* Create a v-shaped potential centered an equilibrium distance from
   the path from the ball to the center of the opponent's goal. The
   trough of the V is always on the side of the ball path away from
   the end wall of the field by the opponents' goal.
*/
double PotentialField::getCorridorPF(vector2d point,
				     vector2d score_goal,
				     vector2d ball,
				     int supporter_role,
				     bool leader_is_goalie) {
  
  bool wrong_side_of_path = false;
  // Figure out the line from the point to the goal.
  vector2d delta = score_goal - ball;
  if(delta.y != 0) {
    double m = delta.x/delta.y;
    double b = ball.x;
    
    double dpy = point.y - ball.y;
    double x = m*dpy + b;
    
    if(score_goal.x < 0 &&
       point.x < x)
      wrong_side_of_path = true;
    else if(score_goal.x > 0 &&
	    point.x > x)
      wrong_side_of_path = true;
  }

  double dist = fabs(GVector::offset_to_line(score_goal, ball, point));  
  
  // If the primary attacker is playing goalie, we all want to
  // get out of its way.  
  if(leader_is_goalie &&
     supporter_role==RoleCommand::COM_DEF_SUPP)
    supporter_role = RoleCommand::COM_OFF_SUPP;
  
  switch(supporter_role) {
  case RoleCommand::COM_OFF_SUPP:
  case RoleCommand::COM_OFF_SUPP_LONG:
    /* If we're on the wrong side of the ball, return max. */
    if(((score_goal.x < 0) && (ball.x < point.x)) ||
       ((score_goal.x > 0) && (ball.x > point.x)))
      return corridor_pf_max_value;
    
    if(!wrong_side_of_path) {
      double potential = min(fabs(corridor_equilibrium - dist)*corridor_slope,
			     corridor_pf_max_value);
      return potential;
    } else {
      return min((corridor_equilibrium + dist)*corridor_slope,
		 corridor_pf_max_value);
    }
    
  case RoleCommand::COM_DEF_SUPP:
  default:
    return corridor_pf_max_value;
  }
}

double PotentialField::getBlockOwnGoalPF(vector2d point,
					 vector2d defend_goal,
					 vector2d ball,
					 int supporter_role) {
    
  double dist = fabs(GVector::offset_to_line(defend_goal, ball, point));
  
  switch(supporter_role) {
    case RoleCommand::COM_OFF_SUPP:
    case RoleCommand::COM_OFF_SUPP_LONG:
      return block_pf_max_value;
      
    case RoleCommand::COM_DEF_SUPP:
    default:
      /* If we're on the wrong side of the ball, return max. */
      if(((defend_goal.x < 0) && (ball.x < point.x)) ||
	 ((defend_goal.x > 0) && (ball.x > point.x)))
	return block_pf_max_value;
      
      return min(dist*block_slope,
		 block_pf_max_value);
  }
}

double PotentialField::getYBiasPF(vector2d point, vector2d ball,
			       int supporter_role) {

  double strength = ball.y/halfWidth;
  
  switch(supporter_role) {
    case RoleCommand::COM_OFF_SUPP:
    case RoleCommand::COM_OFF_SUPP_LONG:
      /* How strong should our y bias be? The bias is stronger
	 the further off center the ball is. */
      
      return max(strength*point.y*y_bias_slope, 0.0);
      
    case RoleCommand::COM_DEF_SUPP:
    default:
      return 0.0;
  }
}