/* 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 _WORLD_MODEL_H_
#define _WORLD_MODEL_H_

#include <math.h>
#include <stdio.h>
#include <assert.h>

#include <vector>
#include <deque>
#include <map>

using namespace std;

// defined in "../headers/Sensors.h"
class SensorData;

#include "../headers/field.h"

#include "../headers/MessageStructures.hh"
#include "../headers/MoveDistributions.hh"
#include "../Motion/MotionInterface.h"
#include "../Vision/VisionInterface.h"

#include "../headers/Config.h"
#include "../headers/Geometry.h"
#include "../Main/TeamMsgMgr.h"
#include "../Behaviors/constants.h"
#include "BallTracker.h"
#include "TeammateBallTracker.h"
#include "WorldModelEncoder.h"
#include "SPOutTrackerEncoder.h"
#include "SPOutWMDebugEncoder.h"

// 06/10/04 - PER
// Set this to true to return the old ball estimate in the tracker interfaces
// otherwise set it to false
#define USE_OLD_BALL_ESTIMATE false

class PacketStream;
class PacketStreamCollection;

#define SMALL_ERROR 10.0  /* For now, the small error is 10 centimeters.  
			     I got this from the old world model, so I 
			     assume it has been tweaked. */

// Uncertainty estimates on shared ball information - 
// this used to be 10 mm, but that was a little optimistic.
const double SHARED_BALL_ERROR = 500.0;

#define MINCONF_BALL 0.40
#define MINCONF_OPPONENT 0.40
#define MINCONF_GOAL 0.90

/* Limit how far our uncertainty bounds on the ball can
   converge.
*/
#define CONVERGE_LIMIT 20.0

#define LOST_BALL_TIME 3000000 /* If we haven't seen the ball in 3 seconds,
				  it is totally lost */

/* We don't want the robot to walk through the ball. Looking at data from
   running the robot, it doesn't appear that the ball ever gets much closer
   than 50 mm to our chest. When it's to the side of our legs, we tend to
   see it at a y offset of more than 100 mm. (This seems large). When it's
   actually against our chest, y tends to be around 20.
*/
#define BALL_ON_CHEST_X_MIN 90 // Used to be 70 6/16/2004
#define BALL_ON_CHEST_Y_MAX 90  // Used to be 40 6/16/2004

// Number of frames of ball information that we save
#define BALL_HIST_LEN 25

// If the ball seems to be moving this fast in the ball trajectory
// line fit, ignore it. We probably detected a spurious ball and are
// fitting to atrocious data.
const float RIDICULOUS_SPEED = 3000.0;

/*use goal vision in world model*/
static const bool USE_GOAL_VISION = true; 
/*num frames since goal last seen before only localization data is used*/
const double GOAL_INTERP_FRAMES = 200;    

extern double MaxDX;

struct BallHist{
  double confidence; // only meaningful when saw_ball is true
  ulong time;
  vector2d odo_frame;
  bool saw_ball;
};

struct BallTrjInfo{
  // The line defining our estimate of the ball path.
  // t is in seconds and ZERO is the present moment.
  // deltas are in mm/sec
  // offsets are in mm
  // global_x(t) = t*velocity.x + intercept.x;
  // global_y(t) = t*velocity.y + intercept.y;
  
  vector2d velocity;
  vector2d intercept;

  // Here's some information to make capturing the ball
  // easier.
  double approach_vel; // magnitude of velocity component toward us
  double perp_vel; // magnitude of velocity perpendicular to us
  double perp_offset; // y coord at which it will roll by us
  
  // Normalized chi^2 error of the regression that was used to
  // estimate the velocity.
  double chi2;

  // mean squared error in predicted versus actual posns in hist
  // set to -1 if no trajectory available
  double sq_error;
};

//==========================================================================
// These structures are used differently from the above

// This is storage for the world model history
struct BallObservation {
  Gaussian2 position_local_frame;  // Stored in the odometric reference frame
  Gaussian2 position_odo_frame;  // Stored in the odometric reference frame
  Gaussian2 position_global_frame;  // Stored in the odometric reference frame
  ulong obs_time;                // Time when this observation was made
  BallObservation() { obs_time = 0; }
};

