/* 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.
  ========================================================================= */

#include "../headers/Sensors.h"
#include "../headers/Utility.h"

#include "BotModel.h"

using namespace BotModelSpace;

void BotModel::init() {
  for (int j=0; j<NUM_BOT_TYPES; j++) {
    for (int i=0; i<NUM_BOTS; i++) {
      bot[j][i].time = 0;
      bot[j][i].loc.set(0,0,0);
      bot[j][i].confidence = 0;
    }
  }
}

void BotModel::updateModelMovement(Motion::MotionLocalizationUpdate *move) {
  if ((move->heading_delta.x == 0.0) && (move->heading_delta.y == 0.0)) {
    return; // feet planted
  }
  
  double angle = atan2a(move->heading_delta.y, move->heading_delta.x);
  vector2d trans = move->pos_delta;
  
  for (int j=0; j<NUM_BOT_TYPES; j++) {
    for (int i=0; i<NUM_BOTS; i++) {
      if (0.0 != bot[j][i].time) {
        bot[j][i].loc.x -= trans.x;
        bot[j][i].loc.y -= trans.y;
        bot[j][i].loc = bot[j][i].loc.rotate_z(-angle);
      }
    }
  }
}

void BotModel::updateModelSensors(VisionInterface::ObjectInfo *obj_info, SensorData* sensor_data,
    vector3d const & camera_loc, vector3d const & camera_dir, ulong time) {
  // modify bots we should be able to see
  // they time out faster
  for (int type = 0; type< NUM_BOT_TYPES; type++) {
    for (int i=0; i<NUM_BOTS; i++) {
      if (bot[type][i].time < (int)(time - timeLimit)) {
        continue;
      }
      vector3d loc = bot[type][i].loc - camera_loc;
      if ((loc.sqlength() < 40) ||
          (loc.norm().dot(camera_dir) > lookingAtBotAngleCos)) {
        bot[type][i].time -= lookingAtBotPenalty;
      }
    }
  }

// install new bots
  for (int i=0; i<3; i++) {
    // red pet i
    if (obj_info->red_robots[i].confidence > minConfidence) {
      // feed the NOIR type - IR is added later
      installBot(obj_info->red_robots[i], true, sensor_data, camera_loc, camera_dir, time);
    }
    // blue pet i
    if (obj_info->blue_robots[i].confidence > minConfidence) {
      // feed the NOIR type - IR is added later
      installBot(obj_info->blue_robots[i], false, sensor_data, camera_loc, camera_dir, time);
    }
  }
}

const double IRClosest  = 100.0;
const double IRFarthest = 1.0e4;
const double IRTooFar   = 850.0;
const double IRCutoffDistMax = 5.0e3; // any value >900 and <IRFarthest should work here
inline double getIRDistance(SensorData* sensor_data) {
  double distance = sensor_data->getFrame(0)->IRDistance;
  
  if(distance > IRTooFar)
    distance = IRFarthest;
  else
    distance = distance - 65.0;

  if(distance < IRClosest)
    distance = IRClosest;
  
  return distance;
}


void BotModel::installBot(VisionInterface::VObject const &obj, bool red, SensorData* sensor_data,
      vector3d const & camera_loc, vector3d const & camera_dir, ulong time) {
  const bool debug = true;
  bool IR = (obj.confInFrontOfIR > inFrontOfIRCutoff);
  
  vector3d loc = obj.loc;
  
  if (IR) {
    double distance = getIRDistance(sensor_data);
    vector3d lDir = loc - camera_loc;
    
    if ((distance < IRCutoffDistMax) && (lDir.sqlength() > 1)) {      
      loc = lDir.norm()*distance + camera_loc;
      if ((loc.z < 20) || (loc.z > 300)) {
        return; // don't store dogs in the ground or in the air
      }
    } else {
      return;
    }
  }
  
  int type = red ? RED_BOT_NOIR : BLUE_BOT_NOIR;
  
  if (IR) {
    type++;
  }
  
  int worst_bot = -1;
  int worst_bot_time = time + 2*one_second;
  for (int i=0; i<NUM_BOTS; i++) {
    int this_time = bot[type][i].time;
    
    if (this_time < worst_bot_time) {
      worst_bot_time = this_time;
      worst_bot = i;
      if (!debug) {
        if (this_time < (int)(time - timeLimit)) {
          break;
        }
      }
    }
  }
  
  if (worst_bot == -1) {
    worst_bot = time % NUM_BOTS;  // pick a semi-random bot
  }
  
  bot[type][worst_bot].confidence = obj.confidence;
  bot[type][worst_bot].loc = loc;
  bot[type][worst_bot].time = time + SecToTime(obj.confidence*2);
}

double BotModel::BotInRangePolar(int type_mask, int time, double minAngle, double maxAngle, double minDist, double maxDist) {
  double confidence = 0;

  for (int type = 0; type<NUM_BOT_TYPES; type++) {
    if (!(type_mask & (1<<type)))
      continue;
    for (int i=0; i<NUM_BOTS; i++) {
      if (bot[type][i].time < time - timeLimit)
        continue;
      
      vector2d loc(bot[type][i].loc.x, bot[type][i].loc.y);
      
      double angle = loc.angle();
      
      if (angle < minAngle)
        continue;
      if (angle > maxAngle)
        continue;
      
      double distance = loc.length();
      
      if (distance < minDist)
        continue;
      if (distance > maxDist)
        continue;
      
      confidence += bound(bot[type][i].confidence * (timeLimit/(double)(time - bot[type][i].time)), 0, 1);
    }
  }
  
  return confidence;
}

double BotModel::BotInRangeRect(int type_mask, int time, double minX, double maxX, double minY, double maxY) {
  double confidence = 0;

  for (int type = 0; type<NUM_BOT_TYPES; type++) {
    if (!(type_mask & (1<<type)))
      continue;
    for (int i=0; i<NUM_BOTS; i++) {
      if (bot[type][i].time < time - timeLimit)
        continue;
      
      vector2d loc(bot[type][i].loc.x, bot[type][i].loc.y);
      
      if (loc.x < minX)
        continue;
      if (loc.x > maxX)
        continue;
      if (loc.y < minY)
        continue;
      if (loc.y > maxY)
        continue;
      
      confidence += bound(bot[type][i].confidence * (timeLimit/(double)(time - bot[type][i].time)), 0, 1);
    }
  }
  
  return confidence;
}