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

#include "../../headers/system_config.h"

#ifdef PLATFORM_LINUX
#include <iostream>
#endif

#include <limits.h>
#include <math.h>

#include "../../headers/Geometry.h"

class Environment;
class MovementPrimitive;
class Random;
class ProbEvaluator;

#include "Constants.h"
#include "Sample.h"
#include "Sampler.h"

struct LocaleSampled {
  Sample *samples;

  double totalWeight;

  static const int numSamples=100;

  bool totalWeightValid;
  bool samplesNormalized;
};

class LocalizationEngine {
public:
  LocalizationEngine(Environment *environ_param);

  void init();

  void reset();

  void setPos(double *pos, double *dev);
  void setPosUniform();

  void updateForMovement(MovementPrimitive *move_updater);
  void updateForSensors (int num_primitives, ProbEvaluator **primitives_p);
  
  void getPosition(Sample *pos, Sample *dev, double *cov_xy);

#ifdef PLATFORM_LINUX
  void dumpSamples(std::ostream &os) const;
#endif

  void copySamples(Sample *samples) const;

private:
  double calcTotalSampleWeight();

  void normalizeSamples();

  void sortPrimitives(int num_primitives, ProbEvaluator **primitives_p);

  double weightSampleForPrimitives(Sample *sample,
                                   int num_primitives, ProbEvaluator **primitives_p,
                                   double *effects);

  void generateSamples(int num_samples_to_generate,
                       int num_samples,Sample *samples,
                       int num_primitives, ProbEvaluator **primitives_p);

  Environment *environ;
  LocaleSampled locale;

  double avg_prob_fast;
  double avg_prob_slow;
  int num_samples_generated_last_update;

  bool pos_cache_valid;
  Sample pos_cache_mean;
  Sample pos_cache_std_dev;
  double pos_cache_cov_xy;

  Sampler sampler;
  PointDistProbEvaluator  pt_dist_evals [10];
  PointDistWithUniformProbEvaluator pt_dist_with_uniform_evals[10];
  PointAngleProbEvaluator pt_angle_evals[10];
  LinePointProbEvaluator  line_pt_evals [10];
  OnFieldProbEvaluator    on_field_eval;
  NearOldValueProbEvaluator near_old_eval;
};

#endif