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

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

#include "../headers/Geometry.h"
#include "../headers/Gaussian2.h"
#include "../headers/DogTypes.h"
#include "../headers/CircBufPacket.h"

bool DEBUG = false;

Gaussian2::Gaussian2() 
{
  mean = vector2d(0.0,0.0);
  sMaj = 1.0;
  sMin = 1.0;
  thetaMaj = 0.0;
  sx = 1.0;
  sy = 1.0;
  psxsy = 0.0;
  time = (ulong)0.0;
}

Gaussian2::Gaussian2(ulong timestamp, vector2d m, double s1, double s2, 
		     double c, bool global) 
{
  mean = vector2d(m);
  time = timestamp;
  if (global) 
  {
    setsxsy(s1,s2,c);
  }
  else 
  {
    setsMajsMin(s1,s2,c);
  }
}

Gaussian2::Gaussian2(const Gaussian2& g) 
{
  mean = vector2d(g.mean);
  sMaj = g.sMaj;
  sMin = g.sMin;
  thetaMaj = g.thetaMaj;
  sx = g.sx;
  sy = g.sy;
  psxsy = g.psxsy;
  time = g.time;
}

void Gaussian2::setsxsy(double s1, double s2, double c)
{
  sx = fabs(s1);
  sy = fabs(s2);
  psxsy = c;
  if (psxsy==0)
  {
    if (sx > sy) 
    {
      sMaj = sx;
      sMin = sy;
      thetaMaj = 0.0;
    }
    else 
    {
      sMaj = sy;
      sMin = sx;
      thetaMaj = M_PI/2.0;
    }
  }
  else
  {
    thetaMaj = 0.5*atan2a(2*psxsy,sq(sx)-sq(sy));
    if (thetaMaj > M_PI)
    {
      thetaMaj = thetaMaj - 2.0*M_PI;
    }
    else if (thetaMaj <= -M_PI)
    {
      thetaMaj = thetaMaj + 2.0*M_PI;
    }
    double ct = cos(thetaMaj);
    double st = sin(thetaMaj);
    /*
    sMaj = sqrt(sq(ct*sx) - 2*ct*st*psxsy 
                + sq(st*sy));
    sMin = sqrt(sq(st*sx) + 2*ct*st*psxsy
                + sq(ct*sy));
    */
    double vMaj = sq(ct*sx) + 2*ct*st*psxsy + sq(st*sy);
    double vMin = sq(st*sx) - 2*ct*st*psxsy + sq(ct*sy);

    sMaj = sqrt(vMaj);
    sMin = sqrt(vMin);
    /*
    sMaj = sqrt(sq(ct*sx) + 2*ct*st*psxsy + sq(st*sy));
    sMin = sqrt(sq(st*sx) - 2*ct*st*psxsy + sq(ct*sy));
    */
    }
}

void Gaussian2::setsMajsMin(double s1, double s2, double c) 
{
  if (s1>=s2)
  {
    sMaj = s1;
    sMin = s2;
    thetaMaj = c;
  }
  else
  {
    sMaj = s2;
    sMin = s1;
    thetaMaj = c + M_PI/2.0;
  }
  while (thetaMaj > M_PI) thetaMaj -= 2.0*M_PI;
  while (thetaMaj <= -M_PI) thetaMaj += 2.0*M_PI;
  double ct = cos(thetaMaj);
  double st = sin(thetaMaj);

  sx = sqrt(sq(ct*sMaj) + sq(st*sMin));
  sy = sqrt(sq(st*sMaj) + sq(ct*sMin));
  psxsy = st*ct*(sq(sMaj) - sq(sMin));
}


Gaussian2 G2Multiply(Gaussian2 g1, Gaussian2 g2) 
{
  double x1 = g1.mean.x;
  double y1 = g1.mean.y;
  double vx1 = sq(g1.sx);
  double vy1 = sq(g1.sy);
  double p1 = g1.psxsy;
  double x2 = g2.mean.x;
  double y2 = g2.mean.y;
  double vx2 = sq(g2.sx);
  double vy2 = sq(g2.sy);
  double p2 = g2.psxsy;

#ifndef PLATFORM_LINUX
  if (DEBUG) 
  {
    pprintf(TextOutputStream,
      "G1:  (%g,%g) (%g,%g,%g) (%g,%g,%g)\n",
       x1,y1,g1.sx,g1.sy,p1,g1.sMaj,g1.sMin,g1.thetaMaj);
    pprintf(TextOutputStream,
      "G2:  (%g,%g) (%g,%g,%g) (%g,%g,%g)\n",
      x2,y2,g2.sx,g2.sy,p2,g2.sMaj,g2.sMin,g2.thetaMaj);
  }
#endif

  if ( vx1==vy1 && vx2==vy2 )
  {
    vector2d mean = vector2d((x1*vx2 + x2*vx1)/(vx1+vx2),
		             (y1*vx2 + y2*vx1)/(vx1+vx2));
    double new_sx = sqrt(vx1*vx2/(vx1+vx2));
    double new_sy = new_sx;
    double new_p = 0.0;

    return(Gaussian2(g1.time,mean,new_sx,new_sy,new_p,true));
  }
  else
  {
    // Find K = C1*inv(C1+C2)
    double A1 = vx1 + vx2;
    double B1 = p1 + p2;
    double D1 = vy1 + vy2;
    double det = A1*D1 - sq(B1);

    double KA = ( vx1*D1 - p1*B1)/det;
    double KB = (-vx1*B1 + p1*A1)/det;
    double KC = (-vy1*B1 + p1*D1)/det;
    double KD = ( vy1*A1 - p1*B1)/det;
    
    // Find Cnew = C1 - K*C1
    double new_vx = vx1*(1 - KA) - KB*p1;
    double new_vy = vy1*(1 - KD) - KC*p1;
    double new_p  = p1*(1 - KA) - KB*vy1;

    // Find Xnew = X1 + K*(X2-X1)
    double new_x = x1 + KA*(x2-x1) + KB*(y2-y1);
    double new_y = y1 + KC*(x2-x1) + KD*(y2-y1);

    Gaussian2 result = 
      Gaussian2(g1.time,vector2d(new_x,new_y),sqrt(new_vx),
		sqrt(new_vy),new_p,true);

#ifndef PLATFORM_LINUX
    if (DEBUG)
      pprintf(TextOutputStream,
        "G2Mult Result:  (%g,%g) (%g,%g,%g) (%g,%g,%g)\n",
        result.mean.x,result.mean.y,result.sx,result.sy,
        result.psxsy,result.sMaj,result.sMin,result.thetaMaj);
#endif

    return(result);
  }  
}

