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

/* The potential field that is used to position supporters
   on the field. */

#ifndef PotentialField_h
#define PotentialField_h

#include "../headers/gvector.h"
#include "../headers/Geometry.h"

class FeatureSet;

class PotentialField {

 private:
  
  /* Repulsion from the walls falls off linearly with
     distance.
  */
  static const double wall_repulsion = 1000;
  static const double wall_falloff = -2.0;

  static const double defense_zone_repulsion = 1000;
  static const double defense_zone_radius = 800;
  static const double defense_zone_fall_off = 
    -(defense_zone_repulsion/defense_zone_radius);
  
  static const double ball_equilibrium = 1000;
  // ball_slope = ball_repulsion/ball_equilibrium
  static const double ball_slope = .3;
  static const double ball_pf_max_value = ball_equilibrium*ball_slope;

  static const double teammate_repulsion = 1000;
  static const double teammate_falloff = -1.5;
  
  /* This bias guides robots in front of the 
     primary attacker to supporting positions in front.
     It guides robots behind the attacker to 
     supporting positions behind it.
  */
  static const double x_bias_gain = 0.250;
  // the defensive supporter bias begins x_bias_def_offset mm 
  // behind the primary attacker
  static const double x_bias_def_offset = 1000;

  static const double corridor_slope = .4;
  static const double corridor_equilibrium = 750;
  static const double corridor_pf_max_value =
    corridor_slope*corridor_equilibrium;

  static const double block_slope = .25;
  static const double block_pf_max_value = 300;

  static const double y_bias_slope = .1;
  
  // These parameters control how we look for minima on the surface.
  // Each frame we scan across a single row of points. This leads to ~2
  // or 3 seconds between completely covering the field, but checking
  // ~5000 points (required for 5 cm accuracy) in a single frame is
  // really pushing it. (For the record, gradient descent based methods
  // did not do terribly well, but I didn't really tweak the number
  // of random restarts/etc.
  double scan_x;
  
  // We don't need to track this since we cover a complete row each
  // scan.
  // double scan_y;

  vector2d last_min_point;

 public:

  PotentialField();

  vector2d getSupporterPath(FeatureSet *features);
  vector2d getMinPoint(FeatureSet *features);
  void printField(FeatureSet *features, double resolution_in_mm);

 private:
  vector2d getGrad(FeatureSet *features);
  vector2d getGrad(vector2d use_point, FeatureSet *features);

  // For debugging only - this will eat a lot of CPU
  vector2d gridGetMinPoint(FeatureSet *features);

  vector2d incrementalGetMinPt(FeatureSet *features);

  double calcPoint(vector2d point,
		   FeatureSet *features);

  double getWallPF(vector2d point);
  double getDefenseZonePF(vector2d point, int uniform_color);
  double getBallPF(vector2d point, vector2d ball,
		   int supporter_role);
  double getTeammatePF(vector2d point, vector2d tm);
  double getXBiasPF(vector2d point, vector2d primary,
		    int uniform_color,
		    int supporter_role,
		    bool leader_is_goalie);
  double getCorridorPF(vector2d point, 
		       vector2d score_goal,
		       vector2d ball,
		       int supporter_role,
		       bool leader_is_goalie);
  double getBlockOwnGoalPF(vector2d point,
			   vector2d defend_goal,
			   vector2d ball,
			   int supporter_role);
  double getYBiasPF(vector2d point, vector2d ball,
		    int supporter_role);
};

#endif