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

#include "../Motion/MotionInterface.h"
using namespace Motion;

#include "state_machine.h"
#include "../headers/field.h"
#include "FeatureSet.h"
#include "HeadLowLevel.h"

class BeFindBall;
class BeGotoPointGlobal;
class Turn;

class BeFindBall
{
public:
  static const int beh_id;

  static char const * const beh_name;
  
  static const int NumStates=5;
  enum State { SETTLE, LOOK_AT_BALL,
               SEARCH_WITH_HEAD, SEARCH_SPIN, 
               WALK_AROUND };

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;

  enum WalkTarget {BALL, MIDDLE};

  WalkTarget goto_state;

  double spin_direction;
  double ball_distance;
  ulong timer;

  BeGotoPointGlobal *goto_point;
  BeTrackObjects *track_objects;

  /* This is a *heuristic* that estimates whether or not
     the dog should reliably see the ball given its current
     position in the world model. Should this go in the feature
     set?
  */
  bool shouldSeeBall(FeatureSet *features);

  void resetVars();

public:
  BeFindBall();
  ~BeFindBall();
  void reset(ulong timestamp);
  void sleep();

  double operator()(FeatureSet *features, 
		    MotionCommand *command);

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

// Position the body at point specified.
class BeGotoPointGlobal {
public:
  static const int beh_id;

  static char const * const beh_name;  

  static const int NumStates = 1;
  enum State { CLEAR_PATH };

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;

  vector2d target_pt;
  double target_angle;
  double heading_ego;
  double heading_global;
  double spin_dir;

  bool obey_final_angle;
  bool obey_heading_ego;
  bool obey_heading_global;

  int walk;

  void resetVars();

public:
  BeGotoPointGlobal();
  ~BeGotoPointGlobal();
  void reset(ulong timestamp);
  void sleep();
  void setWalk(int walk); // one of MOTION_WALK_*
  void setTarget(double X, double Y);
  void setTarget(vector2d target);
  void setTarget(double X, double Y, double A);
  vector2d getTarget();
  void setHeadingEgo(double A);
  void setHeadingGlobal(double A);

  double operator()(FeatureSet *features,
		    MotionCommand *command);

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

class BeNavToPointGlobal {
public:
  static const int beh_id;

  static char const * const beh_name;  

  static const int NumStates = 2;
  enum State { NAV_TO_POINT, STUCK };

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;

  OccGridEntry cell;

  vector2d target_pt;
  bool global_coord;
  bool no_walk;

  //Params for checkObsAngle
  double rec_length;	//Length of one rectangle
  double rec_width;	//Width of one rectangle
  int num_recs;		//Number of rectangles forward to check

  vector3d free_obs_params;	//Params for when robot is not stuck (free)
  vector3d stuck_obs_params;	//Params for when robot is stuck

  bool black_obs;	//Consider black obstacles in query
  bool free_black_obs;	//Use black obstacles when not stuck
  bool stuck_black_obs;	//Use black obstacles when stuck

  bool use_stuck_detector; //Use stuck detector (duh!)

  ulong start_time_stuck;  //Start time of current stuck state
  ulong start_time_walk;   //Start time of current walk state

  void resetVars(ulong timestamp);
  bool cellIsClear();
  bool checkObsAngle(FeatureSet *features, double a);
  vector2d pickClosestAngleToTarget(FeatureSet *features, vector2d rel_target);
  void setNavParams();
  double navToPoint(FeatureSet *features, MotionCommand *command, bool trot_fast);

public:
  BeNavToPointGlobal();
  ~BeNavToPointGlobal();
  void reset(ulong timestamp);
  void sleep();
  void setTargetGlobal(double X, double Y);
  void setTargetGlobal(vector2d target);
  void setTargetEgo(double X, double Y);
  void setTargetEgo(vector2d target);
  void setCheckObstParams(double length, double width, int recs, bool blkobs=false);
  void setCheckObstParamsStuck(double length, double width, int recs, bool blkobs=true);
  void setBlackObs(bool blkobs);
  void setBlackObsStuck(bool blkobs);
  void setStuckDetector(bool stuck);
  bool isStuck();

  double operator()(FeatureSet *features,
		    MotionCommand *command,
		    bool trot_fast=false);

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


struct BeGetBallInfo {
  bool have_ball; // true if BeGetBall thinks it has control of the ball
  double free_kick_angle;  
};

class BeGetBall {
public:
  static const int beh_id;

  static char const * const beh_name;
  
  static const int NumStates=5;
  enum State { APPROACH, LOST_BALL, LOCALIZE, LOCALIZE_LOST, LOOK_BALL};

  double near_ball_threshold;

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;

