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

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

#include "../headers/MessageStructures.hh"
#include "../headers/MoveDistributions.hh"
#include "../Motion/MotionInterface.h"
#include "../Vision/VisionInterface.h"
#include "../headers/field.h"
#include "constants.h"

namespace BotModelSpace {

// N.B. The code relies on the fact that the Blah_IR == Blah_NOIR + 1;
const int RED_BOT_NOIR = 0;
const int RED_BOT_IR = 1;
const int BLUE_BOT_NOIR = 2;
const int BLUE_BOT_IR = 3;

const int NUM_BOT_TYPES = 4;

const int RED_BOT_NOIR_MASK = 1<<RED_BOT_NOIR;
const int RED_BOT_IR_MASK = 1<<RED_BOT_IR;
const int BLUE_BOT_NOIR_MASK = 1<<BLUE_BOT_NOIR;
const int BLUE_BOT_IR_MASK = 1<<BLUE_BOT_IR;


const int NUM_BOTS = 100;

const int timeLimit = 5*one_second;

const double minConfidence = 0.5;
const double inFrontOfIRCutoff = 0.8;

const double lookingAtBotAngleCos = cos(visionDegHalf/2.0/180.0*M_PI);
const int lookingAtBotPenalty = one_second/4;  // how long we penalize a point if we're looking there

struct botInfo {
  double confidence;
  vector3d loc;
  int time; // relies on int being longer than ulong :)
};

}

class BotModel {
  void installBot(VisionInterface::VObject const &obj, bool red, SensorData* sensor_data,
                              vector3d const & camera_loc, vector3d const & camera_dir, ulong time);

public:

  BotModelSpace::botInfo bot[4][BotModelSpace::NUM_BOTS];
  
  void init();
  void updateModelMovement(Motion::MotionLocalizationUpdate *move);
  void updateModelSensors(VisionInterface::ObjectInfo *obj_info, SensorData* sensor_data,
    			     vector3d const & camera_loc, vector3d const & camera_dir, ulong time);
  double BotInRangePolar(int type_mask, int time, double minAngle, double maxAngle, double minDist, double maxDist);
  double BotInRangeRect(int type_mask, int time, double minX, double maxX, double minY, double maxY);
};

#endif