/* 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 "colors.h"
#include "Vision.h"
#include "../headers/Geometry.h"
#include "../headers/Reporting.h"

#include "DetectBall.h"

static const double AlmostZero = 1e-6;

bool IsDuplicate(vector2i new_pt,int num_pts,vector2i *edge_pts)
{
  for(int i=0; i<num_pts; i++){
    if(new_pt == edge_pts[i])
      return true;
  }

  return false;
}

double CalcDeterminant(bool &ok,int n_rc,double *mat)
{
  static const int MaxRC=5; // maximum rows/cols

  static double mat_copy[MaxRC][MaxRC];

  ok = false;

  if(n_rc > MaxRC)
    return 0.0;

  // copy matrix
  for(int r=0; r<n_rc; r++){
    for(int c=0; c<n_rc; c++){
      mat_copy[r][c] = mat[r*n_rc + c];
    }
  }

  double det=1.0;

  // row_to_fix is first remaining row which is not upper
  //   triangular with a 1.0 diagonal value
  for(int row_to_fix=0; row_to_fix<n_rc; row_to_fix++){
    // if diagonal element is zero, find a row below this one that's non-zero
    //   to swap with
    if(fabs(mat_copy[row_to_fix][row_to_fix]) < AlmostZero){
      int row_to_swap;
      for(row_to_swap=row_to_fix+1; row_to_swap<n_rc; row_to_swap++){
        if(fabs(mat_copy[row_to_swap][row_to_fix]) >= AlmostZero){
          break;
        }
      }
      // uh-oh, no non-zero row to swap, implies determinate is 0
      if(row_to_swap == n_rc){
        ok = true;
        return 0.0;
      }
      // swap rows
      for(int c=0; c<n_rc; c++){
        swap(mat_copy[row_to_fix][c],mat_copy[row_to_swap][c]);
      }
      det = -det;
    }
    // diagonal element is now non-zero
    // divide by diagonal element to make it 1.0
    // can start at row_to_fix since first elements are already 0.0
    double scale = mat_copy[row_to_fix][row_to_fix];
    det *= scale;
    for(int c=row_to_fix; c<n_rc; c++){
      mat_copy[row_to_fix][c] /= scale;
    }
    // now eliminate this variable from remaining rows
    for(int row_to_clear=row_to_fix+1; row_to_clear<n_rc; row_to_clear++){
      double mult = -mat_copy[row_to_clear][row_to_fix] / mat_copy[row_to_fix][row_to_fix];
      // can start at row_to_fix since first elements are already 0.0
      for(int c=row_to_fix; c<n_rc; c++){
        mat_copy[row_to_clear][c] += mult*mat_copy[row_to_fix][c];
      }      
    }
  }

  // trace of matrix is now 1.0 and matrix is traingular so det is 1.0
  // det has factor to multiply this by to get determinant of orignal matrix
  ok = true;
  return det;
}

// this approximates the distance to the ellipse as the distance from edge_pt to the
// point on the ellipse which is on the line from edge_pt to the ellipse center
double distToEllipse(double *coeffs,vector2f cen,vector2i edge_pt)
{
  vector2f pt;
  double dist = 0,error = 0;

  for(int i=0; i<6; i++) 
    if(!finite(coeffs[i]))
      return 1e100;
  
  pt.set(edge_pt.x,-edge_pt.y);

  //dist = coeffs[0]*sq(pt.x) + coeffs[1]*pt.x*pt.y + coeffs[2]*sq(pt.y) + coeffs[3]*pt.x + coeffs[4]*pt.y + coeffs[5];

  vector2f dir;
  double a,b,c;
  double t,t1,t2;
  if(GVector::sdistance(pt, cen) > sq(1e-3))
    dir = (pt-cen).norm();
  else
    return 1.0e100;
  
  a = coeffs[0]*sq(dir.x) + coeffs[1]*dir.x*dir.y + coeffs[2]*sq(dir.y);
  b = 2*coeffs[0]*cen.x*dir.x + coeffs[1]*(dir.x*cen.y + dir.y*cen.x) + 2*coeffs[2]*dir.y*cen.y + coeffs[3]*dir.x + coeffs[4]*dir.y;
  c = coeffs[0]*sq(cen.x) + coeffs[1]*cen.x*cen.y + coeffs[2]*sq(cen.y) + coeffs[3]*cen.x + coeffs[4]*cen.y + coeffs[5];
  if(fabs(a) < AlmostZero)
    return 1.0e100;

  double det;
  det = sq(b) - 4*a*c;
  if(det < 0.0)
    return 1.0e100;
  t1 = (-b + sqrt(det)) / (2*a);
  t2 = (-b - sqrt(det)) / (2*a);
  t = max(t1,t2);

  vector2f ell_pt;
  ell_pt = cen + dir*t;

  dist = GVector::distance(pt,ell_pt);
  error = dist;

  //pprintf(TextOutputStream,"conic is: %g*x^2 + %g*x*y + %g*y^2 + %g*x + %g*y + %g = 0\n",
  //        coeffs[0],coeffs[1],coeffs[2],coeffs[3],coeffs[4],coeffs[5]);
  //pprintf(TextOutputStream,"pt=(%3g,%3g) ell_pt=(%g,%g) has dist=%g error=%g\n",V2COMP(pt),V2COMP(ell_pt),dist,error);

  return fabs(error);
}

double evaluateEllipseError(double *coeffs,vector2f cen,int num_edge_pts,vector2i *edge_pts)
{
  std::vector<double> dists;

  dists.clear();
  for(int edge_pt_idx=0; edge_pt_idx<num_edge_pts; edge_pt_idx++){
    dists.push_back(distToEllipse(coeffs,cen,edge_pts[edge_pt_idx]));
  }

  sort(dists.begin(),dists.end());

  // median of pts not used to produce fit
  return dists[5+(dists.size()-5)/2];
}