#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


//#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}};
    }

  }
}


#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


#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);
}


///////////////////////////////////////////////////////////////////////////////
// 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 <iostream>
#include <armadillo>
#include <cmath>
#include <cstdlib>
#include <time.h>
#include <fstream>
using namespace std;
using namespace arma;

void RHO_A_FILL(vec &rho, mat &A, int N,double rhoN); //rho, kind of like a linespace
                                                 //A, Tridiagonal matrix
void Maxoff(mat &A, int N,int &k, int &l, double &max);        //Finds the max element of a matrix

void Maxoff_test(double &max,int &k,int &l);

void Ortho_test(mat &S,int N);

int main(){
    int N = 400; //matrix size; N x N
    double rhoN = 60;
    double max;
    int k,l;
    Maxoff_test(max,k,l);

    mat A = mat(N,N,fill::zeros); //indexes go from (0) to (N-1)
    mat S = mat(N,N,fill::eye);
    vec rho = vec(N,fill::zeros);
    vec eigen = vec(N);

    RHO_A_FILL(rho,A,N,rhoN);
    Maxoff(A,N,k,l,max);
    int iterations = 0;

    double eps = 1E-10;

    double tau, t, s, c, il, ik, kk, ll, s_ik, s_il;
    double start, finish;

    start = clock();                      //clock value before eigen solve
    while(max > eps){
        tau = (A(l,l)-A(k,k))/(double(2)*A(k,l));
        if(tau>0){
             t = (-tau - sqrt(1.0 + tau*tau));}
        else{t = ( -tau + sqrt(1.0 + tau*tau));}

        //cosine and sine
        c = double(1)/sqrt(1.+t*t);
        s = t*c;

        //Jacobi rotating A round theta in N-dim space
        for(int i = 0; i<N; i++){
            if ((i != k) && (i !=l)){
                ik = A(i,k)*c - A(i,l)*s;
                il = A(i,l)*c + A(i,k)*s;
                A(i,k) = ik;
                A(k,i) = ik;
                A(i,l) = il;
                A(l,i) = il;
                }
            s_ik = S(i,k);
            s_il = S(i,l);
            S(i,k) = c*s_ik - s*s_il;
            S(i,l) = c*s_il + s*s_ik;
            }

        kk = A(k,k)*c*c - 2.*A(k,l)*c*s + A(l,l)*s*s;
        ll = A(l,l)*c*c + 2.*A(k,l)*c*s + A(k,k)*s*s;

        A(k,k) = kk;
        A(l,l) = ll;
        A(k,l) = 0;
        A(l,k) = 0;

        iterations++;
        Maxoff(A,N,k,l,max);
        //Ortho test
        if (iterations%N*2==0){
            Ortho_test(S,N);
            }

        } //end of while
    finish = clock();                    //clock value after eigen solve

    //[eigen] is now eigenvalues
    for(int i = 0; i<N;i++){
        eigen(i) = A(i,i);
        }

    eigen.print();

    double time = (finish -start)/CLOCKS_PER_SEC/iterations;

    fstream outfile;
    outfile.open("eigenvectors5.dat",ios::out);

    for (int i = 0; i<N; i++){
        for (int j = 0; j<N; j++){
            if(j%N == 0){outfile<<endl;}
            outfile << S(i,j)<<" ";
            }
        }
    outfile.close();

    fstream utfile;
    utfile.open("eigenvalues5.dat",ios::out);

    for (int i = 0; i<N; i++){
        utfile << eigen(i)<<endl;
        }
    utfile.close();

    //fstream Outfile;
    //Outfile.open("steps.dat",ios::out);
    //Outfile << "N     epsilon    steps  step_time"<<endl;

    //Outfile <<N <<" "<<eps<<" "<<iterations<<" "<<time;
    //Outfile<<endl;

    //Outfile.close();

} //end of main

void RHO_A_FILL(vec &rho, mat &A, int N,double rhoN){
    double h = rhoN/(N);
    rho(0) = 0.000001;
    for(int i = 0; i < N;i++){
        if(i > 0) rho(i) = i*h;
        A(i,i) = 2./(h*h)+ double(25)*rho(i)*rho(i)+(double(1)/rho(i));
        if(i<N-1){
            A(i+1,i) = -1./(h*h);
            A(i,i+1) = -1./(h*h);
}
        }
    }

void Maxoff(mat &A, int N,int &k, int &l, double &max){
    N = A.n_rows;
    max = 0;
    for(int i = 0; i < N ; i++){
        for(int j = i+1; j < N ; j++){
            if ( A(i,j)*A(i,j) > max){
                max =A(i,j)*A(i,j);
                k = i;
                l = j;
                }
            }
        }

    }
////////////Unit tests
void Maxoff_test(double &max,int &k,int &l){
    mat matrix = mat(5,5,fill::zeros);
    matrix(0,3) = -9;
    matrix(3,0) = -9;
    matrix(4,4) = -3;
    matrix(3,4) = -1;
    matrix(4,3) = -1;
    Maxoff(matrix,5,k,l,max);
    if(abs(max-81) < 1E-13){cout << "Maxoff function passes the test"<<endl;}
    else{cout<<"Maxoff might not be working properly"<<endl;}
    }
void Ortho_test(mat &S,int N){
    double error = 0;
    error = dot(S.col(int(double(rand())/RAND_MAX*N)),S.col(int(double(rand())/RAND_MAX*N)));
    if (abs(error) < 1e-10){
        cout << "ortho pass" << endl;}
    else{cout<<"ortho fail" << endl;}
    }


#pragma once
// hprblas.hpp :  include file containing templated C++ interfaces to High-Performance Reproducible BLAS routines
//
// Copyright (C) 2017-2021 Stillwater Supercomputing, Inc.
//
// This file is part of the HPRBLAS project, which is released under an MIT Open Source license.
#include <universal/number/posit/posit>
#include <boost/numeric/mtl/mtl.hpp>

