//
// This file is part of MorphoGraphX - http://www.MorphoGraphX.org
// Copyright (C) 2012-2015 Richard S. Smith and collaborators.
//
// If you use MorphoGraphX in your work, please cite:
//   http://dx.doi.org/10.7554/eLife.05864
//
// MorphoGraphX is free software, and is licensed under under the terms of the 
// GNU General (GPL) Public License version 2.0, http://www.gnu.org/licenses.
//
#ifndef CURVATURE_H
#define CURVATURE_H

#include <Config.hpp>

#include <Geometry.hpp>
#include <Information.hpp>

#include <QtCore/QtCore>
#include <math.h>
#include <string>
#include <vector>
#include <CImg.h>

#define CIMat cimg_library::CImg<float>
#define CIMatL cimg_library::CImgList<float>

namespace mgx 
{
  class CurvatureMeasure 
  {
  public:
    virtual ~CurvatureMeasure() {
    }
    virtual float measure(float ev1, float ev2) = 0;
    virtual std::string get_type() = 0;
    virtual CurvatureMeasure* copy() = 0;
  };
  
  class GaussianCurvature : public CurvatureMeasure {
  public:
    float measure(float ev1, float ev2) {
      return ev1 * ev2;
    }
    std::string get_type() {
      return "gaussian";
    }
    virtual CurvatureMeasure* copy() {
      return new GaussianCurvature();
    }
  };
  
  class SumSquareCurvature : public CurvatureMeasure {
  public:
    float measure(float ev1, float ev2) {
      return (ev1 * ev1) + (ev2 * ev2);
    }
    std::string get_type() {
      return "sumsquare";
    }
    virtual CurvatureMeasure* copy() {
      return new SumSquareCurvature();
    }
  };
  
  class RootSumSquareCurvature : public CurvatureMeasure {
  public:
    float measure(float ev1, float ev2) {
      return sqrt((ev1 * ev1) + (ev2 * ev2));
    }
    std::string get_type() {
      return "rootsumsquare";
    }
    virtual CurvatureMeasure* copy() {
      return new RootSumSquareCurvature();
    }
  };
  
  class AverageCurvature : public CurvatureMeasure {
  public:
    float measure(float ev1, float ev2) {
      return (ev1 + ev2) / 2;
    }
    std::string get_type() {
      return "average";
    }
    virtual CurvatureMeasure* copy() {
      return new AverageCurvature();
    }
  };
  
  class MinimalCurvature : public CurvatureMeasure {
  public:
    float measure(float ev1, float ev2) {
      return std::min(ev1, ev2);
    }
    std::string get_type() {
      return "minimal";
    }
    virtual CurvatureMeasure* copy() {
      return new MinimalCurvature();
    }
  };
  
  class MaximalCurvature : public CurvatureMeasure {
  public:
    float measure(float ev1, float ev2) {
      return std::max(ev1, ev2);
    }
    std::string get_type() {
      return "maximal";
    }
    virtual CurvatureMeasure* copy() {
      return new MaximalCurvature();
    }
  };

  class MinimalAbsCurvature : public CurvatureMeasure {
  public:
    float measure(float ev1, float ev2) {
      return std::min(ev1, ev2);
    }
    std::string get_type() {
      return "minimal";
    }
    virtual CurvatureMeasure* copy() {
      return new MinimalAbsCurvature();
    }
  };
  
  class MaximalAbsCurvature : public CurvatureMeasure {
  public:
    float measure(float ev1, float ev2) {
      return std::max(ev1, ev2);
    }
    std::string get_type() {
      return "maximal";
    }
    virtual CurvatureMeasure* copy() {
      return new MaximalAbsCurvature();
    }
  }; 
  class AnisotropyCurvature : public CurvatureMeasure {
  public:
    float measure(float ev1, float ev2) {
      return ev1 / ev2;
    }
    std::string get_type() {
      return "anisotropy";
    }
    virtual CurvatureMeasure* copy() {
      return new AnisotropyCurvature();
    }
  };
  
  class SignedAverageAbsCurvature : public CurvatureMeasure {
  public:
    float measure(float ev1, float ev2) {
      return (ev1 * ev2 < 0 ? -1.0f : 1.0f) * (fabs(ev1) + fabs(ev2)) / 2.0f;
    }
    std::string get_type() {
      return "signedaverageabs";
    }
    virtual CurvatureMeasure* copy() {
      return new SignedAverageAbsCurvature();
    }
  };
  
  class Curvature {
  public:
    // default constructor
    Curvature() : W(2, 2), ev1(0), ev2(0)
    {
      cM = new AverageCurvature();
    }
  
    // convenience constructor
    Curvature(std::string s) : W(2, 2), ev1(0), ev2(0)
    {
      if(s == "gaussian")
        cM = new GaussianCurvature();
      else if(s == "average")
        cM = new AverageCurvature();
      else if(s == "minimal")
        cM = new MinimalCurvature();
      else if(s == "maximal")
        cM = new MaximalCurvature();
      else if(s == "sumsquare")
        cM = new SumSquareCurvature();
      else if(s == "rootsumsquare")
        cM = new RootSumSquareCurvature();
      else if(s == "signedaverageabs")
        cM = new SignedAverageAbsCurvature();
      else if(s == "anisotropy")
        cM = new AnisotropyCurvature();
      else
        cM = new AverageCurvature();
    }
  
