//    Copright (C) 1999-2021, Bernd Gaertner
//    November 12, 2021
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.

//    This program 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.

//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see <http://www.gnu.org/licenses/>.
//
//    Contact:
//    --------
//    Bernd Gaertner
//    Institute of Theoretical Computer Science
//    ETH Zuerich
//    CAB G31.1
//    CH-8092 Zuerich, Switzerland
//    http://www.inf.ethz.ch/personal/gaertner

#include <algorithm>
#include <cassert>
#include <ctime>
#include <limits>
#include <list>

namespace Miniball {

// Global Functions
// ================
template <typename NT>
inline NT mb_sqr(NT r) {
  return r * r;
}

// Functors
// ========

// functor to map a point iterator to the corresponding coordinate iterator;
// generic version for points whose coordinate containers have begin()
template <typename Pit_, typename Cit_>
struct CoordAccessor {
  typedef Pit_ Pit;
  typedef Cit_ Cit;
  inline Cit operator()(Pit it) const { return (*it).begin(); }
};

// partial specialization for points whose coordinate containers are arrays
template <typename Pit_, typename Cit_>
struct CoordAccessor<Pit_, Cit_*> {
  typedef Pit_ Pit;
  typedef Cit_* Cit;
  inline Cit operator()(Pit it) const { return *it; }
};

// Class Declaration
// =================

template <typename CoordAccessor>
class Miniball {
 private:
  // types
  // The iterator type to go through the input points
  typedef typename CoordAccessor::Pit Pit;
  // The iterator type to go through the coordinates of a single point.
  typedef typename CoordAccessor::Cit Cit;
  // The coordinate type
  typedef typename std::iterator_traits<Cit>::value_type NT;
  // The iterator to go through the support points
  typedef typename std::list<Pit>::iterator Sit;

  // data members...
  const int d;  // dimension
  Pit points_begin;
  Pit points_end;
  CoordAccessor coord_accessor;
  double time;
  const NT nt0;  // NT(0)

  //...for the algorithms
  std::list<Pit> L;
  Sit support_end;
  int fsize;  // number of forced points
  int ssize;  // number of support points

  // ...for the ball updates
  NT* current_c;
  NT current_sqr_r;
  NT** c;
  NT* sqr_r;

  // helper arrays
  NT* q0;
  NT* z;
  NT* f;
  NT** v;
  NT** a;

  // by how much do we allow points outside?
  NT default_tol;

 public:
  // The iterator type to go through the support points
  typedef typename std::list<Pit>::const_iterator SupportPointIterator;

  // PRE:  [begin, end) is a nonempty range
  // POST: computes the smallest enclosing ball of the points in the range
  //       [begin, end); the functor a maps a point iterator to an iterator
  //       through the d coordinates of the point
  Miniball(int d_, Pit begin, Pit end, CoordAccessor ca = CoordAccessor());

  // POST: returns a pointer to the first element of an array that holds
  //       the d coordinates of the center of the computed ball
  const NT* center() const;

  // POST: returns the squared radius of the computed ball
  NT squared_radius() const;

  // POST: returns the number of support points of the computed ball;
  //       the support points form a minimal set with the same smallest
  //       enclosing ball as the input set; in particular, the support
  //       points are on the boundary of the computed ball, and their
  //       number is at most d+1
  int nr_support_points() const;

  // POST: returns an iterator to the first support point
  SupportPointIterator support_points_begin() const;

  // POST: returns a past-the-end iterator for the range of support points
  SupportPointIterator support_points_end() const;

  // POST: returns the maximum excess of any input point w.r.t. the computed
  //       ball, divided by the squared radius of the computed ball. The
  //       excess of a point is the difference between its squared distance
  //       from the center and the squared radius; Ideally, the return value
  //       is 0. subopt is set to the absolute value of the most negative
  //       coefficient in the affine combination of the support points that
  //       yields the center. Ideally, this is a convex combination, and there
  //       is no negative coefficient in which case subopt is set to 0.
  NT relative_error(NT& subopt) const;

  // POST: return true if the relative error is at most tol, and the
  //       suboptimality is 0; the default tolerance is 10 times the
  //       coordinate type's machine epsilon
  bool is_valid() const;

  // POST: returns the time in seconds taken by the constructor call for
  //       computing the smallest enclosing ball
  double get_time() const;

  // POST: deletes dynamically allocated arrays
  ~Miniball();

