#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 {

// 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 incomingVecToPoint 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 traceVectorInHalfedge; // vector to keep tracing, measured against crossHe

  // If the trace ends (terminated == true)
  SurfacePoint endPoint;      // ending location
  Vector2 incomingVecToPoint; // 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 vecCartesian,
                                             const std::array<bool, 3>& edgeIsHittable, bool errorOnProblem) {

  // 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 (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 "
         << vecCartesian << endl;
  }

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

  // Test if the vector ends in the triangle
  Vector3 endPoint = startPoint + vecBary;
  if (TRACE_PRINT) {
    cout << "    endpoint: " << endPoint << endl;
  }
  if (isInsideTriangle(endPoint)) {
    // The trace ended! Call it a day.
    TraceSubResult result;
    result.terminated = true;
    result.endPoint = SurfacePoint(face, endPoint);
    result.incomingVecToPoint = vecCartesian;
    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 (tRayThis < tRay) {
      // This is the new closest intersection
      tRay = tRayThis;
      crossHe = currHe;
      iOppVertEnd = i;
    }
  }

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

  if (crossHe == Halfedge()) {
    if (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.incomingVecToPoint = vecCartesian;
    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
  Vector2 vecCartesianRemaining = (1.0 - tRay) * vecCartesian;
  Vector2 crossingEdgeVec = (vertexCoords[(iOppVertEnd + 2) % 3] - vertexCoords[(iOppVertEnd + 1) % 3]);
  Vector2 remainingVecInHalfedge = vecCartesianRemaining / crossingEdgeVec.normalize();
  if (!isfinite(remainingVecInHalfedge)) {
    if (TRACE_PRINT) {
      cout << "    NON FINITE REMAINING TRACE" << endl;
      cout << "    vecCartesianRemaining = " << vecCartesianRemaining << endl;
      cout << "    crossingEdgeVec = " << crossingEdgeVec << endl;
      cout << "    remainingVecInHalfedge = " << remainingVecInHalfedge << endl;
    }

    if (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()) {
    // Build the result
    TraceSubResult result;
    result.terminated = true;
    result.endPoint = SurfacePoint(crossHe.edge(), convertTToEdge(crossHe, tCross));
    result.incomingVecToPoint = remainingVecInHalfedge;

    return result;
  }

  // Build the result
  TraceSubResult result;
  result.terminated = false;
  result.crossHe = crossHe;
  result.tCross = tCross;
  result.traceVectorInHalfedge = remainingVecInHalfedge;
  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 vecCartesian,
                                             bool errorOnProblem) {

  // 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 = " << vecCartesian << 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, vecCartesian);
  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, vecCartesian, hittable, errorOnProblem);
}

// 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 traceVecInHalfedge, bool errorOnProblem) {

  // 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 = " << traceVecInHalfedge << endl;

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

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

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

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

    // Manual displacement projection to sum to 0 which perserves above properties
    double diff = -sum(vecBaryFromEdge);
    if (diff > 0) {
      vecBaryFromEdge.z += diff;
    } else {
      vecBaryFromEdge.x += diff / 3.;
      vecBaryFromEdge.y += diff / 3.;
      vecBaryFromEdge.z += diff / 3.;
    }
  }
  if (TRACE_PRINT) cout << "    vec bary after project " << vecBaryFromEdge << 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 vecBaryCanonicalFixed = permuteBarycentricToCanonical(vecBaryFromEdge, faceHe);
  if (TRACE_PRINT) {
    cout << "    iHe = " << iHe << endl;
    cout << "    startPoint = " << startPoint << endl;
    cout << "    canonical bary " << vecBaryCanonicalFixed << endl;
  }
  std::array<bool, 3> hittable = {{true, true, true}};
  hittable[iHe] = false;

  return traceInFaceBarycentric(geom, face, startPoint, vecBaryCanonicalFixed, traceVecInFace, hittable,
                                errorOnProblem);
}

// Trace starting from an edge
inline TraceSubResult traceGeodesic_fromEdge(IntrinsicGeometryInterface& geom, Edge currEdge, double tEdge,
                                             Vector2 currVec, bool errorOnProblem) {

  if (TRACE_PRINT) cout << "  edge trace " << currEdge << " tEdge = " << tEdge << " edge vec = " << currVec << 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 halfedgeTraceVec;
  if (currVec.y >= 0.) {
    traceHe = currEdge.halfedge().twin();
    halfedgeTraceVec = -currVec;
    tEdge = 1.0 - tEdge;
  } else {
    traceHe = currEdge.halfedge();

    // Can't go anyywhere if boundary halfedge
    if (!traceHe.isInterior()) {
      TraceSubResult result;
      result.terminated = true;
      result.endPoint = SurfacePoint(currEdge, tEdge);
      result.incomingVecToPoint = currVec;

      return result;
    }

    halfedgeTraceVec = currVec;
  }

  return traceInFaceFromEdge(geom, traceHe, tEdge, halfedgeTraceVec, errorOnProblem);
}

// Trace starting from a face
inline TraceSubResult traceGeodesic_fromFace(IntrinsicGeometryInterface& geom, Face currFace, Vector3 faceBary,
                                             Vector2 currVec, bool errorOnProblem) {

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

  return traceInFaceBarycentric(geom, currFace, faceBary, vecBary, currVec, {true, true, true}, errorOnProblem);
}

// Trace starting from a vertex (with a rescaled cartesian vector)
inline TraceSubResult traceGeodesic_fromVertex(IntrinsicGeometryInterface& geom, Vertex currVert, Vector2 currVec,
                                               bool errorOnProblem) {
  if (TRACE_PRINT) cout << "  vertex trace " << currVert << " edge vec = " << currVec << endl;

  double traceLen = currVec.norm();

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

  // 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 minCrossHalfedgeVec; // 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, currVec);
    double crossEnd = cross(intervalEnd, currVec);
    if (crossStart > 0. && crossEnd <= 0.) {
      wedgeHe = currHe;
      traceVecRelativeToStart = currVec / intervalStart;
      if (TRACE_PRINT) cout << "    wedge match! relative angle " << traceVecRelativeToStart << 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;
      minCrossHalfedgeVec = Vector2{1, TRACE_EPS_TIGHT} * traceLen;
    }
    if (std::fabs(crossEnd) < minCross) {
      minCross = std::fabs(crossEnd);
      minCrossHalfedge = nextHe;
      minCrossHalfedgeVec = Vector2{1, -TRACE_EPS_TIGHT} * traceLen;
    }

    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 " << minCrossHalfedgeVec << endl;
    // Convert to edge coordinates
    currVec = convertVecToEdge(minCrossHalfedge, minCrossHalfedgeVec);
    return traceGeodesic_fromEdge(geom, minCrossHalfedge.edge(), convertTToEdge(minCrossHalfedge, 0.), currVec,
                                  errorOnProblem);
  }

  // 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;
  traceVecRelativeToStart = traceVecRelativeToStart.pow(geom.vertexAngleSums[currVert] / sum);
  traceVecRelativeToStart = traceVecRelativeToStart.normalize() * currVec.norm(); // fix length

  // Compute the starting vector
  Vector2 startDirInFace = geom.halfedgeVectorsInFace[wedgeHe].normalize();
  Vector2 traceVecInFace = traceVecRelativeToStart * 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 vec in face " << traceVecInFace << endl;
  }

  return traceInFaceTowardsEdge(geom, wedgeHe.next(), traceVecInFace, errorOnProblem);
}

// 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,
                             bool includePath, bool errorOnProblem) {

  // Now, points are always in faces. Trace until termination.
  while (!prevTraceEnd.terminated) {

    // Construct a point where the previous trace ended
    if (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
           << " vec = " << prevTraceEnd.traceVectorInHalfedge << endl;
    }

    // Execute the next step of tracing
    prevTraceEnd = traceInFaceFromEdge(geom, prevTraceEnd.crossHe, prevTraceEnd.tCross,
                                       prevTraceEnd.traceVectorInHalfedge, errorOnProblem);
  }

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

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

} // namespace

TraceGeodesicResult traceGeodesic(IntrinsicGeometryInterface& geom, SurfacePoint startP, Vector2 traceVec,
                                  bool includePath, bool errorOnProblem) {
  geom.requireVertexAngleSums();
  geom.requireHalfedgeVectorsInVertex();
  geom.requireHalfedgeVectorsInFace();

  // The output data
  TraceGeodesicResult result;
  result.hasPath = includePath;
  if (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 (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, traceVec, errorOnProblem);
    break;
  }
  case SurfacePointType::Edge: {
    prevTraceEnd = traceGeodesic_fromEdge(geom, startP.edge, startP.tEdge, traceVec, errorOnProblem);
    break;
  }
  case SurfacePointType::Face: {
    prevTraceEnd = traceGeodesic_fromFace(geom, startP.face, startP.faceCoords, traceVec, errorOnProblem);
    break;
  }
  }

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

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

  return result;
}

TraceGeodesicResult traceGeodesic(IntrinsicGeometryInterface& geom, Face startFace, Vector3 startBary,
                                  Vector3 traceBaryVec, bool includePath, bool errorOnProblem) {

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

  // The output data
  TraceGeodesicResult result;
  result.hasPath = includePath;
  if (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 (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, traceVectorCartesian,
                                                       {true, true, true}, errorOnProblem);

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

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

  return result;
}

} // namespace surface
} // namespace geometrycentral


#include <QatGenericFunctions/Variable.h>
#include <QatGenericFunctions/Parameter.h>
#include <QatGenericFunctions/Square.h>
#include <QatGenericFunctions/Sqrt.h>
#include <QatGenericFunctions/Exp.h>
#include <QatGenericFunctions/VoigtDistribution.h>
#include <QatDataAnalysis/Hist1D.h>
#include <QatDataAnalysis/Table.h>
#include <QatDataModeling/TableLikelihoodFunctional.h>
#include <QatDataModeling/MinuitMinimizer.h>
#include <QatPlotWidgets/PlotView.h>
#include <QatPlotWidgets/MultipleViewWindow.h>
#include <QatPlotting/PlotStream.h>
#include <QatPlotting/PlotHist1D.h>
#include <QatPlotting/PlotFunction1D.h>
#include <QatPlotting/PlotProfile.h>
#include <QatPlotting/PlotKey.h>
#include <Eigen/Dense>
#include <QApplication>
#include <QMainWindow>
#include <QAction>
#include <QToolBar>
#include <QFont>
#include <cmath>
#include <iostream>
#include <string>
#include <vector>
#include <fstream>
#include <algorithm>
#include <random>

PlotView *createPlotView(const std::string& title, const std::string& xLabel, const std::string yLabel, const PRectF& viewRange);

typedef std::vector<double> Data;

Data readData(const std::string& file);
Data refineData(const Data& rawData, const double lowerBound, const double upperBound);
Hist1D binData(const Data& data);
Table tableData(const Data& data);

const std::string dataFileName = "data04.dat";
const bool verbose = true;
const double dataLowerBound = 0;
const double dataUpperBound = 200;
const double nBin = 1000;
const double histogramMin = 0;
const double histogramMax = 200;
const double M_PI4 = M_PI * M_PI * M_PI * M_PI;