namespace sw {
namespace hprblas {

// LEVEL 1 BLAS operators

// 1-norm of a vector: sum of magnitudes of the vector elements, default increment stride is 1
template<typename Vector>
typename Vector::value_type asum(size_t n, const Vector& x, size_t incx = 1) {
	typename Vector::value_type sum = 0;
	size_t ix;
	for (ix = 0; ix < n; ix += incx) {
		sum += (x[ix] < 0 ? -x[ix] : x[ix]);
	}
	return sum;
}

// sum of the vector elements, default increment stride is 1
template<typename Vector>
typename Vector::value_type sum(size_t n, const Vector& x, size_t incx = 1) {
	typename Vector::value_type sum = 0;
	size_t ix;
	for (ix = 0; ix < n; ix += incx) {
		sum += x[ix];
	}
	return sum;
}

// a time x plus y
template<typename Scalar, typename Vector>
void axpy(size_t n, Scalar a, const Vector& x, size_t incx, Vector& y, size_t incy) {
	using namespace mtl;
	size_t cnt, ix, iy;
	for (cnt = 0, ix = 0, iy = 0; cnt < n && ix < size(x) && iy < size(y); ++cnt, ix += incx, iy += incy) {
		y[iy] += a * x[ix];
	}
}

// vector copy
template<typename Vector>
void copy(size_t n, const Vector& x, size_t incx, Vector& y, size_t incy) {
	using namespace mtl;
	size_t cnt, ix, iy;
	for (cnt = 0, ix = 0, iy = 0; cnt < n && ix < size(x) && iy < size(y); ++cnt, ix += incx, iy += incy) {
		y[iy] = x[ix];
	}
}

// adapter for STL vectors
//template<typename Scalar> auto size(const std::vector<Scalar>& v) { return v.size(); }

// dot product: the operator vector::x[index] is limited to uint32_t, so the arguments are limited to uint32_t as well
// The library does support arbitrary posit configuration conversions, but to simplify the
// behavior of the dot product, the element type of the vectors x and y are declared to be the same.
// TODO: investigate if the vector<> index is always a 32bit entity?
template<typename Vector>
typename Vector::value_type dot(size_t n, const Vector& x, size_t incx, const Vector& y, size_t incy) {
	using namespace mtl;
	typename Vector::value_type product = 0;
	size_t cnt, ix, iy;
	for (cnt = 0, ix = 0, iy = 0; cnt < n && ix < mtl::size(x) && iy < mtl::size(y); ++cnt, ix += incx, iy += incy) {
		product += x[ix] * y[iy];
	}
	return product;
}
// specialized dot product
template<typename Vector>
typename Vector::value_type dot(const Vector& x, const Vector& y) {
	using namespace mtl;
	typename Vector::value_type product = 0;
	size_t cnt, ix, iy;
	for (cnt = 0, ix = 0, iy = 0; cnt < mtl::size(x); ++cnt, ++ix, ++iy) {
		product += x[ix] * y[iy];
	}
	return product;
}
///
/// fused dot product operators

// Fused dot product with quire continuation
template<typename Quire, typename Vector>
void fdp_qr(Quire& sum_of_products, size_t n, const Vector& x, size_t incx, const Vector& y, size_t incy) {
	size_t ix, iy;
	for (ix = 0, iy = 0; ix < n && iy < n; ix = ix + incx, iy = iy + incy) {
		sum_of_products += sw::universal::quire_mul(x[ix], y[iy]);
	}
}
// Resolved fused dot product, with the option to control capacity bits in the quire
template<typename Vector, size_t capacity = 10>
typename Vector::value_type fdp_stride(size_t n, const Vector& x, size_t incx, const Vector& y, size_t incy) {
	constexpr size_t nbits = Vector::value_type::nbits;
	constexpr size_t es = Vector::value_type::es;
	sw::universal::quire<nbits, es, capacity> q(0);
	size_t ix, iy;
	for (ix = 0, iy = 0; ix < n && iy < n; ix = ix + incx, iy = iy + incy) {
		q += sw::universal::quire_mul(x[ix], y[iy]);
		if (sw::universal::_trace_quire_add) std::cout << q << '\n';
	}
	typename Vector::value_type sum;
	sw::universal::convert(q.to_value(), sum);     // one and only rounding step of the fused-dot product
	return sum;
}
// Specialized resolved fused dot product that assumes unit stride and a standard vector,
// with the option to control capacity bits in the quire
template<typename Vector, size_t capacity = 10>
typename Vector::value_type fdp(const Vector& x, const Vector& y) {
	using namespace mtl;
	constexpr size_t nbits = Vector::value_type::nbits;
	constexpr size_t es = Vector::value_type::es;
	sw::universal::quire<nbits, es, capacity> q(0);
	size_t ix, iy, n = mtl::size(x);
	for (ix = 0, iy = 0; ix < n && iy < n; ++ix, ++iy) {
		q += sw::universal::quire_mul(x[ix], y[iy]);
	}
	typename Vector::value_type sum;
	sw::universal::convert(q.to_value(), sum);     // one and only rounding step of the fused-dot product
	return sum;
}

// rotation of points in the plane
template<typename Rotation, typename Vector>
void rot(size_t n, Vector& x, size_t incx, Vector& y, size_t incy, Rotation c, Rotation s) {
	using namespace mtl;
	// x_i = c*x_i + s*y_i
	// y_i = c*y_i - s*x_i
	size_t cnt, ix, iy;
	for (cnt = 0, ix = 0, iy = 0; cnt < n && ix < size(x) && iy < size(y); ++cnt, ix += incx, iy += incy) {
		Rotation x_i = c*x[ix] + s*y[iy];
		Rotation y_i = c*y[iy] - s*x[ix];
		y[iy] = y_i;
		x[ix] = x_i;
	}
}

// compute parameters for a Givens rotation
template<typename T>
void rotg(T& a, T& b, T& c, T&s) {
	// Given Cartesian coordinates (a,b) of a point, return parameters c,s,r, and z associated with the Givens rotation.
}

// scale a vector
template<typename Scalar, typename Vector>
void scale(size_t n, Scalar a, Vector& x, size_t incx) {
	using namespace mtl;
	size_t cnt, ix;
	for (cnt = 0, ix = 0; cnt < n && ix < size(x); ix += incx) {
		x[ix] *= a;
	}
}

// swap two vectors
template<typename Vector>
void swap(size_t n, Vector& x, size_t incx, Vector& y, size_t incy) {
	using namespace mtl;
	size_t cnt, ix, iy;
	for (cnt = 0, ix = 0, iy = 0; cnt < n && ix < size(x) && iy < size(y); ++cnt, ix += incx, iy += incy) {
		typename Vector::value_type tmp = x[ix];
		x[ix] = y[iy];
		y[iy] = tmp;
	}
}

// find the index of the element with maximum absolute value
template<typename Vector>
size_t amax(size_t n, const Vector& x, size_t incx) {
	using namespace mtl;
	typename Vector::value_type running_max = -INFINITY;
	size_t ix, index;
	for (ix = 0; ix < size(x); ix += incx) {
		if (x[ix] > running_max) {
			index = ix;
			running_max = x[ix];
		}
	}
	return index;
}

// find the index of the element with minimum absolute value
template<typename Vector>
size_t amin(size_t n, const Vector& x, size_t incx) {
	using namespace mtl;
	typename Vector::value_type running_min = INFINITY;
	size_t ix, index;
	for (ix = 0; ix < size(x); ix += incx) {
		if (x[ix] < running_min) {
			index = ix;
			running_min = x[ix];
		}
	}
	return index;
}

// absolute value of a complex number
template<typename T>
T cabs(T z) {
}

// print a vector
template<typename Vector>
void strided_print(std::ostream& ostr, size_t n, Vector& x, size_t incx = 1) {
	using namespace mtl;
	size_t cnt, ix;
	for (cnt = 0, ix = 0; cnt < n && ix < size(x); ++cnt, ix += incx) {
		cnt == 0 ? ostr << "[" << x[ix] : ostr << ", " << x[ix];
	}
	ostr << "]";
}

// LEVEL 2 BLAS operators

// Matrix-vector product: b = A * x
template<typename Matrix, typename Vector>
void matvec(Vector& b, const Matrix& A, const Vector& x) {
	b = A * x;
}

// Matrix-vector product: b = A * x, posit specialized
template<size_t nbits, size_t es>
void matvec(mtl::vec::dense_vector< sw::universal::posit<nbits, es> >& b, const mtl::mat::dense2D< sw::universal::posit<nbits, es> >& A, const mtl::vec::dense_vector< sw::universal::posit<nbits, es> >& x) {
	using namespace mtl;
	// preconditions
	assert(A.num_cols() == size(x));
	assert(size(b) == size(x));

#if HPRBLAS_TRACE_ROUNDING_EVENTS
	unsigned errors = 0;
#endif
	size_t nr = size(b);
	size_t nc = size(x);
	for (size_t i = 0; i < nr; ++i) {
		sw::universal::quire<nbits, es> q(0);
		for (size_t j = 0; j < nc; ++j) {
			q += sw::universal::quire_mul(A[i][j], x[j]);
		}
		sw::universal::convert(q.to_value(), b[i]);     // one and only rounding step of the fused-dot product
#if HPRBLAS_TRACE_ROUNDING_EVENTS
		sw::universal::quire<nbits, es> qdiff = q;
		sw::universal::quire<nbits, es> qsum = b[i];
		qdiff -= qsum;
		if (!qdiff.iszero()) {
			++errors;
			std::cout << "q    : " << q << std::endl;
			std::cout << "qsum : " << qsum << std::endl;
			std::cout << "qdiff: " << qdiff << std::endl;
			sw::universal::posit<nbits, es> roundingError;
			convert(qdiff.to_value(), roundingError);
			std::cout << "matvec b[" << i << "] = " << posit_format(b[i]) << " rounding error: " << posit_format(roundingError) << " " << roundingError << std::endl;
		}
#endif
	}
#if HPRBLAS_TRACE_ROUNDING_EVENTS
	if (errors) {
		std::cout << "HPR-BLAS: tracing found " << errors << " rounding errors in matvec operation\n";
	}
#endif
}

// A times x = b fused matrix-vector product
template<size_t nbits, size_t es>
mtl::vec::dense_vector< sw::universal::posit<nbits, es> > fmv(const mtl::mat::dense2D< sw::universal::posit<nbits, es> >& A, const mtl::vec::dense_vector< sw::universal::posit<nbits, es> >& x) {
	using namespace mtl;
	// preconditions
	assert(A.num_cols() == size(x));
	mtl::vec::dense_vector< sw::universal::posit<nbits, es> > b(size(x));

#if HPRBLAS_TRACE_ROUNDING_EVENTS
	unsigned errors = 0;
#endif
	size_t nr = size(b);
	size_t nc = size(x);
	for (size_t i = 0; i < nr; ++i) {
		sw::universal::quire<nbits, es> q(0);
		for (size_t j = 0; j < nc; ++j) {
			q += sw::universal::quire_mul(A[i][j], x[j]);
		}
		sw::universal::convert(q.to_value(), b[i]);     // one and only rounding step of the fused-dot product
#if HPRBLAS_TRACE_ROUNDING_EVENTS
		sw::universal::quire<nbits, es> qdiff = q;
		sw::universal::quire<nbits, es> qsum = b[i];
		qdiff -= qsum;
		if (!qdiff.iszero()) {
			++errors;
			std::cout << "q    : " << q << std::endl;
			std::cout << "qsum : " << qsum << std::endl;
			std::cout << "qdiff: " << qdiff << std::endl;
			sw::universal::posit<nbits, es> roundingError;
			convert(qdiff.to_value(), roundingError);
			std::cout << "matvec b[" << i << "] = " << posit_format(b[i]) << " rounding error: " << posit_format(roundingError) << " " << roundingError << std::endl;
		}
#endif
	}
#if HPRBLAS_TRACE_ROUNDING_EVENTS
	if (errors) {
		std::cout << "HPR-BLAS: tracing found " << errors << " rounding errors in matvec operation\n";
	}
#endif
	return b;
}

// LEVEL 3 BLAS operators

template<typename Matrix>
void matmul(Matrix& C, const Matrix& A, const Matrix& B) {
	C = A * B;
}

// C = A * B fused matrix-matrix product when posits are used
template<size_t nbits, size_t es>
void matmul(mtl::mat::dense2D< sw::universal::posit<nbits, es> >& C, const mtl::mat::dense2D< sw::universal::posit<nbits, es> >& A, const mtl::mat::dense2D< sw::universal::posit<nbits, es> >& B) {
	// precondition
	assert(A.num_cols() == B.num_rows());
	size_t nr = A.num_rows();
	size_t nc = B.num_cols();
	size_t nk = A.num_cols();
	// TODO: add asserts to make certain that C is the right size

	for (size_t i = 0; i < nr; ++i) {
		for (size_t j = 0; j < nc; ++j) {
			sw::universal::quire<nbits, es> q(0);
			for (size_t k = 0; k < nk; ++k) {
				q += sw::universal::quire_mul(A[i][k], B[k][j]);
			}
			sw::universal::convert(q.to_value(), C[i][j]);     // one and only rounding step of the fused-dot product
		}
	}
}

// C = A * B fused matrix-matrix product when posits are used
template<size_t nbits, size_t es>
mtl::mat::dense2D< sw::universal::posit<nbits, es> > fmm(const mtl::mat::dense2D< sw::universal::posit<nbits, es> >& A, const mtl::mat::dense2D< sw::universal::posit<nbits, es> >& B) {
	// precondition
	assert(A.num_cols() == B.num_rows());
	size_t nr = A.num_rows();
	size_t nc = B.num_cols();
	size_t nk = A.num_cols();
	mtl::mat::dense2D< sw::universal::posit<nbits, es> > C(nr, nc);

	for (size_t i = 0; i < nr; ++i) {
		for (size_t j = 0; j < nc; ++j) {
			sw::universal::quire<nbits, es> q(0);
			for (size_t k = 0; k < nk; ++k) {
				q += sw::universal::quire_mul(A[i][k], B[k][j]);
			}
			sw::universal::convert(q.to_value(), C[i][j]);     // one and only rounding step of the fused-dot product
		}
	}
	return C;
}

template<typename Scalar>
inline Scalar minimum(const Scalar& a, const Scalar& b) {
	return (a < b ? a : b);
}

// if you only specify one set of block parameters in the specification then by
// the virtue of doing block matrix multiplication you generate the invariant : blockHeight == blockWidth
// Thus, no need to specify blockHeight and blockWidth: we can simplify to blockSize
// blockHeight = blockWidth = blockSize

// subBlockMM generates the partial sums of a sub-block matrix multiply
// the QuireMatrix is [blockHeight][blockWidth] submatrix
// A and B matrices are full [n][m] and [m][n] matrices
template<typename Matrix>
void subBlockMM(Matrix& C_partial, const Matrix& A, unsigned Ai, unsigned Aj, const Matrix& B, unsigned Bi, unsigned Bj) {
	assert(mtl::mat::num_rows(C_partial) == mtl::mat::num_cols(C_partial));
	using Scalar = typename Matrix::value_type;
//	constexpr size_t nbits = Scalar::nbits;
//	constexpr size_t es = Scalar::es;

	unsigned aRows = unsigned(mtl::mat::num_rows(A));
	unsigned aCols = unsigned(mtl::mat::num_cols(A));
	unsigned bRows = unsigned(mtl::mat::num_rows(B));
	unsigned bCols = unsigned(mtl::mat::num_cols(B));

	unsigned blockSize = unsigned(mtl::mat::num_rows(C_partial));

	unsigned aRow = Ai * blockSize;
	unsigned aCol = Aj * blockSize;
	unsigned bRow = Bi * blockSize;
	unsigned bCol = Bj * blockSize;

	// calculate the shape of submatrix A
	unsigned ar = (aRow + blockSize < aRows) ? blockSize : aRows - aRow;
	unsigned ac = (aCol + blockSize < aCols) ? blockSize : aCols - aCol;
	unsigned br = (bRow + blockSize < bRows) ? blockSize : bRows - bRow;
	unsigned bc = (bCol + blockSize < bCols) ? blockSize : bCols - bCol;

//	std::cout << "A=" << ar << "x" << ac << "  B=" << br << "x" << bc << std::endl;
	assert(ac == br);
	// A(ar,ac) x B(br,bc) = C(ar,bc) if ac == br
	for (unsigned i = 0; i < ar; ++i) {
		for (unsigned j = 0; j < bc; ++j) {
			for (unsigned k = 0; k < ac; ++k) {
				C_partial[i][j] += A[aRow + i][aCol + k] * B[bRow + k][bCol + j];
			}
//			std::cout << "A(" << Ai << "," << Aj << ") B(" << Bi << "," << Bj << ") C(" << i << "," << j << ")\n";
//			printMatrix(std::cout, "partial", C_partial);
		}
	}
}

// copySubBlockInto copies a subblock matrix into of the mother matrix
template<typename Matrix>
void copySubBlockInto(Matrix& A, unsigned ai, unsigned aj, const Matrix& SubBlock) {
	unsigned blockHeight = unsigned(mtl::mat::num_rows(SubBlock));
	unsigned blockWidth = unsigned(mtl::mat::num_cols(SubBlock));

	unsigned aRows = unsigned(mtl::mat::num_rows(A));
	unsigned aCols = unsigned(mtl::mat::num_cols(A));

	unsigned aRow = ai * blockHeight;
	unsigned aCol = aj * blockWidth;

	unsigned maxRow = (aRow + blockHeight < aRows) ? blockHeight : aRows - aRow;
	unsigned maxCol = (aCol + blockWidth < aCols) ? blockWidth : aCols - aCol;

	for (unsigned i = 0; i < maxRow; ++i) {
		for (unsigned j = 0; j < maxCol; ++j) {
			A[aRow + i][aCol + j] = SubBlock[i][j];
		}
	}
}

// bmm is a blocked matrix multply: C = A * B
template<typename Matrix>
Matrix bmm(const Matrix& A, const Matrix& B, unsigned blockSize) {
	// precondition
	assert(A.num_cols() == B.num_rows());
	unsigned nr = unsigned(A.num_rows());
	unsigned nc = unsigned(B.num_cols());
	unsigned nk = unsigned(A.num_cols());
	Matrix C(nr, nc);

	using Scalar = typename Matrix::value_type;
	Matrix C_partial(blockSize, blockSize);

	unsigned nrRowBlocks = nr % blockSize ? nr / blockSize + 1 : nr / blockSize;
	unsigned nrColBlocks = nc % blockSize ? nr / blockSize + 1 : nc / blockSize;
	for (unsigned bi = 0; bi < nrRowBlocks; ++bi) {			// row block index
		for (unsigned bj = 0; bj < nrColBlocks; ++bj) {		// col block index
			C_partial = Scalar(0);
			for (unsigned bk = 0; bk < nrRowBlocks; ++bk) { // block iterator
				subBlockMM(C_partial, A, bi, bk, B, bk, bj);
			}
			copySubBlockInto(C, bi, bj, C_partial);
		}
	}
	return C;
}

// subBlockMM generates the partial sums of a sub-block matrix multiply
// the QuireMatrix is a square matrix
// A and B matrices are full [n][m] and [m][n] matrices
template<typename QuireMatrix, typename Matrix>
void subBlockMM(QuireMatrix& C, const Matrix& A, unsigned Ai, unsigned Aj, const Matrix& B, unsigned Bi, unsigned Bj) {
	assert(mtl::mat::num_rows(C) == mtl::mat::num_cols(C));
	using Scalar = typename Matrix::value_type;
	constexpr size_t nbits = Scalar::nbits;
	constexpr size_t es = Scalar::es;

	unsigned aRows = unsigned(mtl::mat::num_rows(A));
	unsigned aCols = unsigned(mtl::mat::num_cols(A));
	unsigned bRows = unsigned(mtl::mat::num_rows(B));
	unsigned bCols = unsigned(mtl::mat::num_cols(B));

	unsigned blockSize = unsigned(mtl::mat::num_rows(C));

	unsigned aRow = Ai * blockSize;
	unsigned aCol = Aj * blockSize;
	unsigned bRow = Bi * blockSize;
	unsigned bCol = Bj * blockSize;

	// calculate the shape of submatrix A
	unsigned ar = (aRow + blockSize < aRows) ? blockSize : aRows - aRow;
	unsigned ac = (aCol + blockSize < aCols) ? blockSize : aCols - aCol;
	unsigned br = (bRow + blockSize < bRows) ? blockSize : bRows - bRow;
	unsigned bc = (bCol + blockSize < bCols) ? blockSize : bCols - bCol;

	//	std::cout << "A=" << ar << "x" << ac << "  B=" << br << "x" << bc << std::endl;
	assert(ac == br);
	// A(ar,ac) x B(br,bc) = C(ar,bc) if ac == br
	for (unsigned i = 0; i < ar; ++i) {
		for (unsigned j = 0; j < bc; ++j) {
			for (unsigned k = 0; k < ac; ++k) {
				C[i][j] += sw::universal::quire_mul<nbits,es>(A[aRow+i][aCol+k], B[bRow+k][bCol+j]);
			}
		}
	}
}

// subBlockRound takes a sub-block address and a QuireMatrix and rounds the partial sums

template<typename Matrix, typename QuireMatrix>
void subBlockRound(Matrix& C, unsigned ci, unsigned cj, const QuireMatrix& C_partial) {
	unsigned blockHeight = unsigned(mtl::mat::num_rows(C_partial));
	unsigned blockWidth = unsigned(mtl::mat::num_cols(C_partial));

	unsigned cRows = unsigned(mtl::mat::num_rows(C));
	unsigned cCols = unsigned(mtl::mat::num_cols(C));

	unsigned cRow = ci * blockHeight;
	unsigned cCol = cj * blockWidth;

	unsigned maxRow = (cRow + blockHeight < cRows) ? blockHeight : cRows - cRow;
	unsigned maxCol = (cCol + blockWidth < cCols) ? blockWidth : cCols - cCol;

	for (unsigned i = 0; i < maxRow; ++i) {
		for (unsigned j = 0; j < maxCol; ++j) {
			sw::universal::convert(C_partial[i][j].to_value(), C[cRow + i][cCol + j]);
		}
	}
}

// copySubBlock copies a subblock matrix out of the mother matrix
// This function is more generic than used in block matmul, as this can take non-square matrices
template<typename Matrix>
void copySubBlock(Matrix& SubBlock, const Matrix& M, unsigned bi, unsigned bj) {
	unsigned blockHeight = unsigned(mtl::mat::num_rows(SubBlock));
	unsigned blockWidth = unsigned(mtl::mat::num_cols(SubBlock));
	for (unsigned i = 0; i < blockHeight; ++i) {
		for (unsigned j = 0; j < blockWidth; ++j) {
			SubBlock[i][j] = M[bi*blockHeight + i][bj*blockWidth + j];
		}
	}
}

// bfmm is a blocked fused matrix multply: C = A * B
template<typename Matrix>
Matrix bfmm(const Matrix& A, const Matrix& B, unsigned blockSize) {
	// precondition
	assert(A.num_cols() == B.num_rows());
	unsigned nr = unsigned(A.num_rows());
	unsigned nc = unsigned(B.num_cols());
	unsigned nk = unsigned(A.num_cols());
	Matrix C(nr, nc);

	using Scalar = typename Matrix::value_type;
	constexpr size_t nbits = Scalar::nbits;
	constexpr size_t es = Scalar::es;
	using Quire = typename sw::universal::quire<nbits, es>;
	using QuireMatrix = typename mtl::mat::dense2D<Quire>;
	QuireMatrix C_partial(blockSize, blockSize);

	unsigned nrRowBlocks = nr % blockSize ? nr / blockSize + 1 : nr / blockSize;
	unsigned nrColBlocks = nc % blockSize ? nr / blockSize + 1  : nc / blockSize;
	for (unsigned bi = 0; bi < nrRowBlocks; ++bi) {			// row block index
		for (unsigned bj = 0; bj < nrColBlocks; ++bj) {		// col block index
			C_partial = Scalar(0);
			for (unsigned bk = 0; bk < nrRowBlocks; ++bk) { // block iterator
				subBlockMM(C_partial, A, bi, bk, B, bk, bj);
			}
			subBlockRound(C, bi, bj, C_partial);  // C_sub(i,j) = round(QuireMatrix)
		}
	}
	return C;
}

}} // namespace sw::hprblas


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2019, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#include <logog/include/logog.hpp>

