#include "geometrycentral/surface/trace_geodesic.h"

#include "geometrycentral/surface/barycentric_coordinate_helpers.h"
#include "geometrycentral/surface/vertex_position_geometry.h"

#include <Eigen/Dense>

#include <iomanip>

using std::cout;
using std::endl;

namespace geometrycentral {
namespace surface {

// The default trace options
const TraceOptions defaultTraceOptions;

// Helper functions which support tracing
namespace {

// === Geometric subroutines for tracing

// a few parameters
const double TRACE_EPS_TIGHT = 1e-12;
const double TRACE_EPS_LOOSE = 1e-9;
const bool TRACE_PRINT = false;

inline std::array<Vector2, 3> vertexCoordinatesInTriangle(IntrinsicGeometryInterface& geom, Face face) {
  return {Vector2{0., 0.}, geom.halfedgeVectorsInFace[face.halfedge()],
          -geom.halfedgeVectorsInFace[face.halfedge().next().next()]};
}

inline Vector3 faceCoordsToBaryCoords(const std::array<Vector2, 3>& vertCoords, Vector2 faceCoord) {

  // Warning: bakes in assumption that vertCoords[0] == (0,0)

  // Invert the system to solve for coordinates
  double b2 = faceCoord.y / vertCoords[2].y;
  b2 = clamp(b2, 0.0, 1.0); // comment these to get useful errors rather than clamping
  double b1 = (faceCoord.x - b2 * vertCoords[2].x) / vertCoords[1].x;
  b1 = clamp(b1, 0.0, 1.0 - b2);
  double b0 = 1.0 - b1 - b2;
  b0 = clamp(b0, 0.0, 1.0);
  Vector3 result{b0, b1, b2};

  return result;
}

inline Vector2 baryCoordsToFaceCoords(const std::array<Vector2, 3>& vertCoords, Vector3 baryCoord) {
  return vertCoords[0] * baryCoord.x + vertCoords[1] * baryCoord.y + vertCoords[2] * baryCoord.z;
}

inline Vector2 barycentricDisplacementToCartesian(const std::array<Vector2, 3>& vertCoords, Vector3 baryVec) {
  // Note: happens to be the same as baryCoordsToFaceCoords()
  return vertCoords[0] * baryVec.x + vertCoords[1] * baryVec.y + vertCoords[2] * baryVec.z;
}

inline Vector3 cartesianVectorToBarycentric(const std::array<Vector2, 3>& vertCoords, Vector2 faceVec) {

  // Build matrix for linear transform problem
  // (last constraint comes from chosing the displacement vector with sum = 0)
  Eigen::Matrix3d A;
  Eigen::Vector3d rhs;
  const std::array<Vector2, 3>& c = vertCoords; // short name
  A << c[0].x, c[1].x, c[2].x, c[0].y, c[1].y, c[2].y, 1., 1., 1.;
  rhs << faceVec.x, faceVec.y, 0.;

  // Solve
  Eigen::Vector3d result = A.colPivHouseholderQr().solve(rhs);
  Vector3 resultBary{result(0), result(1), result(2)};

  resultBary = normalizeBarycentricDisplacement(resultBary);

  if (TRACE_PRINT) {
    cout << "       cartesianVectorToBarycentric() " << endl;
    cout << "         input = " << faceVec << endl;
    cout << "         positions = " << vertCoords[0] << " " << vertCoords[1] << " " << vertCoords[2] << endl;
    cout << "         transform result = " << resultBary << endl;
    cout << "         transform back = " << barycentricDisplacementToCartesian(vertCoords, resultBary) << endl;
    cout << " A = " << endl << A << endl;
    cout << " rhs = " << endl << rhs << endl;
    cout << " Ax = " << endl << A * result << endl;
  }

  return resultBary;
}

// Converts tCross from halfedge to edge coordinates, handling sign conventions
inline double convertTToEdge(Halfedge he, double tCross) {
  if (he == he.edge().halfedge()) return tCross;
  return 1.0 - tCross;
}
// Converts vectors in halfedge basis from halfedge to edge coordinates, handling sign conventions
inline Vector2 convertVecToEdge(Halfedge he, Vector2 halfedgeVec) {
  if (he == he.edge().halfedge()) return halfedgeVec;
  return -halfedgeVec;
}

// === Tracing subroutines

// Return type from tracing subroutines
// When trace ends, will set newFace = Face() and newVector = Vector3::zero(). only then is incomingDirToPoint populated
struct TraceSubResult {
  // Did the trace end?
  bool terminated;

  // One of the two sets of values will be defined:

  // If the trace continues (terminated == false)
  Halfedge crossHe;                 // halfedge we crossed over from (crossHe.twin().face() is new face)
  double tCross;                    // t along crossHe
  Vector2 traceVectorInHalfedgeDir; // vector to keep tracing, measured against crossHe
  double traceVectorInHalfedgeLen;  // vector to keep tracing, measured against crossHe

