/*
 *
 * This file is part of Tulip (http://tulip.labri.fr)
 *
 * Authors: David Auber and the Tulip development Team
 * from LaBRI, University of Bordeaux
 *
 * Tulip 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 3
 * of the License, or (at your option) any later version.
 *
 * Tulip 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 General Public License for more details.
 *
 */
#include <cstdlib>
#include <tulip/TlpTools.h>

template <typename Obj, typename OTYPE>
tlp::Circle<Obj, OTYPE> &tlp::Circle<Obj, OTYPE>::merge(const tlp::Circle<Obj, OTYPE> &c) {
  Vector<Obj, 2, OTYPE> p1(*this);
  Vector<Obj, 2, OTYPE> p2(c);
  Vector<Obj, 2, OTYPE> c12 = p2 - p1;
  double norm = c12.norm();

  if (norm < 0.0000001) {
    if (radius < c.radius)
      return (*this) = c;
    else
      return *this;
  }

  double dx = c12[0] / norm;
  double dy = c12[1] / norm;
  p1[0] -= dx * radius;
  p1[1] -= dy * radius;
  p2[0] += dx * c.radius;
  p2[1] += dy * c.radius;
  Obj tmpDist = p1.dist(p2) / 2.;

  if ((tmpDist < radius) || (tmpDist < c.radius)) {
    if (radius > c.radius)
      return *this;
    else
      return (*this) = c;
  } else {
    p1 += p2;
    p1 /= 2.;
    (*this)[0] = p1[0];
    (*this)[1] = p1[1];
    radius = tmpDist;
    return *this;
  }
}

template <typename Obj, typename OTYPE>
bool tlp::Circle<Obj, OTYPE>::isIncludeIn(const tlp::Circle<Obj, OTYPE> &c) const {
  Vector<Obj, 2, OTYPE> dir = c - *this;
  return (dir.norm() + radius) <= c.radius;
}

template <typename Obj, typename OTYPE>
tlp::Circle<Obj, OTYPE> tlp::enclosingCircle(const tlp::Circle<Obj, OTYPE> &c1,
                                             const tlp::Circle<Obj, OTYPE> &c2) {
  Vector<Obj, 2, OTYPE> dir = c2 - c1;
  Obj n = dir.norm();

  if (n == 0)
    return Circle<Obj, OTYPE>(c1, std::max(c1.radius, c2.radius));

  dir /= n;
  Vector<Obj, 2, OTYPE> ext1 = c1 - dir * c1.radius;
  Vector<Obj, 2, OTYPE> ext2 = c2 + dir * c2.radius;
  return Circle<Obj, OTYPE>((ext1 + ext2) / Obj(2), (ext2 - ext1).norm() / Obj(2));
}

template <typename Obj, typename OTYPE>
tlp::Circle<Obj, OTYPE>
tlp::lazyEnclosingCircle(const std::vector<tlp::Circle<Obj, OTYPE>> &circles) {
  // compute bounding box of a
  tlp::Vector<Obj, 4, OTYPE> boundingBox;
  //  for (int i=0;i<4;++i) boundingBox[i]=0;
  typename std::vector<tlp::Circle<Obj, OTYPE>>::const_iterator it = circles.begin();
  boundingBox[0] = (*it)[0] - (*it).radius;
  boundingBox[1] = (*it)[1] - (*it).radius;
  boundingBox[2] = (*it)[0] + (*it).radius;
  boundingBox[3] = (*it)[1] + (*it).radius;
  ++it;

  for (; it != circles.end(); ++it) {
    boundingBox[0] = std::min(boundingBox[0], ((*it)[0] - (*it).radius));
    boundingBox[1] = std::min(boundingBox[1], ((*it)[1] - (*it).radius));
    boundingBox[2] = std::max(boundingBox[2], ((*it)[0] + (*it).radius));
    boundingBox[3] = std::max(boundingBox[3], ((*it)[1] + (*it).radius));
  }

  tlp::Vector<Obj, 2, OTYPE> center;
  center[0] = (boundingBox[0] + boundingBox[2]) / 2.;
  center[1] = (boundingBox[1] + boundingBox[3]) / 2.;
  Obj radius =
      std::max((boundingBox[2] - boundingBox[0]) / 2., (boundingBox[3] - boundingBox[1]) / 2.);
  tlp::Circle<Obj, OTYPE> result(center, radius);

  // compute circle hull
  for (typename std::vector<tlp::Circle<Obj, OTYPE>>::const_iterator it = circles.begin();
       it != circles.end(); ++it)
    result.merge(*it);

  return result;
}

