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

#include "Bezier.hpp"
#include <QFileInfo>

namespace mgx
{

  // Compute binomial coeff of k elements from set of n elements ("n choose k")
  int binomCoeff(int n, int k)
  {
    if(k < 0 || k > n)
      return 0;
    if(k == 0 || k == n)
      return 1;
    //  take advantage of symmetry, k = min(k, n-k)
    if((n-k) < k)
      k = n-k;
    int c = 1;
    for(int i = 0; i < k; i++)
      c = c * (n - i) / (i + 1);
    return c;
  }

  // Copy and scale
  void Bezier::scale(float scaleX, float scaleY)
  {
    int count = _bezierV.size();
    for(int i = 0; i < count; i++){
      _bezierV[i].x() = scaleX * _bezierV[i].x();
      _bezierV[i].y() = scaleY * _bezierV[i].y();
    }
  }
  
  
  // Interpolate (plus scale) between two patches
  void Bezier::interpolate(Bezier &b1, Bezier &b2,
                           double scale1, double scale2, double s)
  {
    if(b1.bezierV().size() != b2.bezierV().size()){
      throw QString("Bezier::interpolate::Error:Size of Bezier surfaces does not match");
    }
    int count = b1.bezierV().size();
    _bezierV.resize(count);
    s = trim(s, 0.0, 1.0);
    for(int i = 0; i < count; i++)
      _bezierV[i] = ((1.0 - s) * scale1 * b1.bezierV()[i] + s * scale2 * b2.bezierV()[i]);
  }
  

  // Read cutting plane parameters from a view (.mgxv) file
  void Bezier::readParms(Parms& parms, QString section)
  {
    Point2u BezLines;
    parms(section, "SurfSize", BezLines, Point2u(15, 15));
    setBezLines(BezLines);
    Point2d BezSize;
    parms(section, "BezSize", BezSize, Point2d(15, 15));
    setBezSize(BezSize);
    Point2u BezPoints;
    parms(section, "BezPoints", BezPoints, Point2u(5 ,5));
    // SS: is there a reason why there has to be at least 4 Bez Points per dimension?
    //if(BezPoints.x() < 4)
    //  BezPoints.x() = 4;
    //if(BezPoints.y() < 4)
    //  BezPoints.y() = 4;
    setBezPoints(BezPoints);

    initBez();
    std::vector<Point3d> bezierT;
    parms.all(section, "BezPointList", bezierT);
    setBezierV(bezierT);
  }
  
  // Load Bezier from a (.bez) file
  void Bezier::loadBezier(QString fileName)
  {
    Parms bezParms(fileName);
    readParms(bezParms, QString("ControlPoints"));
    // Check if bezier parms red correctly
    Information::out << "loadBezier Size Bezier in x: " << bezPoints().x() << ", in y:" << bezPoints().y() << endl;
    Information::out << "Loaded " << bezierV().size() << " Bezier control points from file: " << fileName << endl;
  }

  // Save Bezier to a (.bez) file
  void Bezier::saveBezier(QString fileName)
  {
    // Open the file for write
    QFile file(fileName);
    if(!file.open(QIODevice::WriteOnly | QIODevice::Text))
      throw(QString("Bezier::saveBezier: Cannot open parms file for writing: %1").arg(fileName));

    QTextStream pout(&file);
    pout.setCodec("UTF-8");

    pout << "[ControlPoints]" << endl;
    pout << "BezPoints: " << bezPoints() << endl;
    for(size_t i = 0; i <  bezierV().size(); i++)
      pout << "BezPointList: " << bezierV()[i] << endl;
  }

  
  void Bezier::initBez()
  {
    uint maxU = _bezPoints.x();
    uint maxV = _bezPoints.y();
    float sizeX = _bezSize.x();
    float sizeY = _bezSize.y();

    _binomV.clear();
    _bezierV.clear();

    _binomV.resize(maxU * maxV);
    _bezierV.resize(maxU * maxV);

    // Fill in table for binomial coefficients
    for(uint u = 0; u < maxU; u++)
      for(uint v = 0; v < maxV; v++) {
        _binomV[idx(u, v)] = binomCoeff(maxU - 1, u) * binomCoeff(maxV - 1, v);
        // Fill in position of control points. NB: u is tied to x axis, v to y axis
        _bezierV[idx(u, v)] = Point3d(sizeX*(-0.5f + float(u) / float(maxU - 1)),
                                      sizeY*(-0.5f + float(v) / float(maxV - 1)),
                                      0.0f);
      }
  }

