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

class BehaviorManager;
class EventManager;
class PacketStreamCollection;

#include <string>
#include "../headers/system_config.h"

#include "../headers/DogTypes.h"
#include "../headers/RoboCupGameControlData.h"
#include "../Behaviors/Behaviors.h"
#include "../Motion/MotionInterface.h"
#include "../Vision/VisionInterface.h"
#include "../WorldModel/PostureDetector.h"
#include "../WorldModel/StuckDetector.h"

using namespace VisionInterface;
using namespace Motion;

class CameraFrameSource;
class WorldModel;

// *******************************************************
// This is the class to add general stuff needed by a lot
// of things that do NOT depend on Aperios.  Aperios specific
// stuff should be put in MainObject instead.
// *******************************************************

/*------------------------------------------------------------------
CLASS
  RobotMain

DESCRIPTION
  Holds all the objects for MainObject.
------------------------------------------------------------------*/
class RobotMain {
private:
  MotionCommand command;
  ulong available_command_time;
  ulong last_sent_command_time;

  EventManager *event_mgr;
  BehaviorManager *beh_mgr;
  CameraFrameSource *camera_frame_source;

  uint camera_frame_source_id;
  uint motion_update_source_id;
  uint sensor_data_id;

public:
  static RobotMain &GetObject();

  bool robot_is_paused;
  bool we_scored;
  bool opp_scored;
  int our_old_score;
  int opp_old_score;

  int back_pressed_count;

  WorldModel world_model;
  PostureDetector posture;
  BehaviorSystem behave;
  StuckDetector stuck;
  SensorData *sensorData;
  TeamMsgMgr teamMsgMgr;
  //CommMgr commMgr;

  /* Robot state variables */
  RState robot_state;
  bool have_kickoff;
  bool state_changed;
  bool trans_to_play;
  int transition_pressed_count;
  int team_color_pressed_count;
  int kickoff_pressed_count;

  static const int MAX_BOTS = 25;
  enum RobotNames{MOON,CLOUD,SILVER,MIST,ICE,SNOW,FROST,COTTON};
  int robotID;  //assigned ID from macaddr.cfg
  int robotName;//give name (MOON, CLOUD, etc) from macadd.cfg

  RobotMain();
  void init();
  void start(PacketStreamCollection *out_psc);
  void destroy();
  MotionCommand *processImage(ulong time,uchar *bytes_y,uchar *bytes_u,uchar *bytes_v,
			      int width,int height,int skip, ulong sensor_frame);
  void newMotionCommandAvailable(ulong time);
  void processSensorFrame(ulong time);

  Vision *getVision();

  int getRobotName();
  int getRobotID();

private:
  void updateHeadPosition(const double *hpi);
  void updateHeadVelocities(const double *head_angles);
  void readRobotInfo();
};


#endif