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

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

#include "BotModel.h"
#include "constants.h"
#include "FeatureSet.h"
#include "../headers/SPOutEncoder.h"
#include "../Main/globals.h"
#include "../Main/TeamMsgMgr.h"
#include "../Motion/MotionInterface.h"
#include "../Localization/GLocalization.h"
#include "../Localization/SRL/Localization.h"
#include "../Vision/VisionInterface.h"
#include "../WorldModel/LocalModel.h"
#include "../WorldModel/PostureDetector.h"
#include "../WorldModel/StuckDetector.h"
#include "../WorldModel/WorldModel.h"

extern double MaxDX;
extern double MaxDY;
extern double MaxDA;

using namespace Motion;
using namespace VisionInterface;
using namespace LocalModel;

/*------------------------------------------------------------------
CLASS
  BehaviorSystem

DESCRIPTION
  The behavior system.
------------------------------------------------------------------*/
const int BEHAVIOR_NEW_ATTACK        =  0;
const int BEHAVIOR_GOALIE            =  1;
const int BEHAVIOR_CHASE_BALL        =  2;
const int BEHAVIOR_CAMERA            =  3;
const int BEHAVIOR_LOCALIZATION_TEST =  4;
const int BEHAVIOR_TEST_SLENSER      =  5;
const int BEHAVIOR_TEST_JBRUCE       =  6;
const int BEHAVIOR_TEST_DVAIL2       =  7;
const int BEHAVIOR_TEST_BRETTB       =  8;
const int BEHAVIOR_OBSTACLE_AVOIDANCE= 13;
const int BEHAVIOR_KICK_TEST         = 14;
const int BEHAVIOR_TEST_GENERIC      = 15;
const int BEHAVIOR_MOTION_TEST       = 16;
const int BEHAVIOR_TEST_JFASOLA      = 18;
const int BEHAVIOR_BWBALL            = 20;


const uchar CALL_CHAIN_MAX       = 100;
const uchar CALL_getcmd          =   1;
const uchar CALL_headTrackObject =   4;
const uchar CALL_findBall        =   5;
const uchar CALL_findMarkers     =   6;
const uchar CALL_getBehindBall   =   7;
const uchar CALL_kickBallDir     =   8;
const uchar CALL_chaseBall       =   9;
const uchar CALL_goalie          =  10;
const uchar CALL_camera          =  12;
const uchar CALL_challenge_two   =  14;
const uchar CALL_challenge_three =  15;
const uchar CALL_test            =  16;

const uint EDGE_posX = 1<<0;
const uint EDGE_negX = 1<<1;
const uint EDGE_posY = 1<<2;
const uint EDGE_negY = 1<<3;

struct MotionCommandDiffs {
  double motion_cmd;      // target locomotion state
  double head_cmd;        // head command type

  double vx,vy,va;        // desired position and angular velocity
                          // specified in mm/sec and rad/sec

  vector3d head_lookat;   // where look at for head (relative to front of body)

  double tail_pan;        // where to point tail in panning [-100,100]
  double tail_tilt;       // where to point tail in panning [-100,100]
  double mouth;           // mouth open fraction [0,100]
  double led;             // LED on state
};

class BehaviorSystem : public SPOutEncoder {
public:
  BehaviorSystem();
  void initializeStreams(PacketStreamCollection *out_psc);
  void prerun(MotionCommand* command_arg,
              SensorData* sens_data,
              TeamMsgMgr *msg_mgr, ulong time);
  void run(MotionCommand* command_arg,
	   SensorData* sens_data,
	   TeamMsgMgr *msg_mgr, ulong time);
  void setPosture(PostureDetector::Posture _posture)
    {posture = _posture;}
  void setStuckInfo(StuckInfo _stuck_info) {
    stuck_info = _stuck_info; }
  void readConfig(const char *filename);
  void changeTeamColor(int color);

  void updateFeatureSet();

  int setupStates;

  /* // BehaviorSystem is deprecated. 
  int goalColor;
  int teamColor;
  int fieldSide;  // for the attacker, which side of the field do we play on, +1 or -1
  int ownKickOff;
  */
 public:
  int topFunc;
  int subRole;

  FeatureSet *features;
  WorldModel *modeller;
  BotModel botModeller;
  LModel local_model;
  ObjectInfo* object_info;
  RadialObjectMap *radial_map;
  MotionCommand* command;
  MotionCommand lastCommand;
  MotionCommandDiffs commandDiffs;
  SensorData* sensor_data;
  TeamMsgMgr *team_msg_mgr;
  HeadVelocity *head_vel;
  vector2d neck_offset;
  short actual_walk_state;
  ulong timestamp;
  vector3d camera_loc;
  vector3d camera_dir;
  vector2d ball_world;

