//
// 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 <PolarSurface.hpp>
#include <Function.hpp>
#include <Contour.hpp>
#include <Information.hpp>

namespace mgx
{
  const double DX = 0.00015;
	const int ARCLENGTH = 0;	// Growth types

  // Find polar angle of a point
  double polarAngle(double x, double y)
  {
    double angle = acos(x/sqrt(pow(x, 2) + pow(y, 2)));
    if(y < 0.0)
      angle = M_PI + M_PI - angle;
    return(angle);
  }
   
  bool PolarSurface::initialize(VertexAttr *vAttr, const QStringList &parms) 
  {
    // Process the parameters
    processParms(parms);

    // Get the attribute maps
    vertexAttr = vAttr;

    return true;
  }
  
  bool PolarSurface::integrateFunction(const QString &funcFile, double *v)
  {
    Function func(funcFile);
    double dx = 1.0/SAMPLES, x = 0, sum = 0, y;
    int negy = 0;
    
    for(int i = 0; i <= SAMPLES; i++) {
      v[i] = sum;
      x += dx;
      y = func(x);
      if(y < 0.0)
        negy = 1;
      sum += dx * y;
    }
  
    if(negy)
      Information::out << "Surface::IntegrateFunction:Warning " << funcFile << 
                                                   " goes negative" << endl;
    return true;
  }

  // Reparamaterize contour by arclength
  bool PolarSurface::arcLengthParam(const QString &contFile, Point3d *v) 
  {
    Contour surfCont(contFile);
    Point3d curr, prev, last;
    double totArcLen = 0.0;
    // First calculate arc length of contour based on initial paramaterization
    prev = surfaceScale * surfCont((double)0.0);
    for(int i = 1; i <= SAMPLES; i++) {
      curr = surfaceScale * surfCont((double)i/(double)SAMPLES);
      totArcLen += norm(prev - curr);
      prev = curr;
    }
    // re-parameterize by arc length
    dArc  = totArcLen/(double)SAMPLES;
    v[0] = prev = curr = surfaceScale * surfCont((double)0.0);
      
    { // MS C++ requires this
      int i = 1, j = 1;
      for(; i <= SAMPLES; i++) {
        double dis;
        while((dis = norm(v[i - 1] - curr)) < dArc && j <= SAMPLES)
          curr = surfaceScale * surfCont((double)j++/(double)SAMPLES);
        
        if(j >= SAMPLES)
          break;
        v[i] = (v[i - 1] * (dis - dArc) + curr * dArc)/dis;
      }
      last = v[i - 1];
      dPos = v[i - 1] - v[i - 2];
      j = i - 1;
      for(; i <= SAMPLES; i++)
        v[i] = last + dPos * (double)(i - j);
    }
    // Scale dPos to 1.0
    dPos /= dArc;
      
    // translate so start of curve is at 0,0,0
    Point3d v0 = v[0];
    for(int i = 0; i <= SAMPLES; i++)
      v[i].set(v[i].x() - v0.x(), v[i].y() - v0.y(), v[i].z());
      //v[i].set(v[i].x() - minx, v[i].y() - miny, v[i].z());
  // Not sure how to do this now
  //  SurfacePoint pt0, pt1;
  //  SetPoint(pt0, 0.0, 0.0);
  //  SetPoint(pt0, 0.0, dArc * (double)SAMPLES);
  //  
  //  // Check x value to see if contour backwards
  //  if(pt0._pos.x() > pt1._pos.x())
  //    Information::out << "Surface::ParamArcLength:Warning " << contFile << 
  //                                                  " curve backwards?" << endl;
    return true;
  }