  ulong timer;
  ulong last_lost_localize;
  ulong defensebox_timer;
  BeFindBall *find_ball;
  BeActiveLocalize *localize;
  BeLookAtPoint *look_at;
  BeTrackObjects *track_objects;
  BeLookAtObject *look_at_object;
  BeLookAtObjectInfo look_at_object_info;

  /* The angle formed between the dog->ball and ball->goal
     vectors. This is tracked via a running average to smooth
     out noise.
  */
  vector3d prev_ball_loc;

  double p2b2g_angle;

  vector2d getMove(FeatureSet *features,MotionCommand *command,bool &use_verbatim);
  void checkForObstacles(FeatureSet *features,BeGetBallInfo *info);

public:
  BeGetBall();
  ~BeGetBall();

  void reset(ulong timestamp);
  void sleep();

  double operator()(FeatureSet *features,
                    BeGetBallInfo *info,
		    MotionCommand *command);

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



class BeTurn {
 private:
  static const int NumStates = 2;
  enum State { START, MAIN_TURN};

  double near_ball_threshold;

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

  FSM fsm;

  static double getTurnTime(double da, double &t_accel,
			    double &t_cruise, double &t_decel);
  
  ulong timer;
  bool added_extra;
  ulong last_ball_on_chest_reading;
  
 public:
  BeTurn();
  ~BeTurn();

  void reset(ulong timestamp);

  //turn to a specified angle
  double turnToAngle(FeatureSet *features,
		     MotionCommand *command,
		     double targetAngle,
		     double thresh);

  //turn to specified angle with a ball
  double turnBallToAngle(FeatureSet *features,
			 MotionCommand *command,
			 double targetAngle, 
			 double thresh);

  //circle the ball to the desired angle
  double circleBallToAngle(FeatureSet *features,
			 MotionCommand *command,
			 double targetAngle, 
			 double thresh);

};


/**
 * FindBallNear - when the world model says the ball
 * is really close to us (after messing up the approach
 * for example)
 */

class FindBallNear {
private:
  static const int beh_id;

  static char const * const beh_name;
  
  static const int NumStates=3;
  enum State {LOOK, SCAN, BACKUP};

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

  FSM fsm;
  FeatureSet* fs;

public:
  FindBallNear();
  ~FindBallNear();
  void reset(ulong timestamp);
  void sleep();

  double operator()(FeatureSet *features,MotionCommand *command);
};


class BeApproachBall {
public:
  static const int beh_id;

  static char const * const beh_name;
  
  static const int NumStates = 6;
  enum State { APPROACH_CENTER, APPROACH_SIDE, APPROACH_NEAR,GRAB_SLOW,GRAB_FAST,GOALIE };
  enum Sides {LEFT, RIGHT, CENTER};

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;

  vector2d ball_v;
  vector2d walk_rel;
  vector2d gpoint;

  BeTrackObjects *track_objects;

  ulong last_ball_on_chest_reading;
public:

  BeApproachBall();
  ~BeApproachBall();
  void reset(ulong timestamp);
  void sleep();

  void setApproachType(State s, int side, ulong timestamp);

  double operator()(FeatureSet *features,
		    MotionCommand *command);

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



class BeDribble {
public:
  static const int NumStates = 2;
  enum State {TURN, WALK};

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;

  static const int beh_id;
  static char const * const beh_name;

  BeTurn *turn;
  double turn_angle;

  ulong start_time;
  ulong turn_time;

  double thresh_x, thresh_y;

public:
  BeDribble();
  ~BeDribble();
  void reset(ulong timestamp);
  double operator()(FeatureSet *FS_p,MotionCommand *command, vector2d p);
  void setThresholds(double x, double y);
  void setDefaultThresholds();  
};



class BeGetNearBall {
public:
  static const int NumStates = 8;
  enum State {LOOK_FORWARD,LOOK_NEAR,SPIN,SETTLE,WANDER_BALL,WANDER_RANDOM,GET_BALL,LOST_SCAN};

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;

  static const int beh_id;
  static char const * const beh_name;

  ulong timer;
  double spin_direction;
  bool first_spin;
  int prev_ball_hyp;
  int prev_state;
  vector3d look_target;

  vector2d walk_target_bound1;
  vector2d walk_target_bound2;

  double nav_retval;
  BeNavToPointGlobal *navtopoint;
  BeGotoPointGlobal *gotopoint;
  BeTrackObjects *track_objects;

public:
  BeGetNearBall();
  ~BeGetNearBall();
  void reset(ulong timestamp);
  void resetVars();
  double operator()(FeatureSet *features,MotionCommand *command);
  
  void setTargetBounds(double pt1_x, double pt1_y, 
		       double pt2_x, double pt2_y);
  void setTargetBounds(vector2d pt1, vector2d pt2);
  void setRandomTarget(FeatureSet *features);
  vector2d avoidDefenseBoxTarget(FeatureSet *features, vector2d global_target);

};

#endif