  // If the trace ends (terminated == true)
  SurfacePoint endPoint;      // ending location
  Vector2 incomingDirToPoint; // final incoming direction
};

// General form for tracing barycentrically within a face
// Assumes that approriate projects have already been performed such that startPoint and vectors are valid (inside
// triangle and pointing in the right direction)
//
// This function is tightly coupled with the routines which call it. They prepare the values startPoint, vecBary, and
// vecCartesian, ensuring that those values satisify basic properties (essentially that the trace points in a vallid
// direction).
//
// Note that this expects to be given the trace vector in both barycentric _and_ cartesian coordinates. These are two
// different representations of the same data! This is useful the barycentric representation is good for relilably
// performing tracing, while the cartesian representation is good for transforming the trace vector between triangles.
inline TraceSubResult traceInFaceBarycentric(IntrinsicGeometryInterface& geom, Face face, Vector3 startPoint,
                                             Vector3 vecBary, Vector2 vecCartesianDir, double vecCartesianLen,
                                             std::array<bool, 3> edgeIsHittable, const TraceOptions& traceOptions) {

  // Gather values
  std::array<Vector2, 3> vertexCoords = vertexCoordinatesInTriangle(geom, face);
  Vector3 triangleLengths{geom.edgeLengths[face.halfedge().edge()], geom.edgeLengths[face.halfedge().next().edge()],
                          geom.edgeLengths[face.halfedge().next().next().edge()]};

  if (sum(startPoint) < 0.5) {
    if (TRACE_PRINT) {
      cout << "  bad bary point: " << startPoint << endl;
    }
    if (traceOptions.errorOnProblem) {
      throw std::runtime_error("bad bary point");
    }
  }

  if (TRACE_PRINT) {
    cout << "  general trace in face: " << endl;
    cout << "  face: " << face << " startPoint " << startPoint << " vecBary = " << vecBary << " vecCartesian "
         << vecCartesianDir << " " << vecCartesianLen << endl;
  }

  if (TRACE_PRINT) {
    cout << "  vec bary  = " << vecBary << endl;
    cout << "  reconvert = " << cartesianVectorToBarycentric(vertexCoords, vecCartesianDir) * vecCartesianLen << endl;
  }

  // Test if the vector ends in the triangle
  Vector3 endPoint = startPoint + vecBary;
  if (TRACE_PRINT) {
    cout << "    endpoint: " << endPoint << endl;
  }

  /*
  if (isInsideTriangle(endPoint)) {
    // Fancy test if ending on edges
    // (to detect when we're within eps of edge)
    bool foundEpsEdge = false;
    if (traceOptions.allowEndOnEdge) {
      double A = 0.5 * cross(vertexCoords[1] - vertexCoords[0], vertexCoords[2] - vertexCoords[0]);
      Halfedge endHe = face.halfedge();
      for (int i = 0; i < 3; i++) {
        double dist = 2. * endPoint[i] * A / triangleLengths[i]; // perp distance to edge
        if (endPoint[i] > 0 && dist < traceOptions.allowEndOnEdgeEps) {
          // force the process below to hit this edge, let other logic proceed
          edgeIsHittable[i] = true;
          edgeIsHittable[(i + 1) % 3] = false;
          edgeIsHittable[(i + 2) % 3] = false;
          foundEpsEdge = true;
          break;
        }
      }
    }
    // Simple test if not ending on edges
    if (!foundEpsEdge) {
      // The trace ended! Call it a day.
      TraceSubResult result;
      result.terminated = true;
      result.endPoint = SurfacePoint(face, endPoint);
      result.incomingDirToPoint = vecCartesian;
      return result;
    }
  }
  */

  if (isInsideTriangle(endPoint)) {
    // The trace ended! Call it a day.
    TraceSubResult result;
    result.terminated = true;
    result.endPoint = SurfacePoint(face, endPoint);
    result.incomingDirToPoint = vecCartesianDir;
    return result;
  }

  // The vector did not end in this triangle. Pick an appropriate point along some edge
  double tRay = std::numeric_limits<double>::infinity();
  Halfedge crossHe = Halfedge();
  int iOppVertEnd = -777;
  Halfedge currHe = face.halfedge();
  for (int i = 0; i < 3; i++) {
    currHe = currHe.next(); // always opposite the i'th vertex

    // Check the crossing
    double tRayThisRaw = -startPoint[i] / vecBary[i];
    double tRayThis = clamp(tRayThisRaw, 0., 1. - TRACE_EPS_LOOSE);

    if (TRACE_PRINT) {
      cout << "    considering intersection:" << endl;
      cout << std::boolalpha;
      cout << "      hittable[(i+1)%3]: " << edgeIsHittable[(i + 1) % 3] << endl;
      cout << "      vecBary[i]: " << vecBary[i] << endl;
      cout << "      startPoint[i]: " << startPoint[i] << endl;
      cout << "      tRayThisRaw: " << tRayThisRaw << endl;
      cout << "      tRayThis: " << tRayThis << endl;
    }

    if (!edgeIsHittable[(i + 1) % 3] || vecBary[i] >= 0) {
      // note should ALWAYS satisfy precondition that vecBary[i] is negative for at least one hittable edge.
      // if not, fix projection of inputs in caller
      continue;
    }

    if (tRayThisRaw < tRay) {
      // This is the new closest intersection
      tRay = tRayThisRaw;
      crossHe = currHe;
      iOppVertEnd = i;
    }
  }

  if (TRACE_PRINT) {
    cout << "    selected intersection:" << endl;
    cout << "      crossHe: " << crossHe << endl;
    cout << "      tRay: " << tRay << endl;
    cout << "      iOppVertEnd: " << iOppVertEnd << endl;
  }

  // Clamp to a sane range
  tRay = clamp(tRay, 0., 1. - TRACE_EPS_LOOSE);

  if (crossHe == Halfedge()) {
    if (traceOptions.errorOnProblem) {
      throw std::logic_error("no halfedge intersection was selected, precondition problem?");
    }
    if (TRACE_PRINT) {
      cout << "    PROBLEM PROBLEM NO INTERSECTION:" << endl;
    }

    // End immediately
    TraceSubResult result;
    result.terminated = true;
    result.endPoint = SurfacePoint(face, startPoint);
    result.incomingDirToPoint = vecCartesianDir;
    return result;
  }

  // Compute some useful info about the endpoint
  Vector3 endPointOnEdge = startPoint + tRay * vecBary;
  double tCross = endPointOnEdge[(iOppVertEnd + 2) % 3] /
                  (endPointOnEdge[(iOppVertEnd + 1) % 3] + endPointOnEdge[(iOppVertEnd + 2) % 3]);
  if (TRACE_PRINT) {
    cout << "    end point on edge: " << endPointOnEdge << endl;
    cout << "    tCross raw: " << tCross << endl;
  }
  tCross = clamp(tCross, 0., 1.);

  // Rotate the vector in to the frame of crossHe and shorten it
  double lenRemaining = (1.0 - tRay) * vecCartesianLen;
  Vector2 crossingEdgeVec = (vertexCoords[(iOppVertEnd + 2) % 3] - vertexCoords[(iOppVertEnd + 1) % 3]);
  Vector2 remainingDirInHalfedge = vecCartesianDir / crossingEdgeVec.normalize();
  if (!isfinite(remainingDirInHalfedge)) {
    if (TRACE_PRINT) {
      cout << "    NON FINITE REMAINING TRACE" << endl;
      cout << "    lenRemaining = " << lenRemaining << endl;
      cout << "    crossingEdgeVec = " << crossingEdgeVec << endl;
      cout << "    remainingDirInHalfedge = " << remainingDirInHalfedge << endl;
    }

    if (traceOptions.errorOnProblem) {
      throw std::runtime_error("bad value transforming to new edge. is there a zero-length edge?");
    }
  }

  // Stop tracing if we hit a boundary
  if (!crossHe.twin().isInterior() || (traceOptions.barrierEdges && (*traceOptions.barrierEdges)[crossHe.edge()])) {
    // Build the result
    TraceSubResult result;
    result.terminated = true;
    result.endPoint = SurfacePoint(crossHe.edge(), convertTToEdge(crossHe, tCross));
    result.incomingDirToPoint = remainingDirInHalfedge;

    return result;
  }

  // Build the result
  TraceSubResult result;
  result.terminated = false;
  result.crossHe = crossHe;
  result.tCross = tCross;
  result.traceVectorInHalfedgeDir = remainingDirInHalfedge;
  result.traceVectorInHalfedgeLen = lenRemaining;
  return result;
}

// Trace within a face towards a given edge. The trace is assumed to start at the vertex opposite towardsHe.
//   - towardsHe: the halfedge we are tracing towards (opposite the source vertex)
//   - vecCartesian: vector to trace, in the cartesian basis of the face
inline TraceSubResult traceInFaceTowardsEdge(IntrinsicGeometryInterface& geom, Halfedge towardsHe,
                                             Vector2 vecCartesianDir, double vecCartesianLen,
                                             const TraceOptions& traceOptions) {

  // Gather some values
  Face face = towardsHe.face();
  Halfedge rootHe = towardsHe.next().next();
  std::array<Vector2, 3> vertexCoords = vertexCoordinatesInTriangle(geom, face);

  if (TRACE_PRINT) {
    cout << "  face trace towards edge " << towardsHe << " vec = " << vecCartesianDir << endl;
    cout << "  wedge vec right = " << geom.halfedgeVectorsInFace[towardsHe.next().next()] << endl;
    cout << "  wedge vec left  = " << -geom.halfedgeVectorsInFace[towardsHe.next()] << endl;
    cout << "  wedge vec opp = " << geom.halfedgeVectorsInFace[towardsHe] << endl;
  }

  // TODO do some reasonable angular projection on the cartesian vector

  // Convert to barycentric
  Vector3 vecBaryCanonical = cartesianVectorToBarycentric(vertexCoords, vecCartesianDir) * vecCartesianLen;
  Vector3 vecBaryFromRoot = permuteBarycentricFromCanonical(vecBaryCanonical, towardsHe.next().next());

  if (TRACE_PRINT) {
    cout << "  canonical bary vec" << vecBaryCanonical << endl;
    cout << "  bary vec before projection " << vecBaryFromRoot << endl;
  }

  { // Project to ensure the vector is inside the triangle
    vecBaryFromRoot.x = std::fmin(vecBaryFromRoot.x, TRACE_EPS_TIGHT);
    vecBaryFromRoot.y = std::fmax(vecBaryFromRoot.y, 0.);
    vecBaryFromRoot.z = std::fmax(vecBaryFromRoot.z, 0.);

    // Manual displacement projection to sum to 0 while perserving above properties
    double diff = -sum(vecBaryFromRoot);
    if (diff > 0) {
      vecBaryFromRoot.y += diff / 2;
      vecBaryFromRoot.z += diff / 2;
    } else {
      vecBaryFromRoot.x += diff;
    }
  }

  if (TRACE_PRINT) {
    cout << "  bary vec after projection " << vecBaryFromRoot << endl;
  }

  // Assemble data to call the general trace function
  int iHe = halfedgeIndexInTriangle(towardsHe.next().next());
  Vector3 startPoint{0., 0., 0.};
  startPoint[iHe] = 1.0;
  Vector3 vecBaryCanonicalFixed = permuteBarycentricToCanonical(vecBaryFromRoot, rootHe);
  std::array<bool, 3> hittable = {{false, false, false}};
  hittable[(iHe + 1) % 3] = true;

  return traceInFaceBarycentric(geom, face, startPoint, vecBaryCanonicalFixed, vecCartesianDir, vecCartesianLen,
                                hittable, traceOptions);
}

// Trace within a face away from a given edge. The trace must hit one of the two opposite edges
//   - fromHe: the halfedge we enter from along the face
//   - tCrossFrom: t value in [0, 1] along fromHe we we enter the face
//   - traceVecInHalfedge: vector to trace, in the basis of fromHe
inline TraceSubResult traceInFaceFromEdge(IntrinsicGeometryInterface& geom, Halfedge fromHe, double tCrossFrom,
                                          Vector2 traceVecInHalfedgeDir, double traceVecInHalfedgeLen,
                                          const TraceOptions& traceOptions) {

  // Possibly terminate at this edge
  /*
  std::cout << "  norm tracevec = " << norm(traceVecInHalfedge) << std::endl;
  if (traceOptions.allowEndOnEdge && norm(traceVecInHalfedge) < traceOptions.allowEndOnEdgeEps) {
    std::cout << "TERMINATING AT EDGE" << std::endl;
    TraceSubResult result;
    result.terminated = true;
    result.endPoint = SurfacePoint(fromHe, tCrossFrom);
    result.incomingDirToPoint = traceVecInHalfedge;
    if (fromHe != fromHe.edge().halfedge()) result.incomingDirToPoint *= -1.;
    return result;
  }
  */

  // Gather some values
  Halfedge faceHe = fromHe.twin(); // the halfedge in hte face we're heading in to
  Face face = faceHe.face();
  std::array<Vector2, 3> vertexCoords = vertexCoordinatesInTriangle(geom, face);

  if (TRACE_PRINT) cout << "  face trace from edge " << fromHe << " vec = " << traceVecInHalfedgeDir << endl;

  // Project the cartesian vector to definitely point in the right direction
  Vector2 traceVecInFaceHalfedgeDir = -traceVecInHalfedgeDir;
  if (TRACE_PRINT) cout << "    vec in face before project " << traceVecInFaceHalfedgeDir << endl;
  traceVecInFaceHalfedgeDir.y = std::fmax(traceVecInFaceHalfedgeDir.y, TRACE_EPS_LOOSE);
  if (TRACE_PRINT) cout << "    vec in face after project " << traceVecInFaceHalfedgeDir << endl;

  // Convert to face coordinates
  Vector2 heDir = geom.halfedgeVectorsInFace[faceHe].normalize();
  Vector2 traceVecInFaceDir = heDir * traceVecInFaceHalfedgeDir;
  if (TRACE_PRINT) cout << "    traceVec in face " << traceVecInFaceDir << endl;

  // Convert to barycentric
  Vector3 vecBaryCanonicalDir = cartesianVectorToBarycentric(vertexCoords, traceVecInFaceDir);
  if (TRACE_PRINT) cout << "    vecBaryCanonical " << vecBaryCanonicalDir << endl;
  Vector3 vecBaryFromEdgeDir = permuteBarycentricFromCanonical(vecBaryCanonicalDir, faceHe);

  if (TRACE_PRINT) cout << "    vec bary before project " << vecBaryFromEdgeDir << endl;
  { // Project to ensure the vector is in the right direction
    vecBaryFromEdgeDir.z = std::fmax(vecBaryFromEdgeDir.z, TRACE_EPS_TIGHT);

    // Manual displacement projection to sum to 0 which perserves above properties
    double diff = -sum(vecBaryFromEdgeDir);
    if (diff > 0) {
      vecBaryFromEdgeDir.z += diff;
    } else {
      vecBaryFromEdgeDir.x += diff / 3.;
      vecBaryFromEdgeDir.y += diff / 3.;
      vecBaryFromEdgeDir.z += diff / 3.;
    }
  }
  if (TRACE_PRINT) cout << "    vec bary after project " << vecBaryFromEdgeDir << endl;

  // Project ensure tCrossFrom is valid
  tCrossFrom = clamp(tCrossFrom, 0., 1.);

  // Assemble data to call the general trace function
  int iHe = halfedgeIndexInTriangle(faceHe);
  Vector3 startPoint{0., 0., 0.};
  startPoint[iHe] = tCrossFrom; // notice: switched from what you'd expect becasue tCrossFrom is defined on twin
  startPoint[(iHe + 1) % 3] = 1.0 - tCrossFrom;
  Vector3 vecBaryCanonicalFixedDir = permuteBarycentricToCanonical(vecBaryFromEdgeDir, faceHe);
  if (TRACE_PRINT) {
    cout << "    iHe = " << iHe << endl;
    cout << "    startPoint = " << startPoint << endl;
    cout << "    canonical bary " << vecBaryCanonicalFixedDir << endl;
  }
  std::array<bool, 3> hittable = {{true, true, true}};
  hittable[iHe] = false;

  return traceInFaceBarycentric(geom, face, startPoint, vecBaryCanonicalFixedDir * traceVecInHalfedgeLen,
                                traceVecInFaceDir, traceVecInHalfedgeLen, hittable, traceOptions);
}

// Trace starting from an edge
inline TraceSubResult traceGeodesic_fromEdge(IntrinsicGeometryInterface& geom, Edge currEdge, double tEdge,
                                             Vector2 currVecDir, double currVecLen, const TraceOptions& traceOptions) {

  if (TRACE_PRINT) cout << "  edge trace " << currEdge << " tEdge = " << tEdge << " edge vec = " << currVecDir << endl;

  // Project to ensure tEdge is valid
  tEdge = clamp(tEdge, 0., 1.);

  // Find coordinates in adjacent face

  // Check which side of the face we're exiting
  Halfedge traceHe;
  Vector2 halfedgeTraceDir;
  if (currVecDir.y >= 0.) {
    traceHe = currEdge.halfedge().twin();
    halfedgeTraceDir = -currVecDir;
    tEdge = 1.0 - tEdge;
  } else {
    traceHe = currEdge.halfedge();

    // Can't go anyywhere if boundary halfedge
    if (!traceHe.twin().isInterior() || (traceOptions.barrierEdges && (*traceOptions.barrierEdges)[traceHe.edge()])) {
      TraceSubResult result;
      result.terminated = true;
      result.endPoint = SurfacePoint(currEdge, tEdge);
      result.incomingDirToPoint = currVecDir;

      return result;
    }

    halfedgeTraceDir = currVecDir;
  }

  return traceInFaceFromEdge(geom, traceHe, tEdge, halfedgeTraceDir, currVecLen, traceOptions);
}

// Trace starting from a face
inline TraceSubResult traceGeodesic_fromFace(IntrinsicGeometryInterface& geom, Face currFace, Vector3 faceBary,
                                             Vector2 currVecDir, double currVecLen, const TraceOptions& traceOptions) {

  // Convert the vector to barycentric
  std::array<Vector2, 3> vertexCoords = vertexCoordinatesInTriangle(geom, currFace);
  Vector3 vecBary = cartesianVectorToBarycentric(vertexCoords, currVecDir) * currVecLen;

  return traceInFaceBarycentric(geom, currFace, faceBary, vecBary, currVecDir, currVecLen, {true, true, true},
                                traceOptions);
}

// Trace starting from a vertex (with a rescaled cartesian vector)
inline TraceSubResult traceGeodesic_fromVertex(IntrinsicGeometryInterface& geom, Vertex currVert, Vector2 currVecDir,
                                               double currVecLen, const TraceOptions& traceOptions) {
  if (TRACE_PRINT) cout << "  vertex trace " << currVert << " edge vec = " << currVecDir << endl;

  // Find the halfedge opening the wedge where tracing will start
  Halfedge wedgeHe;
  Vector2 traceDirRelativeToStart;

  // Normally, one of the interval tests below will return positive and we'll simply launch the trace in to that
  // interval. However, due to numerical misfortune, it is possible that none of the intervals will test positive. In
  // that case, we'll simply launch along whichever halfedge was closest.
  double minCross = std::numeric_limits<double>::infinity();
  Halfedge minCrossHalfedge;
  Vector2 minCrossHalfedgeDir; // the trace vector in this closest halfedge

  Halfedge currHe = currVert.halfedge();
  do {

    // Once we hit the boundary we're done
    // (and traversal below doesn't work on boundary loop, so need to exit specially)
    if (!currHe.isInterior()) {
      break;
    }

    Halfedge nextHe = currHe.next().next().twin();

    // The interval spanned by this edge, which we are currently testing
    Vector2 intervalStart = geom.halfedgeVectorsInVertex[currHe].normalize();
    Vector2 intervalEnd = geom.halfedgeVectorsInVertex[nextHe].normalize();

    if (TRACE_PRINT) {
      cout << "  testing wedge " << intervalStart << " -- " << intervalEnd << endl;
      cout << "    testing wedge (un norm) " << geom.halfedgeVectorsInVertex[currHe] << " -- "
           << geom.halfedgeVectorsInVertex[nextHe] << endl;
      cout << "    corner angle " << geom.cornerAngles[currHe.corner()] << endl;
      cout << "    corner " << currHe.corner() << endl;
      Vector2 relAngle = intervalEnd / intervalStart;
      cout << "    wedge width " << relAngle << " radians: " << relAngle.arg() << endl;
    }

    // Check if our trace vector lies within the interval
    double crossStart = cross(intervalStart, currVecDir);
    double crossEnd = cross(intervalEnd, currVecDir);
    if (crossStart > 0. && crossEnd <= 0.) {
      wedgeHe = currHe;
      traceDirRelativeToStart = currVecDir / intervalStart;
      if (TRACE_PRINT) cout << "    wedge match! relative angle " << traceDirRelativeToStart << endl;
      if (TRACE_PRINT) cout << "    cross start = " << crossStart << " cross end = " << crossEnd << endl;
      break;
    }

    // Keep track of the closest halfedge, as described above
    if (std::fabs(crossStart) < minCross) {
      minCross = std::fabs(crossStart);
      minCrossHalfedge = currHe;
      minCrossHalfedgeDir = Vector2{1, TRACE_EPS_TIGHT};
    }
    if (std::fabs(crossEnd) < minCross) {
      minCross = std::fabs(crossEnd);
      minCrossHalfedge = nextHe;
      minCrossHalfedgeDir = Vector2{1, -TRACE_EPS_TIGHT};
    }

    currHe = nextHe;
  } while (currHe != currVert.halfedge());

  // None of the interval tests passed (probably due to unfortunate numerics), so just trace along the closest
  // halfedge
  if (wedgeHe == Halfedge()) {
    if (TRACE_PRINT) cout << "  no wedge worked. following closest edge with dir " << minCrossHalfedgeDir << endl;
    // Convert to edge coordinates
    currVecDir = convertVecToEdge(minCrossHalfedge, minCrossHalfedgeDir);
    return traceGeodesic_fromEdge(geom, minCrossHalfedge.edge(), convertTToEdge(minCrossHalfedge, 0.), currVecDir,
                                  currVecLen, traceOptions);
  }

  // Compute the actual starting face point, slightly inside and adjacent face
  Face startFace = wedgeHe.face();
  int iHe = halfedgeIndexInTriangle(wedgeHe);

  // Need to convert from "powered" representation to flat vector in face
  double sum = currVert.isBoundary() ? M_PI : 2. * M_PI;
  traceDirRelativeToStart = traceDirRelativeToStart.pow(geom.vertexAngleSums[currVert] / sum);
  traceDirRelativeToStart = traceDirRelativeToStart.normalize();

  // Compute the starting vector
  Vector2 startDirInFace = geom.halfedgeVectorsInFace[wedgeHe].normalize();
  Vector2 traceDirInFace = traceDirRelativeToStart * startDirInFace;
  if (TRACE_PRINT) {
    cout << "  starting vector" << endl;
    cout << "    start wedge vec " << geom.halfedgeVectorsInFace[wedgeHe] << endl;
    cout << "    start wedge vec unit " << geom.halfedgeVectorsInFace[wedgeHe].normalize() << endl;
    cout << "    trace dir in face " << traceDirInFace << endl;
  }

  return traceInFaceTowardsEdge(geom, wedgeHe.next(), traceDirInFace, currVecLen, traceOptions);
}

// Run tracing iteratively in faces, after on of the variants below has gotten it started.
// Will internally add the point path point encoded by prevTraceEnd, don't add beforehand.
void traceGeodesic_iterative(IntrinsicGeometryInterface& geom, TraceGeodesicResult& result, TraceSubResult prevTraceEnd,
                             const TraceOptions& traceOptions) {

  // Now, points are always in faces. Trace until termination.
  size_t iter = 0;
  while (!prevTraceEnd.terminated) {

    // Terminate on iterations
    if (traceOptions.maxIters != INVALID_IND && iter >= traceOptions.maxIters) {

      // Use the last trace as ending data
      result.endPoint = SurfacePoint(prevTraceEnd.crossHe, prevTraceEnd.tCross);
      result.endingDir = prevTraceEnd.traceVectorInHalfedgeDir;

      return;
    }

    // Construct a point where the previous trace ended
    if (traceOptions.includePath) {
      SurfacePoint currPoint(prevTraceEnd.crossHe.edge(), convertTToEdge(prevTraceEnd.crossHe, prevTraceEnd.tCross));
      result.pathPoints.push_back(currPoint);
    }

    if (TRACE_PRINT) {
      cout << "> tracing from " << prevTraceEnd.crossHe << " t = " << prevTraceEnd.tCross
           << " dir = " << prevTraceEnd.traceVectorInHalfedgeDir << endl;
    }

    // Execute the next step of tracing
    prevTraceEnd =
        traceInFaceFromEdge(geom, prevTraceEnd.crossHe, prevTraceEnd.tCross, prevTraceEnd.traceVectorInHalfedgeDir,
                            prevTraceEnd.traceVectorInHalfedgeLen, traceOptions);
    iter++;
  }

  // Add the final ending point
  if (traceOptions.includePath) {
    result.pathPoints.push_back(prevTraceEnd.endPoint);
  }
  result.endPoint = prevTraceEnd.endPoint;
  result.endingDir = prevTraceEnd.incomingDirToPoint;

  // if (std::abs(norm(result.endingDir) - 1.) > .1) throw std::runtime_error("norm problem");

  if (prevTraceEnd.endPoint.type == SurfacePointType::Edge) {
    result.hitBoundary = true;
  }
}

} // namespace

TraceGeodesicResult traceGeodesic(IntrinsicGeometryInterface& geom, SurfacePoint startP, Vector2 traceVec,
                                  const TraceOptions& traceOptions) {
  geom.requireVertexAngleSums();
  geom.requireHalfedgeVectorsInVertex();
  geom.requireHalfedgeVectorsInFace();

  // The output data
  TraceGeodesicResult result;
  result.hasPath = traceOptions.includePath;
  if (traceOptions.includePath) {
    result.pathPoints.push_back(startP);
  }

  if (TRACE_PRINT) cout << "\n>>> Trace query from " << startP << " vec = " << traceVec << endl;

  // Quick out with a zero vector
  if (traceVec.norm2() == 0) {
    geom.unrequireVertexAngleSums();
    geom.unrequireHalfedgeVectorsInVertex();
    geom.unrequireHalfedgeVectorsInFace();

    result.endingDir = Vector2::zero();

    // probably want to ensure we still return a point in a face...
    if (traceOptions.errorOnProblem) {
      throw std::runtime_error("zero vec passed to trace, do something good here");
    }

    return result;
  }

  // Trace the first point, based on what kind of input we got
  TraceSubResult prevTraceEnd;
  switch (startP.type) {
  case SurfacePointType::Vertex: {
    prevTraceEnd = traceGeodesic_fromVertex(geom, startP.vertex, unit(traceVec), norm(traceVec), traceOptions);
    break;
  }
  case SurfacePointType::Edge: {
    prevTraceEnd =
        traceGeodesic_fromEdge(geom, startP.edge, startP.tEdge, unit(traceVec), norm(traceVec), traceOptions);
    break;
  }
  case SurfacePointType::Face: {
    prevTraceEnd =
        traceGeodesic_fromFace(geom, startP.face, startP.faceCoords, unit(traceVec), norm(traceVec), traceOptions);
    break;
  }
  }

  // Keep tracing through triangles until finished
  traceGeodesic_iterative(geom, result, prevTraceEnd, traceOptions);

  geom.unrequireVertexAngleSums();
  geom.unrequireHalfedgeVectorsInVertex();
  geom.unrequireHalfedgeVectorsInFace();

  return result;
}

TraceGeodesicResult traceGeodesic(IntrinsicGeometryInterface& geom, Face startFace, Vector3 startBary,
                                  Vector3 traceBaryVec, const TraceOptions& traceOptions) {

  geom.requireVertexAngleSums();
  geom.requireHalfedgeVectorsInVertex();
  geom.requireHalfedgeVectorsInFace();

  // The output data
  TraceGeodesicResult result;
  result.hasPath = traceOptions.includePath;
  if (traceOptions.includePath) {
    result.pathPoints.push_back(SurfacePoint(startFace, startBary));
  }

  if (TRACE_PRINT) {
    cout << "\n>>> Trace query (barycentric) from " << startFace << " " << startBary << " vec = " << traceBaryVec
         << endl;
  }

  // Early-out if zero
  if (traceBaryVec.norm2() == 0) {
    geom.unrequireVertexAngleSums();
    geom.unrequireHalfedgeVectorsInVertex();
    geom.unrequireHalfedgeVectorsInFace();

    // probably want to ensure we still return a point in a face...
    if (traceOptions.errorOnProblem) {
      throw std::runtime_error("zero vec passed to trace, do something good here");
    }

    result.endingDir = Vector2::zero();
    return result;
  }

  // Make sure the input is sane
  startBary = projectInsideTriangle(startBary);
  traceBaryVec -= Vector3::constant(sum(traceBaryVec) / 3);

  // Construct the cartesian equivalent
  std::array<Vector2, 3> vertexCoords = vertexCoordinatesInTriangle(geom, startFace);
  Vector2 traceVectorCartesian = barycentricDisplacementToCartesian(vertexCoords, traceBaryVec);

  // Trace the first point starting inside the face
  TraceSubResult prevTraceEnd =
      traceInFaceBarycentric(geom, startFace, startBary, traceBaryVec, unit(traceVectorCartesian),
                             norm(traceVectorCartesian), {true, true, true}, traceOptions);

  // Keep tracing through triangles until finished
  traceGeodesic_iterative(geom, result, prevTraceEnd, traceOptions);

  geom.unrequireVertexAngleSums();
  geom.unrequireHalfedgeVectorsInVertex();
  geom.unrequireHalfedgeVectorsInFace();

  return result;
}

bool trimTraceResult(TraceGeodesicResult& traceResult, Vertex targetVertex) {

  while (traceResult.pathPoints.size() > 1) {
    SurfacePoint& b = traceResult.pathPoints.back();

    // Remove any edge crossings connected to the target vertex: they're numerical noise because we're already in the
    // 1-ring
    if (b.type == SurfacePointType::Edge &&
        (b.edge.halfedge().vertex() == targetVertex || b.edge.halfedge().twin().vertex() == targetVertex)) {
      traceResult.pathPoints.pop_back();
      traceResult.endingDir = Vector2::undefined();
      continue;
    }

    // Always trim face points
    if (b.type == SurfacePointType::Face) {
      traceResult.pathPoints.pop_back();
      traceResult.endingDir = Vector2::undefined();
      continue;
    }

    // Always trim vertex points
    if (b.type == SurfacePointType::Vertex) {
      traceResult.pathPoints.pop_back();
      traceResult.endingDir = Vector2::undefined();
      continue;
    }

    // we're done here
    break;
  }

  // Check success
  if (traceResult.pathPoints.empty()) return false;

  SurfacePoint& b = traceResult.pathPoints.back();

  switch (b.type) {
  case SurfacePointType::Vertex: {
    if (b.vertex == targetVertex) return true;
    for (Vertex n : b.vertex.adjacentVertices()) {
      if (n == targetVertex) return true;
    }
    break;
  }
  case SurfacePointType::Edge: {
    Halfedge bHe = b.edge.halfedge();
    if (bHe.vertex() == targetVertex) return true;
    if (bHe.twin().vertex() == targetVertex) return true;
    if (bHe.next().next().vertex() == targetVertex) return true;
    if (bHe.twin().next().next().vertex() == targetVertex) return true;
    return false;
    break;
  }
  case SurfacePointType::Face: {
    for (Vertex v : b.face.adjacentVertices()) {
      if (v == targetVertex) return true;
    }
    return false;
    break;
  }
  }

  return false;
}

} // namespace surface
} // namespace geometrycentral


// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>
#include <OpenTissue/core/spline/spline.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>

// Boost Test declaration and Checking macros
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/test_tools.hpp>
#include <boost/test/floating_point_comparison.hpp>

typedef OpenTissue::spline::MathTypes<double, size_t>    math_types;
typedef math_types::vector_type                                vector_type;
typedef std::vector<double>                                    knot_container;
typedef std::vector<vector_type>                               point_container;

typedef OpenTissue::spline::NUBSpline<knot_container, point_container> spline_type;

BOOST_AUTO_TEST_SUITE(opentissue_spline_compute_point_on_spline);

BOOST_AUTO_TEST_CASE(test_compute_point_on_spline)
{
  knot_container U;

  U.push_back(0.0);
  U.push_back(0.0);
  U.push_back(0.0);  //k = 3
  U.push_back(1.0);
  U.push_back(2.0);
  U.push_back(3.0);
  U.push_back(4.0);  // n = 6  => |P| = 7
  U.push_back(5.0);
  U.push_back(5.0);
  U.push_back(5.0);  // m = 9  => |U| = 10

  // Indices of basis functions belongs to the interval [0..n]

  point_container P;
  vector_type p0(2);  p0(0) = 0.0; p0(1) = 0.0;
  vector_type p1(2);  p1(0) = 1.0; p1(1) = 0.0;
  vector_type p2(2);  p2(0) = 2.0; p2(1) = 0.0;
  vector_type p3(2);  p3(0) = 3.0; p3(1) = 0.0;
  vector_type p4(2);  p4(0) = 4.0; p4(1) = 0.0;
  vector_type p5(2);  p5(0) = 5.0; p5(1) = 0.0;
  vector_type p6(2);  p6(0) = 6.0; p6(1) = 0.0;

  P.push_back(p0);
  P.push_back(p1);
  P.push_back(p2);
  P.push_back(p3);
  P.push_back(p4);
  P.push_back(p5);
  P.push_back(p6);

  spline_type spline(3,U,P);

  double const tolerance = 0.00001;

  vector_type q0(2);
  vector_type q1(2);
  vector_type q2(2);
  vector_type q3(2);
  vector_type q4(2);
  vector_type q5(2);
  vector_type q6(2);

  double const du = 5.0/6.0;
  double u = 0.0;
  OpenTissue::spline::compute_point_on_spline(u, spline, q0);
  u += du;
  OpenTissue::spline::compute_point_on_spline(u, spline, q1);
  u += du;
  OpenTissue::spline::compute_point_on_spline(u, spline, q2);
  u += du;
  OpenTissue::spline::compute_point_on_spline(u, spline, q3);
  u += du;
  OpenTissue::spline::compute_point_on_spline(u, spline, q4);
  u += du;
  OpenTissue::spline::compute_point_on_spline(u, spline, q5);
  u += du;
  OpenTissue::spline::compute_point_on_spline(u, spline, q6);

  BOOST_CHECK_CLOSE( p0(0), q0(0), tolerance );
  BOOST_CHECK_CLOSE( p0(1), q0(1), tolerance );

  BOOST_CHECK_CLOSE( q0(1), 0.0, tolerance );
  BOOST_CHECK_CLOSE( q1(1), 0.0, tolerance );
  BOOST_CHECK_CLOSE( q2(1), 0.0, tolerance );
  BOOST_CHECK_CLOSE( q3(1), 0.0, tolerance );
  BOOST_CHECK_CLOSE( q4(1), 0.0, tolerance );
  BOOST_CHECK_CLOSE( q5(1), 0.0, tolerance );
  BOOST_CHECK_CLOSE( q6(1), 0.0, tolerance );

  BOOST_CHECK( q1(0) > q0(0));
  BOOST_CHECK( q2(0) > q1(0));
  BOOST_CHECK( q3(0) > q2(0));
  BOOST_CHECK( q4(0) > q3(0));
  BOOST_CHECK( q5(0) > q4(0));
  BOOST_CHECK( q6(0) > q5(0));

  BOOST_CHECK_CLOSE( p6(0), q6(0), tolerance );
  BOOST_CHECK_CLOSE( p6(1), q6(1), tolerance );
}

