/////////////////////////////////////////////////////////////
//                                                         //
// Copyright (c) 2007-2017 by The University of Queensland //
// Centre for Geoscience Computing                         //
// http://earth.uq.edu.au/centre-geoscience-computing      //
//                                                         //
// Primary Business: Brisbane, Queensland, Australia       //
// Licensed under the Open Software License version 3.0    //
// http://www.apache.org/licenses/LICENSE-2.0              //
//                                                         //
/////////////////////////////////////////////////////////////

#include "Sphere2DFitter.h"

// --- project includes ---
#include "sphere_fitting/utils/nvector.h"
#include "sphere_fitting/utils/nfunction.h"
#include "sphere_fitting/utils/simplex.h"
#include "fit_2d_sphere.h"

// --- IO includes ---
#include <iostream>

Sphere FitSphere2D(const AGeometricObject* GO1,
		   const AGeometricObject* GO2,
		   const AGeometricObject* GO3,
		   const Vector3& spos,
		   int max_iter,double prec)
{
  Sphere res;

  simplex_method<double,2> *simplex;
  nvector<double,2> start,sol;
  fit_2d_sphere_fn* sfn;

  // set initial position to barycenter of input spheres
  start[0]=spos.x();
  start[1]=spos.y();

  // set fitting function
  sfn=new fit_2d_sphere_fn(GO1,GO2,GO3);
  
  // solve for center 
  simplex=new simplex_method<double,2>(sfn);
  sol=simplex->solve(prec,start,max_iter);

  // calc radius : min radius relative to 3 spheres - tol
  Vector3 center=Vector3(sol[0],sol[1],0.0);
  double r1=GO1->getDist(center);
  double r2=GO2->getDist(center);
  double r3=GO3->getDist(center);

  double r=(r1<r2) ? r1 : r2;
  r= (r < r3) ? r : r3;

  res=Sphere(center,r-0.1*prec);

  // clean up
  delete simplex;
  delete sfn;

  return res;
}

