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

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

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

class BeLookAtPoint;
class BeLookAtVisibleObject;
class BeLookAtObject;
class BeActiveLocalize;
class BeTrackObjects;


/* Fixate time is a misnomer; it's really the amount of
   time we'll spend moving to look at the point and staring
   at it in total (i.e. move time counts). Perhaps "settle_time"
   would be a better name.
*/
class BeLookAtPoint {
 private:

 public:

  BeLookAtPoint();
  ~BeLookAtPoint();
  void reset();

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

/* Look at a particular object that is visible.
*/
class BeLookAtVisibleObject {
public:
  static const int beh_id;

  static char const * const beh_name;
  
  static const int NumStates = 2;
  enum State { LOOK_AT_VIS_OBJECT, ERROR };

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;

  int object_idx;
  BeLookAtPoint *look_at_point;

public:

  BeLookAtVisibleObject();
  ~BeLookAtVisibleObject();
  void reset(ulong timestamp);
  void sleep();
  void setTargetNoReset(int _object_idx);
  void setTarget(ulong timestamp,int _object_idx);

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

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

struct BeLookAtObjectInfo {
  bool fixated;
  bool scanning;
};

/* Look at a particular object, locating it if necessary.
*/
class BeLookAtObject {
public:
  static const int beh_id;

  static char const * const beh_name;
  
  static const int NumStates = 4;
  enum State { LOOK_AT_VIS_OBJECT, 
               LOOK_AT_MODEL_OBJECT, 
               LOOK_AT_TEAMMATE_MODEL_OBJECT,
               FIND_OBJECT };

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;

  // requested time to fixate on model location before giving up
  ulong model_fixate_time;

  ulong fixate_timeout;
  ulong last_seen_time; // last time object was seen
  // time the object has been seen minus time it hasn't been seen since we've been
  //   trying to look at it (always >=0)
  ulong time_seen_balance;
  // last time this behavior was run
  ulong last_time;
  int object_idx;
  BeLookAtPoint *look_at_point;

  void resetVars();

public:

  BeLookAtObject();
  ~BeLookAtObject();
  void reset(ulong timestamp);
  void sleep();
  void setTarget(ulong timestamp,int _object_idx);
  void setTargetNoReset(int _object_idx);
  void setFixateTime(ulong timestamp,ulong fixate_time);
  void setFixateTimeNoReset(ulong fixate_time);

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

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

struct BeActiveLocalizeInfo {
  bool fixated; // true when fixated on target
};

class BeActiveLocalize {
public:
  static const int beh_id;

  static char const * const beh_name;
  
  static const int NumStates=2;
  enum State {PICK_POINT, FIXATE_ON_POINT};

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;

  vector3d point;
  int marker_id;
  bool looking_at_ball;
  bool looking_at_goal;

  BeLookAtPoint *look_at;

  int pickRandomMarker(FeatureSet *features);

  void resetVars();

public:
  BeActiveLocalize();
  ~BeActiveLocalize();

  void reset(ulong timestamp);
  void sleep();
  double operator()(FeatureSet *features,
                    BeActiveLocalizeInfo *info,
		    MotionCommand *command,
		    bool also_use_ball=false);

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

class BeTrackObjects {
public:
  static const int beh_id;

  static char const * const beh_name;

  static const int NumStates = 2;
  enum State { PICKING_OBJECT, LOCATING_OBJECT };

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;

  bool last_tried_goal;
  bool last_tried_ball;
  bool find_ball_failed;
  bool saw_marker_while_scanning;

  int num_landmarks;
  int object_to_find; // ball is 6, markers are their number
  ulong last_seen[NUM_MARKERS+7]; // markers, goal posts, ball.  Ball = 12

  vector3d last_loc;
  ulong timer;

  BeLookAtObject *look_at_object;
  BeLookAtObjectInfo look_at_object_info;

  void resetVars();

public:
  BeTrackObjects();
  ~BeTrackObjects();

  void reset(ulong timestamp);
  void sleep();
  double operator ()(FeatureSet *features,
                     MotionCommand *command,
                     bool UseMarkers=true,
                     bool UseBall=true);

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


#endif