/* 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 <iostream>
#include <math.h>
#include <stdio.h>

using namespace std;

#include "Constants.h"

#include "../../headers/Util.h"
#include "Environment.h"
#include "Functions.h"
#include "Primitives.h"

double eval_gaussian(double sigmas)
{
  return exp(-sigmas*sigmas/2.0);
}

ProbEvaluator::ProbEvaluator()
{
  matchedTo = NULL;
}

void PointDistProbEvaluator::calc_prob(double *prob,const Sample *samp,bool scaled)
{
  double dist;
  
  dist = GVector::distance(samp->loc,loc);
  
  double sigmas = (dist - dist_obs) / (dist_dev_frac*dist_obs);
  *prob = eval_gaussian(sigmas);

  //printf("eval pdpe: samp->loc=(%g,%g) loc=(%g,%g) dist_obs=%g dist_dev_frac=%g prob=%g\n",
  //       V2COMP(samp->loc),V2COMP(loc),dist_obs,dist_dev_frac,*prob);
}

void PointDistProbEvaluator::calc_exp_prob(double *exp_prob,bool scaled)
{
  *exp_prob = .28209*sqrt(2*M_PI);
}

void PointDistProbEvaluator::calc_energy(double *energy,const Sample *samp)
{
  double dist;
  
  dist = GVector::distance(samp->loc,loc);
  
  double sigmas = (dist - dist_obs) / (dist_dev_frac*dist_obs);
  *energy = (sigmas*sigmas/2.0);
}

void PointDistProbEvaluator::calc_energy_der(Sample *der,const Sample *samp)
{
  double dist;
  
  dist = GVector::distance(samp->loc,loc);
  
  double sigmas = (dist - dist_obs) / (dist_dev_frac*dist_obs);
  double der_len;
  vector2d der_xy;
  vector2d der_dir;
  
  der_len = sigmas;
  der_xy.set(der_len,0.0);
  der_dir = (samp->loc - loc).norm() / (dist_dev_frac*dist_obs);
  der_xy = der_xy.project(der_dir);
  
  der->loc = der_xy;
  der->angle = 0.0;

  //printf("eval der for samp->loc=(%g,%g) loc=(%g,%g) der->loc=(%g,%g)\n",
  //       V2COMP(samp->loc),V2COMP(loc),V2COMP(der->loc));
}

void PointDistWithUniformProbEvaluator::calc_prob(double *prob,const Sample *samp,bool scaled)
{
  double dist;
  double dist_off;
  
  dist = GVector::distance(samp->loc,loc);
  dist_off = dist - dist_obs;
  if(dist_off < 0.0){
    dist_off += dist_unif;
    if(dist_off > 0.0){
      *prob = 1.0;
      return;
    }
  }
  
  double sigmas = dist_off / (dist_dev_frac*dist_obs);
  *prob = eval_gaussian(sigmas);
}

void PointDistWithUniformProbEvaluator::calc_exp_prob(double *exp_prob,bool scaled)
{
  double gauss_area;
  double unif_area;
  double total_area;
  double gauss_exp;
  double unif_exp;

  gauss_area = dist_dev_frac*dist_obs*sqrt(2*M_PI);
  unif_area = dist_unif;
  total_area = gauss_area + unif_area;
  
  gauss_exp = .28209*sqrt(2*M_PI);
  unif_exp = 1.0;

  *exp_prob = (gauss_area/total_area)*gauss_exp + (unif_area/total_area)*unif_exp;
}

void PointDistWithUniformProbEvaluator::calc_energy(double *energy,const Sample *samp)
{
  double dist;
  double dist_off;
  
  dist = GVector::distance(samp->loc,loc);
  dist_off = dist - dist_obs;
  if(dist_off < 0.0){
    dist_off += dist_unif;
    if(dist_off > 0.0){
      *energy = 0.0;
      return;
    }
  }
  
  double sigmas = dist_off / (dist_dev_frac*dist_obs);
  *energy = (sigmas*sigmas/2.0);
}

void PointDistWithUniformProbEvaluator::calc_energy_der(Sample *der,const Sample *samp)
{
  double dist;
  double dist_off;
  
  dist = GVector::distance(samp->loc,loc);
  dist_off = dist - dist_obs;
  if(dist_off < 0.0){
    dist_off += dist_unif;
    if(dist_off > 0.0){
      dist_off = 0.0;
    }
  }
  
  double sigmas = dist_off / (dist_dev_frac*dist_obs);
  double der_len;
  vector2d der_xy;
  vector2d der_dir;
  
  der_len = sigmas;
  der_xy.set(der_len,0.0);
  der_dir = (samp->loc - loc).norm() / (dist_dev_frac*dist_obs);
  der_xy = der_xy.project(der_dir);
  
  der->loc = der_xy;
  der->angle = 0.0;
}

void PointAngleProbEvaluator::calc_prob(double *prob,const Sample *samp,bool scaled)
{
  double ego_exp;
  double angle_error;
  double sigmas;

  ego_exp = (loc - samp->loc).angle();
  if(scaled)
    ego_exp -= samp->angle/Sample::AngleScale;
  else
    ego_exp -= samp->angle;
  ego_exp = norm_angle(ego_exp);
  angle_error = angle_obs - ego_exp;
  angle_error = norm_angle(angle_error);
  sigmas = angle_error / angle_dev;

  *prob = eval_gaussian(sigmas);

  //printf("eval pape: samp->loc=(%g,%g) samp->angle=%g loc=(%g,%g) angle_obs=%g angle_dev=%g prob=%g\n",
  //       V2COMP(samp->loc),samp->angle,V2COMP(loc),angle_obs,angle_dev,*prob);
}

void PointAngleProbEvaluator::calc_exp_prob(double *exp_prob,bool scaled)
{
  *exp_prob = .28209*sqrt(2*M_PI);
}

void PointAngleProbEvaluator::calc_energy(double *energy,const Sample *samp)
{
  double ego_exp;
  double angle_error;
  double sigmas;

  ego_exp = (loc - samp->loc).angle();
  ego_exp -= samp->angle/Sample::AngleScale;
  ego_exp = norm_angle(ego_exp);
  angle_error = angle_obs - ego_exp;
  angle_error = norm_angle(angle_error);
  sigmas = angle_error / angle_dev;
  
  *energy = (sigmas*sigmas)/2.0;
}

void PointAngleProbEvaluator::calc_energy_der(Sample *der,const Sample *samp)
{
  double ego_exp;
  double angle_error;
  double sigmas;
  vector2d disp;
  double sqlength;

  disp = loc - samp->loc;
  sqlength = disp.sqlength();

  ego_exp = disp.angle();
  ego_exp -= samp->angle/Sample::AngleScale;
  ego_exp = norm_angle(ego_exp);
  angle_error = angle_obs - ego_exp;
  angle_error = norm_angle(angle_error);
  sigmas = angle_error / angle_dev;

  double der_common;
  der_common = sigmas / angle_dev;

  der->loc.x = -(der_common)*(disp.y/sqlength);
  der->loc.y =  (der_common)*(disp.x/sqlength);
  der->angle =  (der_common)/Sample::AngleScale;
}

static double LineDistSigmaFrac = .03;
static double LineAngleSigma = .03;

void LinePointProbEvaluator::init_internals()
{
  obs_angle = obs_ego.angle();
  obs_dist  = obs_ego.length();

  for(int line_idx=0; line_idx<num_lines; line_idx++){
    pd[line_idx] = p1[line_idx] - p0[line_idx];
    pd_sqlength[line_idx] = pd[line_idx].sqlength();
  }
}

void LinePointProbEvaluator::calc_prob(double *prob,const Sample *samp,bool scaled)
{
  vector2d pt_wall_glob; // closest point on wall
  vector2d obs_glob;
  vector2d disp;
  double dist_error,angle_error;
  double dist_sigmas,angle_sigmas;
  int line_idx;

  if(scaled)
    obs_glob = samp->loc + obs_ego.rotate(samp->angle/Sample::AngleScale);
  else
    obs_glob = samp->loc + obs_ego.rotate(samp->angle);

  double best_line_dist=HUGE_VAL;
  line_idx = 0;
  for(int i=0; i<num_lines; i++){
    double line_dist;
    line_dist = GVector::distance_to_line(p0[i],p1[i],obs_glob);
    if(line_dist < best_line_dist){
      line_idx = i;
      best_line_dist = line_dist;
    }
  }

  pt_wall_glob = GVector::point_on_line(p0[line_idx],p1[line_idx],obs_glob);
  disp = pt_wall_glob - samp->loc;
  dist_error = obs_dist - disp.length();
  dist_sigmas = dist_error / (LineDistSigmaFrac*obs_dist);

  angle_error = obs_angle - disp.angle() + samp->angle/Sample::AngleScale;
  angle_sigmas = angle_error / LineAngleSigma;

  *prob = exp((-dist_sigmas*dist_sigmas - angle_sigmas*angle_sigmas) / 2.0);
}

void LinePointProbEvaluator::calc_exp_prob(double *exp_prob,bool scaled)
{
  *exp_prob = sq(.28209)*2*M_PI;
}

void LinePointProbEvaluator::calc_energy(double *energy,const Sample *samp)
{
  vector2d pt_wall_glob; // closest point on wall
  vector2d obs_glob;
  vector2d disp;
  double dist_error,angle_error;
  double dist_sigmas,angle_sigmas;
  int line_idx;

  obs_glob = samp->loc + obs_ego.rotate(samp->angle/Sample::AngleScale);

  double best_line_dist=HUGE_VAL;
  line_idx = 0;
  for(int i=0; i<num_lines; i++){
    double line_dist;
    line_dist = GVector::distance_to_line(p0[i],p1[i],obs_glob);
    //line_dist = GVector::distance(GVector::point_on_line(p0[i],p1[i],obs_glob),obs_glob);
    if(line_dist < best_line_dist){
      line_idx = i;
      best_line_dist = line_dist;
    }
  }

  //((Sample *)samp)->line_idx = line_idx;

  pt_wall_glob = GVector::point_on_line(p0[line_idx],p1[line_idx],obs_glob);
  disp = pt_wall_glob - samp->loc;
  dist_error = obs_dist - disp.length();
  dist_sigmas = dist_error / (LineDistSigmaFrac*obs_dist);

  angle_error = obs_angle - disp.angle() + samp->angle/Sample::AngleScale;
  angle_sigmas = angle_error / LineAngleSigma;

  *energy = (dist_sigmas*dist_sigmas + angle_sigmas*angle_sigmas) / 2.0;
}

void LinePointProbEvaluator::calc_energy_der(Sample *der,const Sample *samp)
{
  vector2d pt_wall_glob; // closest point on wall
  vector2d obs_glob;
  vector2d disp;
  double dist_error,angle_error;
  double dist_sigmas,angle_sigmas;
  double disp_sqlength,disp_length;
  int line_idx;

  obs_glob = samp->loc + obs_ego.rotate(samp->angle/Sample::AngleScale);

  double best_line_dist=HUGE_VAL;
  line_idx = 0;
  for(int i=0; i<num_lines; i++){
    double line_dist;
    line_dist = GVector::distance_to_line(p0[i],p1[i],obs_glob);
    if(line_dist < best_line_dist){
      line_idx = i;
      best_line_dist = line_dist;
    }
  }

  pt_wall_glob = GVector::point_on_line(p0[line_idx],p1[line_idx],obs_glob);
  disp = pt_wall_glob - samp->loc;
  disp_sqlength = disp.sqlength();
  disp_length = sqrt(disp_sqlength);
  dist_error = obs_dist - disp_length;
  dist_sigmas = dist_error / (LineDistSigmaFrac*obs_dist);

  angle_error = obs_angle - disp.angle() + samp->angle/Sample::AngleScale;
  angle_sigmas = angle_error / LineAngleSigma;

  double cos_samp_angle,sin_samp_angle;
  cos_samp_angle = cosf(samp->angle/Sample::AngleScale);
  sin_samp_angle = sinf(samp->angle/Sample::AngleScale);

  double der_dist_sigmas_common;
  double der_dx_loc_x;
  double der_dx_loc_y;
  double der_dy_loc_x;
  double der_dy_loc_y;
  double der_obs_glob_x_angle;
  double der_obs_glob_y_angle;
  double der_dxy_common_angle;
  double der_dx_angle;
  double der_dy_angle;
  der_dist_sigmas_common = -1/((LineDistSigmaFrac+LineAngleSigma)*obs_dist * disp_length);
  der_dx_loc_x = -(pd[line_idx].y * pd[line_idx].y) / pd_sqlength[line_idx];
  der_dy_loc_x =  (pd[line_idx].y * pd[line_idx].x) / pd_sqlength[line_idx];
  der_dx_loc_y =  (pd[line_idx].x * pd[line_idx].y) / pd_sqlength[line_idx];
  der_dy_loc_y = -(pd[line_idx].x * pd[line_idx].x) / pd_sqlength[line_idx];
  der_obs_glob_x_angle = (- obs_ego.x * sin_samp_angle - obs_ego.y * cos_samp_angle) / Sample::AngleScale;
  der_obs_glob_y_angle = (  obs_ego.x * cos_samp_angle - obs_ego.y * sin_samp_angle) / Sample::AngleScale;
  der_dxy_common_angle = (pd[line_idx].x * der_obs_glob_x_angle + pd[line_idx].y * der_obs_glob_y_angle) / pd_sqlength[line_idx];
  der_dx_angle =  pd[line_idx].x * der_dxy_common_angle;
  der_dy_angle =  pd[line_idx].y * der_dxy_common_angle;

  double der_dist_sigmas_loc_x;
  double der_angle_sigmas_loc_x;
  double der_dist_sigmas_loc_y;
  double der_angle_sigmas_loc_y;
  double der_dist_sigmas_angle;
  double der_angle_sigmas_angle;

  der_dist_sigmas_loc_x  = der_dist_sigmas_common * (disp.x * der_dx_loc_x + disp.y * der_dy_loc_x);
  der_angle_sigmas_loc_x = (-1/LineAngleSigma) * (disp.x * der_dy_loc_x - disp.y * der_dx_loc_x) / disp_sqlength;
  der->loc.x = dist_sigmas * der_dist_sigmas_loc_x + angle_sigmas * der_angle_sigmas_loc_x;

  der_dist_sigmas_loc_y  = der_dist_sigmas_common * (disp.x * der_dx_loc_y + disp.y * der_dy_loc_y);
  der_angle_sigmas_loc_y = (-1/LineAngleSigma) * (disp.x * der_dy_loc_y - disp.y * der_dx_loc_y) / disp_sqlength;
  der->loc.y = dist_sigmas * der_dist_sigmas_loc_y + angle_sigmas * der_angle_sigmas_loc_y;

  der_dist_sigmas_angle  = der_dist_sigmas_common * (disp.x * der_dx_angle + disp.y * der_dy_angle);
  der_angle_sigmas_angle = (1/LineAngleSigma) * (1/Sample::AngleScale - (disp.x * der_dy_angle - disp.y * der_dx_angle)/disp_sqlength);
  der->angle = dist_sigmas * der_dist_sigmas_angle + angle_sigmas * der_angle_sigmas_angle;
}

void CornerProbEvaluator::pickBestCorner(const Sample *samp,bool scaled)
{
  vector2d obs_glob;
  vector2d obs_ego;
  int corner_idx;

  obs_ego.set(1.0,0.0);
  obs_ego = obs_ego.rotate(angle_obs);
  obs_ego = obs_ego * dist_obs;

  if(scaled)
    obs_glob = samp->loc + obs_ego.rotate(samp->angle/Sample::AngleScale);
  else
    obs_glob = samp->loc + obs_ego.rotate(samp->angle);

  double best_corner_dist=HUGE_VAL;
  corner_idx = 0;
  for(int i=0; i<num_corners; i++){
    double corner_dist;
    corner_dist = GVector::distance(corners[i],obs_glob);
    if(corner_dist < best_corner_dist){
      corner_idx = i;
      best_corner_dist = corner_dist;
    }
  }

  //printf("chose corner=%d,dist=%g at (%g,%g)\n",corner_idx,best_corner_dist,V2COMP(corners[corner_idx]));

  pt_dist_pe.loc.set(corners[corner_idx]);
  pt_dist_pe.dist_obs      = dist_obs;
  pt_dist_pe.dist_dev_frac = dist_dev_frac;

  pt_angle_pe.loc.set(corners[corner_idx]);
  pt_angle_pe.angle_obs = angle_obs;
  pt_angle_pe.angle_dev = angle_dev;
}

void CornerProbEvaluator::calc_prob(double *prob,const Sample *samp,bool scaled)
{
  double prob1=1.0,prob2=1.0;

  pickBestCorner(samp,scaled);

  pt_dist_pe.calc_prob (&prob1,samp,scaled);
  pt_angle_pe.calc_prob(&prob2,samp,scaled);

  *prob = prob1 * prob2;
}

void CornerProbEvaluator::calc_exp_prob(double *exp_prob,bool scaled)
{
  double exp_prob1=1.0,exp_prob2=1.0;

  pt_dist_pe.loc.set(corners[0]);
  pt_dist_pe.dist_obs      = dist_obs;
  pt_dist_pe.dist_dev_frac = dist_dev_frac;

  pt_angle_pe.loc.set(corners[0]);
  pt_angle_pe.angle_obs = angle_obs;
  pt_angle_pe.angle_dev = angle_dev;

  pt_dist_pe .calc_exp_prob(&exp_prob1,scaled);
  pt_angle_pe.calc_exp_prob(&exp_prob2,scaled);

  *exp_prob = exp_prob1 * exp_prob2;
}

void CornerProbEvaluator::calc_energy(double *energy,const Sample *samp)
{
  double energy1=0.0,energy2=0.0;

  pickBestCorner(samp,true);

  pt_dist_pe .calc_energy(&energy1,samp);
  pt_angle_pe.calc_energy(&energy2,samp);

  *energy = energy1 + energy2;
}

void CornerProbEvaluator::calc_energy_der(Sample *energy_der,const Sample *samp)
{
  Sample energy_der1,energy_der2;

  pickBestCorner(samp,true);

  energy_der1.init();
  energy_der2.init();

  pt_dist_pe.calc_energy_der (&energy_der1,samp);
  pt_angle_pe.calc_energy_der(&energy_der2,samp);

  energy_der->copy_der(&energy_der1);
  energy_der->add_der (&energy_der2);
}

MovementPrimitive::MovementPrimitive() :
  environ(NULL)
{
}

void MovementPrimitive::velWalkModelUpdater(Sample *sample) const
{
  double x            =sample->loc.x;
  double y            =sample->loc.y;
  double theta        =sample->angle;

  double ego_move_x;
  double ego_move_y;
  double ego_dir_chg;

  ego_move_x =data.vel_walk_data.x             +GlobalGaussianSampler.gen_sample()*data.vel_walk_data.x_dev             ;
  ego_move_y =data.vel_walk_data.y             +GlobalGaussianSampler.gen_sample()*data.vel_walk_data.y_dev             ;
  ego_dir_chg=data.vel_walk_data.heading_change+GlobalGaussianSampler.gen_sample()*data.vel_walk_data.heading_change_dev;

  vector2d ego_move;
  ego_move.set(ego_move_x,ego_move_y);

  double new_x;
  double new_y;
  double new_dir;

  double movement_angle = theta;

  ego_move = ego_move.rotate_fast(movement_angle);

  new_x = x + ego_move.x;
  new_y = y + ego_move.y;

  new_dir = theta + ego_dir_chg;
  while(new_dir < -M_PI)
    new_dir+= 2*M_PI;
  while(new_dir >= M_PI)
    new_dir-= 2*M_PI;

  sample->loc.x=new_x;
  sample->loc.y=new_y;
  sample->angle=new_dir;

  clipToEnviron(sample);
}

void
MovementPrimitive::clipToEnviron(Sample *sample) const {
  bool moved=environ->placeInEnviron(sample, robotSideOffset);

  if(!moved)
    return;

  Sample::Element &x            =sample->loc.x;
  Sample::Element &y            =sample->loc.y;
  Sample::Element &theta        =sample->angle;

  double delta_x=robotSideStdDev*GlobalGaussianSampler.gen_sample();
  x+=delta_x;

  double delta_y=robotSideStdDev*GlobalGaussianSampler.gen_sample();
  y+=delta_y;

  double delta_theta=robotSideDirStdDev*GlobalGaussianSampler.gen_sample();
  theta+=delta_theta;

  while(theta<-M_PI)
    theta+=2*M_PI;
  while(theta>=M_PI)
    theta-=2*M_PI;
}