/* 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/system_config.h"

#include "Constants.h"

#include <math.h>
#include <iostream>
#ifdef PLATFORM_LINUX
#include <fstream>
#endif
#include <stdio.h>

using namespace std;

#include "../../headers/Config.h"
#include "../../headers/Geometry.h"
#include "../../headers/MessageStructures.hh"
#include "../../headers/random.h"
#include "../../headers/Reporting.h"
#include "../../headers/Util.h"
#include "../../headers/Utility.h"
#include "../../Motion/MotionInterface.h"
#include "../../Vision/Vision.h"
#include "../../Vision/VisionInterface.h"
#include "../../WorldModel/summarize.h"
#include "Environment.h"
#include "Localization.h"
#include "LocalizationEngine.h"
#include "Primitives.h"
#include "SensorInterp.h"

using namespace VisionInterface;

#ifdef DEBUG_LOCALIZATION_LINUX
static const bool debug=true;
#else
static const bool debug=false;
#endif
static const bool dump_marker_conf=false;

static const bool UseMarkers=true;
static const bool UseGoals  =true;
static const bool UseLines  =false;
bool UseCorners=false;

static const double DistDevFactor   =0.2; // deviation per step as fraction of distance moved
static const double AngleDevFactor  =0.3; // deviation per step as fraction of heading change
static const double HeadingDevFactor=0.3; // deviation per step as fraction of amount turned

static const double MinDevDist         =20.0;
static const double MinDevMoveAngle    =0.0175; // 1 degree
static const double MinDevHeadingChange=0.0175; // 1 degree

static bool use_starting_position=false;
static const double starting_x        =   0.0L;
static const double starting_x_dev    =  10.0L;
static const double starting_y        =   0.0L;
static const double starting_y_dev    =  10.0L;
static const double starting_theta    =   0.0L;
static const double starting_theta_dev=   0.01L;

static const bool LARGE_ERRORS   = false; /* Increased error in models for motion calibration */

/* We want a standard deviation of 0.1 at 1.0 meter 
 * (100)^2 = 10000 mm^2 per m = 10 mm^2 per mm
 * (125)^2 = 15625 mm^2 per m = 15.625 mm^2 per mm
 * Was 2.5 */
static const double MOVE_X_VAR_PER_MM = (LARGE_ERRORS ? 40.0 : 10.0);
static const double MOVE_Y_VAR_PER_MM = (LARGE_ERRORS ? 40.0 : 10.0);

/* We want some small error for x and y when there is rotational but no
 * translational motion (turning in place). This scales with time.
 * 2 cm per second = 20 mm per sec standard deviation. Variance = 400 mm^2/sec 
 */
/* Was 0.001 */
static const double MOVE_X_VAR_PER_SEC = (LARGE_ERRORS ? 2500.0 : 400.0);
static const double MOVE_Y_VAR_PER_SEC = (LARGE_ERRORS ? 2500.0 : 400.0);

/* We want a standard deviation of 15 degrees per revolution */
/* This is 15*2*PI/360 radians per revolution = PI/12
 * variance is (PI/6)^2 per revolution or ((PI/6)^2)/2PI per radian = (pi/72).
 */
/* Was: 4pi^2/81^2 ??? */ 
static const double MOVE_T_VAR_PER_RAD = (LARGE_ERRORS ? 1.0 : (M_PI/72.0));

/* We also want the heading to drift a little when we are moving.
 * This scales with time.
 */
static const double MOVE_T_VAR_PER_SEC_MOVING = (LARGE_ERRORS ? sq(30.0*M_PI/180.0) : sq(10.0*M_PI/180.0));

/* We also want the heading to drift a little even when it isn't 
 *   trying to change. This scales with time.
 */
static const double MOVE_T_VAR_PER_SEC = (LARGE_ERRORS ? sq(4.0*M_PI/180.0) : sq(2.0*M_PI/180.0));

//===== SRLocalizer implementation =====================================//

SRLocalizer::SRLocalizer() :
  engine(NULL)
{
}

void
SRLocalizer::init() {
  environ=new RoboCupFieldEnvironment;
  engine =new LocalizationEngine(environ);
  if(use_starting_position) {
    double pos[3]={starting_x,     starting_y,     starting_theta    };
    double dev[3]={starting_x_dev, starting_y_dev, starting_theta_dev};
    resetPosition(pos,dev);

    use_starting_position=false;
  }
  else {
    resetPositionUniform();
  }

  last_motion_update_time = 0UL;
  next_line_point_to_use = 0;
}

void
SRLocalizer::reset() {
  engine->reset();

  last_motion_update_time = 0UL;
  next_line_point_to_use = 0;
}