  // Version of evalCoord needed for CuttingSurface
  Point3d Bezier::evalCoord(float u, float v) const
  {
    // Make sure u and v between 0.0 and 1.0
    u = trim(u, 0.0f, 1.0f);
    v = trim(v, 0.0f, 1.0f);

    // Set Pointer (k) to bezier points, evaluate Bezier
    uint maxU = _bezPoints.x();
    uint maxV = _bezPoints.y();
    Point3d pos;
    for(uint i = 0, k = 0; i < maxU; i++)
      for(uint j = 0; j < maxV; j++) {
        k = idx(i,j);
        // Compute Bernstein polynomials
        float s = (float(_binomV[k]) * pow(u, (float)i) * pow(1.0f - u, (float)(maxU - 1 - i))
                   * pow(v, (float)j) * pow(1.0f - v, (float)(maxV -1 - j)));
        // Multiply polynomials by control points to evaluate position
        pos += s * _bezierV[k];
      }
    return pos;
  }

  double expSpline(double x, double k, double c)
  {
    
    if(x > c or x < -c) return 0.;
    else{
      double xSq = x*x;
      return std::exp(-k*xSq/(c*c - xSq));
    } 
  }

  Point3d Bezier::evalCoordSpline(double u, double v)
 {
    Point3d pos;
    // Make sure u and v between 0.0 and 1.0
    u = trim(u, 0., 1.);
    v = trim(v, 0., 1.);

    double c = 2;
    double k = 1;

    // Set Pointer (k) to bezier points, evaluate Bezier
    uint maxU = _bezPoints.x();
    uint maxV = _bezPoints.y();

    for(uint i = 0; i < maxU; i++)
      for(uint j = 0; j < maxV; j++) {

        double sum = 0;

        for(double ic = -c; ic <= c; ic++){
          if(ic + i < 0 or ic + i >= maxU) continue;
          for(double jc = -c; jc <= c; jc++){
            if(jc + j < 0 or jc + j >= maxV) continue;
            double x = sqrt(ic*ic) + sqrt(jc*jc);
            sum += expSpline(x,k,c);
          }
        }

        //double s

        // k = idx(i,j);
        // // Compute Bernstein polynomials
        // float s = (float(_binomV[k]) * pow(u, (float)i) * pow(1.0f - u, (float)(maxU - 1 - i))
        //            * pow(v, (float)j) * pow(1.0f - v, (float)(maxV -1 - j)));
        // // Multiply polynomials by control points to evaluate position
        // pos += s * _bezierV[k];

      }

    return pos;
 }

  // Returns the u,v coordinates trimed and evaluates 3D position
  Point3d Bezier::evalCoordTrim(double& u, double& v)
  {
    // Make sure u and v between 0.0 and 1.0
    u = trim(u, 0.0d, 1.0d);
    v = trim(v, 0.0d, 1.0d);

    // Set Pointer (k) to bezier points, evaluate Bezier
    uint maxU = _bezPoints.x();
    uint maxV = _bezPoints.y();
    Point3d pos;
    for(uint i = 0, k = 0; i < maxU; i++)
      for(uint j = 0; j < maxV; j++) {
        k = idx(i,j);
        // Compute Bernstein polynomials
        float s = (float(_binomV[k]) * pow(u, (float)i) * pow(1.0f - u, (float)(maxU - 1 - i))
                   * pow(v, (float)j) * pow(1.0f - v, (float)(maxV -1 - j)));
        // Multiply polynomials by control points to evaluate position
        pos += s * _bezierV[k];
      }
    return pos;
  }