BOOST_AUTO_TEST_CASE(test_bezier_curve_case)
{
  knot_container U;

  U.push_back(0.0);
  U.push_back(0.0);
  U.push_back(0.0);
  U.push_back(0.0);
  U.push_back(1.0);
  U.push_back(1.0);
  U.push_back(1.0);
  U.push_back(1.0);

  // k = 4
  // n = 3  => |P| = 4
  // m = 7  => |U| = 8

  point_container P;
  vector_type p0(2);  p0(0) = 0.0; p0(1) = 0.0;
  vector_type p1(2);  p1(0) = 1.0; p1(1) = 1.0;
  vector_type p2(2);  p2(0) = 2.0; p2(1) = 1.0;
  vector_type p3(2);  p3(0) = 3.0; p3(1) = 0.0;

  P.push_back(p0);
  P.push_back(p1);
  P.push_back(p2);
  P.push_back(p3);

  spline_type bezier(4,U,P);

  double const tolerance = 10e-15;

  // Now we have constructed the equivalent of a bezier curve
  //
  //   B(t) =     a t^3 +   b t^2 + c t + d
  //   B'(t) =  3 a t^2 + 2 b t   + c
  //
  //   B(0) = p0  = d
  //   B(1) = p3  = a + b + c + d
  //   B'(0) = 3*(p1-p0) = c
  //   B'(1) = 3*(p1-p0) = 3 a + 2 b + c
  //
  //  working out the equations one would get
  vector_type a = -1*p0 + 3*p1 - 3*p2 + 1*p3;
  vector_type b =  3*p0 - 6*p1 + 3*p2;
  vector_type c = -3*p0 + 3*p1;
  vector_type d =  1*p0;

  double u = 0.0;
  int const N = 11;
  double const du = 1.0 / (N-1);
  for(int i=0;i<N;++i)
  {
    vector_type p(2);
    OpenTissue::spline::compute_point_on_spline(u, bezier, p);
    vector_type q(2);
    q = (u*u*u)*a + (u*u)*b + u*c + d;
    BOOST_CHECK( std::fabs( p(0) - q(0) ) < tolerance );
    BOOST_CHECK( std::fabs( p(1) - q(1) ) < tolerance );
    u += du;
  }
}

BOOST_AUTO_TEST_SUITE_END();


//#define EIGEN_RUNTIME_NO_MALLOC

#include "linmath.h"
#include "sv_matrix.h"
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/QR>
#include <Eigen/SVD>

#include <iostream>

#ifdef EIGEN_RUNTIME_NO_MALLOC
#define EIGEN_RUNTIME_SET_IS_MALLOC_ALLOWED(v) EIGEN_RUNTIME_SET_IS_MALLOC_ALLOWED(v)
#else
#define EIGEN_RUNTIME_SET_IS_MALLOC_ALLOWED(v)
#endif

#ifdef SV_MATRIX_IS_COL_MAJOR
typedef Eigen::Matrix<FLT, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor, 50, 50> MatrixType;
#else
typedef Eigen::Matrix<FLT, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor, 50, 50> MatrixType;
#endif
typedef Eigen::Map<MatrixType> MapType;

#define CONVERT_TO_EIGEN(A) MapType(A ? SV_FLT_PTR(A) : 0, A ? (A)->rows : 0, A ? (A)->cols : 0)

double svInvert(const SvMat *srcarr, SvMat *dstarr, enum svInvertMethod method) {
	auto src = CONVERT_TO_EIGEN(srcarr);
	auto dst = CONVERT_TO_EIGEN(dstarr);

	EIGEN_RUNTIME_SET_IS_MALLOC_ALLOWED(false);
	if (method == SV_INVERT_METHOD_LU) {
		dst.noalias() = src.inverse();
	} else {
		dst.noalias() = src.completeOrthogonalDecomposition().pseudoInverse();
	}
	return 0;
}

extern "C" void svGEMM(const SvMat *_src1, const SvMat *_src2, double alpha, const SvMat *_src3, double beta,
					   SvMat *_dst, enum svGEMMFlags tABC) {
	auto src1 = CONVERT_TO_EIGEN(_src1);
	auto src2 = CONVERT_TO_EIGEN(_src2);

	auto dst = CONVERT_TO_EIGEN(_dst);

	EIGEN_RUNTIME_SET_IS_MALLOC_ALLOWED(false);
	if (tABC & SV_GEMM_FLAG_A_T)
		if (tABC & SV_GEMM_FLAG_B_T)
			dst.noalias() = alpha * src1.transpose() * src2.transpose();
		else
			dst.noalias() = alpha * src1.transpose() * src2;
	else {
		if (tABC & SV_GEMM_FLAG_B_T)
			dst.noalias() = alpha * src1 * src2.transpose();
		else
			dst.noalias() = alpha * src1 * src2;
	}

	if (_src3) {
		auto src3 = CONVERT_TO_EIGEN(_src3);
		if (tABC & SV_GEMM_FLAG_C_T)
			dst.noalias() += beta * src3.transpose();
		else
			dst.noalias() += beta * src3;
	}
}

const int DECOMP_SVD = 1;
const int DECOMP_LU = 2;

extern "C" int svSolve(const SvMat *_Aarr, const SvMat *_Barr, SvMat *_xarr, enum svInvertMethod method) {
	auto Aarr = CONVERT_TO_EIGEN(_Aarr);
	auto Barr = CONVERT_TO_EIGEN(_Barr);
	auto xarr = CONVERT_TO_EIGEN(_xarr);

	if (method == SV_INVERT_METHOD_LU) {
		xarr.noalias() = Aarr.partialPivLu().solve(Barr);
	} else if (method == SV_INVERT_METHOD_QR) {
		xarr.noalias() = Aarr.colPivHouseholderQr().solve(Barr);
	} else {
		EIGEN_RUNTIME_SET_IS_MALLOC_ALLOWED(true);
		auto svd = Aarr.bdcSvd(
			Eigen::ComputeFullU |
			Eigen::ComputeFullV); // Eigen::JacobiSVD<MatrixType>(Aarr, Eigen::ComputeFullU | Eigen::ComputeFullV);
		EIGEN_RUNTIME_SET_IS_MALLOC_ALLOWED(false);
		xarr.noalias() = svd.solve(Barr);
	}
	return 0;
}

extern "C" void svSVD(SvMat *aarr, SvMat *warr, SvMat *uarr, SvMat *varr, enum svSVDFlags flags) {
	auto aarrEigen = CONVERT_TO_EIGEN(aarr);
	auto warrEigen = CONVERT_TO_EIGEN(warr);

	int options = 0;
	if (uarr)
		options |= Eigen::ComputeFullU;
	if (varr)
		options |= Eigen::ComputeFullV;
	EIGEN_RUNTIME_SET_IS_MALLOC_ALLOWED(true);
	auto svd = aarrEigen.bdcSvd(options);
	EIGEN_RUNTIME_SET_IS_MALLOC_ALLOWED(false);

	if (warrEigen.cols() == 1) {
		warrEigen.noalias() = svd.singularValues();
	} else if (warrEigen.rows() == 1) {
		warrEigen.noalias() = svd.singularValues().transpose();
	} else {
		warrEigen.diagonal().noalias() = svd.singularValues();
	}

	if (uarr) {
		auto uarrEigen = CONVERT_TO_EIGEN(uarr);
		if (flags & SV_SVD_U_T)
			uarrEigen.noalias() = svd.matrixU().transpose();
		else
			uarrEigen.noalias() = svd.matrixU();
	}

	if (varr) {
		auto varrEigen = CONVERT_TO_EIGEN(varr);
		if (flags & SV_SVD_V_T)
			varrEigen.noalias() = svd.matrixV().transpose();
		else
			varrEigen.noalias() = svd.matrixV();
	}
}

void svMulTransposed(const SvMat *src, SvMat *dst, int order, const SvMat *delta, double scale) {
	auto srcEigen = CONVERT_TO_EIGEN(src);
	auto dstEigen = CONVERT_TO_EIGEN(dst);

	if (delta) {
		auto deltaEigen = CONVERT_TO_EIGEN(delta);
		if (order == 0)
			dstEigen.noalias() = scale * (srcEigen - deltaEigen) * (srcEigen - deltaEigen).transpose();
		else
			dstEigen.noalias() = scale * (srcEigen - deltaEigen).transpose() * (src - delta);
	} else {
		if (order == 0)
			dstEigen.noalias() = scale * srcEigen * srcEigen.transpose();
		else
			dstEigen.noalias() = scale * srcEigen.transpose() * srcEigen;
	}
}

void svTranspose(const SvMat *M, SvMat *dst) {
	auto src = CONVERT_TO_EIGEN(M);
	auto dstEigen = CONVERT_TO_EIGEN(dst);
	if (SV_FLT_PTR(M) == SV_FLT_PTR(dst))
		dstEigen = src.transpose().eval();
	else
		dstEigen.noalias() = src.transpose();
}