  PacketStream *behavior_names_stream;
  PacketStream *behavior_trace_stream;
  
  bool set_localization_leds;
  bool set_ball_led;
  bool silence_is_golden;

  PostureDetector::Posture posture;
  StuckInfo stuck_info;
  int getUpDir;
  int robotID;

protected:
  void forwardOdometry();
  bool handleGetUp();
  bool ballInFront();
  bool knowBallLoc();
  bool shouldDodgeClockwise(double forward_angle);
  double angleToObject(vector3d obj);
  double getAttackAngle();  // returns the angle we want to attack along
  bool stopGoalieCharge();  // handles goalie charge prevention logic

  // these calls test odometry.
  // distanceTravelled outputs the distance travelled since the last time it was called
  void distanceReset();
  void distanceTravelled();
  void modifyMoveToHeadCoords(MotionLocalizationUpdate *move);
  vector2d relative_dist;
  double relative_angle;
  int dist_calls;
  void distanceUpdate(Motion::MotionLocalizationUpdate *move);

  // Utility behaviour to verify that the distances we're getting from
  // the vision are reasonable

  int cachedmarkers;
  double markerdist[6], markerang[6], markerconf[6];
  void verifyAngles();

  // There is a robot in front of us in the world model
  // if lookForAlly is true it returns true only for ally robots
  // if lookForAlly is false it returns true only for opponent robots
  bool robotInFront(bool lookForAlly);
    
  // Guess what this does...
  void wagTail();
  
  // this command moves the legs only - it spins the robot in the correct
  // direction to hit the ball the right way
  void legTurnToLook();

  // This command only moves the head to look at an object we can see
  void headTrackObject(VObject *obj,double tilt_offset);
  void headTrackObject(VObject *obj)
    {headTrackObject(obj,0);}

  // This command finds and tracks the ball
  // It uses the legs when told to
  // It does not give a leg command unless it needs to
  // returns true if we can see the ball with high confidence
  ulong startBallSearchTime;
  bool findBall(bool useLegs);
  
  // This command finds and tracks the ball
  // it looks up regularly at the markers
  void findBallAndMarkers(bool useLegs);
  
  // controls the head while we're dribling the ball
  // looks up for most of the time
  // looks down when we're near the end of the dribble time limit
  void headCheckBallInFront();
  
  // This command finds and tracks the markers
  // it uses the legs when told to
  // It does not give a leg command unless it needs to
  // Returns: true if we can see the marker we're after with high confidence
  bool findMarkers(bool useLegs);
  // N.B. includeBots currently ignored - bots are not included
  bool findObjects(bool useLegs, bool includeBall, bool includeGoals,
        bool includeMarkers, bool includeBots);
  
  void getBehindBall(double angle, double accuracy_estimate);

  // kick or dribble the ball in the direction specified
  //  (or work towards making this happen)
  // angle = ego angle to move ball
  // required_accuracy = total angle spread allowed when moving ball to hit goal
  // accuracy_estimate = approx. standard deviation of angle specified
  // avoid_allies turns on some ally avoidance code
  void kickBallDir(double angle, double required_accuracy, double accuracy_estimate, bool avoid_allies);

  // main behaviour entry points

  ulong reachedGoalTime;

  void goalie           (FeatureSet *Features);
  void chaseBall        (FeatureSet *Features);
  void kickTest         (FeatureSet *Features);
  void camera           (FeatureSet *Features);
  void localization_test(FeatureSet *Features);
  void obstacleAvoidance(FeatureSet *Features);
  void test_slenser     (FeatureSet *Features);
  void test_jbruce      (FeatureSet *Features);
  void test_dvail2      (FeatureSet *Features);
  void new_attack       (FeatureSet *Features);
  void test_brettb      (FeatureSet *Features);
  void test_jfasola     (FeatureSet *Features);
  void test_mroth       (FeatureSet *Features);
  void test_sofia       (FeatureSet *Features);
  void test_generic     (FeatureSet *Features);
  void motion_test      (FeatureSet *Features);
  void bwball_challenge (FeatureSet *Features);

  // call chain logging
  
  uchar callChainList[CALL_CHAIN_MAX];
  uint callChainPosn;
  
  inline void logCallChain(uchar function) { if (callChainPosn < CALL_CHAIN_MAX) { callChainList[callChainPosn++] = function; } };
};

#endif