 private:
  void mtf_mb(Sit n);
  void mtf_move_to_front(Sit j);
  void pivot_mb(Pit n);
  void pivot_move_to_front(Pit j);
  NT excess(Pit pit) const;
  void pop();
  bool push(Pit pit);
  NT suboptimality() const;
  void create_arrays();
  void delete_arrays();
};

// Class Definition
// ================
template <typename CoordAccessor>
Miniball<CoordAccessor>::Miniball(int d_, Pit begin, Pit end, CoordAccessor ca)
    : d(d_),
      points_begin(begin),
      points_end(end),
      coord_accessor(ca),
      time(clock()),
      nt0(NT(0)),
      L(),
      support_end(L.begin()),
      fsize(0),
      ssize(0),
      current_c(NULL),
      current_sqr_r(NT(-1)),
      c(NULL),
      sqr_r(NULL),
      q0(NULL),
      z(NULL),
      f(NULL),
      v(NULL),
      a(NULL),
      default_tol(NT(10) * std::numeric_limits<NT>::epsilon()) {
  assert(points_begin != points_end);
  create_arrays();

  // set initial center
  for (int j = 0; j < d; ++j) c[0][j] = nt0;
  current_c = c[0];

  // compute miniball
  pivot_mb(points_end);

  // update time
  time = (clock() - time) / CLOCKS_PER_SEC;
}

template <typename CoordAccessor>
Miniball<CoordAccessor>::~Miniball() {
  delete_arrays();
}

template <typename CoordAccessor>
void Miniball<CoordAccessor>::create_arrays() {
  c = new NT*[d + 1];
  v = new NT*[d + 1];
  a = new NT*[d + 1];
  for (int i = 0; i < d + 1; ++i) {
    c[i] = new NT[d];
    v[i] = new NT[d];
    a[i] = new NT[d];
  }
  sqr_r = new NT[d + 1];
  q0 = new NT[d];
  z = new NT[d + 1];
  f = new NT[d + 1];
}

template <typename CoordAccessor>
void Miniball<CoordAccessor>::delete_arrays() {
  delete[] f;
  delete[] z;
  delete[] q0;
  delete[] sqr_r;
  for (int i = 0; i < d + 1; ++i) {
    delete[] a[i];
    delete[] v[i];
    delete[] c[i];
  }
  delete[] a;
  delete[] v;
  delete[] c;
}

template <typename CoordAccessor>
const typename Miniball<CoordAccessor>::NT* Miniball<CoordAccessor>::center()
    const {
  return current_c;
}

template <typename CoordAccessor>
typename Miniball<CoordAccessor>::NT Miniball<CoordAccessor>::squared_radius()
    const {
  return current_sqr_r;
}

template <typename CoordAccessor>
int Miniball<CoordAccessor>::nr_support_points() const {
  assert(ssize < d + 2);
  return ssize;
}

template <typename CoordAccessor>
typename Miniball<CoordAccessor>::SupportPointIterator
Miniball<CoordAccessor>::support_points_begin() const {
  return L.begin();
}

template <typename CoordAccessor>
typename Miniball<CoordAccessor>::SupportPointIterator
Miniball<CoordAccessor>::support_points_end() const {
  return support_end;
}

template <typename CoordAccessor>
typename Miniball<CoordAccessor>::NT Miniball<CoordAccessor>::relative_error(
    NT& subopt) const {
  NT e, max_e = nt0;
  // compute maximum absolute excess of support points
  for (SupportPointIterator it = support_points_begin();
       it != support_points_end(); ++it) {
    e = excess(*it);
    if (e < nt0) e = -e;
    if (e > max_e) {
      max_e = e;
    }
  }
  // compute maximum excess of any point
  for (Pit i = points_begin; i != points_end; ++i)
    if ((e = excess(i)) > max_e) max_e = e;

  subopt = suboptimality();
  assert(current_sqr_r > nt0 || max_e == nt0);
  return (current_sqr_r == nt0 ? nt0 : max_e / current_sqr_r);
}

template <typename CoordAccessor>
bool Miniball<CoordAccessor>::is_valid() const {
  NT suboptimality;
  return ((relative_error(suboptimality) <= default_tol) &&
          (suboptimality == 0));
}

template <typename CoordAccessor>
double Miniball<CoordAccessor>::get_time() const {
  return time;
}

template <typename CoordAccessor>
void Miniball<CoordAccessor>::mtf_mb(Sit n) {
  // Algorithm 1: mtf_mb (L_{n-1}, B), where L_{n-1} = [L.begin, n)
  // B: the set of forced points, defining the current ball
  // S: the superset of support points computed by the algorithm
  // --------------------------------------------------------------
  // from B. Gaertner, Fast and Robust Smallest Enclosing Balls, ESA 1999,
  // http://www.inf.ethz.ch/personal/gaertner/texts/own_work/esa99_final.pdf

  //   PRE: B = S
  assert(fsize == ssize);

  support_end = L.begin();
  if ((fsize) == d + 1) return;

  // incremental construction
  for (Sit i = L.begin(); i != n;) {
    // INV: (support_end - L.begin() == |S|-|B|)
    assert(std::distance(L.begin(), support_end) == ssize - fsize);

    Sit j = i++;
    if (excess(*j) > nt0)
      if (push(*j)) {  // B := B + p_i
        mtf_mb(j);     // mtf_mb (L_{i-1}, B + p_i)
        pop();         // B := B - p_i
        mtf_move_to_front(j);
      }
  }
  // POST: the range [L.begin(), support_end) stores the set S\B
}

template <typename CoordAccessor>
void Miniball<CoordAccessor>::mtf_move_to_front(Sit j) {
  if (support_end == j) support_end++;
  L.splice(L.begin(), L, j);
}

template <typename CoordAccessor>
void Miniball<CoordAccessor>::pivot_mb(Pit n) {
  // Algorithm 2: pivot_mb (L_{n-1}), where L_{n-1} = [L.begin, n)
  // --------------------------------------------------------------
  // from B. Gaertner, Fast and Robust Smallest Enclosing Balls, ESA 1999,
  // http://www.inf.ethz.ch/personal/gaertner/texts/own_work/esa99_final.pdf
  const NT* c;
  Pit pivot, k;
  NT e, max_e, sqr_r;
  Cit p;
  unsigned int loops_without_progress = 0;
  NT best_sqr_r = current_sqr_r;
  do {
    sqr_r = current_sqr_r;

    pivot = points_begin;
    max_e = nt0;
    for (k = points_begin; k != n; ++k) {
      p = coord_accessor(k);
      e = -sqr_r;
      c = current_c;
      for (int j = 0; j < d; ++j) e += mb_sqr<NT>(*p++ - *c++);
      if (e > max_e) {
        max_e = e;
        pivot = k;
      }
    }

    if (sqr_r < nt0 || max_e > nt0) {
      // check if the pivot is already contained in the support set
      if (std::find(L.begin(), support_end, pivot) == support_end) {
        assert(fsize == 0);
        if (push(pivot)) {
          mtf_mb(support_end);
          pop();
          pivot_move_to_front(pivot);
        }
      }
    }
    if (best_sqr_r < current_sqr_r) {
      best_sqr_r = current_sqr_r;
      loops_without_progress = 0;
    } else
      ++loops_without_progress;
  } while (loops_without_progress < 2);
}

template <typename CoordAccessor>
void Miniball<CoordAccessor>::pivot_move_to_front(Pit j) {
  L.push_front(j);
  if (std::distance(L.begin(), support_end) == d + 2) support_end--;
}

template <typename CoordAccessor>
inline typename Miniball<CoordAccessor>::NT Miniball<CoordAccessor>::excess(
    Pit pit) const {
  Cit p = coord_accessor(pit);
  NT e = -current_sqr_r;
  NT* c = current_c;
  for (int k = 0; k < d; ++k) {
    e += mb_sqr<NT>(*p++ - *c++);
  }
  return e;
}

template <typename CoordAccessor>
void Miniball<CoordAccessor>::pop() {
  --fsize;
}

template <typename CoordAccessor>
bool Miniball<CoordAccessor>::push(Pit pit) {
  int i, j;
  NT eps = mb_sqr<NT>(std::numeric_limits<NT>::epsilon());

  Cit cit = coord_accessor(pit);
  Cit p = cit;

  if (fsize == 0) {
    for (i = 0; i < d; ++i) q0[i] = *p++;
    for (i = 0; i < d; ++i) c[0][i] = q0[i];
    sqr_r[0] = nt0;
  } else {
    // set v_fsize to Q_fsize
    for (i = 0; i < d; ++i)
      // v[fsize][i] = p[i]-q0[i];
      v[fsize][i] = *p++ - q0[i];

    // compute the a_{fsize,i}, i< fsize
    for (i = 1; i < fsize; ++i) {
      a[fsize][i] = nt0;
      for (j = 0; j < d; ++j) a[fsize][i] += v[i][j] * v[fsize][j];
      a[fsize][i] *= (2 / z[i]);
    }

    // update v_fsize to Q_fsize-\bar{Q}_fsize
    for (i = 1; i < fsize; ++i) {
      for (j = 0; j < d; ++j) v[fsize][j] -= a[fsize][i] * v[i][j];
    }

    // compute z_fsize
    z[fsize] = nt0;
    for (j = 0; j < d; ++j) z[fsize] += mb_sqr<NT>(v[fsize][j]);
    z[fsize] *= 2;

    // reject push if z_fsize too small
    if (z[fsize] < eps * current_sqr_r) {
      return false;
    }

    // update c, sqr_r
    p = cit;
    NT e = -sqr_r[fsize - 1];
    for (i = 0; i < d; ++i) e += mb_sqr<NT>(*p++ - c[fsize - 1][i]);
    f[fsize] = e / z[fsize];

    for (i = 0; i < d; ++i)
      c[fsize][i] = c[fsize - 1][i] + f[fsize] * v[fsize][i];
    sqr_r[fsize] = sqr_r[fsize - 1] + e * f[fsize] / 2;
  }
  current_c = c[fsize];
  current_sqr_r = sqr_r[fsize];
  ssize = ++fsize;
  return true;
}

template <typename CoordAccessor>
typename Miniball<CoordAccessor>::NT Miniball<CoordAccessor>::suboptimality()
    const {
  NT* l = new NT[d + 1];
  NT min_l = nt0;
  l[0] = NT(1);
  for (int i = ssize - 1; i > 0; --i) {
    l[i] = f[i];
    for (int k = ssize - 1; k > i; --k) l[i] -= a[k][i] * l[k];
    if (l[i] < min_l) min_l = l[i];
    l[0] -= l[i];
  }
  if (l[0] < min_l) min_l = l[0];
  delete[] l;
  if (min_l < nt0) return -min_l;
  return nt0;
}

}  // end Namespace Miniball