void print_mat(const SvMat *M);

double svDet(const SvMat *M) {
	EIGEN_RUNTIME_SET_IS_MALLOC_ALLOWED(false);
	auto MEigen = CONVERT_TO_EIGEN(M);
	return MEigen.determinant();
}


/** @file main.cpp Demonstrates purely functional, tail-recursive,
  * arbitrary-precision factorial and Fibonacci functions using
  * Boost.Multiprecision.
  *
  * @note This implementation uses a concise style, passing and returning
  * objects by value.
  */

#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>

using BigInt = boost::multiprecision::cpp_int;

BigInt factorial(int n, BigInt r =1) {
    return n ? factorial(n - 1, r * n) : r;
}

BigInt fibonacci(int n, BigInt a =1, BigInt b =1) {
    return n ? fibonacci(n - 1, b, a + b) : a;
}

int main() {
    for (int i = 0; i < 100; ++i)
        std::cout << i << ' ' << factorial(i) << ' ' << fibonacci(i) << '\n';
}


#pragma once

// C++ standard library
#include <cmath>
#include <iostream>
#include <functional>

// Armadillo
#include <armadillo>

// Mantella
#include <mantella>

namespace mant{
  namespace itd{

    double stumpffC(
        const double parameter){
      if(parameter > 0){
        return (1.0 - std::cos(std::sqrt(parameter))) / parameter;
      }else if (parameter < 0){
        return (std::cosh(std::sqrt(-parameter)) - 1.0) / -parameter;
      }else{
        return 0.5;
      }
    }

    double stumpffS(
        const double parameter){

      if(parameter > 0){
        return (std::sqrt(parameter) - std::sin(std::sqrt(parameter))) / std::pow(std::sqrt(parameter), 3.0);
      }else if (parameter < 0){
        return (std::sinh(std::sqrt(-parameter)) - std::sqrt(-parameter)) / std::pow(std::sqrt(-parameter), 3.0);
      }else{
        return 1.0 / 6.0;
      }
    }

    std::pair<arma::Col<double>::fixed<3>, arma::Col<double>::fixed<3>> lambert(
        const arma::Col<double>::fixed<3> &startPosition,
        const arma::Col<double>::fixed<3> &endPosition,
        const double flightTime,
        const bool isPrograde){

      double startPositionNorm = arma::norm(startPosition);
      double endPositionNorm = arma::norm(endPosition);

      arma::Col<double> positionsCrossProduct = arma::cross(startPosition, endPosition);
      double theta = std::acos(arma::norm_dot(startPosition, endPosition));

      if(isPrograde){
        if(positionsCrossProduct(2) < 0){
          theta = 2.0 * arma::datum::pi - theta;
        }
      }else{
        if(positionsCrossProduct(2) >= 0){
          theta = 2.0 * arma::datum::pi - theta;
        }
      }

      double a = std::sin(theta) * std::sqrt((startPositionNorm * endPositionNorm) / (1.0 - std::cos(theta)));

      std::function<double(const double)> yFunction = [&](
          const double parameter){
        return startPositionNorm + endPositionNorm + a * ((parameter * stumpffS(parameter) - 1.0) / std::sqrt(stumpffC(parameter)));
      };

      double z = mant::brent(
        [&](
            const double parameter) {

          return stumpffS(parameter) * std::pow(yFunction(parameter) / stumpffC(parameter), 3.0/2.0) + a * std::sqrt(yFunction(parameter)) - std::sqrt(3.986e+5) * flightTime; //heliocentric should be 1.32712440018e11

        }, -2.0, 2.0, 100); //TODO: remove magic numbers

      double y = yFunction(z);

      double f = 1.0 - y / startPositionNorm;
      double g = a * std::sqrt(y / 3.986e+5); //heliocentric should be 1.32712440018e11
      double gDot = 1.0 - y / endPositionNorm;

      return {{(1.0 / g) * (endPosition - f * startPosition)}, {(1.0 / g) * (gDot * endPosition - startPosition)}};

    }

    std::pair<arma::Col<double>::fixed<3>, arma::Col<double>::fixed<3>> positionAndVelocityOnOrbit(
        const arma::Col<double>::fixed<6>& keplerValues,
        const arma::Col<double>::fixed<6>& dKeplerValues,
        const arma::Col<double>::fixed<6>& date) {
      // parameters
      // -keplerValues as known (a, e, i, omega, w, L)
      // -date in [yyyy,(mon)mon, (d)d, (h)h, (min)min, (s)s]

      double jd = 367.0 * date(0) - std::trunc((7.0 * (date(0) + std::trunc((date(1) + 9.0) / 12.0))) / 4.0) + std::trunc(275.0 * date(1) / 9.0) + date(2) + 1721013.5 + (date(3) + date(4) / 60.0 + date(5) / 3600.0) / 24;

      double t0 = (jd - 2451545.0) / 36525.0;

      arma::Col<double>::fixed<6> keplerValAtTime = keplerValues + dKeplerValues * t0;
      keplerValAtTime(0) = keplerValAtTime(0) * 149597871.464; //au to km
      keplerValAtTime(2) = std::fmod(keplerValAtTime(2), 360.0) * arma::datum::pi / 180.0;
      keplerValAtTime(3) = std::fmod(keplerValAtTime(3), 360.0) * arma::datum::pi / 180.0;
      keplerValAtTime(4) = std::fmod(keplerValAtTime(4), 360.0) * arma::datum::pi / 180.0;
      keplerValAtTime(5) = std::fmod(keplerValAtTime(5), 360.0) * arma::datum::pi / 180.0;

      double angularMomentum = std::sqrt(1.32712440018e11 * keplerValAtTime(0) * (1.0 - std::pow(keplerValAtTime(1), 2.0))); //sun gravity!!!

      double argumentPerihelion = keplerValAtTime(4) - keplerValAtTime(3);
      double meanAnomaly = keplerValAtTime(5) - keplerValAtTime(4);

      double eccentricAnomaly0;
      if(meanAnomaly < arma::datum::pi) {
        eccentricAnomaly0 =  meanAnomaly + keplerValAtTime(1) / 2.0;
      } else {
        eccentricAnomaly0 =  meanAnomaly - keplerValAtTime(1) / 2.0;
      }

      double eccentricAnomaly = mant::brent(
        [&](
            const double parameter) {

          return parameter - keplerValAtTime(1) * std::sin(parameter) - meanAnomaly;

        }, -10.0, 10.0, 100); //TODO: remove magic numbers

      double trueAnomaly = std::fmod(2.0 * std::atan(std::sqrt((1.0 + keplerValAtTime(1)) / (1.0 - keplerValAtTime(1))) * std::tan(eccentricAnomaly / 2.0)), 2.0 * arma::datum::pi);

      arma::Col<double>::fixed<3> helperVectorPos = {std::cos(trueAnomaly), std::sin(trueAnomaly), 0};
      arma::Col<double>::fixed<3> perifocalPosition = (std::pow(angularMomentum, 2.0) / 1.32712440018e11) * (1.0 / (1.0 + keplerValAtTime(1) * std::cos(trueAnomaly))) * helperVectorPos; //sun gravity!!!

      arma::Col<double>::fixed<3> helperVectorVel = {-std::sin(trueAnomaly), (keplerValAtTime(1) + std::cos(trueAnomaly)), 0};
      arma::Col<double>::fixed<3> perifocalVelocity = (1.32712440018e11 / angularMomentum) * helperVectorVel; //sun gravity!!!

      arma::Mat<double>::fixed<3, 3> R3_W = {
        {std::cos(keplerValAtTime(3)),  std::sin(keplerValAtTime(3)), 0.0},
        {-std::sin(keplerValAtTime(3)), std::cos(keplerValAtTime(3)), 0.0},
        {0.0,                           0.0,                          1.0}
      };

      arma::Mat<double>::fixed<3, 3> R1_i = {
        {1.0, 0.0, 0.0},
        {0.0, std::cos(keplerValAtTime(2)), std::sin(keplerValAtTime(2))},
        {0.0, std::sin(keplerValAtTime(2)), std::cos(keplerValAtTime(2))}
      };

      arma::Mat<double>::fixed<3, 3> R3_w = {
        {std::cos(argumentPerihelion),  std::sin(argumentPerihelion), 0.0},
        {-std::sin(argumentPerihelion), std::cos(argumentPerihelion), 0.0},
        {0.0,                           0.0,                          1.0}
      };

      arma::Mat<double>::fixed<3, 3> Q_pX = (R3_w * R1_i * R3_W).t();

      return {{Q_pX * perifocalPosition},{Q_pX * perifocalVelocity}};
    }

  }
}


#include <matplotlibcpp.h>
#include "../simple_lib/include/simple_activation.h"
#include <Eigen/Dense>
#include <vector>

namespace plt = matplotlibcpp;
using namespace Eigen;

int main(){
    using std::vector;

    VectorXd x = VectorXd::LinSpaced(101, -5, 5);
    VectorXd y;

    y = x.unaryExpr([](double p){return MyDL::sigmoid<double>(p);});

    // 変換する要素数で初期化する必要あり
    vector<double> x_vec(101), y_vec(101);

    // 直接EigenのVectorを扱えないので、STLのコンテナに移し替えて使う
    Map<VectorXd>(&x_vec[0], 101) = x;
    Map<VectorXd>(&y_vec[0], 101) = y;

    plt::named_plot("sigmoid", x_vec, y_vec);
    plt::grid(true);
    plt::legend();
    plt::save("./ch3/images/sigmoid.png");

    return 0;
}


/*=============================================================================

  LeastSquaresPackage: A software package for estimating the rotation and translation of rigid bodies.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include "catch.hpp"
#include "lsqCatchMain.h"
#include "lsqComputeRotation.h"
#include <Eigen/Dense>
#include <iostream>

TEST_CASE( "Compute the rotation with the SVD methods", "[svd_rotation]" ) {

  std::unique_ptr<lsq::ComputeRotation> algorithm( new lsq::SVDMethod() );

  Eigen::Matrix3d input_H_matrix = Eigen::Matrix3d::Zero(3, 3);
  Eigen::Matrix3d expected_rotation_matrix = Eigen::Matrix3d::Zero(3, 3);
  Eigen::Matrix3d obtained_rotation_matrix;

  SECTION( "The method fails because the rotation has det = -1 and the singular values are non-negative." ) {
    input_H_matrix(0,2) = -0.2;
    input_H_matrix(1,1) = -0.5;
    input_H_matrix(2,0) = 1.;

    REQUIRE_THROWS( obtained_rotation_matrix = algorithm->find_rotation(input_H_matrix) );
  }

  SECTION( "The method succeeds in finding the rotation, but has to modify the rotation to avoid a reflection." ) {
    input_H_matrix(1,1) = -0.3;
    input_H_matrix(2,0) = -1.;

    expected_rotation_matrix(0,2) = -1.;
    expected_rotation_matrix(1,1) = -1.;
    expected_rotation_matrix(2,0) = -1.;

    obtained_rotation_matrix = algorithm->find_rotation(input_H_matrix);

    REQUIRE( expected_rotation_matrix == obtained_rotation_matrix );

  }

  SECTION( "The method succeeds in finding the rotation." ) {
    input_H_matrix(1,1) = -0.3;
    input_H_matrix(2,0) = 1.;

    expected_rotation_matrix(0,2) = 1.;
    expected_rotation_matrix(1,1) = -1.;
    expected_rotation_matrix(2,0) = 1.;

    obtained_rotation_matrix = algorithm->find_rotation(input_H_matrix);

    REQUIRE( expected_rotation_matrix == obtained_rotation_matrix );

  }

}

TEST_CASE( "Compute the rotation with the Quaternion methods", "[quad_rotation]" ) {

  std::unique_ptr<lsq::ComputeRotation> algorithm( new lsq::QuaternionMethod() );

  Eigen::Matrix3d input_H_matrix = Eigen::Matrix3d::Zero(3, 3);
  Eigen::Matrix3d expected_rotation_matrix = Eigen::Matrix3d::Zero(3, 3);
  Eigen::Matrix3d obtained_rotation_matrix;

  input_H_matrix(1,1) = -0.3;
  input_H_matrix(2,0) = 1.;

  expected_rotation_matrix(0,2) = 1.;
  expected_rotation_matrix(1,1) = -1.;
  expected_rotation_matrix(2,0) = 1.;

  obtained_rotation_matrix = algorithm->find_rotation(input_H_matrix);

  REQUIRE( obtained_rotation_matrix.isApprox(expected_rotation_matrix, 1.e-10) );

}


#pragma once

#include <Eigen/Core>
#include <array>
#include <iostream>
#include <string>
#include <vector>

using Bounds = std::array<double, 2>;

class ShockTubeSolver
{
public:
    ShockTubeSolver(Bounds calc_bounds, double endtime, double dt, int n_cell);
    ~ShockTubeSolver();

    void SetGrid();
    void SetFlowField(double wall_position, double rhoL, double pL, double rhoR,
                      double pR);
    void SetSchemes(std::string convectiveflux_scheme, bool MUSCL, double k);
    void Solve();
    void WriteFlowFile(std::string filename);

    inline int Get_n_cells() const { return n_cells_; };
    inline double Get_dx() const { return dx_; };

private:
    std::vector<double> cells;
    std::vector<Eigen::Vector3d> flowVars;
    std::vector<Eigen::Vector3d> flowConservatives;

    Eigen::Vector3d PrimToCons(Eigen::Vector3d prim);
    Eigen::Vector3d ConsToPrim(Eigen::Vector3d cons);

    // Euler flux
    Eigen::Vector3d Roe(Eigen::Vector3d primL, Eigen::Vector3d primR);
    Eigen::Vector3d vanLeer(Eigen::Vector3d primL, Eigen::Vector3d primR);
    Eigen::Vector3d AUSM(Eigen::Vector3d primL, Eigen::Vector3d primR);

    // Limiter
    double minmod(double a, double b);

    int n_cells_;
    int n_timestep_;
    double dx_;
    Bounds calc_bounds_;
    double endtime_;
    double dt_;
    double CFL_;

    const double GAMMA = 1.40;

    std::string convectiveflux_scheme_;
    bool MUSCL_;
    //! A coefficient of MUSCL scheme
    //! k = -1: 2nd order upwind differencing
    //! k = 0: 2nd order upwind-biased differencing
    //! k = 1/3: 3rd order upwind-biased differencing
    double k_;
};


/*
    Boost Competency Test - GSoC 2020
    digu_J - Digvijay Janartha
    NIT Hamirpur - INDIA
*/

