//
// This file is part of MorphoGraphX - http://www.MorphoGraphX.org
// Copyright (C) 2012-2016 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.
//

// Implement surface and surface point class
#include <iostream>
#include <cstdio>
#include <cmath>

#include <BezierSurface.hpp>
#include <Function.hpp>
#include <Contour.hpp>
#include <Information.hpp>

namespace mgx
{
  const double DX = 0.00015;
  
  bool BezierSurface::initialize(VertexAttr *vData, const QStringList &parms) 
  {
    // Process the parameters
    processParms(parms);

    // Get the attribute maps
    vertexAttr = vData;

		// Set current Bezier 
    surfCurr = surface[0];

    return true;
  }

  // Read in model parameters
  bool BezierSurface::processParms(const QStringList &parms)
  {  
    QString nameSurf = parms[pSurface];
    QStringList nameSurfList = nameSurf.split(" ");

    QString scales = parms[pSurfScale];
    QStringList scaleList = scales.split(" ");

    QString times = parms[pSurfTime];
    QStringList timeList = times.split(" ");

    int nSurf = nameSurfList.size(); // number of surfaces
 
     if(timeList.size() != nSurf)
      throw QString("BezierSurface::processParms::Error: Number of elements in SurfTimes should be equal to number of elements in Surfaces");

     if(scaleList.size() != nSurf)
      throw QString("BezierSurface::processParms::Error: Number of elements in SurfScales should be equal to number of elements in Surfaces");

    surface.resize(nSurf);
    surfScale.resize(nSurf);
    surfTime.resize(nSurf);

    for(int i = 0; i < nSurf; i++) {
			if(!QFile::exists(nameSurfList[i]))
				throw QString("BezierSurface::processParms::Error: '%1' does not exist").arg(nameSurfList[i]);
      surface[i].loadBezier(nameSurfList[i]);
      surfScale[i] = scaleList[i].toDouble();
      surfTime[i] = timeList[i].toDouble();
    }

    surfMaxDist = parms[pSurfMaxDist].toDouble();
    rootSearchMaxSteps = parms[pRootSearchMaxSteps].toInt();    

    return true;
  }
  
  // Find closest point (p) on a bezier surface to a given cartesian point (cp), 
  // starting from a given surface point (sp)
  bool BezierSurface::setPoint(vertex vp, vertex vsp, Point3d cp)
  {
    VertexData &p = vp->*vertexAttr;
    VertexData &sp = vsp->*vertexAttr;

    // Initial guess for p
    updatePos(vp);

    const double DX = 0.00001;
    double lastd = -1000;
    double count = 0;
    double du, dv, step;

    // Calc partial derivatives along u and v coordinates, staring from cp
    Point3d u1, u2, v1, v2; 
		double u1_u, u2_u, v1_u, v2_u;
		double u1_v, u2_v, v1_v, v2_v;

    // Temp for line search
    Point3d t;
		double t_u, t_v;
    while(fabs(lastd - norm(cp - vp->pos)) > DX && count < rootSearchMaxSteps) {
      // Save previous distance
      lastd = norm(cp - vp->pos);
      // Do line minimization one direction at a time
      u1_v = u2_v = p.v;
      u1_u = p.u - DX; u2_u = p.u + DX;
			u1 = Point3d(surfCurr.evalCoordTrim(u1_u, u1_v));
			u2 = Point3d(surfCurr.evalCoordTrim(u2_u, u2_v));
      du = (norm(cp - u1) - norm(cp - u2))/fabs(u1_u - u2_u);
      step = 1;
      while(step > DX) {
        t_v = p.v;
        t_u = p.u + step * du;
				t = Point3d(surfCurr.evalCoordTrim(t_u, t_v));
        if(norm(cp - t) < norm(cp - vp->pos)) {
          step *= 1.11;
          p.u = t_u;
          p.v = t_v;
          updatePos(vp);
        } else
          step /= 2.0;
      }

      v1_u = v2_u = p.u;
      v1_v = p.v - DX; v2_v = p.v + DX;
			v1 = surfCurr.evalCoordTrim(v1_u, v1_v);
			v2 = surfCurr.evalCoordTrim(v2_u, v2_v);
      dv = (norm(cp - v1) - norm(cp - v2))/fabs(v1_v - v2_v);
      step = 1;
      while(step > DX) {
        t_u = p.u;
        t_v = p.v + step * dv;
				t = surfCurr.evalCoordTrim(t_u, t_v);
        if(norm(cp - t) < norm(cp - vp->pos)) {
          step *= 1.11;
					p.u = t_u;
          p.v = t_v;
          updatePos(vp);
        } else
          step /= 2.0;
      }
      count++; 
    }
    updatePos(vp);
    updateNormal(vp);
  
    if(count >= rootSearchMaxSteps || norm(vp->pos - cp) > surfMaxDist) {
      Information::out << "Surface::SetPoint:Error Failed, point " << cp << 
              " closest point " << vp->pos << " du " << du << " dv " << dv << 
              " count " << count << " distance " <<  norm(vp->pos - cp) << endl;
    }
    return(true);
  } 