void
SRLocalizer::resetPosition(double *pos, double *dev) {
  engine->setPos(pos, dev);
}

void
SRLocalizer::resetPositionUniform() {
  engine->setPosUniform();
}

void
SRLocalizer::updatePositionMovement(const Motion::MotionLocalizationUpdate *move) {
  static EventTimeReporter reporter("localization, movement update",1000000UL,10000000UL,1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

  static MovementPrimitive move_primitive;
  static bool mp_initialized=false;
  if(!mp_initialized) {
    mp_initialized=true;
    move_primitive.type=MovementPrimitive::VelWalkModel;
    move_primitive.sampleUpdater=&MovementPrimitive::velWalkModelUpdater;
    move_primitive.environ=environ;
  }

  double delta_time = TimeToSec(move->timestamp - last_motion_update_time);
  last_motion_update_time = move->timestamp;

  double dx=move->pos_delta.x;
  double dy=move->pos_delta.y;
  double heading_change=move->heading_delta.angle();
  
  if(debug){
    cout << "got movement update, dx=" << dx
         << " dy=" << dy 
         << " da=" << heading_change << endl;
  }

  vector2d move_disp(dx,dy);
  double move_dist =move_disp.length();
  vector2d center_offset = move->body.neck_offset;
  vector2d center_offset_rotated;
  center_offset_rotated=center_offset.rotate(heading_change);
  move_disp = move_disp + (center_offset_rotated - center_offset);

  /* Update heading standard deviation. 
   * Small amounts of error are added(based on elapsed time in sec)
   *   if the robot is moving even if heading doesn't change.
   */
  double t_var=0.0;
  t_var = MOVE_T_VAR_PER_SEC_MOVING * delta_time;
  if(move->pos_delta.x !=0.0 || move->pos_delta.y !=0.0){
    t_var += MOVE_T_VAR_PER_SEC_MOVING * delta_time;
  }
  t_var += MOVE_T_VAR_PER_RAD * fabs(heading_change);

  /* x_var and y_var are percentage of distance if non-zero,
   *   and constant with time if the robot is not moving at all.
   */
  double x_var, y_var; // variance along heading direction; to side
  if(move_dist != 0.0){
    x_var = MOVE_X_VAR_PER_MM * move_dist;
    y_var = MOVE_Y_VAR_PER_MM * move_dist;
  }
  else{
    x_var = MOVE_X_VAR_PER_SEC * delta_time;
    y_var = MOVE_Y_VAR_PER_SEC * delta_time;
  }

  move_primitive.data.vel_walk_data.x                 =move_disp.x;
  move_primitive.data.vel_walk_data.x_dev             =sqrt(x_var);
  move_primitive.data.vel_walk_data.y                 =move_disp.y;
  move_primitive.data.vel_walk_data.y_dev             =sqrt(y_var);
  move_primitive.data.vel_walk_data.heading_change    =heading_change;
  move_primitive.data.vel_walk_data.heading_change_dev=sqrt(t_var);

  //move_primitive.data.vel_walk_data.x_dev             =0.0;
  //move_primitive.data.vel_walk_data.y_dev             =0.0;
  //move_primitive.data.vel_walk_data.heading_change_dev=0.0;

  engine->updateForMovement(&move_primitive);
}

void
SRLocalizer::updatePositionSensors(int num_objects, const SensorInfo *obj_info_p) {
  static ProbEvaluator *primitives_p[MaxNumPrimitives];

  int num_primitives;

  // map observations to primitive types
  environ->sensorInterp->interpretSensors(environ,num_objects,obj_info_p,&num_primitives,primitives_p);

  // incorporate sensor reading
  engine->updateForSensors(num_primitives,primitives_p);
}

bool
SRLocalizer::updatePositionSensors(const VisionInterface::ObjectInfo *obj_info,
                                   const VisionInterface::RadialObjectMap *radial_map) {
  static EventTimeReporter reporter("localization, sensor update",1000000UL,10000000UL,1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

  static SensorInfo sensor_readings[MaxNumPrimitives];

  int num_objects;

  num_objects = 0;

  for(int i=0; i<6; i++){
    const VisionInterface::VObject *obj=&obj_info->marker[i];
    if(obj->confidence > ignoreMarkerThreshold && obj->distance > 10.0) {
      sensor_readings[num_objects].readingClass=i;
      sensor_readings[num_objects].type=SensorInfo::PointLandmark;
      sensor_readings[num_objects].data.point_data.dist      = obj->distance;
      sensor_readings[num_objects].data.point_data.dist_dev  = .1*obj->distance;
      sensor_readings[num_objects].data.point_data.angle     = atan2a(obj->loc.y,obj->loc.x);
      sensor_readings[num_objects].data.point_data.angle_dev = deg2rad(15.0);
      if(UseMarkers)
        num_objects++;
      if(debug){
        cout << "see marker " << i << " conf=" << obj->confidence 
             << " at dist=" << obj->distance << " angle=" << atan2a(obj->loc.y,obj->loc.x) << endl;
      }
    }
  }

  for(int i=YELLOW_GOAL_SIDES; i<=CYAN_GOAL_SIDES+RIGHT_SIDE; i++){
    if(i==CYAN_GOAL)
      continue;
    const VisionInterface::VObject *obj=(&obj_info->marker[0])+i;
    if(obj->confidence > ignoreMarkerThreshold && obj->distance > 100.0 &&
       (obj->edge == 0 || obj->distance < 1000.0)) {
      sensor_readings[num_objects].readingClass=i;
      sensor_readings[num_objects].type=SensorInfo::GoalLandmark;
      sensor_readings[num_objects].data.point_data.dist      = obj->distance;
      sensor_readings[num_objects].data.point_data.dist_dev  = (obj->edge==0 ? .2*obj->distance : .35*obj->distance + 100.0);
      sensor_readings[num_objects].data.point_data.angle     = atan2a(obj->loc.y,obj->loc.x);
      sensor_readings[num_objects].data.point_data.angle_dev = deg2rad(15.0);
      if(UseGoals)
        num_objects++;
      if(debug){
        cout << "see goal " << i << " conf=" << obj->confidence 
             << " at dist=" << obj->distance << " angle=" << atan2a(obj->loc.y,obj->loc.x) << endl;
      }
    }
  }

  int num_line_point_readings=0;
  // count number of usable readings
  for(int ang_idx=0; ang_idx<NUM_ANGLES; ang_idx++){
    int num_ros = radial_map->num_ros[ang_idx];
    bool last_was_field=false;
    for(int ro_idx=0; ro_idx<num_ros; ro_idx++){
      const RO *ro;
      
      ro = &radial_map->ros[ang_idx][ro_idx];
      if(ro->start_pt.length() > 1500.0)
        continue;
      switch(ro->type){
        case RO_Stripe:
          num_line_point_readings++;
          break;
        case RO_Wall:
          num_line_point_readings++;
          break;
        case RO_Goal:
          num_line_point_readings++;
          break;
      }
      if(!last_was_field){
        num_line_point_readings--;
      }
      last_was_field = (ro->type == RO_Field);
    }
  }
  int line_point_reading_to_use=0;
  if(num_line_point_readings > 0){
    line_point_reading_to_use = next_line_point_to_use % num_line_point_readings;
  }
  next_line_point_to_use+=23;
  // add reading to use to sensor readings
  int line_point_reading_idx=0;
  bool done=false;
  for(int ang_idx=0; !done && ang_idx<NUM_ANGLES; ang_idx++){
    int num_ros = radial_map->num_ros[ang_idx];
    bool last_was_field=false;
    for(int ro_idx=0; !done && ro_idx<num_ros; ro_idx++){
      const RO *ro;
      
      ro = &radial_map->ros[ang_idx][ro_idx];
      if(ro->start_pt.length() > 1500.0)
        continue;
      switch(ro->type){
        case RO_Stripe:
          line_point_reading_idx++;
          break;
        case RO_Wall:
          line_point_reading_idx++;
          break;
        case RO_Goal:
          line_point_reading_idx++;
          break;
      }
      if(!last_was_field){
        line_point_reading_idx--;
      }
      last_was_field = (ro->type == RO_Field);

      if(line_point_reading_idx == line_point_reading_to_use){
        done = true;

        double dist  = ro->start_pt.length();
        double angle = ro->start_pt.angle();
        int reading_class=12;
        switch(ro->type){
          case RO_Wall:   reading_class = 12; break;
          case RO_Stripe: reading_class = 13; break;
          case RO_Goal:
            if(ro->sub_type == RO_Goal_Yellow)
              reading_class = 14;
            else
              reading_class = 15;
        }
        sensor_readings[num_objects].readingClass = reading_class;
        sensor_readings[num_objects].type = SensorInfo::LinePoint;
        sensor_readings[num_objects].data.line_point_data.dist      = dist;
        sensor_readings[num_objects].data.line_point_data.angle     = angle;
        if(debug)
          cout << "using pt reading of type " << reading_class << " at (" << ro->start_pt.x << "," << ro->start_pt.y << ")" << endl;
        if(UseLines)
          num_objects++;
      }
    }
  }

  {
    const VisionInterface::VObject *obj=&obj_info->stripe_corner;
    if(obj->confidence > ignoreMarkerThreshold && obj->distance > 10.0) {
      sensor_readings[num_objects].readingClass=16;
      sensor_readings[num_objects].type=SensorInfo::CornerLandmark;
      sensor_readings[num_objects].data.point_data.dist      = obj->distance;
      sensor_readings[num_objects].data.point_data.dist_dev  = .2*obj->distance;
      sensor_readings[num_objects].data.point_data.angle     = atan2a(obj->loc.y,obj->loc.x);
      sensor_readings[num_objects].data.point_data.angle_dev = .25;
      if(UseCorners)
        num_objects++;
      if(debug){
        cout << "see corner conf=" << obj->confidence 
             << " at dist=" << obj->distance << " angle=" << atan2a(obj->loc.y,obj->loc.x) << endl;
      }
    }
  }

  if(num_objects > 0) {
    updatePositionSensors(num_objects,sensor_readings);
    return true;
  }else{
    return false;
  }
}

void
SRLocalizer::getPosition(RobotPositionInfo *pos_info) {
  static EventTimeReporter reporter("localization, get position",1000000UL,4000000UL,400UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

  static Sample mean;
  static Sample std_dev;
  static double cov_xy;

  engine->getPosition(&mean,&std_dev,&cov_xy);
  //printf("cov %g\n",cov[0*3+1]);

  pos_info->position = Gaussian2(0UL,vector2d(V2COMP(mean.loc)),std_dev.loc.x,std_dev.loc.y,cov_xy,true);
  pos_info->heading  = Gaussian2(0UL,vector2d(cos(mean.angle),sin(mean.angle)),std_dev.angle,0.0,0.0,false);
  //printf("ang mean %g, vec=(%g,%g), std=%g, sMaj=%g\n",mean[2],
  //       cos(mean[2]),sin(mean[2]),std_dev[2],pos_info->heading.sMaj);
}

void
SRLocalizer::copySamples(Sample *samples) {
  engine->copySamples(samples);
}

#ifdef PLATFORM_LINUX
void
SRLocalizer::dumpSamples(ostream &os) const {
  engine->dumpSamples(os);
}
#endif

//===== SRLocalizerEP implementation =====================================//

// FIXME: This should be made to compile under Linux
#ifdef PLATFORM_APERIOS

std::string SRLocalizerEP::name("SRLocalizer");

SRLocalizerEP::SRLocalizerEP()
{
  pos_info = new RobotPositionInfo;
  mzero(*pos_info);

  localizer.init();
  localizer.getPosition(pos_info);
}

SRLocalizerEP::~SRLocalizerEP()
{
  delete pos_info;
}

bool SRLocalizerEP::initConnections()
{
  EventManager *em;
  uint my_id;

  em = EventManager::getManager();
  my_id = em->getEventProcessorId(name);

  mot_update_id = em->getEventProcessorId(MotionUpdateHeadSummary::name);
  mot_update_ep = dynamic_cast<MotionUpdateHeadSummary *>(em->listenEventProcessor(my_id,mot_update_id));
  
  vis_id = em->getEventProcessorId(Vision::name);
  vis_ep = dynamic_cast<Vision *>(em->listenEventProcessor(my_id,vis_id));

  return true;
}

bool SRLocalizerEP::update(ulong time,const EventList *events)
{
  const std::vector<Motion::MotionLocalizationUpdate> *mot_updates;
  bool new_out=false;

  //pprintf(TextOutputStream,"Updating localization at time %lu\n",time);

  if(events->present(mot_update_id)){
    mot_updates = mot_update_ep->get(time);

    for(uint i=0; i<mot_updates->size(); i++){
      const Motion::MotionLocalizationUpdate *move_update;

      move_update = &((*mot_updates)[i]);

      //pprintf(TextOutputStream,"Update for movement at time %lu\n",time);
      localizer.updatePositionMovement(move_update);

      new_out = true;
    }
  }
  
  if(events->present(vis_id)){
    vis_ep->get(time);

    //pprintf(TextOutputStream,"Update for sensors at time %lu\n",time);
    localizer.updatePositionSensors(vis_ep->object_info,&vis_ep->radial_map);

    new_out = true;
  }

  if(new_out){
    cache_tracker.updateAvailable(time);
  }

  return new_out;
}

const RobotPositionInfo *SRLocalizerEP::get(ulong time)
{
  if(cache_tracker.shouldUpdate(time)){
    localizer.getPosition(pos_info);

    cache_tracker.cacheUpdated(time);
  }

  return pos_info;
}

REGISTER_EVENT_PROCESSOR(SRLocalizerEP,SRLocalizerEP::name,SRLocalizerEP::create);

#endif /* PLATFORM_APERIOS */