/* This file is part of the Gudhi Library. The Gudhi library 
 *    (Geometric Understanding in Higher Dimensions) is a generic C++ 
 *    library for computational topology.
 *
 *    Author(s):       David Salinas
 *
 *    Copyright (C) 2014  INRIA Sophia Antipolis-M�diterran�e (France)
 *
 *    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/>.
 * 
 */

#ifndef GARLAND_HECKBERT_ERROR_QUADRIC_H_
#define GARLAND_HECKBERT_ERROR_QUADRIC_H_

#include <boost/optional/optional.hpp>

#include <vector>
#include <utility>

template <typename Point> class Error_quadric {
 private:
  double coeff[10];

 public:
  Error_quadric() {
    clear();
  }

  /**
   * Quadric corresponding to the L2 distance to the plane.
   *
   * According to the notation of Garland Heckbert, they
   * denote a quadric symetric matrix as :
   * Q = [ q11 q12 q13 q14]
   *     [ q12 q22 q23 q24]
   *     [ q13 q23 q33 q34]
   *     [ q14 q24 q34 q44]
   *
   * which is represented by a vector with 10 elts that
   * are denoted ci for clarity with :
   * Q = [ c0  c1  c2  c3 ]
   *     [ c1  c4  c5  c6 ]
   *     [ c2  c5  c7  c8 ]
   *     [ c3  c6  c8  c9 ]
   *
   * The constructor return the quadrics that represents
   * the squared distance to the plane defined by triangle p0,p1,p2
   * times the area of triangle p0,p1,p2.
   */
  Error_quadric(const Point & p0, const Point & p1, const Point & p2) {
    Point normal(unit_normal(p0, p1, p2));
    double a = normal[0];
    double b = normal[1];
    double c = normal[2];
    double d = -a * p0[0] - b * p0[1] - c * p0[2];
    coeff[0] = a*a;
    coeff[1] = a*b;
    coeff[2] = a*c;
    coeff[3] = a*d;
    coeff[4] = b*b;
    coeff[5] = b*c;
    coeff[6] = b*d;
    coeff[7] = c*c;
    coeff[8] = c*d;
    coeff[9] = d*d;

    double area_p0p1p2 = std::sqrt(squared_area(p0, p1, p2));
    for (auto& x : coeff)
      x *= area_p0p1p2;
  }

  inline double squared_area(const Point& p0, const Point& p1, const Point& p2) {
    // if (x1,x2,x3) = p1-p0 and (y1,y2,y3) = p2-p0
    // then the squared area is = (u^2+v^2+w^2)/4
    // with: u = x2 * y3 - x3 * y2;
    //       v = x3 * y1 - x1 * y3;
    //       w = x1 * y2 - x2 * y1;
    Point p0p1(p1 - p0);
    Point p0p2(p2 - p0);
    double A = p0p1[1] * p0p2[2] - p0p1[2] * p0p2[1];
    double B = p0p1[2] * p0p2[0] - p0p1[0] * p0p2[2];
    double C = p0p1[0] * p0p2[1] - p0p1[1] * p0p2[0];
    return 1. / 4. * (A * A + B * B + C * C);
  }

  void clear() {
    for (auto& x : coeff)
      x = 0;
  }

  Error_quadric& operator+=(const Error_quadric& other) {
    if (this != &other) {
      for (int i = 0; i < 10; ++i)
        coeff[i] += other.coeff[i];
    }
    return *this;
  }

  /**
   * @return The quadric quost defined by the scalar product v^T Q v where Q is the quadratic form of Garland/Heckbert
   */
  inline double cost(const Point& point) const {
    double cost =
        coeff[0] * point.x() * point.x() + coeff[4] * point.y() * point.y() + coeff[7] * point.z() * point.z()
        + 2 * (coeff[1] * point.x() * point.y() + coeff[5] * point.y() * point.z() + coeff[2] * point.z() * point.x())
        + 2 * (coeff[3] * point.x() + coeff[6] * point.y() + coeff[8] * point.z())
        + coeff[9];
    if (cost < 0) {
      return 0;
    } else {
      return cost;
    }
  }

  inline double grad_determinant() const {
    return
    coeff[0] * coeff[4] * coeff[7]
        - coeff[0] * coeff[5] * coeff[5]
        - coeff[1] * coeff[1] * coeff[7]
        + 2 * coeff[1] * coeff[5] * coeff[2]
        - coeff[4] * coeff[2] * coeff[2];
  }

  /**
   * Return the point such that it minimizes the gradient of the quadric.
   * Det must be passed with the determinant value of the gradient (should be non zero).
   */
  inline Point solve_linear_gradient(double det) const {
    return Point({
                 (-coeff[1] * coeff[5] * coeff[8] + coeff[1] * coeff[7] * coeff[6] + coeff[2] * coeff[8] * coeff[4] -
                     coeff[2] * coeff[5] * coeff[6] - coeff[3] * coeff[4] * coeff[7] + coeff[3] * coeff[5] * coeff[5])
                     / det,
                 (coeff[0] * coeff[5] * coeff[8] - coeff[0] * coeff[7] * coeff[6] - coeff[5] * coeff[2] * coeff[3] -
                     coeff[1] * coeff[2] * coeff[8] + coeff[6] * coeff[2] * coeff[2] + coeff[1] * coeff[3] * coeff[7])
                     / det,
                 (-coeff[8] * coeff[0] * coeff[4] + coeff[8] * coeff[1] * coeff[1] + coeff[2] * coeff[3] * coeff[4] +
                     coeff[5] * coeff[0] * coeff[6] - coeff[5] * coeff[1] * coeff[3] - coeff[1] * coeff[2] * coeff[6])
                     / det
    });
  }

  /**
   * returns the point that minimizes the quadric.
   * It inverses the quadric if its determinant is higher that a given threshold .
   * If the determinant is lower than this value the returned value is uninitialized.
   */
  boost::optional<Point> min_cost(double scale = 1) const {
    // const double min_determinant = 1e-4 * scale*scale;
    const double min_determinant = 1e-5;
    boost::optional<Point> pt_res;
    double det = grad_determinant();
    if (std::abs(det) > min_determinant)
      pt_res = solve_linear_gradient(det);
    return pt_res;
  }

  friend std::ostream& operator<<(std::ostream& stream, const Error_quadric& quadric) {
    stream << "\n[ " << quadric.coeff[0] << "," << quadric.coeff[1] << "," << quadric.coeff[2] << "," <<
        quadric.coeff[3] << ";\n";
    stream << " " << quadric.coeff[1] << "," << quadric.coeff[4] << "," << quadric.coeff[5] << "," <<
        quadric.coeff[6] << ";\n";
    stream << " " << quadric.coeff[2] << "," << quadric.coeff[5] << "," << quadric.coeff[7] << "," <<
        quadric.coeff[8] << ";\n";
    stream << " " << quadric.coeff[3] << "," << quadric.coeff[6] << "," << quadric.coeff[8] << "," <<
        quadric.coeff[9] << "]";
    return stream;
  }
};

#endif  // GARLAND_HECKBERT_ERROR_QUADRIC_H_