int main(int argc, char **argv)
{
    QApplication app(argc, argv);
    QMainWindow window;

    Genfun::Square qPow2;
    Genfun::Sqrt qSqrt;
    Genfun::Exp qExp;

    const Data rawData = readData(dataFileName);
    const Data data = refineData(rawData, dataLowerBound, dataUpperBound);
    Hist1D dataHist = binData(data);
    Table dataTable = tableData(data);
    TableLikelihoodFunctional objFunc(dataTable);
    Genfun::Variable X = dataTable.symbol("E");

//  Prepare View
    const double viewXMin = histogramMin;
    const double viewXMax = histogramMax;
    const double viewYMin = 1e-2;
    const double viewYMax = dataHist.maxContents() * 3;
    PRectF viewRange(viewXMin, viewXMax, viewYMin, viewYMax);
    PlotView *view = createPlotView("Spectrum", "Energy / eV", "", viewRange);
    view->setLogY(true);
    window.setCentralWidget(view);

    PlotKey legend(viewXMin, viewYMax);
    legend.setFont(QFont("Times", 25, QFont::Bold));
    view->add(&legend);

// Plot Data
    PlotHist1D dataHistPlot(dataHist);
    {
        PlotHist1D::Properties prop;
        prop.plotStyle = PlotHist1D::Properties::SYMBOLS;
        prop.symbolSize = 10;
        prop.pen.setWidth(1.9);
        dataHistPlot.setProperties(prop);
    }
    view->add(&dataHistPlot);
    legend.add(&dataHistPlot, "Data");

// Voigt Fit
    Genfun::Parameter T("T", dataHist.mean()/3, dataHist.mean()/10, dataHist.mean());
    Genfun::Parameter Mu1("Mu1", 97, 92, 103);
    Genfun::Parameter Delta1("Delta1", 0.3, 0.01, 1);
    Genfun::Parameter Mu2("Mu2", 105, 100, 110);
    Genfun::Parameter Delta2("Delta2", 0.35, 0.01, 1);
    Genfun::Parameter Sigma("Sigma", 1, 0, 5);
    Genfun::Parameter fP("fPlank", 0.8, 0, 1);
    Genfun::Parameter fL1("fLine1", 0.6, 0, 1);

    Genfun::GENFUNCTION PlankPDF = (15/M_PI4)/(T*T*T*T) * (X*X*X) / (qExp(X/T) - 1);

    Genfun::VoigtDistribution VoigtPDF1;
    VoigtPDF1.delta().connectFrom(&Delta1);
    VoigtPDF1.sigma().connectFrom(&Sigma);

    Genfun::VoigtDistribution VoigtPDF2;
    VoigtPDF2.delta().connectFrom(&Delta2);
    VoigtPDF2.sigma().connectFrom(&Sigma);

    Genfun::GENFUNCTION Combo = dataHist.sum() * dataHist.binWidth() *
            (fP * PlankPDF + (1-fP)*fL1*VoigtPDF1(X-Mu1) + (1-fP)*(1-fL1)*VoigtPDF2(X-Mu2));

    MinuitMinimizer comboMinimizer(verbose);
    comboMinimizer.addParameter(&T);
    comboMinimizer.addParameter(&Mu1);
    comboMinimizer.addParameter(&Delta1);
    comboMinimizer.addParameter(&Mu2);
    comboMinimizer.addParameter(&Delta2);
    comboMinimizer.addParameter(&Sigma);
    comboMinimizer.addParameter(&fP);
    comboMinimizer.addParameter(&fL1);
    comboMinimizer.addStatistic(&objFunc, &Combo);
    comboMinimizer.minimize();

    PlotFunction1D comboPlot(Combo);
    {
        PlotFunction1D::Properties prop;
        prop.pen.setColor(Qt::red);
        prop.pen.setWidth(2);
        comboPlot.setProperties(prop);
    }
    view->add(&comboPlot);
    legend.add(&comboPlot, "Fit");

	QToolBar *toolBar = window.addToolBar("Tools");

    QAction *quitAction = toolBar->addAction("Quit (q)");
    quitAction->setShortcut(QKeySequence("q"));
	QObject::connect(quitAction, SIGNAL(triggered()), &app, SLOT(quit()));

    QAction *saveAction = toolBar->addAction("Save as (s)");
    saveAction->setShortcut(QKeySequence("s"));
    QObject::connect(saveAction, SIGNAL(triggered()), view, SLOT(save()));

	window.show();
	app.exec();

    return 0;
}

//////////////////////////////////////////////////////////////////////////////

Data readData(const std::string& file)
{
    std::ifstream dataFile(file);
    double dataIn;
    Data data;

    while (dataFile >> dataIn)
    {
        if (true)
        {
            data.push_back(dataIn);
        }
    }

    std::pair<Data::const_iterator,Data::const_iterator> MIN_MAX = std::minmax_element(data.begin(), data.end());

    std::cout << "Raw Data Min: " << *(MIN_MAX.first) << std::endl;
    std::cout << "Raw Data Max: " << *(MIN_MAX.second) << std::endl;
    std::cout << std::endl;

    return data;
}

Data refineData(const Data& rawData, const double lowerBound, const double upperBound)
{
    std::cout << "Data Selection Range: [" << lowerBound << ", " << upperBound << "]" << std::endl << std::endl;

    Data data;
    for (size_t i = 0; i < rawData.size(); i++)
    {
        if (lowerBound <= rawData[i] && rawData[i] <= upperBound)
        {
            data.push_back(rawData[i]);
        }
    }

    return data;
}

Hist1D binData(const Data& data)
{
    std::pair<Data::const_iterator,Data::const_iterator> MIN_MAX_Iter = std::minmax_element(data.begin(), data.end());
    double dataMin = *(MIN_MAX_Iter.first);
    double dataMax = *(MIN_MAX_Iter.second);

    std::cout << "Data Num: " << data.size() << std::endl;
    std::cout << "Data Min: " << dataMin << std::endl;
    std::cout << "Data Max: " << dataMax << std::endl;

    //Hist1D dataHist(nBin, binMin, binMax);
    Hist1D dataHist(nBin, std::floor(dataMin-1), std::ceil(dataMax+1));
    for (size_t i = 0; i < data.size(); i++)
    {
        dataHist.accumulate(data[i]);
    }

    std::cout << "Data Mean: " << dataHist.mean() << std::endl;
    std::cout << "Data Variance: " << dataHist.variance() << std::endl;
    std::cout << "Data Sigma: " << std::sqrt(dataHist.variance()) << std::endl;
    std::cout << std::endl;

    return dataHist;
}

Table tableData(const Data& data)
{
    Table dataTable("Data Table");

    for (size_t i = 0; i < data.size(); i++)
    {
        dataTable.add("E", data[i]);
        dataTable.capture();
    }

    return dataTable;
}

////////////////////////////////////////////////////////////////////////////////

PlotView *createPlotView(const std::string& title, const std::string& xLabel, const std::string yLabel, const PRectF& viewRange)
{
    PlotView *view_ptr = new PlotView(viewRange);
    view_ptr->setFixedWidth(1200);
    view_ptr->setFixedHeight(800);

    PlotStream titleStream(view_ptr->titleTextEdit());
    titleStream << PlotStream::Clear()
                << PlotStream::Center()
                << PlotStream::Family("Sans Serif")
                << PlotStream::Size(24)
                << title
                << PlotStream::EndP();

    PlotStream xLabelStream(view_ptr->xLabelTextEdit());
    xLabelStream << PlotStream::Clear()
                << PlotStream::Center()
                << PlotStream::Family("Sans Serif")
                << PlotStream::Size(24)
                << xLabel
                << PlotStream::EndP();

    PlotStream yLabelStream(view_ptr->yLabelTextEdit());
    yLabelStream << PlotStream::Clear()
                << PlotStream::Center()
                << PlotStream::Family("Sans Serif")
                << PlotStream::Size(24)
                << yLabel
                << PlotStream::EndP();

    return view_ptr;
}


#include "EigenUtil.h"
#include <boost/format.hpp>

using namespace boost;

namespace cnoid {

Matrix3 rotFromRpy(double r, double p, double y)
{
    const double cr = cos(r);
    const double sr = sin(r);
    const double cp = cos(p);
    const double sp = sin(p);
    const double cy = cos(y);
    const double sy = sin(y);

    Matrix3 R;
    R << cp*cy, sr*sp*cy - cr*sy, cr*sp*cy + sr*sy,
        cp*sy, sr*sp*sy + cr*cy, cr*sp*sy - sr*cy,
        -sp  , sr*cp           , cr*cp;

    return R;
}

Vector3 rpyFromRot(const Matrix3& R)
{
    double roll, pitch, yaw;

    if((fabs(R(0,0)) < fabs(R(2,0))) && (fabs(R(1,0)) < fabs(R(2,0)))) {
        // cos(p) is nearly = 0
        double sp = -R(2,0);
        if (sp < -1.0) {
            sp = -1.0;
        } else if (sp > 1.0) {
            sp = 1.0;
        }
        pitch = asin(sp); // -pi/2< p < pi/2

        roll = atan2(sp * R(0,1) + R(1,2),  // -cp*cp*sr*cy
                     sp * R(0,2) - R(1,1)); // -cp*cp*cr*cy

        if (R(0,0) > 0.0) { // cy > 0
            (roll < 0.0) ? (roll += PI) : (roll -= PI);
        }
        const double sr = sin(roll);
        const double cr = cos(roll);
        if(sp > 0.0){
            yaw = atan2(sr * R(1,1) + cr * R(1,2), //sy*sp
                        sr * R(0,1) + cr * R(0,2));//cy*sp
        } else {
            yaw = atan2(-sr * R(1,1) - cr * R(1,2),
                        -sr * R(0,1) - cr * R(0,2));
        }
    } else {
        yaw = atan2(R(1,0), R(0,0));
        const double sa = sin(yaw);
        const double ca = cos(yaw);
        pitch = atan2(-R(2,0), ca * R(0,0) + sa * R(1,0));
        roll = atan2(sa * R(0,2) - ca * R(1,2), -sa * R(0,1) + ca * R(1,1));
    }
    return Vector3(roll, pitch, yaw);
}

Vector3 omegaFromRot(const Matrix3& R)
{
    double alpha = (R(0,0) + R(1,1) + R(2,2) - 1.0) / 2.0;

    if(fabs(alpha - 1.0) < 1.0e-6) {   //th=0,2PI;
        return Vector3::Zero();

    } else {
        double th = acos(alpha);
        double s = sin(th);

        if (s < std::numeric_limits<double>::epsilon()) {   //th=PI
            return Vector3( sqrt((R(0,0)+1)*0.5)*th, sqrt((R(1,1)+1)*0.5)*th, sqrt((R(2,2)+1)*0.5)*th );
        }

        double k = -0.5 * th / s;

        return Vector3((R(1,2) - R(2,1)) * k,
                       (R(2,0) - R(0,2)) * k,
                       (R(0,1) - R(1,0)) * k);
    }
}

std::string str(const Vector3& v)
{
    return str(format("%1%  %2%  %3%") % v[0] % v[1] % v[2]);
}

bool toVector3(const std::string& s, Vector3& out_v)
{
    const char* nptr = s.c_str();
    char* endptr;
    for(int i=0; i < 3; ++i){
        out_v[i] = strtod(nptr, &endptr);
        if(endptr == nptr){
            return false;
        }
        nptr = endptr;
        while(isspace(*nptr)){
            nptr++;
        }
        if(*nptr == ','){
            nptr++;
        }
    }
    return true;
}

void normalizeRotation(Matrix3& R)
{
    Matrix3::ColXpr x = R.col(0);
    Matrix3::ColXpr y = R.col(1);
    Matrix3::ColXpr z = R.col(2);
    x.normalize();
    z = x.cross(y).normalized();
    y = z.cross(x);
}

void normalizeRotation(Position& T)
{
    typedef Position::LinearPart::ColXpr ColXpr;
    Position::LinearPart R = T.linear();
    ColXpr x = R.col(0);
    ColXpr y = R.col(1);
    ColXpr z = R.col(2);
    x.normalize();
    z = x.cross(y).normalized();
    y = z.cross(x);
}

}