//==========================================================================
class WorldModel {

 public:

  int goalColor;
  int teamColor;
  int myID;

  double ball_on_chest_prob;
 
 private:

  // Logging variables
  SPOutWorldModelEncoder wm_encoder;
  SPOutTrackerEncoder tracker_encoder;
  SPOutWMDebugEncoder wmdebug_encoder;
  ulong lastUpdateModelTime;
  PacketStream *wmOutStream;
  PacketStream *trackerOutStream;
  PacketStream *wmDebugOutStream;

  Gaussian2 ball;
  Gaussian2 self_position;
  vector2d self_heading;
  
  int hist_cnt;
  BallHist ballHistory[BALL_HIST_LEN];
  void ballHistAdd(bool see_ball, 
		   double loc_x, double loc_y,
		   ulong time,
                   double confidence);

  vector2d odo_position;
  vector2d odo_heading;

  //===============================================================
  // A queue of shapes to display on the GUI
  std::deque<WMDebug> wmdebug_deque;

  //===============================================================
  // This is the definitive world model history which will make its
  // data available to any method which requires it.  It consists of
  // positions of objects observed from our own camera as well as the
  // positions our teammates and positions of objects their cameras
  //
  // Eventually, it would probably make sense for the other methods
  // like the ball trajectory fitting stuff use this history, but
  // for now, some duplication isn't going to hurt anything
  //
  // Four seconds
  static const ulong max_obs_age = 4000000L;

  // Store our own ball history
  std::deque<BallObservation> ball_obs;
  // Store red robot observations
  std::deque<RobotObservation> red_robot_obs;
  // Store blue robot observations
  std::deque<RobotObservation> blue_robot_obs;

  // Shared data from our teammates
  std::map<int, std::deque<StatusMsgEntry> > teammate_msgs;

  // Kalman Filter ball tracker object
  BallTracker ball_tracker;
  // Kalman Filter teammate ball observation tracker object
  TeammateBallTracker teammate_ball_tracker;

  //===============================================================
  // Track what motion we were doing in the previous frame.
  // This lets us do things immediately after motions (e.g. kicks)
  // terminate. Like update the posn of the ball after we kick it.
  short last_motion_state;
  short last_motion_state_type;

 public:

  WorldModel();
  void init(int gColor, int tColor, int ID, ulong time);
  void initializeStreams(PacketStreamCollection *out_psc);

  void updateModel(VisionInterface::ObjectInfo *obj_info,
		   SensorData *sensor_data, TeamMsgMgr *msg_mgr,
		   RobotPositionInfo const posn,
		   VisionInterface::HeadVelocity *head_vel, ulong time);

  // We move odometry data into the world model. This is different from
  // position info, which comes cooked from localization - we use this
  // info to track an odometry only pseudo-global frame.
  void updateModelMotion(const Motion::MotionLocalizationUpdate *move);
  void updateModelTeamMsg(int sender_id, const StatusMsgEntry &msg);
  void fillInStatusMsg(StatusMsgEntry &entry);

  int validBallsInHist();

  vector2d estimateBallPosn(double secs_from_now, 
                            int min_hist_len,
                            int max_hist_len = BALL_HIST_LEN);
  
  BallTrjInfo getBallTrajectory(int min_hist_len, 
                                int max_hist_len = BALL_HIST_LEN);

  short lastMotionStateType();

  // Prediction of where we should go to intercept the ball.  We do
  // this by drawing a line perpendicular to the dog, and finding out
  // where the predicted ball trajectory crosses this line. Returns a
  // vector giving the location of the intercept, and a double giving
  // the expected time that the intercept will occur. If there is no
  // intercept, the time returned is -1.0.
  std::pair<vector2d, double> getBallIntercept(int min_hist_len);

  //--------------------------------------------------------------------------
  //--------------------------------------------------------------------------
  //--------------------------------------------------------------------------
  // Provide interfaces for the stored world model history information

  // Provide interfaces for the robot ball position that are indexed by
  // delta time from now
  //
  // These store a history of raw vision readings
  bool ball_mean(vector2d & pos, ulong dt=0);
  bool ball_observation(Gaussian2 &  pos, ulong dt=0);