  void Bezier::discretizeLine(int discPoints, double vPos, Matrix4d rotMat,
                              std::vector<Point3d>& discretizedBez, std::vector<Point3d>& differentialBez)
  {
    // save the out put vectors here
    std::vector<Point3d> discretizedBezNew;
    std::vector<Point3d> differentialBezNew;

    // create bezier vector and its derivative
    double bezStart = 0.0;
    double bezEnd = 1.0;
    double stepSize = (double)(bezEnd-bezStart)/(double)(discPoints-1);

    differentialBezNew.push_back(Point3d(0,0,0));

    // loop over discretization points and calculate their coords
    for(int i=0; i<discPoints; i++){
      double u = bezStart+i*stepSize;
      Point3d p(evalCoord(u,vPos));
      p = multMatrix4Point3(rotMat,p); // correct rotations
      discretizedBezNew.push_back(p);
      if(i>0){
        Point3d dif = discretizedBezNew[i]-discretizedBezNew[i-1];
        differentialBezNew.push_back(dif/norm(dif));
      }
    }

    differentialBezNew[0] = differentialBezNew[1];

    // save output
    discretizedBez = discretizedBezNew;
    differentialBez = differentialBezNew;

  }


  void Bezier::discretizeLineEqualWeight(int discPoints, double vPos, Matrix4d rotMat,
                                         std::vector<Point3d>& discretizedBez, std::vector<Point3d>& differentialBez, double& totalLength)
  {

    std::vector<Point3d> discretizedBezNew;
    std::vector<Point3d> differentialBezNew;

    // create bezier vector and its derivative
    double bezStart = 0.0;
    double bezEnd = 1.0;
    double stepSize = (double)(bezEnd-bezStart)/(double)(discPoints-1);

    int discPointsRefined = discPoints*100;
    double stepSizeFine = (double)(bezEnd-bezStart)/(double)(discPointsRefined-1);

    std::vector<Point3d> discretizedBezFine, differentialBezFine;

    // first calculate bezier points with very fine resolution
    for(int i=0; i<discPointsRefined; i++){
      double u = bezStart+i*stepSizeFine;
      Point3d p(evalCoord(u,vPos));
      p = multMatrix4Point3(rotMat,p); // correct rotations
      discretizedBezFine.push_back(p);
    }

    // now measure the total distance of the curve
    double totDis = 0;
    for(int i=1; i<discPointsRefined; i++){
      totDis += norm(discretizedBezFine[i] - discretizedBezFine[i-1]);
    }

    double dis = 0;
    int idxCounter = 1;

    // first point
    Point3d dif0 = discretizedBezFine[1] - discretizedBezFine[0];
    differentialBezNew.push_back(dif0/norm(dif0));
    discretizedBezNew.push_back(discretizedBezFine[0]);

    // now create more coarse discretization
    for(int i=1; i<discPointsRefined; i++){
      dis += norm(discretizedBezFine[i] - discretizedBezFine[i-1])/totDis;

      if(dis>idxCounter*stepSize){
        Point3d dif = discretizedBezFine[i+1] - discretizedBezFine[i-1];
        differentialBezNew.push_back(dif/norm(dif));
        discretizedBezNew.push_back(discretizedBezFine[i]);
        idxCounter++;
      }

    }
    // last point
    Point3d difL = discretizedBezFine[discPointsRefined-1] - discretizedBezFine[discPointsRefined-2];
    differentialBezNew.push_back(difL/norm(difL));
    discretizedBezNew.push_back(discretizedBezFine[discPointsRefined-1]);

    // save output
    discretizedBez = discretizedBezNew;
    differentialBez = differentialBezNew;
    totalLength = totDis;
  }

  void Bezier::discretizeGrid(int discPoints, Matrix4d rotMat,
                              std::vector<std::vector<Point3d> >& discretizedBez)
  {

    // create bezier vector and its derivative
    double bezStart = 0.0;
    double bezEnd = 1.0;
    double stepSize = (double)(bezEnd-bezStart)/(double)(discPoints-1);

    // save the output vectors here
    std::vector<std::vector<Point3d> > discretizedBezNew(discPoints);
    std::vector<std::vector<Point3d> > discretizedBezDiffNew(discPoints);

    // loop over discretization points and calculate their coords
    for(int i=0; i<discPoints; i++){
      std::vector<Point3d> currentCol(discPoints);
      discretizedBezNew[i] = currentCol;
      for(int j=0; j<discPoints; j++){
        double u = bezStart+i*stepSize;
        double v = bezStart+j*stepSize;
        Point3d p(evalCoord(u,v));
        p = multMatrix4Point3(rotMat,p); // correct rotations
        discretizedBezNew[i][j] = p;
      }
    }

    // save output
    discretizedBez = discretizedBezNew;
  }
}