//  (C) Copyright Nick Thompson 2019.
//  (C) Copyright Matt Borland 2021.
//  Use, modification and distribution are subject to 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_MATH_STATISTICS_T_TEST_HPP
#define BOOST_MATH_STATISTICS_T_TEST_HPP

#include <cmath>
#include <cstddef>
#include <iterator>
#include <utility>
#include <type_traits>
#include <vector>
#include <stdexcept>
#include <boost/math/distributions/students_t.hpp>
#include <boost/math/statistics/univariate_statistics.hpp>

namespace boost { namespace math { namespace statistics { namespace detail {

template<typename ReturnType, typename T>
ReturnType one_sample_t_test_impl(T sample_mean, T sample_variance, T num_samples, T assumed_mean)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;
    using std::sqrt;
    typedef boost::math::policies::policy<
          boost::math::policies::promote_float<false>,
          boost::math::policies::promote_double<false> >
          no_promote_policy;

    Real test_statistic = (sample_mean - assumed_mean)/sqrt(sample_variance/num_samples);
    auto student = boost::math::students_t_distribution<Real, no_promote_policy>(num_samples - 1);
    Real pvalue;
    if (test_statistic > 0) {
        pvalue = 2*boost::math::cdf<Real>(student, -test_statistic);;
    }
    else {
        pvalue = 2*boost::math::cdf<Real>(student, test_statistic);
    }
    return std::make_pair(test_statistic, pvalue);
}

template<typename ReturnType, typename ForwardIterator>
ReturnType one_sample_t_test_impl(ForwardIterator begin, ForwardIterator end, typename std::iterator_traits<ForwardIterator>::value_type assumed_mean)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;
    std::pair<Real, Real> temp = mean_and_sample_variance(begin, end);
    Real mu = std::get<0>(temp);
    Real s_sq = std::get<1>(temp);
    return one_sample_t_test_impl<ReturnType>(mu, s_sq, Real(std::distance(begin, end)), Real(assumed_mean));
}

// https://en.wikipedia.org/wiki/Student%27s_t-test#Equal_or_unequal_sample_sizes,_unequal_variances_(sX1_%3E_2sX2_or_sX2_%3E_2sX1)
template<typename ReturnType, typename T>
ReturnType welchs_t_test_impl(T mean_1, T variance_1, T size_1, T mean_2, T variance_2, T size_2)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;
    using no_promote_policy = boost::math::policies::policy<boost::math::policies::promote_float<false>, boost::math::policies::promote_double<false>>;
    using std::sqrt;

    Real dof_num = (variance_1/size_1 + variance_2/size_2) * (variance_1/size_1 + variance_2/size_2);
    Real dof_denom = ((variance_1/size_1) * (variance_1/size_1))/(size_1 - 1) +
                     ((variance_2/size_2) * (variance_2/size_2))/(size_2 - 1);
    Real dof = dof_num / dof_denom;

    Real s_estimator = sqrt((variance_1/size_1) + (variance_2/size_2));

    Real test_statistic = (static_cast<Real>(mean_1) - static_cast<Real>(mean_2))/s_estimator;
    auto student = boost::math::students_t_distribution<Real, no_promote_policy>(dof);
    Real pvalue;
    if (test_statistic > 0)
    {
        pvalue = 2*boost::math::cdf<Real>(student, -test_statistic);;
    }
    else
    {
        pvalue = 2*boost::math::cdf<Real>(student, test_statistic);
    }

    return std::make_pair(test_statistic, pvalue);
}

// https://en.wikipedia.org/wiki/Student%27s_t-test#Equal_or_unequal_sample_sizes,_similar_variances_(1/2_%3C_sX1/sX2_%3C_2)
template<typename ReturnType, typename T>
ReturnType two_sample_t_test_impl(T mean_1, T variance_1, T size_1, T mean_2, T variance_2, T size_2)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;
    using no_promote_policy = boost::math::policies::policy<boost::math::policies::promote_float<false>, boost::math::policies::promote_double<false>>;
    using std::sqrt;

    Real dof = size_1 + size_2 - 2;
    Real pooled_std_dev = sqrt(((size_1-1)*variance_1 + (size_2-1)*variance_2) / dof);
    Real test_statistic = (mean_1-mean_2) / (pooled_std_dev*sqrt(1.0/static_cast<Real>(size_1) + 1.0/static_cast<Real>(size_2)));

    auto student = boost::math::students_t_distribution<Real, no_promote_policy>(dof);
    Real pvalue;
    if (test_statistic > 0)
    {
        pvalue = 2*boost::math::cdf<Real>(student, -test_statistic);;
    }
    else
    {
        pvalue = 2*boost::math::cdf<Real>(student, test_statistic);
    }

    return std::make_pair(test_statistic, pvalue);
}

template<typename ReturnType, typename ForwardIterator>
ReturnType two_sample_t_test_impl(ForwardIterator begin_1, ForwardIterator end_1, ForwardIterator begin_2, ForwardIterator end_2)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;
    using std::sqrt;
    auto n1 = std::distance(begin_1, end_1);
    auto n2 = std::distance(begin_2, end_2);

    ReturnType temp_1 = mean_and_sample_variance(begin_1, end_1);
    Real mean_1 = std::get<0>(temp_1);
    Real variance_1 = std::get<1>(temp_1);
    Real std_dev_1 = sqrt(variance_1);

    ReturnType temp_2 = mean_and_sample_variance(begin_2, end_2);
    Real mean_2 = std::get<0>(temp_2);
    Real variance_2 = std::get<1>(temp_2);
    Real std_dev_2 = sqrt(variance_2);

    if(std_dev_1 > 2 * std_dev_2 || std_dev_2 > 2 * std_dev_1)
    {
        return welchs_t_test_impl<ReturnType>(mean_1, variance_1, Real(n1), mean_2, variance_2, Real(n2));
    }
    else
    {
        return two_sample_t_test_impl<ReturnType>(mean_1, variance_1, Real(n1), mean_2, variance_2, Real(n2));
    }
}

// https://en.wikipedia.org/wiki/Student%27s_t-test#Dependent_t-test_for_paired_samples
template<typename ReturnType, typename ForwardIterator>
ReturnType paired_samples_t_test_impl(ForwardIterator begin_1, ForwardIterator end_1, ForwardIterator begin_2, ForwardIterator end_2)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;
    using no_promote_policy = boost::math::policies::policy<boost::math::policies::promote_float<false>, boost::math::policies::promote_double<false>>;
    using std::sqrt;

    std::vector<Real> delta;
    ForwardIterator it_1 = begin_1;
    ForwardIterator it_2 = begin_2;
    std::size_t n = 0;
    while(it_1 != end_1 && it_2 != end_2)
    {
        delta.emplace_back(static_cast<Real>(*it_1++) - static_cast<Real>(*it_2++));
        ++n;
    }

    if(it_1 != end_1 || it_2 != end_2)
    {
        throw std::domain_error("Both sets must have the same number of values.");
    }

    std::pair<Real, Real> temp = mean_and_sample_variance(delta.begin(), delta.end());
    Real delta_mean = std::get<0>(temp);
    Real delta_std_dev = sqrt(std::get<1>(temp));

    Real test_statistic = delta_mean/(delta_std_dev/sqrt(n));

    auto student = boost::math::students_t_distribution<Real, no_promote_policy>(n - 1);
    Real pvalue;
    if (test_statistic > 0)
    {
        pvalue = 2*boost::math::cdf<Real>(student, -test_statistic);;
    }
    else
    {
        pvalue = 2*boost::math::cdf<Real>(student, test_statistic);
    }

    return std::make_pair(test_statistic, pvalue);
}
} // namespace detail

template<typename Real, typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline auto one_sample_t_test(Real sample_mean, Real sample_variance, Real num_samples, Real assumed_mean) -> std::pair<double, double>
{
    return detail::one_sample_t_test_impl<std::pair<double, double>>(sample_mean, sample_variance, num_samples, assumed_mean);
}

template<typename Real, typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline auto one_sample_t_test(Real sample_mean, Real sample_variance, Real num_samples, Real assumed_mean) -> std::pair<Real, Real>
{
    return detail::one_sample_t_test_impl<std::pair<Real, Real>>(sample_mean, sample_variance, num_samples, assumed_mean);
}

template<typename ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline auto one_sample_t_test(ForwardIterator begin, ForwardIterator end, Real assumed_mean) -> std::pair<double, double>
{
    return detail::one_sample_t_test_impl<std::pair<double, double>>(begin, end, assumed_mean);
}

template<typename ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline auto one_sample_t_test(ForwardIterator begin, ForwardIterator end, Real assumed_mean) -> std::pair<Real, Real>
{
    return detail::one_sample_t_test_impl<std::pair<Real, Real>>(begin, end, assumed_mean);
}

template<typename Container, typename Real = typename Container::value_type,
         typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline auto one_sample_t_test(Container const & v, Real assumed_mean) -> std::pair<double, double>
{
    return detail::one_sample_t_test_impl<std::pair<double, double>>(std::begin(v), std::end(v), assumed_mean);
}

template<typename Container, typename Real = typename Container::value_type,
         typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline auto one_sample_t_test(Container const & v, Real assumed_mean) -> std::pair<Real, Real>
{
    return detail::one_sample_t_test_impl<std::pair<Real, Real>>(std::begin(v), std::end(v), assumed_mean);
}

template<typename ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline auto two_sample_t_test(ForwardIterator begin_1, ForwardIterator end_1, ForwardIterator begin_2, ForwardIterator end_2) -> std::pair<double, double>
{
    return detail::two_sample_t_test_impl<std::pair<double, double>>(begin_1, end_1, begin_2, end_2);
}

template<typename ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline auto two_sample_t_test(ForwardIterator begin_1, ForwardIterator end_1, ForwardIterator begin_2, ForwardIterator end_2) -> std::pair<Real, Real>
{
    return detail::two_sample_t_test_impl<std::pair<Real, Real>>(begin_1, end_1, begin_2, end_2);
}

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline auto two_sample_t_test(Container const & u, Container const & v) -> std::pair<double, double>
{
    return detail::two_sample_t_test_impl<std::pair<double, double>>(std::begin(u), std::end(u), std::begin(v), std::end(v));
}

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline auto two_sample_t_test(Container const & u, Container const & v) -> std::pair<Real, Real>
{
    return detail::two_sample_t_test_impl<std::pair<Real, Real>>(std::begin(u), std::end(u), std::begin(v), std::end(v));
}