template <typename Obj, typename OTYPE>
tlp::Circle<Obj, OTYPE> tlp::enclosingCircle(const std::vector<tlp::Circle<Obj, OTYPE>> &circles) {
  class OptimumCircleHull {
    const std::vector<tlp::Circle<Obj, OTYPE>> *circles;
    std::vector<unsigned> enclosedCircles;
    unsigned first, last;
    unsigned b1, b2;
    tlp::Circle<Obj, OTYPE> result;

    static tlp::Circle<Obj, OTYPE> enclosingCircle(const tlp::Circle<Obj, OTYPE> &c1,
                                                   const tlp::Circle<Obj, OTYPE> &c2,
                                                   const tlp::Circle<Obj, OTYPE> &c3) {
      Obj a1 = c1[0];
      Obj b1 = c1[1];
      Obj r1 = c1.radius;
      Obj a2 = c2[0];
      Obj b2 = c2[1];
      Obj r2 = c2.radius;
      Obj a3 = c3[0];
      Obj b3 = c3[1];
      Obj r3 = c3.radius;
      Obj b = -b1 * r2 * a1 * a1 * b3 + a3 * a3 * r2 * r2 * r2 + b2 * b2 * r3 * r3 * r3 +
              b1 * b1 * r3 * r3 * r3 + b1 * b1 * r2 * r2 * r2 + r1 * r1 * r1 * b3 * b3 +
              r1 * r1 * r1 * b2 * b2 + r2 * r2 * r2 * b3 * b3 + a1 * a1 * r3 * r3 * r3 +
              a2 * a2 * r1 * r1 * r1 + a2 * a2 * r3 * r3 * r3 + a1 * a1 * r2 * r2 * r2 +
              a3 * a3 * r1 * r1 * r1 - b2 * b2 * r3 * a3 * a3 - b2 * b2 * r3 * b3 * b3 -
              2 * b2 * r3 * r3 * r3 * b1 - b1 * b1 * r3 * a3 * a3 - b1 * b1 * r3 * b3 * b3 -
              b1 * b1 * r2 * a2 * a2 + 2 * b1 * b1 * r2 * b3 * b3 - b1 * b1 * r2 * r3 * r3 -
              r1 * b3 * b3 * b3 * b2 + r1 * b3 * b3 * b3 * b1 + 2 * r1 * b2 * b2 * b3 * b3 -
              r1 * b2 * b2 * r3 * r3 + r2 * b3 * b3 * b3 * b2 - r2 * b3 * b3 * b3 * b1 -
              b2 * b2 * b2 * r3 * b1 + 2 * b2 * b2 * r3 * b1 * b1 + b2 * b2 * b2 * r3 * b3 -
              b2 * b2 * r3 * r1 * r1 + b1 * b1 * b1 * r3 * b3 - b1 * b1 * b1 * r3 * b2 -
              b1 * b1 * r3 * r2 * r2 - b1 * b1 * r2 * b2 * b2 - b1 * b1 * b1 * r2 * b3 +
              b1 * b1 * b1 * r2 * b2 - 2 * b1 * r2 * r2 * r2 * b3 - r1 * b3 * b3 * b1 * b1 -
              2 * r1 * r1 * r1 * b3 * b2 - r1 * b3 * b3 * r2 * r2 - r1 * b3 * b3 * a1 * a1 +
              r1 * b2 * b2 * b2 * b1 - r1 * b2 * b2 * b1 * b1 - r1 * b2 * b2 * b2 * b3 -
              r1 * b2 * b2 * a1 * a1 - r2 * b3 * b3 * a2 * a2 - r2 * b3 * b3 * b2 * b2 -
              r2 * b3 * b3 * r1 * r1 - b2 * r3 * b1 * b1 * b3 + b2 * r3 * a2 * a2 * b3 +
              b2 * r3 * r1 * r1 * b3 - b2 * r3 * r2 * r2 * b3 + b2 * r3 * a1 * a1 * b3 -
              2 * b2 * r3 * a1 * a2 * b3 + a1 * a3 * b2 * b2 * r3 - 2 * a1 * a3 * b2 * b1 * r3 -
              2 * a1 * a3 * b2 * b1 * r2 - 2 * a1 * a3 * b2 * r1 * b3 + a1 * a3 * b2 * b2 * r1 -
              2 * a1 * a3 * b2 * r2 * b3 - b2 * r3 * b1 * a2 * a2 + 2 * b2 * r3 * b1 * a3 * a3 +
              2 * b2 * r3 * b1 * b3 * b3 + b1 * r2 * b2 * a3 * a3 - b1 * r2 * b2 * b3 * b3 +
              b1 * r2 * b2 * r3 * r3 + r1 * b3 * b1 * a2 * a2 - r1 * b3 * b2 * a3 * a3 +
              r1 * b3 * b2 * r3 * r3 + r1 * b3 * b1 * a3 * a3 - r1 * b3 * b1 * r3 * r3 +
              r1 * b2 * b1 * a2 * a2 + r1 * b2 * b1 * a3 * a3 - r1 * b2 * b1 * b3 * b3 +
              r1 * b2 * b1 * r3 * r3 + 2 * r2 * b3 * b1 * a2 * a2 + r2 * b3 * b2 * a3 * a3 -
              r2 * b3 * b2 * r3 * r3 - r2 * b3 * b1 * a3 * a3 + r2 * b3 * b1 * r3 * r3 +
              b2 * r3 * b1 * r2 * r2 + 4 * b2 * r3 * a1 * a2 * b1 + b1 * r3 * a2 * a2 * b3 -
              b1 * r3 * b2 * b2 * b3 - b1 * r3 * r1 * r1 * b3 + b1 * r3 * r1 * r1 * b2 +
              b1 * r3 * r2 * r2 * b3 + b1 * r3 * a1 * a1 * b3 - b1 * r3 * a1 * a1 * b2 -
              2 * b1 * r3 * a1 * a2 * b3 - b1 * b1 * r3 * a1 * a2 + b1 * b1 * r3 * a1 * a3 +
              2 * b1 * r2 * b2 * b2 * b3 + b1 * r2 * r1 * r1 * b3 - b1 * r2 * r1 * r1 * b2 +
              b1 * r2 * a1 * a1 * b2 - 2 * b1 * r2 * a1 * a2 * b3 + b1 * b1 * r2 * a1 * a2 -
              b1 * b1 * r2 * a1 * a3 - r1 * b3 * b1 * b2 * b2 + 2 * r1 * b3 * b1 * b1 * b2 +
              2 * r1 * b3 * a1 * a1 * b2 + r1 * b3 * b1 * r2 * r2 + r1 * b3 * b3 * a1 * a2 -
              r1 * b2 * a2 * a2 * b3 + r1 * b2 * r2 * r2 * b3 - r1 * b2 * b1 * r2 * r2 -
              2 * r1 * b2 * a1 * a2 * b3 - r2 * b3 * b1 * b1 * b2 + r2 * b3 * r1 * r1 * b2 +
              r2 * b3 * a1 * a1 * b2 + r2 * b3 * b3 * a1 * a2 + 4 * r2 * b3 * a1 * a3 * b1 -
              a1 * a1 * a3 * a3 * r3 + 2 * a1 * a1 * a3 * a3 * r2 + a1 * a3 * a3 * a3 * r1 -
              a1 * a1 * b3 * b3 * r3 - a1 * b3 * b3 * a3 * r2 + a1 * r3 * r3 * a3 * r2 -
              a2 * a1 * a1 * a3 * r2 + a2 * b1 * b1 * a3 * r2 + a2 * b3 * b3 * a3 * r2 -
              a1 * a3 * a3 * a2 * r1 + 2 * a1 * a3 * a3 * a2 * r3 + 2 * a1 * b3 * b3 * a2 * r3 +
              a1 * b3 * b3 * a3 * r1 - 2 * a1 * r3 * r3 * r3 * a2 - a1 * a1 * r3 * r3 * r2 -
              a2 * a1 * a1 * a1 * r3 - a2 * a2 * a1 * a1 * r1 + 2 * a2 * a2 * a1 * a1 * r3 +
              a2 * a1 * a1 * a1 * r2 - a2 * a2 * b1 * b1 * r1 + 2 * a2 * a2 * a3 * a3 * r1 -
              a2 * a2 * a3 * a3 * r3 - a2 * a3 * a3 * a3 * r1 - a2 * a2 * b3 * b3 * r3 -
              a2 * a2 * r1 * r1 * r3 - 2 * a2 * r1 * r1 * r1 * a3 + a1 * r3 * r3 * a2 * r1 -
              a1 * r3 * r3 * a3 * r1 + 2 * a2 * a1 * a1 * a3 * r1 + 2 * a2 * b1 * b1 * a3 * r1 -
              a2 * a3 * a3 * a1 * r2 - a2 * b3 * b3 * a3 * r1 + a2 * r1 * r1 * a1 * r3 -
              a2 * r1 * r1 * a1 * r2 - a2 * a2 * r3 * r3 * r1 - a1 * a3 * a3 * a3 * r2 +
              a2 * a3 * a3 * a3 * r2 - a1 * a1 * r3 * r2 * r2 + a1 * a1 * a1 * r3 * a3 +
              a2 * a2 * a2 * r1 * a1 - a2 * a2 * a2 * r1 * a3 - a2 * a2 * a2 * r3 * a1 +
              a2 * a2 * a2 * r3 * a3 - a1 * a1 * r2 * a2 * a2 - a1 * a1 * r2 * b2 * b2 -
              a1 * a1 * a1 * r2 * a3 - 2 * a1 * r2 * r2 * r2 * a3 - a3 * a3 * r1 * a1 * a1 -
              a3 * a3 * r1 * b1 * b1 - a3 * a3 * r1 * r2 * r2 - a3 * a3 * r2 * a2 * a2 -
              a3 * a3 * r2 * b2 * b2 - a3 * a3 * r2 * r1 * r1 + a2 * r3 * r3 * a1 * r2 +
              a2 * r3 * r3 * a3 * r1 - 2 * a3 * r2 * b1 * a2 * b3 + a2 * r1 * r1 * a3 * r2 -
              a2 * r3 * r3 * a3 * r2 - a1 * r3 * a3 * a2 * a2 - a1 * r3 * a3 * r1 * r1 +
              a1 * r3 * a3 * r2 * r2 + a2 * r1 * a1 * b2 * b2 - a2 * r1 * a1 * r2 * r2 -
              a2 * r1 * a3 * b2 * b2 + a2 * r1 * a3 * r2 * r2 - 2 * a2 * r1 * b1 * a3 * b2 -
              a2 * r3 * a1 * b2 * b2 + a2 * r3 * a1 * r2 * r2 - a2 * r3 * a3 * a1 * a1 +
              a2 * r3 * a3 * b1 * b1 + a2 * r3 * a3 * b2 * b2 + a2 * r3 * a3 * r1 * r1 -
              a2 * r3 * a3 * r2 * r2 - 2 * a2 * r3 * b1 * a3 * b2 + 2 * a1 * r2 * a3 * a2 * a2 +
              2 * a1 * r2 * a3 * b2 * b2 + a1 * r2 * a3 * r1 * r1 - a3 * r1 * a1 * a2 * a2 +
              a3 * r1 * a1 * r2 * r2 - 2 * a3 * r1 * b1 * a2 * b3 + 4 * r1 * a3 * b2 * a2 * b3;
      Obj tmp = a2 * b3 - a3 * b2 - a2 * b1 - a1 * b3 + a1 * b2 + a3 * b1;
      Obj d = sqrt((a2 * a2 + b2 * b2 - r2 * r2 - 2 * a2 * a3 + a3 * a3 - r3 * r3 - 2 * b3 * b2 +
                    b3 * b3 + 2 * r3 * r2) *
                   (b3 * b3 + b1 * b1 - r1 * r1 - r3 * r3 - 2 * b1 * b3 + 2 * r3 * r1 + a3 * a3 -
                    2 * a1 * a3 + a1 * a1) *
                   (a1 * a1 - 2 * a1 * a2 - r1 * r1 + b1 * b1 + b2 * b2 - r2 * r2 - 2 * b2 * b1 +
                    2 * r2 * r1 + a2 * a2) *
                   tmp * tmp);
      Obj v = d - b;

      if (v < 0)
        return tlp::Circle<Obj, OTYPE>(0, 0, 0);

      Obj aa = -2 * a3 * b2 * b2 * a1 - 2 * a2 * b3 * b3 * a1 - 2 * a1 * a1 * b3 * b2 +
               a3 * a3 * b2 * b2 + a2 * a2 * b3 * b3 - r1 * r1 * b3 * b3 - r1 * r1 * b2 * b2 +
               a2 * a2 * b1 * b1 + a3 * a3 * b1 * b1 - a2 * a2 * r1 * r1 - a2 * a2 * r3 * r3 -
               a3 * a3 * r2 * r2 - 2 * a3 * b2 * a2 * b3 + 2 * a3 * b2 * a2 * b1 +
               2 * a2 * b3 * a3 * b1 - 2 * b1 * a2 * a2 * b3 - a3 * a3 * r1 * r1 -
               2 * a3 * a3 * b2 * b1 + 2 * b1 * r3 * r3 * b2 + 2 * r1 * r1 * b3 * b2 +
               2 * b3 * b1 * r2 * r2 - 2 * a2 * b1 * b1 * a3 + 2 * a2 * a3 * r1 * r1 -
               b2 * b2 * r3 * r3 - b1 * b1 * r3 * r3 - b1 * b1 * r2 * r2 - r2 * r2 * b3 * b3 -
               a1 * a1 * r3 * r3 - a1 * a1 * r2 * r2 + a1 * a1 * b3 * b3 + a1 * a1 * b2 * b2 +
               2 * b2 * b2 * r3 * r1 + 2 * b1 * b1 * r3 * r2 + 2 * r1 * b3 * b3 * r2 -
               2 * b2 * r3 * b1 * r2 - 2 * b2 * r3 * r1 * b3 + 2 * b2 * r3 * r2 * b3 +
               2 * b1 * r3 * r1 * b3 - 2 * b1 * r3 * r1 * b2 - 2 * b1 * r3 * r2 * b3 -
               2 * b1 * r2 * r1 * b3 + 2 * b1 * r2 * r1 * b2 - 2 * r1 * b2 * r2 * b3 +
               2 * a1 * r3 * r3 * a2 + 2 * a1 * a1 * r3 * r2 + 2 * a2 * a2 * r1 * r3 +
               2 * a1 * r2 * r2 * a3 + 2 * a3 * a3 * r1 * r2 - 2 * a1 * r3 * a2 * r1 +
               2 * a1 * r3 * a3 * r1 + 2 * a2 * r1 * a1 * r2 - 2 * a2 * r3 * a1 * r2 -
               2 * a2 * r3 * a3 * r1 - 2 * a1 * r2 * a3 * r1 - 2 * a1 * r3 * a3 * r2 -
               2 * a2 * r1 * a3 * r2 + 2 * a2 * r3 * a3 * r2 + 2 * a2 * b3 * a1 * b2 +
               2 * a3 * b2 * a1 * b3 + 2 * a2 * b1 * a1 * b3 - 2 * a2 * b1 * a1 * b2 -
               2 * a1 * b3 * a3 * b1 + 2 * a1 * b2 * a3 * b1;
      Obj R = 0.5 * v / aa;
      Obj y = -0.5 *
              (-2 * a1 * R * r2 - 2 * a3 * R * r1 + 2 * a2 * R * r1 + 2 * a3 * R * r2 -
               2 * a2 * R * r3 + a1 * a3 * a3 + a1 * b3 * b3 - a1 * r3 * r3 + a2 * a1 * a1 +
               a2 * b1 * b1 + a1 * r2 * r2 + 2 * a1 * R * r3 - a3 * b1 * b1 + a3 * a2 * a2 +
               a3 * b2 * b2 + a3 * r1 * r1 - a3 * r2 * r2 - a2 * a3 * a3 - a2 * b3 * b3 -
               a2 * r1 * r1 + a2 * r3 * r3 - a1 * a2 * a2 - a1 * b2 * b2 - a3 * a1 * a1) /
              (a2 * b3 + a3 * b1 - a2 * b1 - a1 * b3 + a1 * b2 - a3 * b2);
      Obj x = 0.5 *
              (-a1 * a1 * b3 + a1 * a1 * b2 + 2 * R * r2 * b3 + b1 * a3 * a3 + b1 * b3 * b3 +
               2 * b1 * R * r3 - 2 * R * r1 * b3 + 2 * R * r1 * b2 + b1 * r2 * r2 - b2 * a3 * a3 -
               b2 * b3 * b3 + b2 * r3 * r3 - 2 * b2 * R * r3 - b1 * r3 * r3 - r2 * r2 * b3 +
               a2 * a2 * b3 - r1 * r1 * b2 - b1 * b2 * b2 + b1 * b1 * b2 - b1 * b1 * b3 -
               b1 * a2 * a2 + b2 * b2 * b3 + r1 * r1 * b3 - 2 * b1 * R * r2) /
              (a2 * b3 + a3 * b1 - a2 * b1 - a1 * b3 + a1 * b2 - a3 * b2);
      return tlp::Circle<Obj, OTYPE>(x, y, R);
    }

    void process2() {
      if (isEmpty()) {
        result = tlp::enclosingCircle((*circles)[b1], (*circles)[b2]);
      } else {
        unsigned selectedCircle = popBack();
        process2();

        if (!(*circles)[selectedCircle].isIncludeIn(result)) {
          result = enclosingCircle((*circles)[b1], (*circles)[b2], (*circles)[selectedCircle]);
          pushFront(selectedCircle);
        } else {
          pushBack(selectedCircle);
        }
      }
    }

    void process1() {
      if (isEmpty()) {
        result = (*circles)[b1];
      } else {
        unsigned selectedCircle = popBack();
        process1();

        if (!(*circles)[selectedCircle].isIncludeIn(result)) {
          b2 = selectedCircle;
          process2();
          pushFront(selectedCircle);
        } else {
          pushBack(selectedCircle);
        }
      }
    }
    void process0() {
      if (isEmpty()) {
        result = Circle<Obj, OTYPE>(0, 0, 0);
      } else {
        unsigned selectedCircle = popBack();
        process0();

        if (!(*circles)[selectedCircle].isIncludeIn(result)) {
          b1 = selectedCircle;
          process1();
          pushFront(selectedCircle);
        } else {
          pushBack(selectedCircle);
        }
      }
    }
    bool isEmpty() const {
      return first == (last + 1) % enclosedCircles.size();
    }
    void pushFront(unsigned c) {
      first = (first + enclosedCircles.size() - 1) % enclosedCircles.size();
      enclosedCircles[first] = c;
    }
    unsigned popBack() {
      unsigned result = enclosedCircles[last];
      last = (last + enclosedCircles.size() - 1) % enclosedCircles.size();
      return result;
    }
    void pushBack(unsigned c) {
      last = (last + 1) % enclosedCircles.size();
      enclosedCircles[last] = c;
    }

  public:
    OptimumCircleHull() : circles(nullptr), first(0), last(0), b1(0), b2(0) {}

    tlp::Circle<Obj, OTYPE> operator()(const std::vector<tlp::Circle<Obj, OTYPE>> &circlesSet) {
      circles = &circlesSet;
      enclosedCircles.resize(circlesSet.size() + 1);
      first = 0;
      last = circlesSet.size() - 1;

      for (unsigned i = 0; i < circlesSet.size(); ++i)
        enclosedCircles[i] = i;

      for (unsigned i = circlesSet.size(); i > 0;) {
        unsigned idx = tlp::randomUnsignedInteger(i - 1);
        --i;
        std::swap(enclosedCircles[idx], enclosedCircles[i]);
      }

      process0();
      return result;
    }
  };
  return OptimumCircleHull()(circles);
}

template <typename Obj, typename OTYPE>
std::ostream &tlp::operator<<(std::ostream &os, const tlp::Circle<Obj, OTYPE> &a) {
  os << "((" << a[0] << "," << a[1] << ")," << a.radius << ")";
  return os;
}