#include <Eigen/Dense>

#include "Point3d.h"
#include "Vector3.h"

#include "GeometricBasics.h"

namespace MathLib
{
double orientation3d(MathLib::Point3d const& p,
                     MathLib::Point3d const& a,
                     MathLib::Point3d const& b,
                     MathLib::Point3d const& c)
{
    MathLib::Vector3 const ap (a, p);
    MathLib::Vector3 const bp (b, p);
    MathLib::Vector3 const cp (c, p);
    return MathLib::scalarTriple(bp,cp,ap);
}

double calcTetrahedronVolume(MathLib::Point3d const& a,
                             MathLib::Point3d const& b,
                             MathLib::Point3d const& c,
                             MathLib::Point3d const& d)
{
    const MathLib::Vector3 ab(a, b);
    const MathLib::Vector3 ac(a, c);
    const MathLib::Vector3 ad(a, d);
    return std::abs(MathLib::scalarTriple(ac, ad, ab)) / 6.0;
}

double calcTriangleArea(MathLib::Point3d const& a, MathLib::Point3d const& b,
                        MathLib::Point3d const& c)
{
    MathLib::Vector3 const u(a, c);
    MathLib::Vector3 const v(a, b);
    MathLib::Vector3 const w(MathLib::crossProduct(u, v));
    return 0.5 * w.getLength();
}

bool isPointInTetrahedron(MathLib::Point3d const& p, MathLib::Point3d const& a,
                          MathLib::Point3d const& b, MathLib::Point3d const& c,
                          MathLib::Point3d const& d, double eps)
{
    double const d0 (MathLib::orientation3d(d,a,b,c));
    // if tetrahedron is not coplanar
    if (std::abs(d0) > std::numeric_limits<double>::epsilon())
    {
        bool const d0_sign (d0>0);
        // if p is on the same side of bcd as a
        double const d1 (MathLib::orientation3d(d, p, b, c));
        if (!(d0_sign == (d1 >= 0) || std::abs(d1) < eps))
        {
            return false;
        }
        // if p is on the same side of acd as b
        double const d2 (MathLib::orientation3d(d, a, p, c));
        if (!(d0_sign == (d2 >= 0) || std::abs(d2) < eps))
        {
            return false;
        }
        // if p is on the same side of abd as c
        double const d3 (MathLib::orientation3d(d, a, b, p));
        if (!(d0_sign == (d3 >= 0) || std::abs(d3) < eps))
        {
            return false;
        }
        // if p is on the same side of abc as d
        double const d4 (MathLib::orientation3d(p, a, b, c));
        return d0_sign == (d4 >= 0) || std::abs(d4) < eps;
    }
    return false;
}

bool isPointInTriangle(MathLib::Point3d const& p,
                       MathLib::Point3d const& a,
                       MathLib::Point3d const& b,
                       MathLib::Point3d const& c,
                       double eps_pnt_out_of_plane,
                       double eps_pnt_out_of_tri,
                       MathLib::TriangleTest algorithm)
{
    switch (algorithm)
    {
        case MathLib::GAUSS:
            return gaussPointInTriangle(p, a, b, c, eps_pnt_out_of_plane,
                                        eps_pnt_out_of_tri);
        case MathLib::BARYCENTRIC:
            return barycentricPointInTriangle(p, a, b, c, eps_pnt_out_of_plane,
                                              eps_pnt_out_of_tri);
        default:
            ERR("Selected algorithm for point in triangle testing not found, "
                "falling back on default.");
    }
    return gaussPointInTriangle(p, a, b, c, eps_pnt_out_of_plane,
                                eps_pnt_out_of_tri);
}

bool gaussPointInTriangle(MathLib::Point3d const& q,
                          MathLib::Point3d const& a,
                          MathLib::Point3d const& b,
                          MathLib::Point3d const& c,
                          double eps_pnt_out_of_plane,
                          double eps_pnt_out_of_tri)
{
    MathLib::Vector3 const v(a, b);
    MathLib::Vector3 const w(a, c);

    Eigen::Matrix2d mat;
    mat(0, 0) = v.getSqrLength();
    mat(0, 1) = v[0] * w[0] + v[1] * w[1] + v[2] * w[2];
    mat(1, 0) = mat(0, 1);
    mat(1, 1) = w.getSqrLength();
    Eigen::Vector2d y;
    y << v[0] * (q[0] - a[0]) + v[1] * (q[1] - a[1]) + v[2] * (q[2] - a[2]),
        w[0] * (q[0] - a[0]) + w[1] * (q[1] - a[1]) + w[2] * (q[2] - a[2]);

    y = mat.partialPivLu().solve(y);

    const double lower(eps_pnt_out_of_tri);
    const double upper(1 + lower);

    if (-lower <= y[0] && y[0] <= upper && -lower <= y[1] && y[1] <= upper &&
        y[0] + y[1] <= upper)
    {
        MathLib::Point3d const q_projected(std::array<double, 3>{
            {a[0] + y[0] * v[0] + y[1] * w[0], a[1] + y[0] * v[1] + y[1] * w[1],
             a[2] + y[0] * v[2] + y[1] * w[2]}});
        if (MathLib::sqrDist(q, q_projected) <= eps_pnt_out_of_plane)
        {
            return true;
        }
    }

    return false;
}

bool barycentricPointInTriangle(MathLib::Point3d const& p,
                                MathLib::Point3d const& a,
                                MathLib::Point3d const& b,
                                MathLib::Point3d const& c,
                                double eps_pnt_out_of_plane,
                                double eps_pnt_out_of_tri)
{
    if (std::abs(MathLib::orientation3d(p, a, b, c)) > eps_pnt_out_of_plane)
    {
        return false;
    }

    MathLib::Vector3 const pa(p, a);
    MathLib::Vector3 const pb(p, b);
    MathLib::Vector3 const pc(p, c);
    double const area_x_2(calcTriangleArea(a, b, c) * 2);

    double const alpha((MathLib::crossProduct(pb, pc).getLength()) / area_x_2);
    if (alpha < -eps_pnt_out_of_tri || alpha > 1 + eps_pnt_out_of_tri)
    {
        return false;
    }
    double const beta((MathLib::crossProduct(pc, pa).getLength()) / area_x_2);
    if (beta < -eps_pnt_out_of_tri || beta > 1 + eps_pnt_out_of_tri)
    {
        return false;
    }
    double const gamma(1 - alpha - beta);
    return !(gamma < -eps_pnt_out_of_tri || gamma > 1 + eps_pnt_out_of_tri);
}

bool isPointInTriangleXY(MathLib::Point3d const& p,
                         MathLib::Point3d const& a,
                         MathLib::Point3d const& b,
                         MathLib::Point3d const& c)
{
    // criterion: p-a = u0 * (b-a) + u1 * (c-a); 0 <= u0, u1 <= 1, u0+u1 <= 1
    Eigen::Matrix2d mat;
    mat(0, 0) = b[0] - a[0];
    mat(0, 1) = c[0] - a[0];
    mat(1, 0) = b[1] - a[1];
    mat(1, 1) = c[1] - a[1];
    Eigen::Vector2d y;
    y << p[0] - a[0], p[1] - a[1];

    y = mat.partialPivLu().solve(y);

    // check if u0 and u1 fulfills the condition
    return 0 <= y[0] && y[0] <= 1 && 0 <= y[1] && y[1] <= 1 && y[0] + y[1] <= 1;
}

bool dividedByPlane(const MathLib::Point3d& a, const MathLib::Point3d& b,
                    const MathLib::Point3d& c, const MathLib::Point3d& d)
{
    for (unsigned x = 0; x < 3; ++x)
    {
        const unsigned y = (x + 1) % 3;
        const double abc =
            (b[x] - a[x]) * (c[y] - a[y]) - (b[y] - a[y]) * (c[x] - a[x]);
        const double abd =
            (b[x] - a[x]) * (d[y] - a[y]) - (b[y] - a[y]) * (d[x] - a[x]);

        if ((abc > 0 && abd < 0) || (abc < 0 && abd > 0))
        {
            return true;
        }
    }
    return false;
}

bool isCoplanar(const MathLib::Point3d& a, const MathLib::Point3d& b,
                const MathLib::Point3d& c, const MathLib::Point3d& d)
{
    const MathLib::Vector3 ab(a, b);
    const MathLib::Vector3 ac(a, c);
    const MathLib::Vector3 ad(a, d);

    if (ab.getSqrLength() < pow(std::numeric_limits<double>::epsilon(), 2) ||
        ac.getSqrLength() < pow(std::numeric_limits<double>::epsilon(), 2) ||
        ad.getSqrLength() < pow(std::numeric_limits<double>::epsilon(), 2))
    {
        return true;
    }

    // In exact arithmetic <ac*ad^T, ab> should be zero
    // if all four points are coplanar.
    const double sqr_scalar_triple(
        pow(MathLib::scalarProduct(MathLib::crossProduct(ac, ad), ab), 2));
    // Due to evaluating the above numerically some cancellation or rounding
    // can occur. For this reason a normalisation factor is introduced.
    const double normalisation_factor =
        (ab.getSqrLength() * ac.getSqrLength() * ad.getSqrLength());

    // tolerance 1e-11 is choosen such that
    // a = (0,0,0), b=(1,0,0), c=(0,1,0) and d=(1,1,1e-6) are considered as
    // coplanar
    // a = (0,0,0), b=(1,0,0), c=(0,1,0) and d=(1,1,1e-5) are considered as not
    // coplanar
    return (sqr_scalar_triple / normalisation_factor < 1e-11);
}

}  // end namespace MathLib