template<typename ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline auto paired_samples_t_test(ForwardIterator begin_1, ForwardIterator end_1, ForwardIterator begin_2, ForwardIterator end_2) -> std::pair<double, double>
{
    return detail::paired_samples_t_test_impl<std::pair<double, double>>(begin_1, end_1, begin_2, end_2);
}

template<typename ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline auto paired_samples_t_test(ForwardIterator begin_1, ForwardIterator end_1, ForwardIterator begin_2, ForwardIterator end_2) -> std::pair<Real, Real>
{
    return detail::paired_samples_t_test_impl<std::pair<Real, Real>>(begin_1, end_1, begin_2, end_2);
}

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline auto paired_samples_t_test(Container const & u, Container const & v) -> std::pair<double, double>
{
    return detail::paired_samples_t_test_impl<std::pair<double, double>>(std::begin(u), std::end(u), std::begin(v), std::end(v));
}

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline auto paired_samples_t_test(Container const & u, Container const & v) -> std::pair<Real, Real>
{
    return detail::paired_samples_t_test_impl<std::pair<Real, Real>>(std::begin(u), std::end(u), std::begin(v), std::end(v));
}

}}} // namespace boost::math::statistics
#endif


#include <Eigen/Dense>
#include <boost/math/distributions.hpp>
#include <math.h>
#include <iostream>

#ifdef BAZEL
#include "Stats/Stats.hpp"
#else
#include "Stats.hpp"
#endif

using namespace std;
using namespace Eigen;

float Stats::ChiSquaredTest(MatrixXf observed, MatrixXf expected) {
    MatrixXf diff = observed - expected;
    MatrixXf x = diff.cwiseProduct(diff).cwiseQuotient(expected);
    return x.sum();
}

float Stats::ChiToPValue(float chisqr_value, int dof) {
    boost::math::chi_squared dist(dof);
    return 1 - boost::math::cdf(dist, chisqr_value);
}

float Stats::WaldTest(float mle, float var, float candidate) {
    return pow(mle - candidate, 2) / var;
}

float Stats::FisherExactTest(MatrixXf X) {
   int N = X.sum();
   float NFac = boost::math::factorial<float>(N);
   MatrixXf rowSums = X.rowwise().sum();
   MatrixXf colSums = X.colwise().sum();
   MatrixXf comFacs = X.unaryExpr(std::ptr_fun(boost::math::factorial<float>));
   MatrixXf rFacs = rowSums.unaryExpr(std::ptr_fun(boost::math::factorial<float>));
   MatrixXf cFacs = colSums.unaryExpr(std::ptr_fun(boost::math::factorial<float>));
   float rowsFacs = rFacs.prod();
   float colsFacs = cFacs.prod();
   float componentFacs = comFacs.prod();
   return rowsFacs*colsFacs/(NFac*componentFacs);
}

float Stats::BonCorrection(float pVal, int number) {
  return pVal/number;
}

float Stats::get_ts(float beta, float var, float sigma){
  return beta/(sqrt(var * sigma));
}

float Stats::get_qs(float ts, int N, int q){
  return 2*Stats::ChiToPValue(abs(ts), N-q);
}

void StatsBasic::setAttributeMatrix(const string &string1, MatrixXf *xd) {

}

StatsBasic::StatsBasic() {
    shouldCorrect = false;
}

StatsBasic::StatsBasic(const unordered_map<string, string> & options) {
    string tmp;
    try {
        tmp = options.at("correctNum");
        if (tmp == "Bonferroni correction"){
            shouldCorrect = true;
        }
        else{
            shouldCorrect = false;
        }
    } catch (std::out_of_range& oor) {
        shouldCorrect = true;
    }
}

void StatsBasic::checkGenoType() {
    long r = X.rows();
    long c = X.cols();
    int s = 0;
    bool go = true;
    for (long i=0;i<r&&go;i++){
        for (long j=0;j<c&&go;j++){
            if (X(i,j) == 2){
                s = 1;
                go = false;
            }
        }
    }
    if (s == 0){
        genoType = 1;
    }
    else{
        genoType = 2;
    }
}

void StatsBasic::assertReadyToRun() {
    beta = MatrixXf::Zero(X.cols(), y.cols());
    checkGenoType();
}

void StatsBasic::BonferroniCorrection() {
    if (shouldCorrect){
        beta = beta*X.rows();
        MatrixXf m = MatrixXf::Ones(beta.rows(), beta.cols());
        beta = beta.cwiseMin(m);
    }
}

float StatsBasic::getProgress() {
    return progress;
}

bool StatsBasic::getIsRunning() {
    return isRunning;
}

void StatsBasic::stop() {
    shouldStop = true;
}

void StatsBasic::setUpRun() {
    isRunning = true;
    progress = 0.0;
    shouldStop = false;
}

void StatsBasic::finishRun() {
    isRunning = false;
    progress = 1.0;
}

MatrixXf StatsBasic::getBeta() {
    MatrixXf tmp = MatrixXf::Zero(beta.rows(), beta.cols());
    for (long i = 0; i<beta.rows(); i++){
        for (long j=0; j<beta.cols(); j++){
            if (beta(i,j)>0){
                tmp(i,j) = -log10(beta(i,j));
            }
            else{
                tmp(i,j) = 0;
            }
        }
    }
    return tmp;
}


/*
 * qball.cpp
 *
 * Created on: 10.07.2012
 * @author Ralph Schurade
 */
#include "qball.h"
#include "sharpqballthread.h"

#include "fmath.h"

#include "../data/datasets/datasetdwi.h"

#include "../gui/gl/glfunctions.h"

#include <QDebug>
#include <qmath.h>

#include <boost/math/special_functions/spherical_harmonic.hpp>

QBall::QBall()
{
}

QBall::~QBall()
{
}

Matrix QBall::calcQBallBase( Matrix gradients, double lambda, int maxOrder )
{
    //qDebug() << "start calculating qBall base";
    double sh_size( ( maxOrder + 1 ) * ( maxOrder + 2 ) / 2 );

    // check validity of input:
    if ( gradients.Nrows() == 0 )
        throw std::invalid_argument( "No gradient directions specified." );

    if ( gradients.Ncols() != 3 )
        throw std::invalid_argument( "Gradients have to be 3D." );

    // calculate spherical harmonics base:
    Matrix SH = FMath::sh_base( gradients, maxOrder );

    // calculate the Laplace-Beltrami and the Funk-Radon transformation:
    ColumnVector LBT( sh_size );
    ColumnVector FRT( sh_size );
    LBT = 0;
    FRT = 0;

    for ( int order = 0; order <= maxOrder; order += 2 )
    {
        double frt_val = 2.0 * M_PI * boost::math::legendre_p<double>( order, 0 );
        double lbt_val = lambda * order * order * ( order + 1 ) * ( order + 1 );

        for ( int degree( -order ); degree <= order; ++degree )
        {
            int i = order * ( order + 1 ) / 2 + degree;
            LBT( i + 1 ) = lbt_val;
            FRT( i + 1 ) = frt_val;
        }
    }

    // prepare the calculation of the pseudoinverse:
    Matrix B = SH.t() * SH;

    // update with Laplace-Beltrami operator:
    for ( int i = 0; i < sh_size; ++i )
    {
        B( i + 1, i + 1 ) += LBT( i + 1 );
    }

    Matrix out = B.i() * SH.t();

    // the Funk-Radon transformation:
    for ( int i = 0; i < B.Nrows(); ++i )
    {
        for ( int j = 0; j < out.Ncols(); ++j )
        {
            out( i + 1, j + 1 ) *= FRT( i + 1 );
        }
    }

    //qDebug() << "finished calculating qBall base";

    return out;
}

/*
 * CMRImage calc_sharp_q_ball(

 const CMRImage& data,
 const CMRImage& b_zero,
 const matrixT gradients,
 const baseT order,
 const rangeS r )
 {
 *
 */

void QBall::sharpQBall( DatasetDWI* ds, int order, QVector<ColumnVector>& out )
{
    int numThreads = GLFunctions::idealThreadCount;

    QVector<SharpQBallThread*> threads;
    // create threads
    for ( int i = 0; i < numThreads; ++i )
    {
        threads.push_back( new SharpQBallThread( ds, order, i ) );
    }

    // run threads
    for ( int i = 0; i < numThreads; ++i )
    {
        threads[i]->start();
    }

    // wait for all threads to finish
    for ( int i = 0; i < numThreads; ++i )
    {
        threads[i]->wait();
    }

    out.clear();
    // combine fibs from all threads
    for ( int i = 0; i < numThreads; ++i )
    {
        out += threads[i]->getQBallVector();
    }

    for ( int i = 0; i < numThreads; ++i )
    {
        delete threads[i];
    }
}


// BSD 2-Clause License
//
// Copyright (c) 2021, Eijiro SHIBUSAWA
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "mex.h"

#include <Eigen/Dense>

#include <iostream>

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	if ((nlhs != 1) || (nrhs != 2))
	{
		return;
	}

	const mxArray *A = prhs[0];
	const mxArray *B = prhs[1];
	mwSize cA = mxGetNumberOfDimensions(A);
	mwSize cB = mxGetNumberOfDimensions(B);
	if ((cA != 2) || (cB != 2))
	{
		return;
	}

	const mwSize *dA = mxGetDimensions(A);
	const mwSize *dB = mxGetDimensions(B);
	const mwSize nA = dA[0], mA = dA[1]; // [nA, mA] = size(A)
	const mwSize nB = dB[0], mB = dB[1]; // [nB, mB] = size(B)
	if (mA != nB)
	{
		std::cerr << "nonconformant arguments (op1 is " << nA << "x" << mA << ", op2 is " << nB << "x" << mB << ")" << std::endl;
		return;
	}

	int nDims = 2;
	mwSize dims[] = {nA, mB};
	plhs[0] = mxCreateNumericArray(nDims, dims, mxDOUBLE_CLASS, mxREAL);

	double *pA = reinterpret_cast<double *>(mxGetData(A)); // column major
	double *pB = reinterpret_cast<double *>(mxGetData(B)); // column major
	double *pC = reinterpret_cast<double *>(mxGetData(plhs[0]));
	Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> > matA(pA, nA, mA);
	Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> > matB(pB, nB, mB);
	Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> > matC(pC, dims[0], dims[1]);
	matC = matA * matB;
}


#include "../include/util.h"
#include <Eigen/Dense>
#include <vector>
#include <iostream>
#include <tbb/tbb.h>

using namespace std;
using namespace Eigen;

Matrix3f eigenso3hat(Vector3f w)
{
  Matrix3f what;
  what << 0.0, -w(2), w(1),
          w(2), 0.0, -w(0),
          -w(1), w(0), 0.0;
  return what;
}