  // Provide interfaces for the red and blue robot observations that
  // are indexed by time
  bool red_robot_mean(vector2d & pos, ulong dt=0);
  bool blue_robot_mean(vector2d & pos, ulong dt=0);
  bool red_robot_observation(Gaussian2 & pos, ulong dt=0);
  bool blue_robot_observation(Gaussian2 & pos, ulong dt=0);

  // Provide interfaces for the teammate positions indexed by time
  bool teammate_pos_mean(vector2d & pos,
			 int teammate,
			 ulong dt=0);
  bool teammate_pos_observation(Gaussian2 & pos,
				int teammate,
				ulong dt=0);

  // Provide interfaces for the teammate ball observations indexed by time
  bool teammate_ball_mean(vector2d & pos, 
			  int teammate,
			  ulong dt=0);
  bool teammate_ball_observation(Gaussian2 & pos, 
				 int teammate,
				 ulong dt=0);

  // Provide interfaces for the teammate red and blue robot observations
  // that are indexed by time

  // TODO: 6/10/04 PER
  // Pass in a stl::vector of these objects instead of a singleton value
  // since there are likely to be multiple of these per frame
  bool teammate_red_robot_mean(vector2d & pos, 
			       int teammate,
			       ulong dt=0);
  bool teammate_blue_robot_mean(vector2d & pos, 
				int teammate,
				ulong dt=0);
  bool teammate_red_robot_observation(Gaussian2 & pos,
				      int teammate,
				      ulong dt=0);
  bool teammate_blue_robot_observation(Gaussian2 & pos,
				       int teammate,
				       ulong dt=0);
  
  //--------------------------------------------------------------------------
  //--------------------------------------------------------------------------
  //--------------------------------------------------------------------------
  // What are the ball tracker's best ball estimates for position and 
  // velocity.  The position is in odometric coordinates.
  int ball_tracker_hypothesis(bool use_old_ball_estimate=USE_OLD_BALL_ESTIMATE);

  bool ball_tracker_observation(Gaussian2 & pos,
				int hypothesis=0,
				bool use_old_ball_estimate=USE_OLD_BALL_ESTIMATE);
  bool ball_tracker_observation_global(Gaussian2 & pos, 
				       int hypothesis=0,
				       bool use_old_ball_estimate=USE_OLD_BALL_ESTIMATE);

  bool ball_tracker_observation_from_ID(Gaussian2 & pos,
					ulong ID,
					bool use_old_ball_estimate=USE_OLD_BALL_ESTIMATE);
  bool ball_tracker_observation_global_from_ID(Gaussian2 & pos, 
					       ulong ID,
					       bool use_old_ball_estimate=USE_OLD_BALL_ESTIMATE);

  // Return the hypothesis given specified ID
  bool get_ball_tracker_hypothesis_from_ID(int & hypothesis,
					   ulong ID,
					   bool use_old_ball_estimate=USE_OLD_BALL_ESTIMATE);
  // Return the ID of the specified hypothesis
  bool get_ball_tracker_ID_from_hypothesis(ulong & ID,
					   int hypothesis=0,
					   bool use_old_ball_estimate=USE_OLD_BALL_ESTIMATE);
  
  // Return the index of the closest ball to us
  bool ball_tracker_closest(int & idx,
			    bool use_old_ball_estimate=USE_OLD_BALL_ESTIMATE);

  // Return the time of the last ball observation
  bool ball_tracker_observation_time(ulong & t,
				     int hypothesis=0,
				     bool use_old_ball_hypothesis=USE_OLD_BALL_ESTIMATE);
  bool ball_tracker_observation_time_from_ID(ulong & t,
					     ulong ID,
					     bool use_old_ball_hypothesis=USE_OLD_BALL_ESTIMATE);

  //--------------------------------------------------------------------------
  //--------------------------------------------------------------------------
  //--------------------------------------------------------------------------
  int teammate_ball_tracker_hypothesis();

  bool teammate_ball_tracker_observation_global(Gaussian2 & pos, 
						int hypothesis=0);