#include "stdafx.h"
#include "EigenParameterEstimator.h"

#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;

EigenParameterEstimator::EigenParameterEstimator()
{
}

EigenParameterEstimator::EigenParameterEstimator(MeasurementDB * mdb) : _mdb(mdb)
{
}

EigenParameterEstimator::~EigenParameterEstimator()
{
}

void EigenParameterEstimator::estimateParameters(AbstractSolution * sol, double newrelerr)
{
	int m = _mdb->get_size();
	int n = 2;

	MatrixXd A = MatrixXd(m, n);
	VectorXd b = VectorXd(m);
	VectorXd x = VectorXd(n);

	// Fill values in vector of right-hand side b
	for (int i = 0; i < m; i++)
		b[i] = _mdb->getPairAt(i).second;

	// Zero the solution vector x
	for (int i = 0; i < n; i++)
		x[i] = 0.0;

	// Fill the coefficient matrix
	for (int row = 0; row < m; row++) {
		A(row, 0) = 1.0;
		A(row, 1) = sol->evaluateConstantTermAt(_mdb->getPairAt(row).first);
	}

	x = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);

	// Update Solution
	for (int i = 1; i <= n; ++i) {
		sol->updateAt(i - 1, x[i - 1]);
	}
}


#ifndef NONLINEAR_BASIS_HPP
#define NONLINEAR_BASIS_HPP