Matrix3f eigenso3exp(Vector3f w)
{
  float theta = w.norm();
  Matrix3f R = Matrix3f::Identity();
  if (theta < 1e-6) {
    return R;
  }
  else {
    Matrix3f what = eigenso3hat(w);
    R += (sin(theta) / theta) * what + ((1 - cos(theta)) / (theta * theta)) * (what * what);
    return R;
  }
}

Vector3f so3log(Matrix3f R)
{
  float d = 0.5 * (R.trace() - 1);

  Matrix3f lnR = (acos(d) / (2 * sqrt(1 - d*d))) * (R - R.transpose());

  Vector3f w;
  w(0) = lnR(2,1);
  w(1) = lnR(0,2);
  w(2) = lnR(1,0);

  return w;
}

MatrixXf reprojectionJacFn(VectorXf cam, Vector3f lmk, std::vector<float> K_vec)
{
  Matrix3f K = Map<Matrix<float,3,3>>(K_vec.data()).transpose();
  Vector3f w;
  Vector3f t;
  t << cam(0), cam(1), cam(2);
  w << cam(3), cam(4), cam(5);
  Matrix3f Rcw = eigenso3exp(w);

  Vector3f lmk_cf = Rcw * lmk + t;
  Vector3f p = K * lmk_cf;

  MatrixXf j_proj(2,3);
  j_proj << 1 / p(2), 0, -p(0) / pow(p(2),2),
           0, 1 / p(2), -p(1) / pow(p(2),2);

  Matrix3f dR_wx_dw = - eigenso3hat(Rcw * lmk);

  MatrixXf jac(2, 9);
  jac.block(0, 0, 2, 3) = j_proj * K;
  jac.block(0, 3, 2, 3) = j_proj * K * dR_wx_dw;
  jac.block(0, 6, 2, 3) = (j_proj * K) * Rcw;

  return jac;
}

void eval_reprojection_error(float* reproj, unsigned n_edges,
                      vector<unsigned int> active_flag,
                      float* cam_beliefs_eta_, float* cam_beliefs_lambda_,
                      float* lmk_beliefs_eta_, float* lmk_beliefs_lambda_,
                      unsigned* measurements_camIDs, unsigned* measurements_lIDs,
                      float* measurements_, float* K_,
                      const vector<unsigned int>& bad_associations)
{
  Matrix3f K = Map<Matrix3f>(K_).transpose();
  reproj[0] = 0.0;
  reproj[1] = 0.0;
  unsigned n_active_edges = 0;

  // Make two vectors for storing every reprojection result computed in
  // parallel so that no locking is required in the parallel loop:
  vector<float> reprojNorm(n_edges, 0.f);
  vector<float> reprojSqNorm(n_edges, 0.f);

  // Use as many threads as there are cores on the CPU:
  tbb::task_scheduler_init init(tbb::task_scheduler_init::automatic);

  for (unsigned e = 0; e < n_edges; ++e) {
    n_active_edges += active_flag[e];
  }

  tbb::parallel_for(0U, n_active_edges, [&](unsigned e) {
    if ((find(bad_associations.begin(), bad_associations.end(), e) == bad_associations.end())) {

      Matrix<float,6,1> cam_eta = Map<Matrix<float,6,1>>(&cam_beliefs_eta_[measurements_camIDs[e] * 6]);
      Matrix<float,6,6> cam_lam = Map<Matrix<float,6,6>>(&cam_beliefs_lambda_[measurements_camIDs[e] * 36]);
      VectorXf cam_mu = cam_lam.transpose().inverse() * cam_eta;

      Vector3f lmk_eta = Map<Vector3f>(&lmk_beliefs_eta_[measurements_lIDs[e] * 3]);
      Matrix3f lmk_lam = Map<Matrix3f>(&lmk_beliefs_lambda_[measurements_lIDs[e] * 9]);
      Vector3f lmk_mu = lmk_lam.transpose().inverse() * lmk_eta;

      Vector3f w;
      w << cam_mu(3), cam_mu(4), cam_mu(5);
      Matrix3f R = eigenso3exp(w);

      Vector3f pcf = R * lmk_mu;

      pcf(0) += cam_mu(0);
      pcf(1) += cam_mu(1);
      pcf(2) += cam_mu(2);

      Vector3f predicted = (K * pcf) / pcf(2);
      Vector2f residuals = Map<Vector2f>(&measurements_[2*e]);

      residuals(0) -= predicted(0);
      residuals(1) -= predicted(1);

      reprojNorm[e] = residuals.norm();
      reprojSqNorm[e] = 0.5 * residuals.squaredNorm();
    }
  });

  n_active_edges -= bad_associations.size();

  // Now sum up the results outside of the prallel loop.
  // No need to exclude inactive edges while summing as their entries
  // will have been initialised to 0 and then not updated in the loop above:
  for (unsigned e = 0; e < n_edges; ++e) {
    // n_active_edges += active_flag[e];
    reproj[0] += reprojNorm[e];
    reproj[1] += reprojSqNorm[e];
  }

  // cout << "Number of active edges: " << n_active_edges << "\n";
  reproj[0] /= n_active_edges;
}

void update_eta(unsigned n_keyframes, unsigned n_points,
                std::vector<float> cam_priors_lambda_,
                std::vector<float> cam_priors_mean_,
                std::vector<float>& cam_priors_eta_,
                std::vector<float> lmk_priors_lambda_,
                std::vector<float> lmk_priors_mean_,
                std::vector<float>& lmk_priors_eta_)
{
  for (unsigned cID = 0; cID < n_keyframes; ++cID) {
    Matrix<float,6,6> lambda;
    Matrix<float,6,1> mu;
    lambda = Map<Matrix<float,6,6>>(&cam_priors_lambda_[cID * 36]);
    mu = Map<Matrix<float,6,1>>(&cam_priors_mean_[cID*6]);

    Matrix<float,6,1> eta = lambda * mu;

    cam_priors_eta_[cID*6] = eta(0,0);
    cam_priors_eta_[cID*6 + 1] = eta(1,0);
    cam_priors_eta_[cID*6 + 2] = eta(2,0);
    cam_priors_eta_[cID*6 + 3] = eta(3,0);
    cam_priors_eta_[cID*6 + 4] = eta(4,0);
    cam_priors_eta_[cID*6 + 5] = eta(5,0);

  }
  for (unsigned lID = 0; lID < n_points; ++lID) {
    Matrix3f lambda;
    Vector3f mu;
    lambda = Map<Matrix3f>(&lmk_priors_lambda_[lID*9]);
    mu = Map<Vector3f>(&lmk_priors_mean_[lID*3]);
    Vector3f eta = lambda * mu;

    lmk_priors_eta_[lID*3] = eta(0);
    lmk_priors_eta_[lID*3 + 1] = eta(1);
    lmk_priors_eta_[lID*3 + 2] = eta(2);
  }
}

void initialise_new_kf(std::vector<float>& cam_priors_eta_, std::vector<float>& lmk_priors_eta_,
                       float* cam_beliefs_eta_, float* cam_beliefs_lambda_,
                       std::vector<float> cam_priors_lambda_, std::vector<float> lmk_priors_lambda_,
                       std::vector<unsigned int> lmk_weaken_flag_,
                       unsigned data_counter, unsigned n_points)
{
  Matrix<float,6,1> previous_kf_eta = Map<Matrix<float,6,1>>(&cam_beliefs_eta_[data_counter * 6]);
  Matrix<float,6,6> previous_kf_lam = Map<Matrix<float,6,6>>(&cam_beliefs_lambda_[data_counter * 36]);
  VectorXf previous_kf_mu = previous_kf_lam.transpose().inverse() * previous_kf_eta;
  Matrix<float,6,6> new_kf_lam = Map<Matrix<float,6,6>>(&cam_priors_lambda_[(data_counter + 1) * 36]);
  VectorXf new_kf_eta = new_kf_lam.transpose() * previous_kf_mu;
  for (unsigned i = 0; i < 6; ++i) {
    cam_priors_eta_[(data_counter + 1) * 6 + i] = new_kf_eta(i);
  }

  // Use prior on keyframe for prior on newly observed landmarks
  Vector3f previous_kf_w;
  previous_kf_w << previous_kf_mu(3), previous_kf_mu(4), previous_kf_mu(5);
  Matrix3f previous_kf_R_w2c = eigenso3exp(previous_kf_w);
  Vector4f loc_cam_frame;
  loc_cam_frame << 0.0, 0.0, 1.0, 1.0;
  Matrix4f Tw2c;
  Tw2c << previous_kf_R_w2c(0,0), previous_kf_R_w2c(0,1), previous_kf_R_w2c(0,2), previous_kf_mu(0),
          previous_kf_R_w2c(1,0), previous_kf_R_w2c(1,1), previous_kf_R_w2c(1,2), previous_kf_mu(1),
          previous_kf_R_w2c(2,0), previous_kf_R_w2c(2,1), previous_kf_R_w2c(2,2), previous_kf_mu(2),
          0.0, 0.0, 0.0, 1.0;
  Vector4f new_lmk_mu_wf_homog = Tw2c.inverse() * loc_cam_frame;
  Vector3f new_lmk_mu_wf;
  new_lmk_mu_wf << new_lmk_mu_wf_homog(0), new_lmk_mu_wf_homog(1), new_lmk_mu_wf_homog(2);
  Matrix3f lmk_prior_lambda;
  Vector3f new_lmk_eta;
  for (unsigned i = 0; i < n_points; ++i) {
    if (lmk_weaken_flag_[data_counter*n_points + i] == 5) {  // newly observed landmark
      lmk_prior_lambda = Map<Matrix3f>(&lmk_priors_lambda_[i*9]);
      new_lmk_eta = lmk_prior_lambda.transpose() * new_lmk_mu_wf;
      for (unsigned j = 0; j < 3; ++j) {
        lmk_priors_eta_[i * 3 + j] = new_lmk_eta(j);
      }
    }
  }
}

float KL_divergence(VectorXf eta1, VectorXf eta2, MatrixXf lambda1, MatrixXf lambda2) {

  VectorXf mu1 = lambda1.inverse() * eta1;
  VectorXf mu2 = lambda2.inverse() * eta2;

  float KL =  0.5 * ( (lambda2 * lambda1.inverse()).trace() + (mu2 - mu1).dot(lambda1 * (mu2 - mu1)) - eta1.size()
              + log( lambda1.determinant() / lambda2.determinant() ) );

  if (isnan(KL)) {
    cout << "was null" << "\n";
    cout << eta1 << "\n";
    cout << lambda1 << "\n";
    cout << eta2 << "\n";
    cout << lambda2 << "\n";
  }

  return KL;

}

float symmetricKL(VectorXf eta1, VectorXf eta2, MatrixXf lambda1, MatrixXf lambda2) {

  return (KL_divergence(eta1, eta2, lambda1, lambda2) + KL_divergence(eta2, eta1, lambda2, lambda1)) / 2;

}


// This section of program is the Levenberg-Marquardt solution to estimate 2 parameters of a and b.
//and was modified from this source: https://github.com/SarvagyaVaish/Eigen-Levenberg-Marquardt-Optimization

#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <math.h>
#include <Eigen/Eigen>