#include <algorithm>
#include <iostream>
#include <utility>

#include "../../includes/concave_hull_k_nearest_neighbours_optimized.hpp"

#include <boost/geometry/geometry.hpp>

namespace bg = boost::geometry;
using bg::dsv;

int main()
{
    typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;
    typedef bg::model::multi_point<point_t> mpoint_t;

    mpoint_t mpt1, hull;
    bg::read_wkt("MULTIPOINT(0 0,6 0,3 1,3 2,4 2,5 3,5 5,4 4,-2 2)", mpt1);

    int k = 3;
    algo3::ConcaveHullKNN(mpt1, hull, k);

    std::cout << "Dataset: " << dsv(mpt1) << std::endl;
    std::cout << "Concave hull for k = " << k << ": " << dsv(hull) << std::endl;

    return 0;
}

/*
Output:
Dataset: ((0, 0), (6, 0), (3, 1), (3, 2), (4, 2), (5, 3), (5, 5), (4, 4), (-2, 2))
Concave hull for k = 3: ((-2, 2), (0, 0), (3, 1), (6, 0), (5, 5), (-2, 2))
*/


#include <cmath>
#include <iostream>
#include <string>
#include <boost/mpi.hpp>

#include "dla.h"
#include "graph_printer.h"

using namespace std;

int main(int argc, char *argv[])
{
  boost::mpi::environment env{argc, argv};
  boost::mpi::communicator world;

  int total_num_graphs = stoi(string(argv[1]));
  int graph_per_process = int(ceil(double(total_num_graphs) / double(world.size())));
  total_num_graphs = graph_per_process * world.size();

  DLA_params params(world.rank(), argc-2, argv+2);
  int seed = 577 + world.rank();
  rgen.seed(seed);

  GraphPrinter graph_printer(string("DLA"), false, true, total_num_graphs);
  if (world.rank() == printer_rank) {
    cout << params;
    cout << "Producing " << total_num_graphs << " on " << world.size() << " processes" << endl;
    graph_printer.open();
  }
  for (int graph_id = 0; graph_id < graph_per_process; ++graph_id) {
    produce_graph(world, graph_printer, params);
  }
}


#pragma once

#include <Eigen/Dense>

namespace icarus
{
    template<typename T, size_t N>
    struct OffsetCalibration
    {
        explicit OffsetCalibration()
        {
            mOffset.setZero();
        }

        explicit OffsetCalibration(Eigen::Matrix<T, N, 1> const & offset) :
            mOffset(-offset)
        {}

        Eigen::Matrix<T, N, 1> adjust(Eigen::Matrix<T, N, 1> const & reading) const
        {
            return reading + mOffset;
        }
    private:
        Eigen::Matrix<T, N, 1> mOffset;
    };
}


#pragma once

#include "constants.hpp"
#include <Eigen/Core>

namespace math {
template <typename T>
T to_radians(T degree) {
    return degree / static_cast<T>(180.0) * pi<T>;
}

template <typename T, int rows, int cols>
Eigen::Matrix<T, rows, cols> to_radians(Eigen::Matrix<T, rows, cols> degree) {
    return degree / static_cast<T>(180.0) * pi<T>;
}

template <typename T>
T to_degree(T radians) {
    return radians * static_cast<T>(180.0) / pi<T>;
}

template <typename T, int rows, int cols>
Eigen::Matrix<T, rows, cols> to_degree(Eigen::Matrix<T, rows, cols> radians) {
    return radians * static_cast<T>(180.0) / pi<T>;
}
} // namespace math