#include <armadillo>
#include <math.h>

class NonLinearBasisFunction : public Basis {

public:

    int _nX;
    int _nK;
    int _nU;
    int _nM;
    int _nKU;

    NonLinearBasisFunction(int _nX = 7, int _nK = 25, int _nU = 2 , int _nM = 6, int _nKU = 18) : Basis(_nX,_nK,_nU,_nM,_nKU) {
        this->_nX = _nX;
        this->_nK = _nK;
        this->_nU = _nU;
        this->_nM = _nM;
        this->_nKU = _nKU;
    }

    arma::vec fk(const arma::vec & x, const arma::vec & u ) {
        return arma::join_cols( fkx(x), fku(x, u) );
    }

    arma::vec fkx( const arma::vec& x ) {
        return arma::vec({
          x[0],
          x[1],
          x[2],
          x[3],
          x[4],
          x[5],
          1
        });
    }

    arma::vec fku( const arma::vec & x, const arma::vec& u ) {

        return arma::vec({
            u[0],
            u[1],
            u[0]*x[0],
            u[0]*x[1],
            u[0]*x[2],
            u[0]*x[3],
            u[0]*x[4],
            u[0]*x[5],
            u[1]*x[0],
            u[1]*x[1],
            u[1]*x[2],
            u[1]*x[3],
            u[1]*x[4],
            u[1]*x[5],
            u[0]*cos(x[2]),
            u[0]*sin(x[2]),
            u[1]*cos(x[2]),
            u[1]*sin(x[2])
          });

    }