  bool teammate_ball_tracker_observation_global_from_ID(Gaussian2 & pos, 
							ulong ID);

  // Return the hypothesis given specified ID
  bool get_teammate_ball_tracker_hypothesis_from_ID(int & hypothesis,
						    ulong ID);

  // Return the ID of the specified hypothesis
  bool get_teammate_ball_tracker_ID_from_hypothesis(ulong & ID,
						    int hypothesis=0);
  
  // Return the index of the closest ball to us
  bool teammate_ball_tracker_closest(int & idx);

  bool teammate_ball_tracker_observation_time(ulong & t,
					      int hypothesis=0);

  bool teammate_ball_tracker_observation_time_from_ID(ulong & t,
						      ulong ID);

  //--------------------------------------------------------------------------
  //--------------------------------------------------------------------------
  //--------------------------------------------------------------------------

  // These are used by the behaviors
  void sendWMDebug(float x, float y,
		   uchar shape,
		   uchar red,
		   uchar green, 
		   uchar blue);

  // The number of debug points in the deque
  int numWMDebug();
  // Returns the 'nth' debug point
  bool getWMDebug(WMDebug & wmd,int idx);
  // Clears all of the debug points
  void clearWMDebug();

  //--------------------------------------------------------------------------
  //--------------------------------------------------------------------------
  //--------------------------------------------------------------------------


  // ================================================ Public Interface
  vector2d my_position();
  Gaussian2 my_position_estimate();
  vector2d my_heading();

  // 06/10/04 - PER
  // These ball positions are calculated in a depricated fashion
  // They will still be kept around, but we should be using the
  // ball_tracker_* methods instead
  vector2d ball_posn();
  vector2d ball_posn_global();
  Gaussian2 ball_estimate();
  Gaussian2 ball_estimate_global();

  // These all return locations in local coordinates.
  vector2d cyan_goal();
  vector2d yellow_goal(); 
  vector2d score_goal(); 
  vector2d defend_goal();

  // ================================================ Public Interface
  // Conversions from local to global
  vector2d LocalToGlobal(vector2d object);
  vector2d LocalToGlobal(vector2d object, vector2d my_position,
			 vector2d my_heading);
  vector2d GlobalToLocal(vector2d object);
  vector2d GlobalToLocal(vector2d object, vector2d my_position,
			 vector2d my_heading);

  // Needed: accessors for all of the data stored in the world model
  //         * return teammate observations for the ball
  //         * return observations for the opponents
  //         * return teammate observations for the opponents
  //         * return locations of the teammates

  /* Returns the confidence that an opponent is within the
     width_maj by width_min rectangle centered at center. The
     width_maj sides are aligned along theta_maj.
  */
  double opponentInRect(vector2d center, double width_maj,
			double width_min, double theta_maj);
  double teammateInRect(vector2d center, double width_maj,
			double width_min, double theta_maj);
  double robotInRect(vector2d center, double width_maj,
		     double width_min, double theta_maj);
  
  
 private:
  //  void dumpBallHist();

  void doChestIRUpdate(SensorData *sensor_data, ulong time);
  void doSensorUpdate(VisionInterface::ObjectInfo *obj_info, 
		      VisionInterface::HeadVelocity *head_vel,
		      SensorData* sensor_data, ulong time);
  void doStatusInfoUpdate(TeamMsgMgr *msg_mgr, ulong time);
  void doTimeUpdate(ulong time);
  void doPosnUpdate(RobotPositionInfo const posn, 
		    ulong const time); // move posn data into world model

  vector2d localToOdo(vector2d local_posn);
  vector2d odoToLocal(vector2d odo_posn);
  vector2d odoToGlobal(vector2d odo_posn);
  vector2d globalToOdo(vector2d global_posn);

  bool pointInRect(const vector2d pt,
		   const vector2d rect_center,
		   double width_maj, double width_min,
		   double theta_maj);

  // how many robot observations are hanging around in the rect?
  int robotObsInRect(int robot_color,
		     bool care_about_color,
		     vector2d center, double width_maj,
		     double width_min, double theta_maj);
};

#endif