    Curvature(const Curvature& copy) 
      : W(copy.W), b1(copy.b1), b2(copy.b2), b3(copy.b3), ed1(copy.ed1), ed2(copy.ed2), ev1(copy.ev1), ev2(copy.ev2), cM(0)
    {
      if(copy.cM)
        cM = copy.cM->copy();
    }
  
    ~Curvature() {
      delete cM;
    }
  
    // update the curvature object, first entry of pos and norm must be the position, surface normal of the point of
    // interest
    void update(const std::vector<Point3f>& pos, const std::vector<Point3f>& nrml)
    {
  
      try {
        if(nrml.size() > 0 && nrml.size() == pos.size()) {
  
          // Calculate local Coordinate system
          update_local_Basis(nrml[0]);
  
          // Set up Least Square Problem
          int cnt = pos.size();
          CIMat X = CIMat(7, 3 * (cnt - 1));
          CIMat b = CIMat(1, 3 * (cnt - 1));
          Point3f vpos = pos[0];
          int id = 0;
  
          for(int i = 1; i < cnt; i++) {
            // Transform into local coordinates
            Point3f vw = pos[i] - vpos;
            float w1 = vw * b1;
            float w2 = vw * b2;
            float w3 = vw * b3;
  
            // Transform into local coordinates
            float n1, n2, n3;
            n1 = nrml[i] * b1;
            n2 = nrml[i] * b2;
            n3 = nrml[i] * b3;
  
            X(0, id) = 0.5 * w1 * w1;
            X(1, id) = w1 * w2;
            X(2, id) = 0.5 * w2 * w2;
            X(3, id) = w1 * w1 * w1;
            X(4, id) = w1 * w1 * w2;
            X(5, id) = w1 * w2 * w2;
            X(6, id) = w2 * w2 * w2;
            b(id) = w3;
            id++;
  
            X(0, id) = w1;
            X(1, id) = w2;
            X(2, id) = 0;
            X(3, id) = 3 * w1 * w1;
            X(4, id) = 2 * w1 * w2;
            X(5, id) = w2 * w2;
            X(6, id) = 0;
            b(id) = -n1 / n3;
            id++;
  
            X(0, id) = 0;
            X(1, id) = w1;
            X(2, id) = w2;
            X(3, id) = 0;
            X(4, id) = w1 * w1;
            X(5, id) = 2 * w1 * w2;
            X(6, id) = 3 * w2 * w2;
            b(id) = -n2 / n3;
            id++;
          }
          CIMat Xt = X.get_transpose();
  
          CIMat Q = Xt * X;
  
          b = Xt * b;
  
          float det = Q.det();
          if(!isNan(det) && !(det == 0)) {
            // solve least-squares problem
            b.solve(Q);
  
            // update Weingarten-matrix
            W(0, 0) = b[0];
            W(1, 0) = W(0, 1) = b[1];
            W(1, 1) = b[2];
  
            // calculate eigenvalues and eigenvectors
            CIMatL bL = W.get_symmetric_eigen();
            ev1 = - bL(0)(0, 0);// ALR: bulges in direction of increasing Z should be positive
            ev2 = - bL(0)(1, 0);// Sorry Pierre, it makes more sense this way!
            ed1 = - bL(1)(0, 0) * b1 - bL(1)(0, 1) * b2;
            ed2 = - bL(1)(1, 0) * b1 - bL(1)(1, 1) * b2;
            if(ev1 < ev2) {
              using std::swap;
              swap(ev1, ev2);
              swap(ed1, ed2);
            }
          }
        }
        return;
      }
      catch(...) {
        Information::out << "Error calculating curvature" << endl;
      }
    }
  
    // get principal directions of curvature
    void get_eigenVectors(Point3f& evec1, Point3f& evec2)
    {
      evec1 = ed1;
      evec2 = ed2;
      return;
    }
  
    // get principal directions of curvature
    void get_eigenValues(float& eval1, float& eval2)
    {
      eval1 = ev1;
      eval2 = ev2;
      return;
    }
  
    // calculate a single-valued measure of curvature (for example "gaussian")
    float get_curvature() {
      return cM->measure(ev1, ev2);
    }
  
    // get the name of the curvature measure (for example "gaussian")
    std::string get_type() {
      return cM->get_type();
    }
  
    // set the curvature measure
    void set_Measure(CurvatureMeasure* _cM)
    {
      if(cM)
        delete cM;
      cM = _cM;
      return;
    }
  
  private:
    // calculate local basis with b1,b2, in-plane and b3 out-of-plane basis vectors
    void update_local_Basis(Point3f nrml)
    {
      nrml = nrml / nrml.norm();
      do {
        b1 = Point3f(cimg_library::cimg::rand(), cimg_library::cimg::rand(), cimg_library::cimg::rand());
        b1 = b1 / b1.norm();
      } while(fabs(b1 * nrml) == 1);
      b1 = b1 - (b1 * nrml) * nrml;
      b1 = b1 / b1.norm();
      b2 = b1.cross(nrml);
      b3 = nrml;
  
      return;
    }
  
    // Weingarten Matrix
    CIMat W;
  
    // Local Basis Vectors
    Point3f b1;
    Point3f b2;
    Point3f b3;
  
    // Principal directions of curvature
    Point3f ed1;
    Point3f ed2;
  
    // Principal values of curvature
    float ev1;
    float ev2;
  
    CurvatureMeasure* cM;
  };
}
#endif