#include <unsupported/Eigen/NonLinearOptimization>
using namespace std;

struct LMFunctor
{
	// 'm' pairs of (x, f(x))
	Eigen::MatrixXf measuredValues;

	// Compute 'm' errors, one for each data point, for the given parameter values in 'x'
	int operator()(const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
	{
		// 'x' has dimensions n x 1
		// It contains the current estimates for the parameters.

		// 'fvec' has dimensions m x 1
		// It will contain the error for each data point.

		float aParam = x(0);
		float bParam = x(1);

		for (int i = 0; i < values(); i++) {
			float xValue = measuredValues(i, 0);
			float yValue = measuredValues(i, 1);

			fvec(i) = yValue - (1.0 / (1.0+ aParam * pow(xValue, 2*bParam)) );
		}
		return 0;
	}

	// Compute the jacobian of the errors
	int df(const Eigen::VectorXf &x, Eigen::MatrixXf &fjac) const
	{
		// 'x' has dimensions n x 1
		// It contains the current estimates for the parameters.

		// 'fjac' has dimensions m x n
		// It will contain the jacobian of the errors, calculated numerically in this case.

		float epsilon;
		epsilon = 1e-5f;

		for (int i = 0; i < x.size(); i++) {
			Eigen::VectorXf xPlus(x);
			xPlus(i) += epsilon;
			Eigen::VectorXf xMinus(x);
			xMinus(i) -= epsilon;

			Eigen::VectorXf fvecPlus(values());
			operator()(xPlus, fvecPlus);

			Eigen::VectorXf fvecMinus(values());
			operator()(xMinus, fvecMinus);

			Eigen::VectorXf fvecDiff(values());
			fvecDiff = (fvecPlus - fvecMinus) / (2.0f * epsilon);

			fjac.block(0, i, values(), 1) = fvecDiff;
		}

		return 0;
	}

	// Number of data points, i.e. values.
	int m;

	// Returns 'm', the number of values.
	int values() const { return m; }

	// The number of parameters, i.e. inputs.
	int n;

	// Returns 'n', the number of inputs.
	int inputs() const { return n; }

};

//
// Goal
//
// Given a non-linear equation: f(x) = 1.0/(1.0+a*pow(x,2*b))
// and 'm' data points (x1, f(x1)), (x2, f(x2)), ..., (xm, f(xm))
// our goal is to estimate 'n' parameters (2 in this case: a, b)
// using LM optimization.
//
void estimateParameters(float &a, float &b, float mindist, float spread, ofstream& logFile)
{

	std::vector<float> x_values;
	std::vector<float> y_values;

	/**
	 * The interval used for data fitting
	 * The values were adopted from https://github.com/lmcinnes/umap/blob/master/umap/umap_.py#L1138
	 */
	const float minInterval=0;
	const float maxInterval=3*spread;
	const int intervalCounts=300;

	for (int i = 0; i<intervalCounts; ++i){

		float tmp=minInterval+float(i)/float(intervalCounts)*(maxInterval-minInterval);
		x_values.push_back(tmp);

		if (tmp <= mindist) y_values.push_back(1.0);
		else if (tmp > mindist) y_values.push_back(exp((mindist-tmp)/spread));
		else {
			logFile<< "Error: Negative x_values during Parameter Estimation"<<endl;
			cout<< "Error: Negative x_values during Parameter Estimation"<<endl;
		}
	}

	// 'm' is the number of data points.
	int m = x_values.size();

	// Move the data into an Eigen Matrix.
	// The first column has the input values, x. The second column is the f(x) values.
	Eigen::MatrixXf measuredValues(m, 2);
	for (int i = 0; i < m; i++) {
		measuredValues(i, 0) = x_values[i];
		measuredValues(i, 1) = y_values[i];
	}

	// 'n' is the number of parameters (a and b) in the function.
	int n = 2;

	// 'x' is vector of length 'n' containing the initial values for the parameters.
	// The parameters 'x' are also referred to as the 'inputs' in the context of LM optimization.
	// The LM optimization inputs should not be confused with the x input values.
	Eigen::VectorXf x(n);
	x(0) = 1.8;             // initial value for 'a'
	x(1) = 0.7;             // initial value for 'b'

	//
	// Run the LM optimization
	// Create a LevenbergMarquardt object and pass it the functor.
	//

	LMFunctor functor;
	functor.measuredValues = measuredValues;
	functor.m = m;
	functor.n = n;

	Eigen::LevenbergMarquardt<LMFunctor, float> lm(functor);
	int status = lm.minimize(x);
	logFile << "LM optimization status: " << status << std::endl;
	cout << "LM optimization status: " << status << std::endl;
	//
	// Results
	// The 'x' vector also contains the results of the optimization.
	//
	logFile << "Optimization results" << std::endl;
	logFile << "\ta: " << x(0) << std::endl;
	logFile << "\tb: " << x(1) << std::endl;
	cout << "Optimization results" << std::endl;
	cout << "\ta: " << x(0) << std::endl;
	cout << "\tb: " << x(1) << std::endl;

	a=x(0);
	b=x(1);

}


#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
//#include <boost/multiprecision/mpfr.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/math/special_functions/bessel.hpp>
#define PREC 300    //change this to set precision

using namespace boost::multiprecision;

typedef number<cpp_dec_float<PREC> > big_float;

big_float dpii_iter( big_float current, big_float previous, big_float alpha, big_float beta, big_float gamma, int n )
{
  return (current * (alpha * n + beta)) / (1 - current * current) - previous;
}

big_float initval( big_float lambda )
{
  using namespace boost::math;
  return cyl_bessel_i(1, lambda) / cyl_bessel_i(0, lambda);
}

big_float jac_offdiag( big_float a, big_float b, big_float c )
{
  return (1 - a)*(1 - b * b)*(1 + c);
}

big_float jac_diag( big_float a, big_float b, big_float c )
{
  return (1 - b)*a - (1 + b)*c;
}

int compute_verb ( std::vector<big_float > &verblunsky, big_float lambda, bool use_bessel, double init )
{
  std::cout << "Computing verblunsky coefficients: initializing..." << std::endl;
  big_float coeff = -1*(2 / lambda);
  if (use_bessel)
    verblunsky[0] = initval( lambda );
  else
    verblunsky[0] = init;
  verblunsky[1] = dpii_iter( verblunsky[0], (big_float) -1, coeff, coeff, (big_float) 0, 0 );
  int kmax = verblunsky.size() - 1;
  std::cout << "Computing verblunsky coefficients: iterating..." << std::endl;
  for (int k = 2; k <= kmax; k++)
  {
    verblunsky[k] = dpii_iter( verblunsky[k-1], verblunsky[k-2], coeff, coeff, (big_float) 0, (k-1) );
  }
  std::cout << "Done computing verblunsky coefficients." << std::endl;
  return 0;
}

int compute_jac ( std::vector<big_float > &diag, std::vector<big_float > &offdiag, const std::vector<big_float > & verblunsky )
{
  std::cout << "Computing jacobi coefficients: initializing..." << std::endl;
  int kmax = offdiag.size() - 1;
  diag[0] = jac_diag( verblunsky[1], verblunsky[0], (big_float) -1 );
  offdiag[0] = 2 * verblunsky[0];
  std::cout << "Computing jacobi coefficients: iterating..." << std::endl;
  for (int k = 1; k <= kmax; k++)
  {
    diag[k] = jac_diag( verblunsky[2*k], verblunsky[2*k - 1], verblunsky[2*k - 2] );
    offdiag[k] = jac_offdiag( verblunsky[2*k + 1], verblunsky[2*k], verblunsky[2*k - 1] );
  }
  std::cout << "Done computing jacobi coefficients." << std::endl;
  return 0;
}

int writeout_csv( std::string fname, int out_digits, std::vector<big_float> verblunsky, std::vector<big_float> diag, std::vector<big_float> offdiag )
{
  std::cout << "Writing to file..." << std::endl;
  int terms = (int) verblunsky.size() / 2 - 1;
  std::ofstream outfile{fname};
  outfile << "Index alpha b square(a) diagnostic(1-sq(a))" << std::endl;
  outfile << std::setprecision(out_digits);
//  outfile << std::setprecision(std::numeric_limits<big_float>::maxdigits10);
  for (int k = 0; k < terms; k++)
  {
    big_float diagnostic;
    diagnostic = log(abs(1. - offdiag[k]));
    outfile << k << " " << verblunsky[k] << " " << diag[k] << " " << offdiag[k] << " " << diagnostic << std::endl;
  }
  outfile.close();
  return 0;
}

int get_input( int &digits, int &terms, double &lambda, std::string &fname, double &init_val, bool &use_bessel )
{
  std::string s;
  while (true)
  {
    std::cout << "Enter number of digits to output: " << std::endl;
    std::getline(std::cin, s);
    std::stringstream instr(s);
    if (instr >> digits)
      break;
    std::cout << "Input should be an integer: " << std::endl;
  }
  while (true)
  {
    std::cout << "Enter number of terms to compute: " << std::endl;
    std::getline(std::cin, s);
    std::stringstream instr(s);
    if (instr >> terms)
      break;
    std::cout << "Input should be an integer: " << std::endl;
  }
  while (true)
  {
    std::cout << "Enter value of lambda to use: " << std::endl;
    std::getline(std::cin, s);
    std::stringstream instr(s);
    if (instr >> lambda)
      break;
    std::cout << "Input should be a float: " << std::endl;
  }
  while (true)
  {
    std::cout << "Use mod bessel initial condition? (y/n) " << std::endl;
    std::getline(std::cin, s);
    if (s == "y")
    {
      use_bessel = true;
      break;
    }
    if (s == "n")
    {
      use_bessel = false;
      break;
    }
    std::cout << "Input should be (y/n) " << std::endl;
  }
  if (!use_bessel)
  {
    while (true)
    {
      std::cout << "Enter alternative initial value: " << std::endl;
      std::getline(std::cin, s);
      std::stringstream instr(s);
      if (instr >> init_val)
        break;
      std::cout << "Input should be a float: " << std::endl;
    }
  }
  std::cout << "Enter output filename: " << std::endl;
  std::getline(std::cin, fname);
}

int main(int argc, char *argv[])
{

  int digits;
  int terms;
  double lambda_in;
  std::string fname;
  bool use_bessel;
  double init_val;

  get_input( digits, terms, lambda_in, fname, init_val, use_bessel );
//  big_float::default_precision(digits);
  big_float lambda(lambda_in);

  int jterms = (terms / 2) - 1;
  std::vector<big_float > verblunsky(terms);
  std::vector<big_float > diag(jterms);
  std::vector<big_float > offdiag(jterms);

  compute_verb (verblunsky, lambda, use_bessel, init_val );
  compute_jac (diag, offdiag, verblunsky);
  writeout_csv( fname, digits, verblunsky, diag, offdiag );
  return 0;
}


#pragma once

//! All the number theory goes in here

#include <boost/multiprecision/gmp.hpp>

namespace rubbishrsa {
  // Saves me a lot of typing
  namespace bmp = boost::multiprecision;
  using bigint = bmp::mpz_int;

