/***************************************************************************
**
**  This file is part of QGpCoreMath.
**
**  This library is free software; you can redistribute it and/or
**  modify it under the terms of the GNU Lesser General Public
**  License as published by the Free Software Foundation; either
**  version 2.1 of the License, or (at your option) any later version.
**
**  This file is distributed in the hope that it will be useful, but WITHOUT
**  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
**  FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
**  License for more details.
**
**  You should have received a copy of the GNU Lesser General Public
**  License along with this library; if not, write to the Free Software
**  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**
**  See http://www.geopsy.org for more information.
**
**  Created: 2003-05-12
**  Copyright: 2003-2019
**    Marc Wathelet
**    Marc Wathelet (ULg, Liège, Belgium)
**    Marc Wathelet (LGIT, Grenoble, France)
**
***************************************************************************/

#include <math.h>

#include "Circle.h"

namespace QGpCoreMath {

Circle::Circle(double x0,double y0, double r)
  : _c(x0, y0)
{
  _r=fabs(r);
}

Circle::Circle(const Point2D &c, double r)
  : _c(c)
{
  _r=fabs(r);
}

Circle::Circle(const Point2D& p1, const Point2D& p2, const Point2D& p3)
{
  // From equation of a circle for the thre points
  double a12=p1.x()*p1.x()-p2.x()*p2.x()+p1.y()*p1.y()-p2.y()*p2.y();
  double a13=p1.x()*p1.x()-p3.x()*p3.x()+p1.y()*p1.y()-p3.y()*p3.y();
  double b12=2.0*(p2.x()-p1.x());
  double b13=2.0*(p3.x()-p1.x());
  double c12=2.0*(p2.y()-p1.y());
  double c13=2.0*(p3.y()-p1.y());
  double m=c13*b12-c12*b13;
  if(m!=0) {
    _c.setX((c12*a13-c13*a12)/m);
    if(c12!=0.0) {
      _c.setY(-(a12+_c.x()*b12)/c12);
      Point2D d=p1;
      d-=_c;
      _r=::sqrt(d.x()*d.x()+d.y()*d.y());
    } else {
      _c.setY(std::numeric_limits<double>::infinity());
      _r=std::numeric_limits<double>::infinity();
    }
  } else {
    _c.setX(std::numeric_limits<double>::infinity());
    _c.setY(std::numeric_limits<double>::infinity());
    _r=std::numeric_limits<double>::infinity();
  }
}

Circle::Circle(const Circle &o)
{
  _c=o._c;
  _r=o._r;
}

double Circle::distanceTo(const Point2D& p) const
{
  TRACE;
  double d=p.distanceTo(_c);
  return fabs(d-_r);
}

/*!
  Calculate intersections between this and \a o.
*/
QVector<Point2D> Circle::intersect(const Circle& o) const
{
  TRACE;
  QVector<Point2D> inter;
  double r02=_r*_r;
  double r12=o._r*o._r;
  double x02=_c.x()*_c.x();
  double y02=_c.y()*_c.y();
  double x12=o._c.x()*o._c.x();
  double y12=o._c.y()*o._c.y();
  double a, b, c, rho, srho, A, B, C, D;
  A=2.0*(o._c.x()-_c.x());
  B=2.0*(o._c.y()-_c.y());
  C=r02-r12-(x02+y02-x12-y12);
  // The solution must satisfy
  //  A * xs + B * ys=C, which is simply the subtraction of circle equations
  if(fabs(A)>fabs(B)) {
    // Prefer working with xs=(C-B*ys)/A transformed into xs=B-A*ys, with B=C/A and A=B/A
    D=1.0/A;
    A=B*D;
    B=C*D;
    a=1.0+A*A;
    b=2*A*_c.x()-2*A*B-2*_c.y();
    c=x02+B*B-2*_c.x()*B+y02-r02;
    rho=b*b-4*a*c;
    if(rho>0) {
      srho=::sqrt(rho);
      inter.resize(2);
      inter[0].setY((-b+srho)/(2.0*a));
      inter[1].setY((-b-srho)/(2.0*a));
      inter[0].setX(B-A*inter[0].y());
      inter[1].setX(B-A*inter[1].y());
      // Check:
      //printf("Check c1 %lf==%lf\n",_r,sqrt((inter[0].x()-_x0)*(inter[0].x()-_x0)+(inter[0].y()-_y0)*(inter[0].y()-_y0)));
      //printf("Check c2 %lf==%lf\n",o._r,sqrt((inter[0].x()-o._x0)*(inter[0].x()-o._x0)+(inter[0].y()-o._y0)*(inter[0].y()-o._y0)));
    } else if(rho==0) {
      inter.resize(1);
      inter[0].setY( -b/a);
      inter[0].setX(B-A*inter[0].y());
    }
  } else {
    // Prefer working with ys=(C-A*xs)/B transformed into ys=B-A*xs, with B=C/A and A=B/A
    D=1.0/B;
    A=A*D;
    B=C*D;
    a=1.0+A*A;
    b=2*A*_c.y()-2*A*B-2*_c.x();
    c=x02+B*B-2*_c.y()*B+y02-r02;
    rho=b*b-4*a*c;
    if(rho>0) {
      srho=::sqrt(rho);
      inter.resize(2);
      inter[0].setX((-b+srho)/(2.0*a));
      inter[1].setX((-b-srho)/(2.0*a));
      inter[0].setY(B-A*inter[0].x());
      inter[1].setY(B-A*inter[1].x());
    } else if(rho==0) {
      inter.resize(1);
      inter[0].setX( -b/a);
      inter[0].setY(B-A*inter[0].x());
    }
  }
  return inter;
}

/*!
  For convenience returns only one solution.
  If two solution are found only the closest to \a disc is returned.
  If no solution is found, \a ok is set to false.
*/
Point2D Circle::intersect(const Circle& o, bool& ok, const Circle& disc) const
{
  TRACE;
  QVector<Point2D> solutions=intersect(o);
  switch(solutions.count()) {
  case 1:
    ok=true;
    return solutions[0];
  case 2:
    ok=true;
    if(disc.distanceTo(solutions[0])<disc.distanceTo(solutions[1]))
      return solutions[0];
    else
      return solutions[1];
  default:
    ok=false;
    return Point2D();
  }
}

} // namespace QGpCoreMath
