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

#ifndef FeatureSet_h
#define FeatureSet_h

#include <math.h>

#include "../headers/MessageStructures.hh"
#include "../Vision/VisionInterface.h"
#include "../Motion/MotionInterface.h"
#include "../Localization/SPOutLocalizationEncoder.h"
#include "../WorldModel/LocalModel.h"

#include "../headers/Geometry.h"
#include "../headers/Util.h"
#include "../headers/CircBufPacket.h"
#include "../Main/Events.h"
#include "../Main/TeamMsgMgr.h"
#include "../headers/gvector.h"

#include "../WorldModel/StuckDetector.h"

#include "BotModel.h"

using namespace BotModelSpace;
using namespace LocalModel;

class LocalizationEP;
class MotionUpdateVelocitySummary;
class Vision;
namespace VisionInterface {
  class RadialObjectMap;
  class ObjectInfo;
  class VObject;
}

using namespace VisionInterface;

#include "../WorldModel/WorldModel.h"
#include "constants.h"

/* If vision reports seeing an object with greater than this
   threshold, assume that we actually did see the object.
   The low, med, and high suffixes indicate how confident we
   are that we're right.
*/
const double have_ball_threshold_ers7 = 120;
const double have_ball_threshold_ers210 = 95;

const double see_threshold_low = 0.4;
const double see_threshold_med = 0.65;
const double see_threshold_high = 0.85;

const double stuck_threshold_low = 0.45;
const double stuck_threshold_med = 0.60;
const double stuck_threshold_high = 0.80;

enum FieldLengthRegion {DEF, MID, FWD};
enum FieldWidthRegion  {CENTER, SIDE};
class FeatureSet : public EventProcessor
{
public:

  /* ========================================> Decision Tree */

  vector2d position;
  double angle;

  double pos_uncert_sMaj;
  double pos_uncert_sMin;


  double def_mid_pos_thresh;
  double mid_fwd_pos_thresh;
  double side_pos_thresh;

  int field_region_x;
  int field_region_y;

  double have_ball_threshold;
  bool have_ball_center;
  bool have_ball_right;
  bool have_ball_left;
  bool have_ball;
  bool near_ball;

  double goal_angle;
  
  bool obst_left_of_ball;
  bool obst_right_of_ball;
  bool ball_near_obst;
  bool ball_near_robot;

  bool saw_ball_recently;

  bool in_front_corner;
  bool facing_own_half;
  bool near_wall;
  bool ball_near_wall;
  bool near_front_wall;

  bool facing_ball;
  bool facing_score_goal;
  bool ball_on_front_wall;

  int game_state;
  bool kickoff;

  bool ball_on_chest;
  bool approach_failed;

  bool high_loc_uncertainty;

  bool canInterceptBall(int which_dive);
  bool canInterceptBallSloppy(int which_dive);
  bool canInterceptBallPassingChallenge(int which_dive);
  // don't call this function directly except from within FeatureSet.
  bool canInterceptBallImpl(int which_dive, 
                            double trj_uncertainty_max,
                            double intercept_early_time,
                            double vel_thresh,
                            double catch_width_mult,
                            int min_hist_len);
  // Determines if blocking is a good idea.  We look to see where the
  // ball would be if we blocked, vs. where the ball would be 0.1
  // seconds after it passed us, and will decide that blocking is OK
  // if the "blocked" position is closer to score_goal than the
  // "un-blocked" position.
  bool shouldInterceptIfPossible();

  int pass_receiver; // 0 if noone is ready for a pass, >0 otherwise
  vector2d pass_receiver_pos_global;

  bool teammate_passed_recently;
  ulong teammate_pass_time;

  int last_supporter_role;
  bool was_primary_last;
  ulong last_role_change_time;
  ulong time_in_role;

  /* ========================================> World */

  /* The current time (in microseconds) */
  ulong current_time;

  vector3d camera_loc;
  vector3d camera_dir;

  int robot_id; // my network id (e.g. ers01 = 1, etc)
  int robot_name; //my assigned name (MOON,CLOUD, etc)

  RobotPositionInfo my_position;
  vector2d my_position_body;
  double my_angle;
  vector2d neck_offset;
  
  TeamMsgMgr *team_msg_mgr;
  WorldModel *world_model;

  SensorData *sensors;

  /* ========================================> Robots */
  
  bool ally_in_front;
  bool opponent_in_front;

  /* ========================================> Ball */
  
  /* A measure of how confident we are that we saw
     the ball in the last vision frame.
  */
  double saw_ball_confidence;
  
  ulong last_saw_marker;
  ulong last_saw_goal;

  /* The timestamp (in microseconds) when we last saw the
     ball with confidence > saw_ball_threshold.
  */
  ulong ball_last_seen_low;
  ulong ball_last_seen_med;
  ulong ball_last_seen_high;

  ulong time_since_last_ball_obs;

  bool see_ball_high;
  bool see_ball_med;
  bool see_ball_low;

  bool stuck_high;
  bool stuck_med;
  bool stuck_low;

  /* True if the ball is in our own defense zone. */
  bool ball_in_defense_box;

  /* A vector pointing from the robot to the ball. */
  Gaussian2 ball_gaus;
  Gaussian2 world_ball_gaus;
  vector3d ball_vector3d;
  vector2d ball_vector;
  bool valid_ball_hyp;

  /* Ball location in world coordinates */
  vector2d world_ball_vector;

  double ball_stddev[2];
  vector2d ball_error_vector;
    
