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

class Environment;
class EnvironmentObject;
class Sample;

//#include "LocalizationEngine.h"

class ProbEvaluator {
public:
  ProbEvaluator();

  int observationId;
  const EnvironmentObject *matchedTo;

  virtual void calc_prob(double *prob,const Sample *samp,bool scaled) = 0;
  virtual void calc_exp_prob(double *exp_prob,bool scaled) = 0;
  virtual void calc_energy(double *energy,const Sample *samp) = 0;
  virtual void calc_energy_der(Sample *der,const Sample *samp) = 0;
};

class PointDistProbEvaluator : public ProbEvaluator {
public:
  vector2d loc; // location of point marker
  double dist_obs; // distance observed
  double dist_dev_frac; // deviation is dist_obs*dist_dev_frac

  virtual void calc_prob(double *prob,const Sample *samp,bool scaled);
  virtual void calc_exp_prob(double *exp_prob,bool scaled);
  virtual void calc_energy(double *energy,const Sample *samp);
  virtual void calc_energy_der(Sample *der,const Sample *samp);
};

class PointDistWithUniformProbEvaluator : public ProbEvaluator {
public:
  vector2d loc; // location of point marker
  double dist_obs; // distance observed
  double dist_unif; // distance by which reading might be long
  double dist_dev_frac; // deviation is dist_obs*dist_dev_frac

  virtual void calc_prob(double *prob,const Sample *samp,bool scaled);
  virtual void calc_exp_prob(double *exp_prob,bool scaled);
  virtual void calc_energy(double *energy,const Sample *samp);
  virtual void calc_energy_der(Sample *der,const Sample *samp);
};

class PointAngleProbEvaluator : public ProbEvaluator {
private:
public:
  vector2d loc; // location of point marker
  double angle_obs; // angle observed
  double angle_dev; // deviation is angle_dev

  virtual void calc_prob(double *prob,const Sample *samp,bool scaled);
  virtual void calc_exp_prob(double *exp_prob,bool scaled);
  virtual void calc_energy(double *energy,const Sample *samp);
  virtual void calc_energy_der(Sample *der,const Sample *samp);
};

class LinePointProbEvaluator : public ProbEvaluator {
public:
  static const int MaxLines=9;

private:
  vector2d pd[MaxLines]; // p1 - p0
  double pd_sqlength[MaxLines];
  double obs_angle;
  double obs_dist;
public:
  int num_lines;
  vector2d p0[MaxLines],p1[MaxLines];
  vector2d obs_ego;

  void init_internals(); // call after setting public variables before evaluating

  virtual void calc_prob(double *prob,const Sample *samp,bool scaled);
  virtual void calc_exp_prob(double *exp_prob,bool scaled);
  virtual void calc_energy(double *energy,const Sample *samp);
  virtual void calc_energy_der(Sample *der,const Sample *samp);
};

class CornerProbEvaluator : public ProbEvaluator {
public:
private:
  PointDistProbEvaluator pt_dist_pe;
  PointAngleProbEvaluator pt_angle_pe;

  void pickBestCorner(const Sample *samp,bool scaled);

public:
  double dist_obs; // distance observed
  double dist_dev_frac; // deviation is dist_obs*dist_dev_frac
  double angle_obs; // angle observed
  double angle_dev; // deviation is angle_dev

  int num_corners;
  vector2d *corners;

  virtual void calc_prob(double *prob,const Sample *samp,bool scaled);
  virtual void calc_exp_prob(double *exp_prob,bool scaled);
  virtual void calc_energy(double *energy,const Sample *samp);
  virtual void calc_energy_der(Sample *der,const Sample *samp);
};

class MovementPrimitive {
public:
  MovementPrimitive();

  enum PrimitiveType {VelWalkModel=0};

  typedef union {
    struct VelWalkData {
      double x; // along heading
      double x_dev;

      double y; // perpendicular to heading
      double y_dev;

      double heading_change;
      double heading_change_dev;
    } vel_walk_data;
  } PrimitiveData;

  typedef void (MovementPrimitive::*SampleUpdaterPtr)(Sample *sample) const;

  SampleUpdaterPtr sampleUpdater;

  void velWalkModelUpdater(Sample *sample) const;

  PrimitiveType type;

  PrimitiveData data;

  Environment *environ;

private:
  void clipToEnviron(Sample *sample) const;
};

#endif