/**
 * MIT License
 *
 * Copyright (c) 2018 Prabhsimran Singh
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
#pragma once

#include <algorithm>
#include <cmath>
#include <iostream>

#include <Eigen/Dense>
using namespace Eigen;

#include "functions.hpp"

namespace nn {

class Dense {
  private:
    MatrixXf W;
    RowVectorXf b;

  public:
    explicit Dense(const int &, const int &);

    MatrixXf operator()(const MatrixXf &);

    MatrixXf forward(const MatrixXf &);

    // MatrixXf backward(const MatrixXf &, const MatrixXf &);
};

Dense::Dense(const int &num_inputs, const int &num_outputs) {
    W = MatrixXf(num_inputs, num_outputs).setRandom() * F::glorot_uniform(num_inputs, num_outputs);
    b = RowVectorXf(num_outputs).setZero();
}

MatrixXf Dense::operator()(const MatrixXf &inputs) {
    return forward(inputs);
}

MatrixXf Dense::forward(const MatrixXf &inputs) {
    MatrixXf Z = inputs * W;
    Z += b.replicate(inputs.rows(), 1);
    return Z;
}

// MatrixXf Dense::backward(const MatrixXf &inputs, const MatrixXf &gradients) {
// }

class Embedding {
  private:
    MatrixXf embeddings;
    int n_tokens;
    int embedding_dim;

  public:
    explicit Embedding(const int &, const int &);

    inline MatrixXf operator()(const MatrixXf &);

    inline MatrixXf forward(const MatrixXf &);

    // MatrixXf backward(const MatrixXf &, const MatrixXf &);
};

Embedding::Embedding(const int &n_tokens, const int &embedding_dim) : n_tokens(n_tokens), embedding_dim(embedding_dim) {
    this->embeddings = MatrixXf(n_tokens, embedding_dim).setRandom();
}

inline MatrixXf Embedding::operator()(const MatrixXf &inputs) {
    return forward(inputs);
}

inline MatrixXf Embedding::forward(const MatrixXf &inputs) {
    // here inputs are one-hot encodings
    return inputs * embeddings;
}

// MatrixXf Embedding::backward(const MatrixXf &inputs, const MatrixXf &gradients) {

// }
} // namespace nn


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file RotationTests.cpp
/// \brief Unit tests for the implementation of the rotation matrix class.
/// \details Unit tests for the various Lie Group functions will test both
/// special cases,
///          and randomly generated cases.
///
/// \author Sean Anderson
//////////////////////////////////////////////////////////////////////////////////////////////

#include <gtest/gtest.h>

#include <math.h>
#include <iomanip>
#include <ios>
#include <iostream>

#include <Eigen/Dense>
#include <lgmath/CommonMath.hpp>

#include <lgmath/so3/Operations.hpp>
#include <lgmath/so3/Rotation.hpp>

/////////////////////////////////////////////////////////////////////////////////////////////
///
/// UNIT TESTS OF ROTATION MATRIX
///
/////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief General test of rotation constructors
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, RotationConstructors) {
  // Generate random transform from most basic constructor
  Eigen::Matrix3d C_ba_rand = lgmath::so3::vec2rot(Eigen::Vector3d::Random());
  lgmath::so3::Rotation rand(C_ba_rand);

  // Rotation();
  {
    lgmath::so3::Rotation cmatrix;
    Eigen::Matrix3d test = Eigen::Matrix3d::Identity();
    std::cout << "cmat: " << cmatrix.matrix() << std::endl;
    std::cout << "test: " << test << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(cmatrix.matrix(), test, 1e-6));
  }

  // Rotation(const Rotation& C);
  {
    lgmath::so3::Rotation test(rand);
    std::cout << "cmat: " << rand.matrix() << std::endl;
    std::cout << "test: " << test.matrix() << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(rand.matrix(), test.matrix(), 1e-6));
  }

  // Rotation(const Eigen::Matrix3d& C);
  {
    lgmath::so3::Rotation test = rand;
    std::cout << "cmat: " << rand.matrix() << std::endl;
    std::cout << "test: " << test.matrix() << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(rand.matrix(), test.matrix(), 1e-6));

    // Test forced reprojection (ones to identity)
    Eigen::Matrix3d notRotation = Eigen::Matrix3d::Ones();
    lgmath::so3::Rotation test_bad(notRotation);  // forces reprojection
    std::cout << "cmat: " << test_bad.matrix() << std::endl;
    std::cout << "test: " << Eigen::Matrix3d::Identity() << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(test_bad.matrix(),
                                          Eigen::Matrix3d::Identity(), 1e-6));
  }

  // Rotation& operator=(Rotation C);
  {
    lgmath::so3::Rotation test(C_ba_rand);
    std::cout << "cmat: " << rand.matrix() << std::endl;
    std::cout << "test: " << test.matrix() << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(rand.matrix(), test.matrix(), 1e-6));
  }

  // Rotation(const Eigen::Vector3d& vec, unsigned int numTerms = 0);
  {
    Eigen::Vector3d vec = Eigen::Vector3d::Random();
    Eigen::Matrix3d cmat = lgmath::so3::vec2rot(vec);
    lgmath::so3::Rotation testAnalytical(vec);
    lgmath::so3::Rotation testNumerical(vec, 15);
    std::cout << "cmat: " << cmat << std::endl;
    std::cout << "testAnalytical: " << testAnalytical.matrix() << std::endl;
    std::cout << "testNumerical: " << testNumerical.matrix() << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(cmat, testAnalytical.matrix(), 1e-6));
    EXPECT_TRUE(lgmath::common::nearEqual(cmat, testNumerical.matrix(), 1e-6));
  }

  // Rotation(const Eigen::VectorXd& vec);
  {
    Eigen::VectorXd vec = Eigen::Vector3d::Random();
    Eigen::Matrix3d tmat = lgmath::so3::vec2rot(vec);
    lgmath::so3::Rotation test(vec);
    std::cout << "tmat: " << tmat << std::endl;
    std::cout << "test: " << test.matrix() << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(tmat, test.matrix(), 1e-6));
  }

  // Rotation(const Eigen::VectorXd& vec);
  {
    Eigen::VectorXd vec = Eigen::Vector3d::Random();
    lgmath::so3::Rotation test(vec);

    Eigen::VectorXd badvec = Eigen::Matrix<double, 6, 1>::Random();
    lgmath::so3::Rotation testFailure;
    try {
      testFailure = lgmath::so3::Rotation(badvec);
    } catch (const std::invalid_argument& e) {
      testFailure = test;
    }
    std::cout << "tmat: " << testFailure.matrix() << std::endl;
    std::cout << "test: " << test.matrix() << std::endl;
    EXPECT_TRUE(
        lgmath::common::nearEqual(testFailure.matrix(), test.matrix(), 1e-6));
  }

  // Rotation(Rotation&&);
  {
    auto rand2 = rand;  // std::move may invalidate the original variable.
    lgmath::so3::Rotation test(std::move(rand));
    rand = rand2;

    std::cout << "tmat: " << test.matrix() << std::endl;
    std::cout << "test: " << rand2.matrix() << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(test.matrix(), rand2.matrix(), 1e-6));
  }

  // Rotation = Rotation&&;
  {
    lgmath::so3::Rotation test;
    auto rand2 = rand;  // std::move may invalidate the original variable.
    test = std::move(rand);
    rand = rand2;

    std::cout << "tmat: " << test.matrix() << std::endl;
    std::cout << "test: " << rand2.matrix() << std::endl;
    EXPECT_TRUE(lgmath::common::nearEqual(test.matrix(), rand2.matrix(), 1e-6));
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Test exponential map construction and logarithmic vec() method
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, RotationToFromSE3Algebra) {
  // Add vectors to be tested
  std::vector<Eigen::Vector3d> trueVecs;
  Eigen::Vector3d temp;
  temp << 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, lgmath::constants::PI;
  trueVecs.push_back(temp);
  temp << -lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, -lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, -lgmath::constants::PI;
  trueVecs.push_back(temp);
  temp << 0.5 * lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.5 * lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.5 * lgmath::constants::PI;
  trueVecs.push_back(temp);
  const unsigned numRand = 20;
  for (unsigned i = 0; i < numRand; i++) {
    trueVecs.push_back(Eigen::Vector3d::Random());
  }

  // Get number of tests
  const unsigned numTests = trueVecs.size();

  // Calc rotation matrices
  std::vector<Eigen::Matrix3d> rotMatrices;
  for (unsigned i = 0; i < numTests; i++) {
    rotMatrices.push_back(lgmath::so3::vec2rot(trueVecs.at(i)));
  }

  // Calc rotations
  std::vector<lgmath::so3::Rotation> rotations;
  for (unsigned i = 0; i < numTests; i++) {
    rotations.push_back(lgmath::so3::Rotation(trueVecs.at(i)));
  }

  // Compare matrices
  {
    for (unsigned i = 0; i < numTests; i++) {
      std::cout << "matr: " << rotMatrices.at(i) << std::endl;
      std::cout << "tran: " << rotations.at(i).matrix() << std::endl;
      EXPECT_TRUE(lgmath::common::nearEqual(rotMatrices.at(i),
                                            rotations.at(i).matrix(), 1e-6));
    }
  }

  // Test logarithmic map
  {
    for (unsigned i = 0; i < numTests; i++) {
      Eigen::Vector3d testVec = rotations.at(i).vec();
      std::cout << "true: " << trueVecs.at(i) << std::endl;
      std::cout << "func: " << testVec << std::endl;
      EXPECT_TRUE(
          lgmath::common::nearEqualAxisAngle(trueVecs.at(i), testVec, 1e-6));
    }
  }

}  // TEST

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Test inverse and operatations
/////////////////////////////////////////////////////////////////////////////////////////////
TEST(LGMath, RotationInverse) {
  // Add vectors to be tested
  std::vector<Eigen::Vector3d> trueVecs;
  Eigen::Vector3d temp;
  temp << 0.0, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, lgmath::constants::PI;
  trueVecs.push_back(temp);
  temp << -lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, -lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, -lgmath::constants::PI;
  trueVecs.push_back(temp);
  temp << 0.5 * lgmath::constants::PI, 0.0, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.5 * lgmath::constants::PI, 0.0;
  trueVecs.push_back(temp);
  temp << 0.0, 0.0, 0.5 * lgmath::constants::PI;
  trueVecs.push_back(temp);
  const unsigned numRand = 20;
  for (unsigned i = 0; i < numRand; i++) {
    trueVecs.push_back(Eigen::Vector3d::Random());
  }

  // Get number of tests
  const unsigned numTests = trueVecs.size();

  // Add vectors to be tested - random
  std::vector<Eigen::Vector3d> landmarks;
  for (unsigned i = 0; i < numTests; i++) {
    landmarks.push_back(Eigen::Vector3d::Random());
  }

  // Calc rotation matrices
  std::vector<Eigen::Matrix3d> rotMatrices;
  for (unsigned i = 0; i < numTests; i++) {
    rotMatrices.push_back(lgmath::so3::vec2rot(trueVecs.at(i)));
  }

  // Calc rotations
  std::vector<lgmath::so3::Rotation> rotations;
  for (unsigned i = 0; i < numTests; i++) {
    rotations.push_back(lgmath::so3::Rotation(trueVecs.at(i)));
  }

  // Compare inverse to basic matrix inverse
  {
    for (unsigned i = 0; i < numTests; i++) {
      std::cout << "matr: " << rotMatrices.at(i).inverse() << std::endl;
      std::cout << "tran: " << rotations.at(i).inverse().matrix() << std::endl;
      EXPECT_TRUE(lgmath::common::nearEqual(rotMatrices.at(i).inverse(),
                                            rotations.at(i).inverse().matrix(),
                                            1e-6));
    }
  }

  // Test that product of inverse and self make identity
  {
    for (unsigned i = 0; i < numTests; i++) {
      std::cout << "C*Cinv: "
                << rotations.at(i).matrix() * rotations.at(i).inverse().matrix()
                << std::endl;
      EXPECT_TRUE(lgmath::common::nearEqual(
          rotations.at(i).matrix() * rotations.at(i).inverse().matrix(),
          Eigen::Matrix3d::Identity(), 1e-6));
    }
  }

  // Test self-product
  {
    for (unsigned i = 0; i < numTests - 1; i++) {
      lgmath::so3::Rotation test = rotations.at(i);
      test *= rotations.at(i + 1);
      Eigen::Matrix3d matrix = rotMatrices.at(i) * rotMatrices.at(i + 1);
      std::cout << "matr: " << matrix << std::endl;
      std::cout << "tran: " << test.matrix() << std::endl;
      EXPECT_TRUE(lgmath::common::nearEqual(matrix, test.matrix(), 1e-6));
    }
  }

  // Test product
  {
    for (unsigned i = 0; i < numTests - 1; i++) {
      lgmath::so3::Rotation test = rotations.at(i) * rotations.at(i + 1);
      Eigen::Matrix3d matrix = rotMatrices.at(i) * rotMatrices.at(i + 1);
      std::cout << "matr: " << matrix << std::endl;
      std::cout << "tran: " << test.matrix() << std::endl;
      EXPECT_TRUE(lgmath::common::nearEqual(matrix, test.matrix(), 1e-6));
    }
  }

  // Test self product with inverse
  {
    for (unsigned i = 0; i < numTests - 1; i++) {
      lgmath::so3::Rotation test = rotations.at(i);
      test /= rotations.at(i + 1);
      Eigen::Matrix3d matrix =
          rotMatrices.at(i) * rotMatrices.at(i + 1).inverse();
      std::cout << "matr: " << matrix << std::endl;
      std::cout << "tran: " << test.matrix() << std::endl;
      EXPECT_TRUE(lgmath::common::nearEqual(matrix, test.matrix(), 1e-6));
    }
  }

  // Test product with inverse
  {
    for (unsigned i = 0; i < numTests - 1; i++) {
      lgmath::so3::Rotation test = rotations.at(i) / rotations.at(i + 1);
      Eigen::Matrix3d matrix =
          rotMatrices.at(i) * rotMatrices.at(i + 1).inverse();
      std::cout << "matr: " << matrix << std::endl;
      std::cout << "tran: " << test.matrix() << std::endl;
      EXPECT_TRUE(lgmath::common::nearEqual(matrix, test.matrix(), 1e-6));
    }
  }

  // Test product with landmark
  {
    for (unsigned i = 0; i < numTests; i++) {
      Eigen::Vector3d mat = rotMatrices.at(i) * landmarks.at(i);
      Eigen::Vector3d test = rotations.at(i) * landmarks.at(i);

      std::cout << "matr: " << mat << std::endl;
      std::cout << "test: " << test << std::endl;
      EXPECT_TRUE(lgmath::common::nearEqual(mat, test, 1e-6));
    }
  }
}

int main(int argc, char** argv) {
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#pragma once

// -*- C++ -*-

// The MIT License (MIT)
//
// Copyright (c) 2017 Alexander Samoilov
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <cmath>
#include <boost/math/special_functions/ellint_1.hpp>
#include <boost/math/special_functions/ellint_2.hpp>
#include "math_consts.hpp"

/*! \file elliptic_integral.h
    \brief computation of elliptic integrals of the first and second kinds by iteration method

    Details.
*/

/**
 * \f$ F\left(k\right)\equiv\int_0^{\pi/2}\frac{d\eta}{\sqrt{1-k^2\cos^2\eta}} \f$
 */
template <typename T>
void elliptic_integral (T const& rk2, T& f, T& e)
{
    constexpr T ACCURACY = T(1.0e-6);
    T rk = std::sqrt(rk2), g = ONE<T>, b = rk, c, d;

    f = HALF<T> * PI<T>;
    e = ONE<T>;
    do {
        c = std::sqrt(ONE<T> - b*b);
        b = (ONE<T> - c) / (ONE<T> + c);
        d = f*b;
        f += d;
        g = HALF<T>*g*b;
        e += g;
    } while (std::fabs(d) > ACCURACY);
    e = f * (ONE<T> - HALF<T>*rk2*e);
}

template <typename T>
void elliptic_integral_new (T const& rk2, T& f, T& e)
{
    T srk = std::sqrt(rk2);
    f = boost::math::ellint_1(srk);
    e = boost::math::ellint_2(srk);
    // `std::comp_ellint_1` and `std::comp_ellint_2` are too slow
    // f = std::comp_ellint_1(srk);
    // e = std::comp_ellint_2(srk);
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)

//! \file
//!\brief Tests for the math root-finding utility function TOMS748 of (fixed_point).

#include <utility>

#define BOOST_TEST_MODULE test_negatable_math_root_find_toms748
#define BOOST_LIB_DIAGNOSTIC

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_CASE(test_negatable_math_root_find_toms748)
{
  typedef boost::fixed_point::negatable<        std::numeric_limits<int>::digits,
                                        -(255 - std::numeric_limits<int>::digits)>
  fixed_point_type;

  const fixed_point_type tol = ldexp(fixed_point_type(1), fixed_point_type::resolution + 8);

  BOOST_CONSTEXPR boost::uintmax_t maximum_allowed_iterations = UINTMAX_C(32);

  boost::uintmax_t iterations = maximum_allowed_iterations;

  // Find the first positive real root of [cos(x) - sqrt(x)] = 0,
  // which is near x = 0.64171437087288265839856530031652237185271781360384.

  using boost::math::tools::toms748_solve;

  const std::pair<fixed_point_type, fixed_point_type> root =
    toms748_solve([](const fixed_point_type& x) -> fixed_point_type
                  {
                    return (cos(x) - sqrt(x));
                  },
                  ldexp(fixed_point_type(1), -1),
                  fixed_point_type(1),
                  [&tol](const fixed_point_type& a, const fixed_point_type& b) -> bool
                  {
                    using std::fabs;

                    return (fabs(a - b) < tol);
                  },
                  iterations);

  // Verify the root with a tight tolerance.

  BOOST_CHECK_CLOSE_FRACTION(cos(root.first), sqrt(root.first), tol * 2);

  BOOST_CHECK(iterations < maximum_allowed_iterations);
}


///////////////////////////////////////////////////////////////////////////////
// kurtosis.hpp
//
//  Copyright 2006 Olivier Gygi, Daniel Egloff. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_KURTOSIS_HPP_EAN_28_10_2005
#define BOOST_ACCUMULATORS_STATISTICS_KURTOSIS_HPP_EAN_28_10_2005