    arma::mat fkudu( const arma::vec & x, const arma::vec & u) {
        return arma::mat({
            {1,0},
            {0,1},
            {x[0],0},
            {x[1],0},
            {x[2],0},
            {x[3],0},
            {x[4],0},
            {x[5],0},
            {0,x[0]},
            {0,x[1]},
            {0,x[2]},
            {0,x[3]},
            {0,x[4]},
            {0,x[5]},
            {cos(x[2]), 0},
            {sin(x[2]), 0},
            {0, cos(x[2])},
            {0, sin(x[2])}
        });
    }

};

#endif


#include "utils.h"

#include <cmath>
#include <fstream>

#include <boost/format.hpp>
#include <boost/algorithm/string.hpp>

#include <Eigen/Dense>

namespace masterthesis {
    double expit(double x) {
        return x > 0 ? 1 / (1 + std::exp(-x)) : 1 - 1 / (1 + std::exp(x));
    }

    double log1exp(double x) {
        return x > 0 ? x + log1p(exp(-x)) : log1p(exp(x));
    }

    void readFeatureFile(std::string path,
                         size_t* const nItems, size_t* const mFeatures,
                         std::vector<double>* const features) {
        std::fstream features_file_input;
        features_file_input.open(path, std::ios::in);

        std::string line;
        std::vector<std::string> tokens;
        getline(features_file_input, line);
        boost::split(tokens, line, boost::is_any_of(","));
        *nItems = stoul(tokens[0]);
        *mFeatures = stoul(tokens[1]);

        features->reserve(*nItems * *mFeatures);
        for (size_t i = 0; i < *nItems; ++i) {
            getline(features_file_input, line);
            boost::split(tokens, line, boost::is_any_of(","));
            for (size_t j = 0; j < *mFeatures; ++j) {
                (*features)[(i * *mFeatures) + j] = stod(tokens[j]);
            }
        }
    }

    void readFeatureFileBoost(std::string path,
                              size_t &nItems, size_t &mFeatures,
                              Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> &features) {
        std::fstream features_file_input;
        features_file_input.open(path, std::ios::in);

        std::string line;
        std::vector<std::string> tokens;
        getline(features_file_input, line);
        boost::split(tokens, line, boost::is_any_of(","));
        nItems = stoul(tokens[0]);
        mFeatures = stoul(tokens[1]);

        features.resize(nItems, mFeatures);
        for (size_t i = 0; i < nItems; ++i) {
            getline(features_file_input, line);
            boost::split(tokens, line, boost::is_any_of(","));
            for (size_t j = 0; j < mFeatures; ++j) {
                features(i, j) = stod(tokens[j]);
            }
        }
    }
}