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

#include "../headers/Geometry.h"
#include "BeLowLevel.h"
#include "FeatureSet.h"
#include "../WorldModel/LocalModel.h"

struct PADoObjectIdentifyInfo {
  bool identified;
  int patternId;
};

class PADoObjectIdentify {
private:
  static const int NumStates = 6;
  enum States { GOTO_VIEW_LOCATION, LOOK_AT_BASE_OBJECT, LOOK_AT_PATTERN, LOCATE_PATTERN, FIXATE_PATTERN, IDENTIFY_PATTERN, NONE};
  int CurrentState;

  int onTopObjIdx;    // object index of the object the pattern is on top of
  vector2d viewLoc;   // location to view the pattern from
  double viewAngle;   // angle to face when viewing object
  double vertDisp;    // height of bottom of pattern relative to object it is on
  vector3d lookPt;

  int patternId;
  ulong startLookTime;

  BeLookAtPoint lookAtPoint;
  BeLookAtObject lookAtObject;
  BeTrackObjects trackObjects;
  BeGotoPointGlobal gotoPointGlobal;

  void resetVars();

public:
  PADoObjectIdentify();
  ~PADoObjectIdentify();

  void reset(ulong timestamp);
  void setTarget(int obj_idx,vector2d view_loc,double view_angle,double vert_disp);
  double operator()(FeatureSet *FS,PADoObjectIdentifyInfo *PadoiInfo,MotionCommand *Command);
};

class PADoObjectReport {
private:
  static const int NumStates = 2;
  enum States { GOTO_REPORT_LOCATION, REPORT_PATTERN, NONE};
  int CurrentState;

  vector2d reportLoc; // location to go to to identify the object

  int patternId;
  int reportCnt; // times reported already
  ulong lastReportTime;

  BeTrackObjects trackObjects;
  BeGotoPointGlobal gotoPointGlobal;

  void resetVars();

public:
  PADoObjectReport();
  ~PADoObjectReport();

  void reset(ulong timestamp);
  void setTarget(ulong timestamp,vector2d report_loc,int pattern_id);
  double operator()(FeatureSet *FS,MotionCommand *Command);
};

class PatternAnalysis {
private:
  static const int NumStates = 2;
  enum States { IDENTIFY_PATTERN, REPORT_PATTERN, NONE };

  int CurrentState;

  int patternNum;
  int patternIds[3];
  vector2d targetLocView;
  vector2d targetLocReport;
  vector2d targetDir;

  PADoObjectIdentify doObjectIdentify;
  PADoObjectReport doObjectReport;

  void resetVars();

public:
  PatternAnalysis();
  ~PatternAnalysis();

  void reset(ulong timestamp);
  double operator()(FeatureSet *FS,MotionCommand *Command);
};



class ObstacleAvoidance {
public:
  static const int beh_id;

  static char const * const beh_name;
  
  static const int NumStates=5;
  enum State { WALK, LOCALIZE,RECORD_OBSTACLE,FINISH,HIT_OBST };

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;

  BeTrackObjects *track_objects;
  BeGotoPointGlobal *goto_point;

  struct Obstacle{
    vector2d center;
    double rep_dist;
    ulong timer;
    int color;
  };

  static const int tot_num_obst = 50;
  Obstacle obstacles[tot_num_obst];
  int num_known_obstacles;
  
  OccGridEntry cell;
  vector2d cell_center;
  
  vector2d ego_move_vec;
  vector2d last_min_point;

  bool checkForObstaclesLeft(FeatureSet *features);
  bool checkForObstaclesRight(FeatureSet *features);
  void calculateObstacleCenter(FeatureSet *features, Obstacle *obst);
  bool probablySameObstacle(vector2d center1, vector2d center2, int color1, int color2, double thresh);
  void updateObstacle(FeatureSet *features,Obstacle new_obst, int idx);
  void addObstacle(FeatureSet *features, Obstacle *new_obst);
  void deleteObstacle(int idx);
  bool cellIsClear();

  vector2d incrementalGetMinPt(FeatureSet *features);
  double calcPoint(vector2d point, FeatureSet *features);

  double getWallPF(vector2d point);
  double getObstaclePF(vector2d point,vector2d obstacle);
  double getFieldPF(vector2d point, vector2d goal);

public:
  ObstacleAvoidance();
  ~ObstacleAvoidance();

  void reset(ulong timestamp);
  void resetVars();

  double operator()(FeatureSet *FS,MotionCommand *Command);
};

#endif