/* This is only valid if g1 and g2 are two independently
   distributed gaussians.
*/
Gaussian2 G2Add(const Gaussian2 &g1, const Gaussian2 &g2) {
  Gaussian2 retval;

  retval.mean = g1.mean + g2.mean;
  retval.setsxsy(g1.sx + g2.sx, g1.sy + g2.sy, g1.psxsy + g2.psxsy);

  return retval;
}

/* This is a horrible cludge that I had to put in for backwards 
   compatibility.  If I had my way, it would never see the light of day
   again, but it's referenced so many times, I don't see any way around it.
*/
double Gaussian2::ang_dev() { /* A fairly poor angle stdev estimation */
  return 4*atan2a((sx + sy)/2.0, mean.length());
}


/* Also, I have to add the ridiculous travesty that is dist_dev. */
double Gaussian2::dist_dev() {
  return (sx+sy)/2.0;
}

/* It would be nice if we were able to evaluate the likelihood of
   points using the gaussian.
*/
double Gaussian2::eval(vector2d point) {
  
  // We need to move from global coordinates into the same coordinate
  // frame as the gaussian. 
  vector2d mean_to_pt = point - mean;

  // We need our variances and covariances.
  double var_xx = sq(sx);
  double var_yy = sq(sy);
  double var_xy = psxsy;
  double var_yx = var_xy;

  // Calculate the determinent of our covariance matrix
  double det = var_xx*var_yy - var_xy*var_xy;

  // We'll neet do invert our covariance matrix as well.
  double s_inv[2][2];

  s_inv[0][0] = var_yy/(var_xx*var_yy - var_xy*var_yx);
  s_inv[0][1] = var_xy/(var_xx*var_yy - var_xy*var_yx);
  s_inv[1][0] = var_yx/(var_xx*var_yy - var_xy*var_yx);
  s_inv[1][1] = var_xx/(var_xx*var_yy - var_xy*var_yx);

  // What goes in the exponent? It should be
  // -.5*x'*s_inv*x
  double x = mean_to_pt.x;
  double y = mean_to_pt.y;
  double exp_val = -.5*(x*(s_inv[0][0]*x + s_inv[0][1]*y) +
			y*(s_inv[1][0]*x + s_inv[1][1]*y));

  if(det < 0)
    return 0;

  return (1.0/(2*M_PI*sqrt(det)))*exp(exp_val);
}


/* It would be nice if we were able to evaluate the likelihood of
   points using the gaussian. Since this can be very small we'll
   return it's natural log instead of the actual value.
*/
double Gaussian2::evalLog(vector2d point) {
  
  // We need to move from global coordinates into the same coordinate
  // frame as the gaussian. 
  vector2d mean_to_pt = point - mean;

  // We need our variances and covariances.
  double var_xx = sq(sx);
  double var_yy = sq(sy);
  double var_xy = psxsy;
  double var_yx = var_xy;

  // Calculate the determinent of our covariance matrix
  double det = var_xx*var_yy - var_xy*var_xy;

  // We'll neet do invert our covariance matrix as well.
  double s_inv[2][2];

  s_inv[0][0] = var_yy/(var_xx*var_yy - var_xy*var_yx);
  s_inv[0][1] = var_xy/(var_xx*var_yy - var_xy*var_yx);
  s_inv[1][0] = var_yx/(var_xx*var_yy - var_xy*var_yx);
  s_inv[1][1] = var_xx/(var_xx*var_yy - var_xy*var_yx);

  // What goes in the exponent? It should be
  // -.5*x'*s_inv*x
  double x = mean_to_pt.x;
  double y = mean_to_pt.y;
  double exp_val = -.5*(x*(s_inv[0][0]*x + s_inv[0][1]*y) +
			y*(s_inv[1][0]*x + s_inv[1][1]*y));

  if(det < 0)
    return 0;

  //  return (1.0/(2*M_PI*sqrt(det)))*exp(exp_val);
  return /* log(1.0)==0 */ -log(2*M_PI) - log(sqrt(det)) + exp_val;
}