  // Read in model parameters
  bool PolarSurface::processParms(const QStringList &parms)
  {
     surfaceScale = parms[pSurfaceScale].toDouble();
     growthScaleOutMult = parms[pGrowthScaleOutMult].toDouble();
     growthScaleInMult = parms[pGrowthScaleInMult].toDouble();
     rootSearchMaxSteps = parms[pRootSearchMaxSteps].toInt();
    
     growthScaleFile = parms[pGrowthScaleFunc];
     growthScaleFunc = new Function(growthScaleFile);

     growthType = parms[pGrowthType].toInt();

     if(growthType == ARCLENGTH) {
		 	// For arc length 1st file is surface contour 2nd is arclength growth func
      surfaceContourFile = parms[pSurfaceContour];
      arcLengthParam(surfaceContourFile, arcLV);
      
      // Integrate growth curve
      arcLengthGrowthFile = parms[pArcLengthGrowth];
      integrateFunction(arcLengthGrowthFile, growthVA);
    
      Information::out << "Surface::Surface:Info Arc Length Growth, " <<
                                "Total Arc Length " << dArc * SAMPLES << endl;
      Information::out << "Surface::Surface:Info Sample size " << SAMPLES << endl;
    }
    return true;
  }

  
  // Find closest vertex (vp) on a polar surface to a given cartesian point (cp), starting from a given vertex (vsp) (Newton)
  bool PolarSurface::setPoint(vertex vp, vertex vsp, Point3d cp)
  {
    VertexData &p = vp->*vertexAttr;
    VertexData &sp = vsp->*vertexAttr;
    
    bool found = true;
    if(norm(cp) <= 0.0)
      p.arcLength = p.angle = 0.0;
    else {
      // Zip close point around to same angle as given point and start from there
      p = sp;
      p.angle = polarAngle(cp.x(), cp.z());
      Point3d zp(cp.x(), 0.0, cp.y());
      if(norm(zp) <= DX) {
        p.arcLength = norm(zp);
        // calculate new xyz position
        updatePos(vp);
      } else {
        int max = rootSearchMaxSteps;
        updatePos(vp);
        Point3d pp;
        do {
          // Save previous position
          pp = vp->pos;
  
          // calculate normal
					double n_arcLength, n_angle; 
					double s_arcLength, s_angle; 
          n_arcLength = p.arcLength - DX;
          if(n_arcLength < 0.0)
            n_arcLength = 0.0;
          s_arcLength = p.arcLength + DX;
          n_angle = s_angle = p.angle;
          Point3d n = polarToCart(n_arcLength, n_angle);
          Point3d s = polarToCart(s_arcLength, s_angle);
          Point3d pn = (s - n);
          pn /= norm(pn);
  
          // Get projection onto normal
          double darc = pn * (cp - vp->pos);
  
          // Exit is small enough
          if(fabs(darc) < DX * 10)
            break;
  
          // Set new arc length
          p.arcLength += darc;
  
          // Set to 0 if hit tip
          if(p.arcLength <= 0.0) {
            p.arcLength = p.angle = 0.0;
            break;
          }
          // calculate new xyz position
          updatePos(vp);
        } while(--max > 0);
  
        // Print error if hit max iterations
        if(max <= 0) {
          Information::out << "Surface::SetPoint:Error failed to find root " << cp << endl;
          found = false;
        }
      }
      updateNormal(vp);
    }
    return(found);
  }
    

  // Grow polar surface point 
  bool PolarSurface::growPoint(vertex vp, double dt, double time)
  {
     VertexData &p = vp->*vertexAttr;
     int i;
      
    if(p.arcLength > 0.0) { // don't grow the tip
      i = (int)(p.arcLength/dArc);
      if(i > SAMPLES)
        i = SAMPLES;
      p.arcLength += growthVA[i] * dt * growthScaleOutMult * (*growthScaleFunc)(time * growthScaleInMult);
    }
    updatePos(vp);
    updateNormal(vp);

    return true;
  }