  /* Distance to the ball */
  double ball_distance;

  /* Angle to the ball */
  double ball_angle;

  /* Ball trajectory  */
  BallTrjInfo ball_trj;

  /* Returns true if we have a valid ball hypothesis, and the most
     likely hypothesis is within the given distance. */
  bool ballClose(double distance);

  /* Gets the best ball position, based on both local observations and
     those of teammates, incorporating substantial hysteresis to
     attempt to avoid oscillations. This function will, by definition,
     be a bit laggy, but should be pretty good for purposes like
     supporter/goalie positioning. Returns an int telling you which
     ball it chose (see the .cc file for details). */
  // FIXME: come up with symbolic names for the enumeration of
  // the values that are returned.
  int getBestBallPosn(vector2d& ball);
  int getBestBallPosnPrimary(vector2d& ball);

  /* ========================================> Goals */
  
  /* Are we in our own half? */
  bool in_own_half;
  
  /* Color of the goal which we are defending. */
  int defend_goal_color;

  /* The color of my team's uniform. */
  int uniform_color;

  double saw_score_goal_confidence;
  double saw_defend_goal_confidence;

  ObjectInfo *vision_info;
  VObject *score_goal;
  VObject *defend_goal;
  RadialObjectMap *radial_map;
  LModel *local_model;

  //  Gaussian2 *score_goal_wm_start;
  //  Gaussian2 *defend_goal_wm_start;
  //  Gaussian2 *score_goal_wm_global_start;
  //  Gaussian2 *defend_goal_wm_global_start;
  
  // these are from the world model.
  //  vector2d score_goal_mean;
  //  vector2d defend_goal_mean;
  //  vector2d score_goal_mean_global;
  //  vector2d defend_goal_mean_global;
  //  double score_goal_left_angle;
  //  double score_goal_right_angle;

  // This is a useful constant vector. It points to the
  // center of the goal we're trying to score on.
  // It gives the position in GLOBAL coordinates.
  vector2d score_goal_const;
  vector2d defend_goal_const;

  /* ========================================> Self */
  
  StuckInfo stuck_info;

  bool robot_is_paused;

  short motion_state;
  short prev_motion_state;
  vector3d velocity;

  static const int kick_hist_len = 3;
  ulong kick_hist[kick_hist_len];
  
  /* Administrative Variables */
  SPOutLocalizationEncoder encoder;
  PacketStream *locOutStream;
  int poseBroadcastCounter;

  FeatureSet(void);
  
  void initializeStreams(PacketStreamCollection *out_psc);

  void update(ulong timestamp, int r_id,
	      int goal_color, int jersey_color,
	      bool goalie,
	      WorldModel *modeller,
	      BotModel *botModeller,
	      ObjectInfo *object_info,
              RadialObjectMap *radial_map_param,
	      LModel *lmodel,
	      TeamMsgMgr *_team_msg_mgr,
              vector2d _neck_offset,
	      StuckInfo _stuck_info,
	      VisionInterface::HeadVelocity *head_vel,
              bool _robot_is_paused,
	      short mot_state);

  /**
   * Checks for obstacles at the given angle.
   * Returns true if there are obstacles at the angle
   * false if it is clear to walk at that angle.
   */
  bool checkObsAngle(double a, double clen);

  /*Check for obstacles next to the ball */
  void checkBallObst(double clen);


  /* Converts rel_vec, a vector in relative coordinates
     to a global position based the position and angle of
     the robot (as contained in the feature set). */
  vector2d getGlobalPosn(vector2d rel_vec);

  /* Converts an object location in global coordinates to
     a relative vector given where we currently are [think we are].
  */
  vector2d getRelativePosn(vector2d global_vec);

  vector3d getRelativePosn(vector3d global_vec);
  
  /* Returns true if rel_vec points toward the goal
     we're trying to score into. (toward is defined
     as rel_vec has a positive component going in
     that direction. NOTE THAT rel_vec IS A RELATIVE
     HEADING BASED ON THE DOG'S CURRENT POSITION.
  */
  bool pointsToScoreGoal(vector2d rel_vec);
  
  bool pointInDefenseBox(vector2d global_pt);
  bool attackerAvoidPoint(vector2d global_pt);

  /* Errs on the side of caution - if we don't know we say
     no so that we don't give up looking for it.
  */
  bool isBallInDefenseBox(void);

  double cosAngleCameraToPoint(vector3d pt);

  bool isPointInFront(vector2d point);
  bool isPointInFrontLocal(vector2d point);

  /* Do we see anything in our CURRENT visual frame
     that indicates kicking would be a silly thing to
     do? (i.e. don't kick if you're staring at your
     own goal)
  */
  bool shouldNotKick(void);

  /* Determines whether or not the robot should 
     attempt to dribble the ball forward.
  */
  bool shouldDribble();

  
  void store_kick_time();
  ulong get_kick_time(int idx);

  void queryLocalModel(OccGridEntry *cell,vector2f direction, vector2f cell_center, vector2f range, bool black_obs=false);
  bool cellIsClear(OccGridEntry cell);

  // Event interface related stuff
  static std::string name;
  static EventProcessor *create();
  virtual bool initConnections();
  virtual bool update(ulong time,const EventList *events);
  FeatureSet *get(ulong time);

  SensorData *ep_sensors;
  Vision *vision;
  LocalizationEP *localization_ep;
  MotionUpdateVelocitySummary *robot_velocity_ep;
};

#endif