  // Grow whole bezier surface 
  bool BezierSurface::growSurface(double time)
  {
    int surf = 1;
    double surftime = 0;
    while(time > surftime + surfTime[surf] && surf < surface.size() - 1)
      surftime += surfTime[surf++ - 1];
    
    surfCurr.interpolate(surface[surf-1], surface[surf], 
        surfScale[surf-1], surfScale[surf], (time - surftime)/surfTime[surf]);

    return true;
  }

  // Create an initial cell (square) in a full cell tissue
  bool BezierSurface::initialCell(CellTissue &T, double squareSize, int cellInitWalls)
  {
    std::vector<vertex> poly;
    // Center
    vertex c; 
    VertexData &cd = c->*vertexAttr;
    cd.u = 0;
    cd.v = 0;
    updatePos(c);
    updateNormal(c);
    poly.push_back(c);

    // Cell corners
    for(int i = 0; i < cellInitWalls; i++) {
      vertex v; 
      VertexData &vd = v->*vertexAttr;

      double s = (double)i/cellInitWalls;
  
      if(s < .25) {
        vd.u = 0;
        vd.v = s * 4;
      } else if(s < .5) {
        vd.u = (s - .25) * 4;
        vd.v = 1.0 * squareSize;
      } else if(s < .75) {
        vd.u = 1.0 * squareSize;
        vd.v = 1  - (s - .5) * 4;
      } else {
        vd.u = 1  - (s - .75) * 4;
        vd.v = 0 * squareSize;
      }
      updatePos(v);
      updateNormal(v);
      poly.push_back(v);
    }

    return T.addCell(poly);
  }

  // Calculate position in Cartesian (xyz) coordinates from Bezier (u,v) coordinates
  bool BezierSurface::updatePos(vertex vp)
  {
    Point3d cartPos;
    VertexData &p = vp->*vertexAttr;
		p.u = trim(p.u, 0.0, 1.0);
    p.v = trim(p.v, 0.0, 1.0);
    cartPos = surfCurr.evalCoordTrim(p.u, p.v);
    vp->pos = cartPos;

    return true;
  }
  
  bool BezierSurface::updateNormal(vertex vp)
  {
    Point3d normal;
    VertexData &p = vp->*vertexAttr;
		
		// temporary cartesian and Bezier coordinates
		Point3d n, s, e, w;
		double n_u, s_u, e_u, w_u;
		double n_v, s_v, e_v, w_v;

    // Calc normal from surface.
    n_u = p.u + DX; n_v = p.v; 
    s_u = p.u - DX; s_v = p.v;
    e_u = p.u; e_v = p.v + DX;
    w_u = p.u; w_v = p.v - DX;
      
		n = surfCurr.evalCoordTrim(n_u, n_v);
		s = surfCurr.evalCoordTrim(s_u, s_v);
		e = surfCurr.evalCoordTrim(e_u, e_v);
		w = surfCurr.evalCoordTrim(w_u, w_v);

    normal = s - n;
    normal = normal ^ (w - e);
    normal /= norm(normal);

    vp->nrml = normal;

    return true;
  }

  // Update edge data, s indicates distance along edge
  // Called when a vertex is inserted in an edge
  bool BezierSurface::Subdivide::updateEdgeData(vertex vl, vertex v, vertex vr, double s)
  {
    if(!surface->setPoint(v, vl, v->pos))
      surface->setPoint(v, vr, v->pos);
    surface->updatePos(v);

    return true;
  }
}