  // Extended Euclid's algorithm is the name of this algorithm (I think)
  struct egcd_result { bigint gcd; std::pair<bigint, bigint> coefficients; };
  egcd_result egcd(const bigint& a, const bigint& b);

  // This is actually implemented in the numeric library I have used, but that would be cheating
  /// Performs the Miller-Rabin primality check `certainty_log_2` times
  bool is_prime(const bigint& candidate, uint_fast8_t certainty_log_4 = 64);

  /// Generates a prime that is at least 2^(bits - 1) long.
  //
  // Apparently "strong primes" are better, but computing these is much harder, and RSA say they are unnecceary
  //
  // Because RSA (company) can be trusted. Yes.
  bigint generate_prime(uint_fast16_t bits);

  /// Calculate the lowest common multiple of two numbers
  bigint lcm(const bigint& a, const bigint& b);

  // Calculating Carmichael's function for an arbitrary number is complex and pointless
  //
  // Instead, we can just calculate it for our special case
  inline bigint carmichael_semiprime(const bigint& p, const bigint& q) {
    return lcm(p - 1, q - 1);
  }

  // Again, exists in our library, but I don't want to cheat
  /// Computes a^(-1) mod n
  bigint modinv(const bigint& a, const bigint& n);

  /// An implementation of Pollard's rho algorithm
  ///
  /// @param a: the x^0 term of the polynomial
  bigint pollard_rho(const bigint& n);

  /// Selects the fastest implemented factorisation algorithm for the given semiprime, and returns the factors
  std::pair<bigint, bigint> factorise_semiprime(const bigint& semiprime);

  // Some functions that convert between ascii and bigint
  bigint ascii2bigint(std::string_view str);
  bigint ascii2bigint(std::istream& str);
  std::string bigint2ascii(bigint str);
  bigint hex2bigint(std::string_view hex);

  inline size_t floor_log2(bigint i) {
    size_t bits = 0;
    while (i) {
      i >>= 1;
      ++bits;
    }
    return bits;
  }
}


/**
 * @file engquistoshernumericalflux.cc
 * @brief NPDE homework "EngquistOsherNumericalFlux" code
 * @author Oliver Rietmann
 * @date 23.04.2019
 * @copyright Developed at ETH Zurich
 */

#include "engquistoshernumericalflux.h"

#include <Eigen/Core>
#include <algorithm>
#include <cmath>

namespace EngquistOsherNumericalFlux {

/* SAM_LISTING_BEGIN_1 */
double EngquistOsherNumFlux(double v, double w) {
  double result;
  //====================
  // Your code goes here
  if(v>=0 && w>=0){
    result=std::cosh(v);
  } else if (w>0 && v<=0){
    result=std::cosh(v)+std::cosh(0);
  } else if (w<0 && v>=0){
    result=std::cosh(v)+std::cosh(w)-std::cosh(0);
  } else if (w<0 && v<0){
    result=std::cosh(w);
  }
  //====================
  return result;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
Eigen::VectorXd solveCP(double a, double b, Eigen::VectorXd u0, double T) {
  // Find the maximal speed of propagation
  double A = u0.minCoeff();
  double B = u0.maxCoeff();
  double K = std::max(std::abs(std::sinh(A)), std::abs(std::sinh(B)));
  // Set uniform timestep according to CFL condition
  int N = u0.size();
  double h = (b - a) / N;
  double tau_max = h / K;
  double timesteps = std::ceil(T / tau_max);
  double tau = T / timesteps;

  // Main timestepping loop
  //====================
  // Your code goes here
  for (int i = 0; i < timesteps; i++)
  {
    u0(0)=u0(0)-tau/h*(EngquistOsherNumericalFlux(u0(0),u0(1))-EngquistOsherNumericalFlux(u0(0),u0(0)));
    for (int j=1; i<N-1; j++){
      u0(j)=u0(j) - tau/h*(EngquistOsherNumFlux(u0(j),u0(j+1))-EngquistOsherNumFlux(u0(j-1),u0(j)));
    }
    u0(N-1)=u0(N-1)-tau/h*(EngquistOsherNumericalFlux(u0(N-1),u0(N-1))-EngquistOsherNumericalFlux(u0(N-2),u0(N-1)));    /* code */
  }

  //===================//
  return u0;
}
/* SAM_LISTING_END_2 */

}  // namespace EngquistOsherNumericalFlux


#include "bayesclassifier.h"

#include <algorithm> // for std::max_element
#include <cassert>

#include <Eigen/LU>

#include "algorithm.h"

using Eigen::VectorXd;
using Eigen::MatrixXd;
using std::vector;

double multivariate_normal_pd(const VectorXd& x,
                              const VectorXd& mu,
                              const MatrixXd& sigma) {
    // XXX would be more efficient to pre-compute sigma.inverse() and
    // sigma.determinant()
    double d = x.size();
    VectorXd v = x - mu;
    double a = pow(2.0 * M_PI, d/2.0);
    double b = sqrt(sigma.determinant());
    double c = v.transpose() * sigma.inverse() * v;
    return (1.0 / (a*b)) * exp( - c / 2.0);
}

BayesClassifier::BayesClassifier(const vector<VectorXd>& x,
                                 const vector<int>& y) :
k(0), p(), mu(), sigma() {
    // n is the number of points
    unsigned n = x.size();
    assert(n > 0);
    assert(y.size() == n);

    // d is the dimensionality
    int d = x[0].size();
    for (const VectorXd& v : x)
        assert(v.size() == d);

    // number of classes
    k = *(std::max_element(y.cbegin(), y.cend())) + 1;

    for (int i = 0; i < k; ++i) {
        // find all points in class i
        vector<VectorXd> xi;
        for (unsigned j = 0; j < n; ++j)
            if (y[j] == i)
                xi.push_back(x[j]);

        // ni is the number of points in class i
        unsigned ni = xi.size();
        assert(ni > 0);

        // prior probability
        p.push_back((double)ni / (double)n);

        // class mean
        VectorXd m = VectorXd::Zero(d);
        for (const VectorXd& v : xi)
            m += v;
        m /= ni;
        mu.push_back(m);

        // centered data matrix
        MatrixXd z(d, ni);
        for (unsigned j = 0; j < ni; ++j)
            z.col(j) = xi[j] - m;

        // covariance matrix
        sigma.push_back((1.0/(double)ni) * z * z.transpose());
    }
}

int BayesClassifier::predict(const VectorXd& x) const {
    return arg_max(0, k, [&x,this](int i) {
        return multivariate_normal_pd(x, mu[i], sigma[i]) * p[i];
    });
}

int BayesClassifier::nClasses() const {
    return k;
}
double BayesClassifier::priorProbability(int i) const {
    return p[i];
}
VectorXd BayesClassifier::mean(int i) const {
    return mu[i];
}
MatrixXd BayesClassifier::covarianceMatrix(int i) const {
    return sigma[i];
}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_math/blob/master/LICENSE

#ifndef CBR_MATH__INTERP__PIECEWISE_LINEAR_HPP_
#define CBR_MATH__INTERP__PIECEWISE_LINEAR_HPP_

#include <Eigen/Dense>

#include <utility>
#include <vector>
#include <array>

#include "piecewise_poly.hpp"

namespace cbr
{

class PiecewiseLinear
{
protected:
  using row_t = Eigen::Matrix<double, 1, Eigen::Dynamic>;
  using matrix_t = Eigen::MatrixXd;

public:
  /**
   * @brief Create a scalar-valued piecewise linear polynomial
   *
   * @tparam T1 PiecewisePoly::row_t
   * @tparam T2 PiecewisePoly::row_t
   * @param x sorted breakpoints
   * @param y values
   *
   * The resulting function f is s.t.
   *  f(t) = y[i] + alpha * (y[i+1] - y[i]) if x[i] <= t < x[i+1]
   *     where alpha = (t - x[i]) / (x[i+1] - x[i])
   *  f(t) = y[0] if t < x[0]
   *  f(t) = y[x.size() - 1] if t >= x[x.size() - 1]
   */
  template<typename T1, typename T2>
  static PiecewisePoly fit(
    T1 && x,
    const Eigen::DenseBase<T2> & y)
  {
    static_assert(is_eigen_dense_v<T1>, "x must be an Eigen::DenseBase object.");

    auto coeffs = generateCoeffs(x, y);
    return PiecewisePoly(std::forward<T1>(x), std::move(coeffs));
  }