  // Create an initial cell (circular) in a full cell tissue
  bool PolarSurface::initialCell(CellTissue &T, double radius, int sides)
  {
    std::vector<vertex> poly;
    // Center
    vertex c; 
    VertexData &cd = c->*vertexAttr;
    cd.arcLength = 0;
    cd.angle = 0;
    updatePos(c);
    updateNormal(c);
    poly.push_back(c);

    // Cell corners
    for(int i = 0; i < sides; i++) {
      vertex v; 
       VertexData &vd = v->*vertexAttr;
      vd.arcLength = radius;
      vd.angle = (M_PI + M_PI) * (double)i/(double)sides;;
      updatePos(v);
      updateNormal(v);
      poly.push_back(v);
    }
    return T.addCell(poly);
  }


  // Updates xyz coordinates (stored in vextex->pos) from polar coordinates (stored in attributes)
  bool PolarSurface::updatePos(vertex vp)
  {
    VertexData &p = vp->*vertexAttr;
    vp->pos = polarToCart(p.arcLength, p.angle);

    return true;
	}

  // Calculate xyz coordinates from polar coordinates
  Point3d PolarSurface::polarToCart(double arcLength, double angle)
	{
	  Point3d cartPos;
    // Get pos from table based on arclength
    if(arcLength <= 0.0)
      cartPos = arcLV[0];
    else {
      int i = (int)(arcLength/dArc);
      if(i >= SAMPLES - 1)
        i = SAMPLES - 1;
      
      // If exact use table value, otherwise extrapolate, or interpolate
      double diff = arcLength - (double)i * dArc;
      if(i >= SAMPLES - 1) // extrapolate, past end of curve
        cartPos = arcLV[SAMPLES - 1] + dPos * diff;
      else if(diff == 0.0) // exact
        cartPos = arcLV[i];
      else // interpolate
        cartPos = (arcLV[i] * (dArc - diff) + arcLV[i + 1] * diff)/dArc;
  
      // Rotate based on angle
      if(angle > 0.0)
        cartPos.set(cartPos.x() * cos(angle), cartPos.y(), cartPos.x() * sin(angle));
    }
    return cartPos; 
  }

  
  bool PolarSurface::updateNormal(vertex vp)
  {
     VertexData &p = vp->*vertexAttr;
    Point3d normal;
    if(p.arcLength <= DX)
      normal.set(0.0, 1.0, 0.0);
    else {
      // calc normal from surface.
			double n_arcLength, s_arcLength, e_arcLength, w_arcLength;
			double n_angle, s_angle, e_angle, w_angle;

      n_arcLength = p.arcLength - DX; n_angle = p.angle;
      s_arcLength = p.arcLength + DX; s_angle = p.angle;
      e_arcLength = p.arcLength; e_angle = p.angle + DX;
      w_arcLength = p.arcLength; w_angle = p.angle - DX;
      
      Point3d n = polarToCart(n_arcLength, n_angle); 
      Point3d s = polarToCart(s_arcLength, s_angle); 
      Point3d e = polarToCart(e_arcLength, e_angle); 
      Point3d w = polarToCart(w_arcLength, w_angle); 

      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 PolarSurface::Subdivide::updateEdgeData(vertex l, vertex v, vertex r, double s)
  {
    // If no surface just average
    bool result = true;
    if(surface) {
      result = surface->setPoint(v, l, v->pos);
      if(!result)
        result = surface->setPoint(v, r, v->pos);
      if(result) {
        surface->updatePos(v);
        return true;
      }
    }
    // If no surface or setPoint fails, average the cylindrical coordinates 
    VertexData &L = l->*vertexAttr;
    VertexData &V = v->*vertexAttr;
    VertexData &R = r->*vertexAttr;
    V.arcLength = (1.0 - s) * L.arcLength + s * R.arcLength;
    // Angle is tricky
    double x = cos(L.angle) + cos(R.angle);
    double y = sin(L.angle) + sin(R.angle);
    V.angle = atan2(y, x);

    if(surface) {
      result = surface->setPoint(v, r, v->pos);
      if(result)
        surface->updatePos(v);
      return result;
    }
    return true; // No surface, should we really return false?
  }
}
