/* LICENSE:
  =========================================================================
    CMPack'03 Source Code Release for OPEN-R SDK v1.0
    Copyright (C) 2003 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#ifndef _GLOCALIZATION_H_
#define _GLOCALIZATION_H_

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

#include "../headers/mat2d.h"
#include "../headers/gvector.h"
#include "../headers/Util.h"
#include "../headers/Geometry.h"
#include "../headers/Gaussian2.h"

#include "../headers/CircBufPacket.h"
#include "../headers/Config.h"
#include "../headers/DogTypes.h"
#include "../headers/MessageStructures.hh"
#include "../headers/MoveDistributions.hh"
#include "../Motion/MotionInterface.h"
#include "../Vision/VisionInterface.h"
#include "../Vision/Vision.h"
#include "../WorldModel/summarize.h"
#include "../headers/distcal.h"

#include <math.h>
#include <stdio.h>

#include "../headers/field.h"
#include "LocalizationInterface.h"

/* Compile Option Flags */
static const bool LARGE_ERRORS   = false; /* Increased error in models for motion calibration */
static const bool FIELD_BOUNDED  =  true; /* Hard limits on position */
static const bool CIRCULAR_DISTS =  true; /* Circular gaussians for vision */
static const bool USE_RANGE      =  true; /* Use marker range for position */
static const bool USE_BEARING    = false; /* Use marker bearing for position */
static const bool CHECK_TELEPORT =  true; /* Check for teleportation */
static const bool USE_GOALS      =  true; /* Use goals as landmarks */
static const bool KNOWN_IC       = false; /* Known IC (low uncertainty) */
static const bool CORRECT_RANGE  = false; /* Use linear bias on range */
static const bool NECK_CONVERT   = false; /* Convert from neck to body */
static const bool FULL_MODEL     = false; /* Use all correlations in motion */
static const bool MINI_MODEL     = false; /* Use no corrleations in motion */
					  /* Default: use xy-correlation */

/* Constant Definitions */

/* Standard deviation is modelled as VISION_RANGE_FRAC_ERROR * distance
 * This number was determined from a large body of data, do not change
 * without good reason.
 */
static const double VISION_RANGE_STD_PERCENT = 0.065;
//static const double VISION_RANGE_STD_PERCENT = 0.05;

/* Angle does not scale with distance. Standard deviation is 3 deg. */
static const double VISION_ANGLE_VAR = sq(3.0*M_PI/180.0);
 
/* 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 even when it isn't 
 *   trying to change. This scales with time. Standard deviation is
 *   10 degree per second
 */
static const double MOVE_T_VAR_PER_SEC = (LARGE_ERRORS ? sq(30.0*M_PI/180.0) : sq(10.0*M_PI/180.0));

/* The maximum uncertainty allowed for position and heading */
static const double MAX_POSITION_SIGMA = 10000.0;
static const double MAX_HEADING_SIGMA = 4.0*M_PI;

/* Conversion factor from timestamps to seconds. */
static const double TIMESTAMPS_PER_SECOND = 1000000.0;

/* Minimum time between localizations off single landmarks, seconds */
static const double LOCALIZATION_INTERVAL = 0.08;

/* Checks for teleportation 
 * If sensors disagree with odometry by this many standard deviations,
 *   we should increase uncertainty to recover faster 
 */
static const double MAX_POSITION_STDS = 5.0;
static const double MAX_HEADING_STDS = 5.0;

/* Minimum confidence required to accept sensor measurement */
static const double MINCONF = 0.60;

/* Maximum estimated range for acceptable markers */
static const double MAX_MARKER_RANGE = 3750.0;

/* Minimum range at which to use goals */
static const double MIN_GOAL_RANGE = 750.0;

class GLocalizer 
{
  public:
    /* Administrative Functions */
    GLocalizer() {}
    void init();

    /* Localization Functions */
    void resetPose(Gaussian2 position, Gaussian2 heading);
    void resetDeviationUniform();
    void updateMotion(const Motion::MotionLocalizationUpdate *move);
    bool updateSensors(const VisionInterface::ObjectInfo *obj_info); 
    bool updateHeading(const VisionInterface::ObjectInfo *obj_info);
    bool updatePosition(const VisionInterface::ObjectInfo *obj_info);

    /* Data Retrieval Functions */
    Gaussian2 getPosition();
    Gaussian2 getHeading();
    void getLocation(RobotPositionInfo * pos);
  
    /* Variables */
    vector2d LocMap[VisionInterface::NUM_MARKERS + 6];
    Gaussian2 position;
    Gaussian2 heading;
    double psxst;
    double psyst;
    int NUM_LANDMARKS;
    int NUM_MARKERS;
    
  private:
    /* Localization Functions */ 
    Gaussian2 updatePositionRange(double measured_distance, 
	 			  vector2d landmark_position);
    Gaussian2 updatePositionBearing(double measured_angle, 
				    vector2d landmark_position);
    Gaussian2 neckToBody(Gaussian2 p);
    Gaussian2 bodyToNeck(Gaussian2 p);
    void loadMap(void);
    bool validLandmark(VisionInterface::VObject landmark_measured, 
		       vector2d landmark_map, int i);
    vector2d projectToField(vector2d p);

    /* Math helper functions */
    double angleDifference(double fromAngle, double toAngle);
    vector2d weightedAverageAngle(vector2d angle1, double weight1,
			          vector2d angle2, double weight2);
    
    /* Configuration Flag Variables */
    bool DEBUG;

    /* Localization Timing Variables */
    ulong localizeInterval;
    ulong lastMotionUpdateTime;
    ulong markerHeadTimer[VisionInterface::NUM_MARKERS + 6];
    ulong markerPosTimer[VisionInterface::NUM_MARKERS + 6];
    ulong lastMarkerTime;

    /* Internal position variables */
    Gaussian2 positionReference;  
    vector2d neck_offset;
};

class GLocalizerEP : public LocalizationEP {
private:
  uint mot_update_id;
  uint vis_id;

  MotionUpdateHeadSummary *mot_update_ep;
  Vision *vis_ep;

  EventCacheTracker cache_tracker;
  RobotPositionInfo *pos_info;

  GLocalizer localizer;

public:
  static std::string name;

  static EventProcessor *create() {return new GLocalizerEP;}

  GLocalizerEP();
  virtual ~GLocalizerEP();

  virtual bool initConnections();

  virtual bool update(ulong time,const EventList *events);

  virtual const RobotPositionInfo *get(ulong time);
};

#endif