  /**
   * @brief Create a vector-valued piecewise linear polynomial
   *
   * @tparam T1 PiecewisePoly::row_t
   * @tparam T2 PiecewisePoly::matrix_t or container_t<PiecewisePoly::row_t>
   * @param x sorted breakpoints
   * @param y values
   *
   * The resulting function f is s.t.
   *  f(t) = y[i] + alpha * (y[i+1] - y[i]) if x[i] <= t < x[i+1]
   *     where alpha = (t - x[i]) / (x[i+1] - x[i])
   *  f(t) = y[0] if t < x[0]
   *  f(t) = y[x.size() - 1] if t >= x[x.size() - 1]
   */
  template<typename T1, typename T2>
  static PiecewisePolyND fitND(
    T1 && x,
    const T2 & ys)
  {
    static_assert(is_eigen_dense_v<T1>, "x must be an Eigen::DenseBase object.");

    if (x.size() < 2) {
      throw std::invalid_argument("x must be of size > 1.");
    }

    std::vector<matrix_t> coefLists;

    if constexpr (is_eigen_dense_v<T2>) {
      if (ys.rows() < 1) {
        throw std::invalid_argument("Dimension of the data must be > 0.");
      }

      if (ys.cols() != x.size()) {
        throw std::invalid_argument("The number of columns of ys must be equal to the size of x.");
      }
      coefLists.reserve(static_cast<std::size_t>(ys.rows()));

      for (Eigen::Index i = 0; i < ys.rows(); i++) {
        coefLists.push_back(generateCoeffs(x, ys.row(i)));
      }
    } else {
      static_assert(
        is_eigen_dense_v<typename T2::value_type>&& T2::value_type::IsVectorAtCompileTime,
        "ys must be a container of Eigen::DenseBase vector objects");

      if (ys.size() < 1) {
        throw std::invalid_argument("Dimension of the data must be > 0.");
      }

      coefLists.reserve(ys.size());

      for (const auto & y : ys) {
        coefLists.push_back(generateCoeffs(x, y));
      }
    }

    return PiecewisePolyND(std::forward<T1>(x), std::move(coefLists));
  }

protected:
  template<typename T1, typename T2>
  static matrix_t generateCoeffs(
    const Eigen::DenseBase<T1> & x,
    const Eigen::DenseBase<T2> & y)
  {
    static_assert(
      T1::IsVectorAtCompileTime && T2::IsVectorAtCompileTime,
      "x and y must be vectors.");

    if (x.size() != y.size()) {
      throw std::invalid_argument("Each element of ys must have the same size as x.");
    }

    Eigen::Index nj = x.size() - 1;
    matrix_t coeffs(2, nj);

    for (Eigen::Index i = 0; i < nj; i++) {
      coeffs(1, i) = y[i];
      coeffs(0, i) = (y[i + 1] - y[i]) / (x[i + 1] - x[i]);
    }

    return coeffs;
  }
};

}  // namespace cbr

#endif  // CBR_MATH__INTERP__PIECEWISE_LINEAR_HPP_


#include <iostream>

#include <stack>

// #include <boost/lexical_cast.hpp>

#include ".\\headers\\min_path.h"

using namespace std;

/**

 * @brief MinPath::initGraph

 * @note the start vertex must start with '0'

 * @todo fix the vertex info(automatically to certain question)

 */

void MinPath::initGraph() {

    M_vertexNum = 6;

    M_edgeNum = 10;

    int i;

    int j;

    for ( i=0; i<M_vertexNum; i++ ) {

        this->M_vertexs[i] = i;

    }

    for ( i=0; i<M_vertexNum; i++ )

        for ( j=0; j<M_vertexNum; j++ )

            this->M_edges[i][j] = MAXWEIGHT;

    M_edges[0][1] = 3;

    M_edges[0][2] = 2;

    M_edges[0][3] = 5;

    M_edges[2][3] = 1;

    M_edges[3][1] = 2;

    M_edges[1][5] = 7;

    M_edges[4][2] = 5;

    M_edges[3][5] = 5;

    M_edges[4][3] = 3;

    M_edges[4][5] = 1;

    return;

}

/**

 * @brief MinPath::computeShortestPath

 * @param v0, the start vertex

 * @param pre, array to store the current vertex's pirror vertex

 * @param dist, array to store the current vertex's weight to the start vertex

 */

void MinPath::computeShortestPath(int v0, int *pre, int *dist) {

    bool final[MAXVERTEXNUM];

    int i;

    int w;

    int v;

    int current_tmp_min;

    // 初始化

    for ( v=0; v<=M_vertexNum-1; v++ ) {

        final[v] = false;

        dist[v] = M_edges[v0][v];       // 如果不直接连通，dist[v]就是MAXWEIGHT,否则是相应的权值, 我称此时dist[v] 为 "估计值"

        pre[v] = -1;      // 所有的顶点都无前驱，置pre数组为 -1

        if ( dist[v] < MAXWEIGHT )  // v 到 v0 (直接)连通

            pre[v] = v0;

    }

    // 开始时V0属于Ｓ集合，默认已经找到最短路径

    dist[v0] = 0;

    final[v0] = true;       // (final[i] = true 相当于把i 加入S集合)

    // main loop

    // 寻找其余6个节点

    for ( i=1; i<M_vertexNum+1; i++ ) {

        v = -1;     // ---> if (v==-1)

        current_tmp_min = MAXWEIGHT;    // 当前距V0路径最短的权值 ,初始化为不连通状态

        // 寻找当前离V0最近的顶点 V

        // 第一次main loop 下, current_tmp_min 变化情况: dist[1] -> dist[2]

        for ( w=0; w<M_vertexNum; w++ ) {

            if ( final[w] == false && dist[w] < current_tmp_min ) {  // w还没找到最短路径，并且d[w]比当前min 还要小

                v = w;                          // 更新

                current_tmp_min = dist[v];

            }

        }

        if ( v == -1 )                      // 所有与V0相通的点都找到了最短路径(不满足line 108 的if )，则退出main loop

            break;

        final[v] = true;		// 把V 加入S集合

        // 更新当前最短路径及距离(V 作为中间点)

        for ( w=0; w<M_vertexNum; w++ ) {

            if ( final[w] == false && (current_tmp_min+(M_edges[v][w]) < dist[w]) )  // v0 -> v -> w 比 v0 -> w 短

            {

                // 第一次main loop, dist[1] 更新成 4, 2 作为1 的前驱

                dist[w] = current_tmp_min + (M_edges[v][w]);     // 更新最短路径长度, 此时dist[w]为 "确定值"

                pre[w] = v;     // v作为w的前驱顶点

            }

        }

    }   //end main loop

    return;

}

/**

 * @brief MinPath::showShortestPath

 * @param v0

 * @param pre

 * @param dist

 */

void MinPath::showShortestPath(int v0, int *pre, int *dist) {

    int v;

    int i;

    stack<int> s;

    cout << "从顶点 " << v0 << "　到其他顶点: " << endl;

    for ( v=0; v<M_vertexNum; v++ ) {

        if ( pre[v] == -1 )   //v0 到 v 不通

        {

            cout << "\t" << v << " 没有通路" << endl << endl;

            continue;

        }

        cout <<  "\t顶点" << v << endl;

        cout << "\t最短路径长度: " << dist[v] << endl;

        i = v;

        while ( pre[i] != -1 ) {

            //cout << "前驱: " << pre[i] << endl;

            s.push(pre[i]);             // 入栈，交换顺序

            i = pre[i];

        }

        cout << "\t最短路径: ";

        for ( unsigned long size = s.size(); size > 0; size -- ) {

            cout << s.top() << " -> ";

            s.pop();

        }

        cout << v << endl << endl;

    }

    return;

}

/**

 * @brief main

 * @return

 */

int main() {

    MinPath G;

    int pre[MAXVERTEXNUM];

    int dist[MAXVERTEXNUM];

    G.initGraph();

    G.showOriginalGraph();

    G.computeShortestPath(0, pre, dist);

    G.showShortestPath(0, pre, dist);

    return 0;

}


/**
 * @section DESCRIPTION
 *
 * Perfect Numbers.
 *
 * An integer is said to be a perfect number if the sum of its divisors,
 * including 1 (but not the number itself), is equal to the number. For
 * example, 6 is a perfect number, because 6=1+2+3. Write a function isPerfect
 * that determines whether parameter number is a perfect number. Use this
 * function in a program that determines and prints all the perfect numbers
 * between 1 and 1000. Print the divisors of each perfect number to confirm
 * that the number is indeed perfect. Challenge the power of your computer by
 * testing numbers much larger than 1000.
 */

#include <boost/multiprecision/cpp_int.hpp>
#include <iomanip>
#include <iostream>
#include <set>
#include <memory>

using boost::multiprecision::pow;
using boost::multiprecision::sqrt;
using boost::multiprecision::uint128_t;
using std::cout;
using std::endl;
using std::set;
using std::setw;
using std::unique_ptr;

using int_set_ptr = unique_ptr<set<uint128_t>>;

template <class T>
struct Accumulator final
{
    Accumulator() : sum {0}
    {
    }

    void operator()(const T &input)
    {
        sum += input;
    }
public:
    T sum;
};

static int_set_ptr FindDivisors(const uint128_t &number)
{
    int_set_ptr divisor_set_ptr {new set<uint128_t>};
    const uint128_t kDivisorLimit {sqrt(number)};

    // Base Case:
    // 1 does not have any valid candidate divisor to be considered as a
    // potential perfect number.
    if (number <= 1) {
        return divisor_set_ptr;
    }

    divisor_set_ptr->insert(1);

    for (uint128_t i {2}; i <= kDivisorLimit; ++i) {
        if (number % i == 0) {
            divisor_set_ptr->insert(i);
            divisor_set_ptr->insert(number / i);
        }
    }
    return divisor_set_ptr;
}

static bool IsPerfectNumber(const uint128_t &number,
                            int_set_ptr &divisor_set_ptr)
{
    // Compiler would use copy elision, so a 'move' in unnecessary.
    divisor_set_ptr = FindDivisors(number);

    if (divisor_set_ptr->size() == 0) {
        return false;
    }

    Accumulator<uint128_t> accumulator;

    for (auto &i : *divisor_set_ptr) {
        accumulator(i);
    }

    return accumulator.sum == number;
}

int main(void)
{
    static constexpr int kWidthLeft {15}, kWidthRight {40};
    int_set_ptr divisor_set_ptr;

    cout \
        << setw(kWidthLeft) << "Perfect Number"
        << setw(kWidthRight) << "Divisors" << endl;

    for (uint128_t i = 1; i <= uint128_t(1) << 16; ++i) {
        if (IsPerfectNumber(i, divisor_set_ptr)) {
            cout << setw(kWidthLeft) << i << setw(kWidthRight);
            for (auto divisor_iterator = divisor_set_ptr->cbegin();
                 divisor_iterator != divisor_set_ptr->cend();
                 ++divisor_iterator) {
                cout << *divisor_iterator;
                if (++divisor_iterator == divisor_set_ptr->cend()) {
                    --divisor_iterator;
                    cout << endl;
                } else {
                    --divisor_iterator;
                    cout << ", ";
                }
            }
        }
    }
    return 0;
}


#include <iostream>
#include <tuple>
#include <Eigen/Dense>
#include "linear_algebra_addon.hpp"
using namespace Eigen;
using namespace std;

MatrixXcd bl_bicr_rq(const MatrixXcd& A, const MatrixXcd& B, const double& tol, const int& itermax)
{
    // Tadano et al 2014, Improvement of the accuracy of the approximate solution of the block BiCR method
  double Bnorm= B.norm();
  MatrixXcd X= MatrixXcd::Zero(B.rows(),B.cols()); // Initial guess of X (zeros)
  MatrixXcd R= B-A*X;
  MatrixXcd Q;
  MatrixXcd xi;
  tie(Q,xi)= qr_reduced(R);

  MatrixXcd R_til= R; // or R_til= R.conjugate();
  MatrixXcd Q_til;
  MatrixXcd xi_til;
  tie(Q_til,xi_til)= qr_reduced(R_til);

  MatrixXcd S= Q;
  MatrixXcd S_til= Q_til;
  MatrixXcd U= A*Q;
  MatrixXcd U_til= A.adjoint()*Q_til;
  MatrixXcd V_til= U_til;

  for(int k= 0; k < itermax; ++k){

      MatrixXcd alpha= (U_til.adjoint()*U).fullPivLu().solve(V_til.adjoint()*Q);
      MatrixXcd alpha_til= (U.adjoint()*U_til).fullPivLu().solve(Q.adjoint()*V_til);

      X= X+S*alpha*xi;

      MatrixXcd Qnew;
      MatrixXcd tau;
      tie(Qnew,tau)= qr_reduced(Q-U*alpha);
      MatrixXcd Qnew_til;
      MatrixXcd tau_til;
      tie(Qnew_til,tau_til)= qr_reduced(Q_til-U_til*alpha_til);

      xi= tau*xi;
      MatrixXcd Vnew_til= A.adjoint()*Qnew_til;

      double err= xi.norm()/Bnorm;
      cout << "bl_bicr_rq: " << "iter= " << k << " relative err= " << err << endl;
      if(err < tol) break;

      MatrixXcd beta= (V_til.adjoint()*Q).fullPivLu().solve(tau_til.adjoint()*Vnew_til.adjoint()*Qnew);
      MatrixXcd beta_til= (Q.adjoint()*V_til).fullPivLu().solve(tau.adjoint()*Qnew.adjoint()*Vnew_til);

      Q= Qnew;
      Q_til= Qnew_til;
      V_til= Vnew_til;
      S= Q+S*beta;
      S_til= Q_til+S_til*beta_til;
      U_til= V_til+U_til*beta_til;
      U= A*S;
  }

  if((A*X-B).norm()/Bnorm > 10*tol){
      cerr << "bl_bicr_rq did not converge to solution within error tolerance !" << endl;
     // exit(EXIT_FAILURE);
  }

  return X;
}