#include <limits>
#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/moment.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // kurtosis_impl
    /**
        @brief Kurtosis estimation

        The kurtosis of a sample distribution is defined as the ratio of the 4th central moment and the square of the 2nd central
        moment (the variance) of the samples, minus 3. The term \f$ -3 \f$ is added in order to ensure that the normal distribution
        has zero kurtosis. The kurtosis can also be expressed by the simple moments:

        \f[
            \hat{g}_2 =
                \frac
                {\widehat{m}_n^{(4)}-4\widehat{m}_n^{(3)}\hat{\mu}_n+6\widehat{m}_n^{(2)}\hat{\mu}_n^2-3\hat{\mu}_n^4}
                {\left(\widehat{m}_n^{(2)} - \hat{\mu}_n^{2}\right)^2} - 3,
        \f]

        where \f$ \widehat{m}_n^{(i)} \f$ are the \f$ i \f$-th moment and \f$ \hat{\mu}_n \f$ the mean (first moment) of the
        \f$ n \f$ samples.
    */
    template<typename Sample>
    struct kurtosis_impl
      : accumulator_base
    {
        // for boost::result_of
        typedef typename numeric::functional::average<Sample, Sample>::result_type result_type;

        kurtosis_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            return numeric::average(
                        moment<4>(args)
                        - 4. * moment<3>(args) * mean(args)
                        + 6. * moment<2>(args) * mean(args) * mean(args)
                        - 3. * mean(args) * mean(args) * mean(args) * mean(args)
                      , ( moment<2>(args) - mean(args) * mean(args) )
                        * ( moment<2>(args) - mean(args) * mean(args) )
                    ) - 3.;
        }
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::kurtosis
//
namespace tag
{
    struct kurtosis
      : depends_on<mean, moment<2>, moment<3>, moment<4> >
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::kurtosis_impl<mpl::_1> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::kurtosis
//
namespace extract
{
    extractor<tag::kurtosis> const kurtosis = {};
}

using extract::kurtosis;

// So that kurtosis can be automatically substituted with
// weighted_kurtosis when the weight parameter is non-void
template<>
struct as_weighted_feature<tag::kurtosis>
{
    typedef tag::weighted_kurtosis type;
};

template<>
struct feature_of<tag::weighted_kurtosis>
  : feature_of<tag::kurtosis>
{
};

}} // namespace boost::accumulators

#endif


#include <iostream>
#include <Eigen/Dense>
#include "../Optimizer/optimizer"
using namespace std;
using namespace Eigen;

double func (double x) {
    return pow(x + 10, 2);
}

double func2 (VectorXd x) {
    return x.squaredNorm();
}

int main () {
    cout << "Using Function: (x + 10)^2 for single variable algorithms testing." << endl;
    cout << "Test Bounding Phase:" << endl;
    double ipt = 5.4;
    Vector2d range = BoundingPhase(func, ipt);
    cout << "Range from bounding Phase for initial point :" << ipt << endl;
    cout << range << endl;

    cout << "Derivatives at " << ipt << endl;
    cout << Derivative(func, ipt) << endl;

    cout << "Finding optimal point using above range for Newton Rapshon Method." << endl;
    cout << "Optimal Point is: ";
    cout << NewtonRapshon (func, range) << endl;;

    cout << "Testing SVOptimize on (x + 10)^2 with initial point 5.4." << endl;
    cout << "The Optimal Point obtained is: ";
    cout << SVOptimize(func, 5.4) << endl;

    cout << "Testing Gradient function using func2 with 3 variables" << endl;
    cout << Gradient(func2, Vector3d(3, 3, 4)) << endl;

    cout << "Testing DFP on sum squared function with 3 variables and initial point (4, -3, 7)." << endl;
    cout << "The Optimal Point obtained is: " << endl;
    Vector3d x(4, -3, 7);
    cout << DFP(func2, x) << endl;

}


#include <Eigen/Eigen>
#include <unsupported/Eigen/KroneckerProduct>

#include <mtvmtl/core/manifold.hpp>
#include <cmath>
#include <cstdlib>

using namespace Eigen;
using namespace tvmtl;

template <typename DerivedY, typename DerivedA>
double F(const MatrixBase<DerivedY>& Y, const MatrixBase<DerivedA>& A){
 return 0.5 * (Y.transpose() * A * Y).trace();
}

template <typename DerivedY, typename DerivedA>
MatrixBase<DerivedY> FY(const MatrixBase<DerivedY>& Y, const MatrixBase<DerivedA>& A){
 return A * Y;
}

int main(){

srand(42);

const int N=4;
const int P=3;

typedef Matrix<double, P, P> ppmat;
typedef Matrix<double, N, P> npmat;
typedef Matrix<double, N, N> nnmat;
typedef Matrix<double, N*P, N*P> np2mat;
typedef Manifold<GRASSMANN, N, P> mf;

nnmat R = nnmat::Random();
nnmat A = 0.5 * (R + R.transpose());

std::cout << "A:\n" << A << std::endl;

npmat Y;
Y = npmat::Random();
HouseholderQR<npmat> qr(Y);
npmat q = qr.householderQ() * npmat::Identity();
Y = q;

std::cout << "Y:\n" << Y << std::endl;

nnmat HYproj = nnmat::Identity() - Y * Y.transpose();

double h = 1e-2;
npmat dY;
dY = npmat::Random();
qr.compute(dY);
q = qr.householderQ() * npmat::Identity();
dY = h * HYproj * q;

std::cout << "DY:\n" << dY << std::endl;

VectorXd vecdY = Map<VectorXd>(dY.data(), dY.size());

npmat FY = A * Y;
std::cout << "\nFY:\n" << FY << std::endl;

np2mat FYY = kroneckerProduct(ppmat::Identity(), A);
std::cout << "FYY:\n" << FYY << std::endl;

npmat YpdY = Y + dY;
//mf::exp(Y,dY,YpdY);
mf::projector(YpdY);

double exact = F(Y + dY, A);
double exact2 = F(YpdY, A);
double firstorder = F(Y,A) + FY.cwiseProduct(dY).sum();
double secondorder_euc = firstorder + 0.5 * FYY.cwiseProduct(vecdY * vecdY.transpose()).sum() ;

npmat FYYdY_proj = HYproj * A * dY;
npmat FYY_corr_term = dY * Y.transpose() * FY;
double secondorder_grass_nv = firstorder + 0.5 * (FYYdY_proj.cwiseProduct(dY).sum() - FYY_corr_term.cwiseProduct(dY).sum());

np2mat FYY_proj = kroneckerProduct(ppmat::Identity(), HYproj) * FYY ;
double secondorder_grass_sv = firstorder + 0.5 * (FYY_proj.cwiseProduct(vecdY * vecdY.transpose()).sum() - FYY_corr_term.cwiseProduct(dY).sum());

np2mat FYY_grass = FYY_proj - kroneckerProduct(FY.transpose() * Y, nnmat::Identity());
double secondorder_grass_fv = firstorder + 0.5 * (FYY_grass.cwiseProduct(vecdY * vecdY.transpose()).sum());
std::cout << "FYY_Grass:\n" << FYY_grass << std::endl;

std::cout << "\n\nFirst order approximation: " << std::abs(exact-firstorder) << std::endl;
std::cout << "Second order approximation (Euclidian): " << std::abs(exact-secondorder_euc) << std::endl;
std::cout << "Second order approximation (Grassmann non-vectorized): " << std::abs(exact2-secondorder_grass_sv) << std::endl;
std::cout << "Second order approximation (Grassmann semi-vectorized): " << std::abs(exact2-secondorder_grass_sv) << std::endl;
std::cout << "Second order approximation (Grassmann vectorized): " << std::abs(exact2-secondorder_grass_fv) << std::endl;

return 0;
}


#include <iostream>
#include <vector>
#include <unordered_map>
#include <Eigen/Dense>
#include <matplotlibcpp.h>
#include "../include/numerical_gradient.h"
#include "../include/optimizer.h"

using namespace Eigen;
namespace plt = matplotlibcpp;

double sample_function(MatrixXd &);
MatrixXd sample_function_gradient(MatrixXd &);

int main()
{
    using std::cout;
    using std::endl;
    using std::unordered_map;
    using std::vector;
    using namespace MyDL;

    vector<vector<double>> x, y, z;
    MatrixXd X = MatrixXd::Zero(1, 2);
    for (double i = -2; i <= 2; i += 0.1)
    {
        vector<double> x_row, y_row, z_row;
        for (double j = -2; j <= 2; j += 0.1)
        {
            X(0) = i;
            X(1) = j;
            x_row.push_back(i);
            y_row.push_back(j);
            z_row.push_back(sample_function(X));
        }
        x.push_back(x_row);
        y.push_back(y_row);
        z.push_back(z_row);
    }

    plt::plot_surface(x, y, z);
    plt::save("suddle_function.png");
    plt::clf();

    // x.clear();
    // y.clear();
    // vector<double> ux, uy, vx, vy;
    // MatrixXd grad = MatrixXd::Zero(1, 2);
    // for (double i = -10; i <= 10; i++)
    // {
    //     for (double j = -5; j <= 5; j++)
    //     {
    //         ux.push_back(i);
    //         uy.push_back(j);
    //         X(0) = i;
    //         X(1) = j;
    //         grad = sample_function_gradient(X);
    //         vx.push_back((double)-grad(0));
    //         vy.push_back((double)-grad(1));
    //     }
    // }

    // Adam optimizer(0.3, 0.7, 0.9);
    // unordered_map<string, MatrixXd> params, grads;
    // int num_update = 30;
    // vector<vector<double>> param_history(2, vector<double>(num_update, 0));
    // X(0) = -6;
    // X(1) = 2;
    // params["X"] = X;
    // param_history[0][0] = X(0);
    // param_history[1][0] = X(1);

    // for (int i = 0; i < num_update; i++)
    // {
    //     cout << "iteration: " << i << endl;
    //     grads["X"] = sample_function_gradient(params["X"]);
    //     optimizer.update(params, grads);

    //     param_history[0][i + 1] = (double)params["X"](0);
    //     param_history[1][i + 1] = (double)params["X"](1);
    // }

    // plt::quiver(ux, uy, vx, vy);
    // plt::save("SGD_function_gradient.png");

    // plt::plot(param_history[0], param_history[1], {{"color", "red"}, {"marker", "o"}, {"linestyle", "--"}, {"label", "$f(x, y) = \\frac{1}{20}x^{2} + y^{2}$"}});
    // plt::plot(vector<double>(1, 0), vector<double>(1, 0), "b+");
    // plt::title("Adam: $f(x,y) = 0.05x^{2} + y^{2}$");
    // plt::legend();
    // plt::save("Adam_function_gradient_descent.png");
    // plt::cla();

    return 0;
}

double sample_function(MatrixXd &x)
{
    return 0.25 * (- x(0) * x(0) + x(1) * x(1));
}

// MatrixXd sample_function_gradient(MatrixXd &x)
// {
//     MatrixXd grad(1, 2);
//     grad(0) = 0.1 * x(0);
//     grad(1) = 2 * x(1);
//     return grad;
// }


#ifndef DCS_TESTBED_DETAIL_VARIANCE_HPP
#define DCS_TESTBED_DETAIL_VARIANCE_HPP

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <cmath>
#include <cstddef>

namespace dcs { namespace testbed { namespace detail {

// See: https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance
template <typename ValueT, typename IterT>
ValueT compensated_variance(IterT first, IterT last)
{
    std::size_t n = 0;
    ValueT mean = 0;

	for (IterT it = first; it != last; ++it)
	{
		const ValueT x = *it;
		++n;
		mean += x;
	}
	mean /= n;

    ValueT sum1 = 0;
    ValueT sum2 = 0;
	for (IterT it = first; it != last; ++it)
	{
		const ValueT x = *it;
		const ValueT dev = x-mean;

        sum1 += dev*dev;
        sum2 += dev;
	}

    return (n > 1) ? ((sum1 - sum2*sum2/n)/(n - 1)) : 0;
}

template <typename ValueT, typename IterT>
ValueT boost_variance(IterT first, IterT last)
{
	boost::accumulators::accumulator_set<ValueT, boost::accumulators::stats<boost::accumulators::tag::variance> > acc;
	std::size_t n = 0;

	while (first != last)
	{
		acc(*first);
		++first;
		++n;
	}

	return (n > 1) ? boost::accumulators::variance(acc)*n/static_cast<ValueT>(n-1) : 0;
}

template <typename ValueT, typename IterT>
ValueT variance(IterT first, IterT last)
{
	return boost_variance<ValueT>(first, last);
}

template <typename ValueT, typename IterT>
ValueT stdev(IterT first, IterT last)
{
	return std::sqrt(variance<ValueT>(first, last));
}

}}} // Namespace dcs::testbed::detail

#endif // DCS_TESTBED_DETAIL_VARIANCE_HPP


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXd m(2,3);
m << 1,2,3,4,5,6;
cout << "here's the 2x3 matrix m:" << endl << m << endl;
cout << "let's resize m to 3x2. This is a conservative resizing because 2*3==3*2." << endl;
m.resize(3,2);
cout << "here's the 3x2 matrix m:" << endl << m << endl;
cout << "now let's resize m to size 2x2. This is NOT a conservative resizing, so it becomes uninitialized:" << endl;
m.resize(2,2);
cout << m << endl;

  return 0;
}