//============================================================================
//
// This file is part of the Thea toolkit.
//
// This software is distributed under the BSD license, as detailed in the
// accompanying LICENSE.txt file. Portions are derived from other works:
// their respective licenses and copyright information are reproduced in
// LICENSE.txt and/or in the relevant source files.
//
// Author: Siddhartha Chaudhuri
// First version: 2011
//
//============================================================================

#include "Math.hpp"
#include "../../Array.hpp"
#include "../../MatVec.hpp"
#include "../../Ray3.hpp"
#include "../../Triangle3.hpp"
#include <Eigen/SVD>

namespace Browse3D {

Matrix3
orthonormalBasisMatrix(Vector3 const & u, Vector3 const & v, Vector3 const & w)
{
  Vector3 w2 = w.normalized();
  Vector3 u2 = v.cross(w2).normalized();
  Vector3 v2 = w2.cross(u2);

  Matrix3 ret; ret << u2, v2, w2;
  return ret;
}

Matrix3
orthonormalBasisMatrix(Matrix3 const & m)
{
  return orthonormalBasisMatrix(m.col(0), m.col(1), m.col(2));
}

Real
estimateScalingFactor(Matrix3 const & m)
{
  return (m.col(0).norm() + m.col(1).norm() + m.col(2).norm()) / 3.0f;
}

Vector3
reflectPoint(Vector3 const & p, Plane3 const & mirror_plane)
{
  // Assume Plane3::normal() is unit
  return p - 2 * (p - mirror_plane.getPoint()).dot(mirror_plane.getNormal()) * mirror_plane.getNormal();
}

Vector3
reflectVector(Vector3 const & v, Plane3 const & mirror_plane)
{
  // Assume Plane3::normal() is unit
  return v - 2 * v.dot(mirror_plane.getNormal()) * mirror_plane.getNormal();
}

bool
lineIntersectsTriangle(Line3 const & line, Vector3 const & v0, Vector3 const & edge01, Vector3 const & edge02, Real * time)
{
  // The code is taken from the ray-triangle intersection test in Dave Eberly's Wild Magic library, v5.3, released under the
  // Boost license: http://www.boost.org/LICENSE_1_0.txt .

  static Real const EPS = 1e-30f;

  Vector3 diff = line.getPoint() - v0;
  Vector3 normal = edge01.cross(edge02);

  // Solve Q + t*D = b1*E1 + b2*E2 (Q = diff, D = line direction, E1 = edge01, E2 = edge02, N = Cross(E1,E2)) by
  //   |Dot(D,N)|*b1 = sign(Dot(D,N))*Dot(D,Cross(Q,E2))
  //   |Dot(D,N)|*b2 = sign(Dot(D,N))*Dot(D,Cross(E1,Q))
  //   |Dot(D,N)|*t = -sign(Dot(D,N))*Dot(Q,N)

  Real DdN = line.getDirection().dot(normal);
  int sign;
  if (DdN > EPS)
    sign = 1;
  else if (DdN < -EPS)
  {
    sign = -1;
    DdN = -DdN;
  }
  else
  {
    // Line and triangle are parallel, call it a "no intersection" even if the line does intersect
    return false;
  }

  Real DdQxE2 = sign * line.getDirection().dot(diff.cross(edge02));
  if (DdQxE2 >= 0)
  {
    Real DdE1xQ = sign * line.getDirection().dot(edge01.cross(diff));
    if (DdE1xQ >= 0)
    {
      if (DdQxE2 + DdE1xQ <= DdN)
      {
        // Line intersects triangle
        Real QdN = -sign * diff.dot(normal);
        if (time) *time = QdN / DdN;
        return true;
      }
      // else: b1 + b2 > 1, no intersection
    }
    // else: b2 < 0, no intersection
  }
  // else: b1 < 0, no intersection

  return false;
}

#define CLOSEST_PT_SEGMENT_SEGMENT(VecType) \
Real \
closestPtSegmentSegment(VecType const & p1, VecType const & q1, VecType const & p2, VecType const & q2, Real & s, Real & t, \
                        VecType & c1, VecType & c2) \
{ \
  static Real const EPSILON = 1e-20f; \
 \
  VecType d1 = q1 - p1; /* Direction vector of segment S1 */ \
  VecType d2 = q2 - p2; /* Direction vector of segment S2 */ \
  VecType r = p1 - p2; \
  Real a = d1.squaredNorm(); /* Squared length of segment S1, always nonnegative */ \
  Real e = d2.squaredNorm(); /* Squared length of segment S2, always nonnegative */ \
  Real f = d2.dot(r); \
 \
  /* Check if either or both segments degenerate into points */ \
  if (a <= EPSILON && e <= EPSILON) \
  { \
    /* Both segments degenerate into points */ \
    s = t = 0; \
    c1 = p1; \
    c2 = p2; \
    return (c1 - c2).dot(c1 - c2); \
  } \
  if (a <= EPSILON) \
  { \
    /* First segment degenerates into a point */ \
    s = 0; \
    t = f / e; /* s = 0 => t = (b*s + f) / e = f / e */ \
    t = Math::clamp(t, (Real)0, (Real)1); \
  } \
  else \
  { \
    Real c = d1.dot(r); \
    if (e <= EPSILON) \
    { \
        /* Second segment degenerates into a point */ \
        t = 0; \
        s = Math::clamp(-c / a, (Real)0, (Real)1); /* t = 0 => s = (b*t - c) / a = -c / a */ \
    } \
    else \
    { \
      /* The general nondegenerate case starts here */ \
      Real b = d1.dot(d2); \
      Real denom = a * e - b * b; /* Always nonnegative */ \
 \
      /* If segments not parallel, compute closest point on L1 to L2, and */ \
      /* clamp to segment S1. Else pick arbitrary s (here 0) */ \
      if (denom != 0) \
        s = Math::clamp((b * f - c * e) / denom, (Real)0, (Real)1); \
      else \
        s = 0; \
 \
      /* Compute point on L2 closest to S1(s) using */ \
      /* t = Dot((P1+D1*s)-P2,D2) / Dot(D2,D2) = (b*s + f) / e */ \
      t = (b * s + f) / e; \
 \
      /* If t in [0,1] done. Else clamp t, recompute s for the new value */ \
      /* of t using s = Dot((P2+D2*t)-P1,D1) / Dot(D1,D1)= (t*b - c) / a */ \
      /* and clamp s to [0, 1] */ \
      if (t < 0) \
      { \
        t = 0; \
        s = Math::clamp(-c / a, (Real)0, (Real)1); \
      } \
      else if (t > 1) \
      { \
        t = 1; \
        s = Math::clamp((b - c) / a, (Real)0, (Real)1); \
      } \
    } \
  } \
 \
  c1 = p1 + s * d1; \
  c2 = p2 + t * d2; \
  return (c1 - c2).squaredNorm(); \
}

CLOSEST_PT_SEGMENT_SEGMENT(Vector2)
CLOSEST_PT_SEGMENT_SEGMENT(Vector3)

#define CLOSEST_PT_SEGMENT_LINE(VecType) \
Real \
closestPtSegmentLine(VecType const & p1, VecType const & q1, VecType const & p2, VecType const & q2, Real & s, Real & t, \
                     VecType & c1, VecType & c2) \
{ \
  static Real const EPSILON = 1e-20f; \
 \
  VecType d1 = q1 - p1; /* Direction vector of segment S1 */ \
  VecType d2 = q2 - p2; /* Direction vector of line L2 */ \
  VecType r = p1 - p2; \
  Real a = d1.squaredNorm(); /* Squared length of segment S1, always nonnegative */ \
  Real e = d2.squaredNorm(); /* Squared length of segment supporting L2, always nonnegative and assumed to be non-zero */ \
  Real f = d2.dot(r); \
 \
  if (a <= EPSILON) \
  { \
    /* Segment degenerates into a point */ \
    s = 0; \
    t = f / e; /* s = 0 => t = (b*s + f) / e = f / e */ \
  } \
  else \
  { \
    /* The general nondegenerate case starts here */ \
    Real c = d1.dot(r); \
    Real b = d1.dot(d2); \
    Real denom = a * e - b * b; /* Always nonnegative */ \
\
    /* If segments not parallel, compute closest point on L1 to L2, and */ \
    /* clamp to segment S1. Else pick arbitrary s (here 0) */ \
    if (denom != 0) \
      s = Math::clamp((b * f - c * e) / denom, (Real)0, (Real)1); \
    else \
      s = 0; \
\
    /* Compute point on L2 closest to S1(s) using */ \
    /* t = Dot((P1+D1*s)-P2,D2) / Dot(D2,D2) = (b*s + f) / e */ \
    t = (b * s + f) / e; \
  } \
 \
  c1 = p1 + s * d1; \
  c2 = p2 + t * d2; \
  return (c1 - c2).squaredNorm(); \
}

CLOSEST_PT_SEGMENT_LINE(Vector2)
CLOSEST_PT_SEGMENT_LINE(Vector3)

Real
closestPtLineTriangle(Line3 const & line, Vector3 const & v0, Vector3 const & edge01, Vector3 const & edge02, Real & s,
                      Vector3 & c1, Vector3 & c2)
{
  if (lineIntersectsTriangle(line, v0, edge01, edge02, &s))
  {
    c1 = c2 = line.getPoint() + s * line.getDirection();
    return 0;
  }

  // Either (1) the line is not parallel to the triangle and the point of
  // intersection of the line and the plane of the triangle is outside the
  // triangle or (2) the line and triangle are parallel.  Regardless, the
  // closest point on the triangle is on an edge of the triangle.  Compare
  // the line to all three edges of the triangle.
  Real min_sqdist = -1;
  Vector3 v[3] = { v0, v0 + edge01, v0 + edge02 };
  Vector3 tmp_c1, tmp_c2;
  float tmp_s, t;
  for (int i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
  {
    Real sqdist = closestPtSegmentLine(v[i0], v[i1], line.getPoint(), line.getPoint() + line.getDirection(),
                                       t, tmp_s, tmp_c2, tmp_c1);
    if (min_sqdist < 0 || sqdist < min_sqdist)
    {
      min_sqdist = sqdist;
      s = tmp_s;
      c1 = tmp_c1;
      c2 = tmp_c2;
    }
  }

  return min_sqdist;
}

Real
closestPtRayTriangle(Ray3 const & ray, Vector3 const & v0, Vector3 const & edge01, Vector3 const & edge02, Real & s,
                     Vector3 & c1, Vector3 & c2)
{
  Real sqdist = closestPtLineTriangle(Line3::fromPointAndDirection(ray.getOrigin(), ray.getDirection()), v0, edge01, edge02,
                                      s, c1, c2);
  if (s >= 0)
    return sqdist;

  // Not the most efficient way but the most convenient
  LocalTriangle3 tri(v0, v0 + edge01, v0 + edge02);
  s = 0;
  c1 = ray.getOrigin();
  c2 = tri.closestPoint(c1);

  return (c1 - c2).squaredNorm();
}

RigidTransform3
closestRigidTransform(std::vector<CoordinateFrame3> const & src, std::vector<CoordinateFrame3> const & dst)
{
  alwaysAssertM(src.size() == dst.size(), "Source and destination sets must have same number of frames");

  if (src.empty())
    return RigidTransform3::identity();
  else if (src.size() == 1)
    return RigidTransform3(dst[0]) * RigidTransform3(src[0].inverse());
  else if (src.size() == 2)
  {
    // First map line segment to line segment...
    Vector3 src_mean = 0.5f * (src[0].getTranslation() + src[1].getTranslation());
    Vector3 dst_mean = 0.5f * (dst[0].getTranslation() + dst[1].getTranslation());

    static Real const MIN_SQLEN = 1.0e-8f;

    Vector3 src_axis = src[1].getTranslation() - src[0].getTranslation();
    Vector3 dst_axis = dst[1].getTranslation() - dst[0].getTranslation();

    if (src_axis.squaredNorm() > MIN_SQLEN && dst_axis.squaredNorm() > MIN_SQLEN)
    {
      Matrix3 rot = Math::rotationArc(src_axis, dst_axis);

      // Now rotate around the mapped segment to align the z axes of the frames...
      Vector3 src_dir = rot * (src[0].getRotation().col(2) + src[1].getRotation().col(2));
      Vector3 dst_dir = dst[0].getRotation().col(2) + dst[1].getRotation().col(2);

      // Transform src_dir and dst_dir to be perpendicular to dst_axis
      dst_axis = dst_axis.normalized();
      src_dir = src_dir - src_dir.dot(dst_axis) * dst_axis;
      dst_dir = dst_dir - dst_dir.dot(dst_axis) * dst_axis;

      if (src_dir.squaredNorm() > MIN_SQLEN && dst_dir.squaredNorm() > MIN_SQLEN)
      {
        src_dir = src_dir.normalized();
        dst_dir = dst_dir.normalized();

        Vector3 src_perp = dst_axis.cross(src_dir);
        Vector3 dst_perp = dst_axis.cross(dst_dir);

        Matrix3 src_basis; src_basis << src_dir, src_perp, dst_axis;
        Matrix3 dst_basis; dst_basis << dst_dir, dst_perp, dst_axis;
        Matrix3 extra_rot = dst_basis * src_basis.transpose();  // inverse == tranpose for rotation matrices

        rot = extra_rot * rot;
      }

      CoordinateFrame3 cf(RigidTransform3::_fromAffine(AffineTransform3(rot, dst_mean - rot * src_mean)));
      // THEA_CONSOLE.nospace() << "R = " << toString(cf.getRotation()) << ", T = " << toString(cf.getTranslation());

      return RigidTransform3(cf);
    }
    else
      return RigidTransform3::translation(dst_mean - src_mean);
  }

  // ICP algorithm of Arun et al. 1987.

  size_t num_frames = src.size();
  Vector3 src_mean = Vector3::Zero(), dst_mean = Vector3::Zero();
  for (size_t i = 0; i < num_frames; ++i)
  {
    CoordinateFrame3 const & src_frame = src[i];
    CoordinateFrame3 const & dst_frame = dst[i];

    // THEA_CONSOLE.nospace() << "src[" << i << "] = R: " << toString(src_frame.getRotation()) << ", T: "
    //                    << toString(src_frame.getTranslation());
    // THEA_CONSOLE.nospace() << "dst[" << i << "] = R: " << toString(dst_frame.getRotation()) << ", T: "
    //                    << toString(dst_frame.getTranslation());

    src_mean += src_frame.getTranslation();
    dst_mean += dst_frame.getTranslation();
  }

  src_mean /= num_frames;
  dst_mean /= num_frames;

  Matrix3 corr = Matrix3::Zero();
  Vector3 src_pt, dst_pt;
  for (size_t i = 0; i < num_frames; ++i)
  {
    CoordinateFrame3 const & src_frame = src[i];
    CoordinateFrame3 const & dst_frame = dst[i];

    src_pt = src_frame.getTranslation() - src_mean;
    dst_pt = dst_frame.getTranslation() - dst_mean;

    for (int r = 0; r < 3; ++r)
      for (int c = 0; c < 3; ++c)
        corr(r, c) += src_pt[r] * dst_pt[c];
  }

  Eigen::JacobiSVD<Matrix3> svd(corr, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Matrix3 U_T = svd.matrixU().transpose();
  Matrix3 V = svd.matrixV();
  Matrix3 rotation = V * U_T;

  if (rotation.determinant() < 0)
  {
    // FIXME: One of the columns (which?) of V should be negated. See Eggert et al. '97, "Estimating 3D rigid transformations: a
    // comparison of four major algorithms".
    //
    // For now, we'll do the dumb but safe thing and test each option for the minimum error.

    THEA_WARNING << "Estimated rigid transform involves reflection, fixing by negating a column of V";

    Real min_sqerr = -1;
    for (int i = 0; i < 3; ++i)
    {
      // Swap i'th column of V
      Matrix3 V_i = V;
      V_i(0, i) = -V_i(0, i);
      V_i(1, i) = -V_i(1, i);
      V_i(2, i) = -V_i(2, i);

      Matrix3 candidate_rot = V_i * U_T;
      Real sqerr = 0;
      for (size_t j = 0; j < num_frames; ++j)
      {
        CoordinateFrame3 const & src_frame = src[j];
        CoordinateFrame3 const & dst_frame = dst[j];

        src_pt = src_frame.getTranslation() - src_mean;
        dst_pt = dst_frame.getTranslation() - dst_mean;
        sqerr += (candidate_rot * src_pt - dst_pt).squaredNorm();
      }

      if (min_sqerr < 0 || sqerr < min_sqerr)
      {
        rotation = candidate_rot;
        min_sqerr = sqerr;
      }
    }
  }

  Vector3 translation = dst_mean - rotation * src_mean;

  CoordinateFrame3 cf(RigidTransform3::_fromAffine(AffineTransform3(rotation, translation)));
  // THEA_CONSOLE.nospace() << "R = " << toString(rotation) << ", T = " << toString(translation);

  return RigidTransform3(cf);
}

int
bvhDepth(intx num_elems, int max_elems_in_leaf)
{
  alwaysAssertM(num_elems >= 0, "Can't compute BVH depth for negative number of elements");
  alwaysAssertM(max_elems_in_leaf > 0, "Can't compute BVH depth for non-positive number of elements at leaf");

  int max_depth = (num_elems > 0 ? (int)std::ceil(Math::fastLog2(num_elems / (double)max_elems_in_leaf)) : 0);
  return max_depth;
}

void
getBarycentricCoordinates2(Real qx, Real qy, Real x0, Real y0, Real x1, Real y1, Real x2, Real y2,
                           Real & bc0, Real & bc1, Real & bc2)
{
  Real a = y1 - y2;
  Real b = x0 - x2;
  Real c = y2 - y0;
  Real d = x2 - x1;

  Real det = a * b - c * d;
  if (std::abs(det) < 1.0e-30f)
    bc0 = bc1 = bc2 = 1.0f / 3;
  else
  {
    Real qx_rel_3 = qx - x2;
    Real qy_rel_3 = qy - y2;

    bc0 = (a * qx_rel_3 + d * qy_rel_3) / det;
    bc1 = (c * qx_rel_3 + b * qy_rel_3) / det;
    bc2 = 1 - bc0 - bc1;
  }
}

void
getBarycentricCoordinates2(Vector2 const & q, Vector2 const & v0, Vector2 const & v1, Vector2 const & v2,
                           Real & bc0, Real & bc1, Real & bc2)
{
  getBarycentricCoordinates2(q.x(), q.y(), v0.x(), v0.y(), v1.x(), v1.y(), v2.x(), v2.y(), bc0, bc1, bc2);
}

void
getBarycentricCoordinates3(Vector3 const & q, Vector3 const & v0, Vector3 const & v1, Vector3 const & v2,
                           Real & bc0, Real & bc1, Real & bc2)
{
  switch (Math::maxAbsAxis((v1 - v0).cross(v2 - v0)))
  {
    case 0:           getBarycentricCoordinates2(q.tail<2>(), v0.tail<2>(), v1.tail<2>(), v2.tail<2>(), bc0, bc1, bc2); break;
    case 1:           getBarycentricCoordinates2(Vector2( q[0],  q[2]),
                                                 Vector2(v0[0], v0[2]),
                                                 Vector2(v1[0], v1[2]),
                                                 Vector2(v2[0], v2[2]), bc0, bc1, bc2); break;
    default /* 2 */:  getBarycentricCoordinates2(q.head<2>(), v0.head<2>(), v1.head<2>(), v2.head<2>(), bc0, bc1, bc2);
  }
}

Vector3
triRandomPoint(Vector3 const & v0, Vector3 const & v1, Vector3 const & v2)
{
  return triRandomPointFromEdges(v0, v1 - v0, v2 - v0);
}

Vector3
triRandomPointFromEdges(Vector3 const & v0, Vector3 const & edge01, Vector3 const & edge02)
{
  // From G3D::Triangle

  // Choose a random point in the parallelogram

  float s = Random::common().uniform01();
  float t = Random::common().uniform01();

  if (t > 1.0f - s)
  {
    // Outside the triangle; reflect about the diagonal of the parallelogram
    t = 1.0f - t;
    s = 1.0f - s;
  }

  return s * edge01 + t * edge02 + v0;
}

Vector3
quadRandomPoint(Vector3 const & v0, Vector3 const & v1, Vector3 const & v2, Vector3 const & v3)
{
  Vector3 edge01 = v1 - v0;
  Vector3 edge03 = v3 - v0;
  Vector3 edge21 = v1 - v2;
  Vector3 edge23 = v3 - v2;

  Real tri0_double_area = edge01.cross(edge03).norm();
  Real tri1_double_area = edge21.cross(edge23).norm();

  float tri = Random::common().uniform(0, tri0_double_area + tri1_double_area);
  if (tri <= tri0_double_area)
    return triRandomPointFromEdges(v0, edge01, edge03);
  else
    return triRandomPointFromEdges(v2, edge21, edge23);
}

Real
raySphereIntersectionTime(Ray3 const & ray, Vector3 const & center, Real radius)
{
  Vector3 pmc = ray.getOrigin() - center;

  double s[3];
  s[2] = ray.getDirection().squaredNorm();
  s[1] = 2 * pmc.dot(ray.getDirection());
  s[0] = pmc.squaredNorm() - radius * radius;

  double roots[2];
  int num_roots = Math::solveQuadratic(s[0], s[1], s[2], roots);

  double min_root = -1;
  for (int i = 0; i < num_roots; ++i)
    if (roots[i] >= 0 && (min_root < 0 || roots[i] < min_root))
      min_root = roots[i];

#if 0
  if (min_root >= 0)
    THEA_CONSOLE << "min_root =" << min_root;
#endif

  return (Real)min_root;
}

Real
rayTorusIntersectionTime(Ray3 const & ray, Real torus_radius, Real torus_width)
{
  double r2pw2 = torus_radius * torus_radius + torus_width * torus_width;
  double r2mw2 = r2pw2 - 2 * torus_width * torus_width;

  Vector3 p2 = ray.getOrigin().cwiseProduct(ray.getOrigin());
  Vector3 pu = ray.getOrigin().cwiseProduct(ray.getDirection());
  Vector3 u2 = ray.getDirection().cwiseProduct(ray.getDirection());

  double s[5];
  s[4] = u2[0] * (u2[0] + 2 * u2[1]) + u2[1] * (u2[1] + 2 * u2[2]) + u2[2] * (u2[2] + 2 * u2[0]);
  s[3] = 4 * (pu[0] + pu[1] + pu[2]) * (u2[0] + u2[1] + u2[2]);
  s[2] = 2 * (r2mw2 * u2[2] - r2pw2 * (u2[0] + u2[1]))
       + 8 * (pu[0] * pu[1] + pu[1] * pu[2] + pu[2] * pu[0])
       + 6 * (pu[0] * pu[0] + pu[1] * pu[1] + pu[2] * pu[2])
       + 2 * (p2[0] * (u2[1] + u2[2]) + p2[1] * (u2[2] + u2[0]) + p2[2] * (u2[0] + u2[1]));
  s[1] = 4 * (r2mw2 * pu[2] - r2pw2 * (pu[0] + pu[1]) + (p2[0] + p2[1] + p2[2]) * (pu[0] + pu[1] + pu[2]));
  s[0] = 2 * (r2mw2 * p2[2] - r2pw2 * (p2[0] + p2[1]) + p2[0] * p2[1] + p2[1] * p2[2] + p2[2] * p2[0])
       + p2[0] * p2[0] + p2[1] * p2[1] + p2[2] * p2[2] + r2mw2 * r2mw2;

  double roots[4];
  int num_roots = Math::solveQuartic(s[0], s[1], s[2], s[3], s[4], roots);

  double min_root = -1;
  for (int i = 0; i < num_roots; ++i)
    if (roots[i] >= 0 && (min_root < 0 || roots[i] < min_root))
      min_root = roots[i];

#if 0
  if (min_root >= 0)
    THEA_CONSOLE << "min_root =" << min_root;
#endif

  return (Real)min_root;
}

} // namespace Browse3D


#include "matmult_impl.h"

#include <Eigen/Dense>

using Eigen::Index;
using Eigen::MatrixXd;
using grpc::StatusCode;

Status MatrixProductAPIImpl::Multiply(ServerContext* context,
                                      const MultiplyRequest* request,
                                      MultiplyResponse* response) {
  const auto& mplier = request->multiplier();
  const int mplier_columns_size = mplier.columns_size();
  const int mplier_rows_size = mplier.columns(0).coefficients_size();
  const auto& mplicand = request->multiplicand();
  const int mplicand_columns_size = mplicand.columns_size();
  const int mplicand_rows_size = mplicand.columns(0).coefficients_size();

  if (mplier_columns_size != mplicand_rows_size) {  // very shallow validation
    return Status(StatusCode::OUT_OF_RANGE, "1-factor cols != 2-factor rows");
  }

  MatrixXd mplier_eigen(mplier_rows_size, mplier_columns_size);
  for (int i = 0; i != mplier_columns_size; ++i) {
    for (int j = 0; j != mplier_rows_size; ++j) {
      mplier_eigen(j, i) = mplier.columns(i).coefficients(j);
    }
  }
  MatrixXd mplicand_eigen(mplicand_rows_size, mplicand_columns_size);
  for (int i = 0; i != mplicand_columns_size; ++i) {
    for (int j = 0; j != mplicand_rows_size; ++j) {
      mplicand_eigen(j, i) = mplicand.columns(i).coefficients(j);
    }
  }

  MatrixXd result_eigen = mplier_eigen * mplicand_eigen;

  auto response_result = response->mutable_result();
  for (Index ic = 0, ec = result_eigen.cols(); ec != ic; ++ic) {
    auto response_result_column = response_result->add_columns();
    for (Index ir = 0, er = result_eigen.rows(); er != ir; ++ir) {
      response_result_column->add_coefficients(result_eigen(ir, ic));
    }
  }

  return Status::OK;
}


/**
 * Discrete time LQR controller implementation
 * @author: Haoguang Yang
 * @date: 09-27-2021
 *
 * Copyright (c) 2021-2022, Haoguang Yang
 * 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.
 *
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * 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.
 */

#ifndef _DLQR_H_
#define _DLQR_H_

#include <utility>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/LU>
#include <eigen3/Eigen/Eigenvalues>
//#include <armadillo>

namespace control
{
  /// @class dLQR
  /// @brief A discrete time LQR controller class. The LQR controller minimizes the Hinf control cost,
  /// as defined by: J = \sum_t[(Xd(t)-X(t))'Q(Xd(t)-X(t)) + u(t)'Ru(t) + 2*(Xd(t)-X(t))'Nu(t)].
  class dLQR
  {
  public:
    /// @brief Constructor
    /// @param[in] K is the LQR gain matrix for u=K(Xd-X), which drives X to Xd.
    dLQR(const Eigen::MatrixXd& K);

    /// @brief Constructor using system x[n+1]=Ax[n]+Bu[n]
    /// @param[in] A specifies the discrete-time state space equation.
    /// @param[in] B specifies the discrete-time state space equation.
    /// @param[in] Q specifies the state space error penalty.
    /// @param[in] R specifies the control effort penalty.
    /// @param[in] N specifies the error-effort cross penalty.
    /// @param[in] dt specifies the control interval.
    dLQR(const Eigen::MatrixXd& A,
         const Eigen::MatrixXd& B,
         const Eigen::MatrixXd& Q,
         const Eigen::MatrixXd& R,
         const Eigen::MatrixXd& N);

    dLQR(){};

    ~dLQR(){ initialized = false; };

    /// @brief Calculate the ARE solution given A, B, Q, R, N matrices. This algorithm is based
    /// on the work of https://github.com/TakaHoribe/Riccati_Solver/ and W. F. Arnold and A. J. Laub,
    /// "Generalized eigenproblem algorithms and software for algebraic Riccati equations," in
    /// Proceedings of the IEEE, vol. 72, no. 12, pp. 1746-1754, Dec. 1984, doi: 10.1109/PROC.1984.13083.
    /// The DARE solver is based on: https://github.com/arunabh1904/LQR-ROS/blob/master/lqr_obsavoid/src/are_solver.cpp
    /// The WIP DARE solver with N term used is based on: https://github.com/scipy/scipy/blob/v1.7.1/scipy/linalg/_solvers.py#L529-L734
    Eigen::MatrixXd care(const Eigen::MatrixXd& A,
                          const Eigen::MatrixXd& B,
                          const Eigen::MatrixXd& Q,
                          const Eigen::MatrixXd& R,
                          const Eigen::MatrixXd& N);

    Eigen::MatrixXd dare(const Eigen::MatrixXd& A,
                          const Eigen::MatrixXd& B,
                          const Eigen::MatrixXd& Q,
                          const Eigen::MatrixXd& R,
                          const Eigen::MatrixXd& N);

    /// @brief Input the current states and desired states, output the control values
    /// @param X is the most recently state space
    /// @param Xd is the most recently desired state space
    /// @return the control value from the LQR algorithm
    const Eigen::VectorXd& calculateControl(const Eigen::VectorXd& X, const Eigen::VectorXd& Xd);

    void balance_matrix(const Eigen::MatrixXd &A, Eigen::MatrixXd &Aprime, Eigen::VectorXd &D);

    /// @brief Get the current LQR gain.
    /// @return The current K matrix.
    inline const Eigen::MatrixXd& getK() const { return K_; };

    /// @brief Get the current value of the control error
    inline const Eigen::VectorXd& CurrentError() const { return e_; };

    /// @brief Get the current control value (calculated based on most recent errors)
    /// @return the current control value
    inline const Eigen::VectorXd& CurrentControl() const { return u_; };
    inline bool isInitialized() const { return initialized; };

  private:
    /// @brief is initialized
    bool initialized = false;

    /// @brief LQR gain.
    Eigen::MatrixXd K_;

    /// @brief Current state space error used in LQR control algorithm
    Eigen::VectorXd e_;

    /// @brief Current control value
    Eigen::VectorXd u_;

  }; // End class dLQR

} // End namespace control

#endif


#ifndef INTEGRALS_HPP_
#define INTEGRALS_HPP_
/**
 * @file integrals.hpp
 * @author Adam Lamson
 * @brief Probility Density Function integrals for KMC algorithm. Used to create
 * lookup tables
 * @version 0.1
 * @date 2019-04-15
 *
 * @copyright Copyright (c) 2019
 *
 */

#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <cassert>
#include <cstdio>
#include <iostream>
#include <string>

/*! \brief Integrate exponential factor with the form of
 * e^{-M * (\sqrt{ s^2 + lm^2}-ell0 -1)^2}
 * from sbound0 to sbound1  with respect to the variable s.
 *
 * \param lm Physically, this is the perpendicular distance above rod
 * \param sbound lowerr limit of integral
 * \param sbound Upper limit of integral
 * \param M exponential constant factor. Physically, this is the product of
 (1-load_sensitivity)*spring_const/(k_B * Temperature)
 * \param ell0 Shift of the integrands mean. Physically, protein rest length
 * \return result The value of the integration

 */
inline double integral(double lm, double sbound0, double sbound1, double M,
                       double ell0) {
    if (sbound0 >= sbound1) {
        return 0;
    }
    auto integrand = [&](double s) {
        // lambda capture variabls ell0 and M
        const double exponent = sqrt(s * s + lm * lm) - ell0 - 1;
        return exp(-M * exponent * exponent);
    };
    double error = 0;
    double result =
        boost::math::quadrature::gauss_kronrod<double, 21>::integrate(
            integrand, sbound0, sbound1, 10, 1e-6, &error);
    return result;
}

#endif /* INTEGRALS_HPP_ */


#include <math.h>
#include <cmath>
//#include <RcppEigen.h>
#include <RcppArmadillo.h>
//#include <iostream>
//#include <vector>
//#include <Eigen/Core>

// [[Rcpp::depends(RcppArmadillo)]]
// [[Rcpp::depends(RcppEigen)]]

typedef Eigen::Triplet<float> T; //tripleta para rellenar matrices sparse
typedef Eigen::MappedSparseMatrix<float> MSpMat; //alias para matriz sparse
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorXd; //alias para vector sparse
using namespace Rcpp;
//using namespace RcppArmadillo;

// [[Rcpp::export]]
float KernelGauss (arma::cube x, arma::cube y, int dimension, double sigJ, double sigd, float distancia)
{
  //Calculo individual del kernel entre dos observaciones
  // input: x observaciones
  //        y observaciones
  // dimension: numero de canales
  //sigJ: sd de la imagen
  //sigd: sd de la posicion
  //distancia: distancia L_2 entre los dos pixeles
  float kernel_value = 0;
  float siJ = (float)sigJ;
  float sid = (float)sigd;
  for (int i = 0; i < dimension; i++ )
  {
    kernel_value +=(float)pow( (float)(x[i] - y[i]),2);
  }
  kernel_value = exp(-(kernel_value/(2*pow(siJ,2)) ) - distancia/(2*pow(sid, 2)));
  return( kernel_value );
}

// [[Rcpp::export]]
Eigen::SparseMatrix<float> Kernel_RGB ( arma::cube  M, int h, int w, double distancia, double sigJ, double sigd, int dimension)
{
  // Funcion para calcular  la matriz de similaridades de todos los puntos de una imagen
  //  input: w: altura en pixeles de la imagen//SINO NO ME SALIAN LAS MULTIPLICACIONES
  //         h: ancho en pixeles de la imagen//SINO NO ME SALIAN LAS MULTIPLICACIONES
  //         distancia: limite de la vecindad del pixel a considerar
  //         M: imagen de 'dimension' dimensiones (numero de canales)
  //         sigJ: sigma para kernel gaussiano, de la imagen
  //         sigd: sigma para kernell gaussiano, de las distancias
  //         dimension: numero de canales
  //output: W, similaridad calculada
  int i, j, k, l = 0 ;
  float vecindad = distancia;
  float d2 = 0.;
  float calculo = 0.;
  std::vector<T> tripletList; //inicializamos tripleta
  tripletList.reserve((int)floor(h*h*w*w*.1));

  //comienza calculo de matriz de similaridades
  for(i=0; i < h; i++)
  {
    for(j=0; j< w; j++ )
    {
      for(k=0; k<h; k++)
      {
        for(l=0; l<w; l++)
        {
          d2 = (float)(pow(i-k,2)+pow(j-l,2));
          if(d2 <= vecindad)
          {
            calculo = KernelGauss( M.tube(j,i), M.tube(l, k), dimension, sigJ, sigd,  vecindad);
            tripletList.push_back(T((j)*h+i, (l)*h+k, calculo ));
          }
        }
      }
    }
  }
  //termina calculo de matriz de similaridades
  Eigen::SparseMatrix<float>  W(h*w, h*w); //construccion de matriz de similaridades
  W.setFromTriplets(tripletList.begin(), tripletList.end());
  return(W);
}

// [[Rcpp::export]]
Eigen::SparseMatrix<float> Kernel_float ( NumericMatrix M, int h, int w, double distancia, double sigJ, double sigd)
{
  // Funcion para calcular  la matriz de similaridades de todos los puntos de una imagen
  //  input: w: altura en pixeles de la imagen//SINO NO ME SALIAN LAS MULTIPLICACIONES
  //         h: ancho en pixeles de la imagen//SINO NO ME SALIAN LAS MULTIPLICACIONES
  //         distancia: limite de la vecindad del pixel a considerar
  //         M: imagen de 2 dimensiones (EN ESCALA DE GRISES)
  //         sigJ: sigma para kernel gaussiano, de la imagen
  //         sigd: sigma para kernell gaussiano, de las distancias
  //output: W, similaridad calculada, sparse
  int i, j, k, l = 0 ;
  float vecindad = (float)distancia;
  float d2 = 0.;
  float calculo = 0.;
  std::vector<T> tripletList; //inicializamos tripleta
  tripletList.reserve((int)(h*h*w*w*.1));
  //comienza calculo de matriz de similaridades
  for(i=0; i < h; i++)
  {
    for(j=0; j< w; j++ )
    {
      for(k=0; k<h; k++)
      {
        for(l=0; l<w; l++)
        {
          d2 = (float)(pow(i-k,2)+pow(j-l,2));
          if(d2 <= vecindad)
          {
            calculo = (float) (exp(-pow(M(j,i)-M(l,k),2)/(2*pow(sigJ,2))-d2/(2*pow(sigd,2))));
            tripletList.push_back(T((j)*h+i, (l)*h+k, calculo ));
          }
        }
      }
    }
  }
  //termina calculo de matriz de similaridades
  Eigen::SparseMatrix<float>  W(h*w, h*w); //construccion de matriz de similaridades
  W.setFromTriplets(tripletList.begin(), tripletList.end());
  return(W);
}


#include "iris/geometry.h"
#include <Eigen/Core>
#include <Eigen/LU>
#include <iostream>
#include "iris_cdd.h"

namespace iris {

int factorial(int n) {
  return n == 0 ? 1 : factorial(n - 1) * n;
}

double nSphereVolume(int dim, double radius) {
  double v;
  int k = std::floor(dim / 2);
  if (dim % 2 == 0) {
    v = std::pow(M_PI, k) / static_cast<double>(factorial(k));
  } else {
    v = (2.0 * factorial(k) * std::pow(4 * M_PI, k)) / static_cast<double>(factorial(2 * k + 1));
  }
  return v * std::pow(radius, dim);
}

Ellipsoid::Ellipsoid(int dim) :
  C_(Eigen::MatrixXd(dim, dim)),
  d_(Eigen::VectorXd(dim)) {}
Ellipsoid::Ellipsoid(Eigen::MatrixXd C, Eigen::VectorXd d):
  C_(C),
  d_(d) {}
const Eigen::MatrixXd& Ellipsoid::getC() const {
  return C_;
}
const Eigen::VectorXd& Ellipsoid::getD() const {
  return d_;
}
void Ellipsoid::setC(const Eigen::MatrixXd &C) {
  C_ = C;
}
void Ellipsoid::setCEntry(Eigen::DenseIndex row,
                          Eigen::DenseIndex col, double value) {
  C_(row, col) = value;
}
void Ellipsoid::setD(const Eigen::VectorXd &d) {
  d_ = d;
}
void Ellipsoid::setDEntry(Eigen::DenseIndex index, double value) {
  d_(index) = value;
}
int Ellipsoid::getDimension() const {
  return C_.cols();
}
double Ellipsoid::getVolume() const {
  return C_.determinant() * nSphereVolume(this->getDimension(), 1.0);
}
Ellipsoid Ellipsoid::fromNSphere(Eigen::VectorXd &center, double radius) {
  const int dim = center.size();
  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim, dim);
  C.diagonal().setConstant(radius);
  Ellipsoid ellipsoid(C, center);
  return ellipsoid;
}

Polyhedron::Polyhedron(int dim):
  A_(0, dim),
  b_(0, 1),
  dd_representation_dirty_(true) {}
Polyhedron::Polyhedron(Eigen::MatrixXd A, Eigen::VectorXd b):
    A_(A),
    b_(b),
    dd_representation_dirty_(true) {}
void Polyhedron::setA(const Eigen::MatrixXd &A) {
  A_ = A;
  dd_representation_dirty_ = true;
}
const Eigen::MatrixXd& Polyhedron::getA() const {
  return A_;
}
void Polyhedron::setB(const Eigen::VectorXd &b) {
  b_ = b;
  dd_representation_dirty_ = true;
}
const Eigen::VectorXd& Polyhedron::getB() const {
  return b_;
}
int Polyhedron::getDimension() const {
  return A_.cols();
}
int Polyhedron::getNumberOfConstraints() const {
  return A_.rows();
}
void Polyhedron::appendConstraints(const Polyhedron &other) {
  A_.conservativeResize(A_.rows() + other.getA().rows(), A_.cols());
  A_.bottomRows(other.getA().rows()) = other.getA();
  b_.conservativeResize(b_.rows() + other.getB().rows());
  b_.tail(other.getB().rows()) = other.getB();
  dd_representation_dirty_ = true;
}
void Polyhedron::updateDDRepresentation() {
  generator_points_.clear();
  generator_rays_.clear();
  getGenerators(A_, b_, generator_points_, generator_rays_);
  dd_representation_dirty_ = false;
}
std::vector<Eigen::VectorXd> Polyhedron::generatorPoints() {
  if (dd_representation_dirty_) {
    updateDDRepresentation();
  }
  return generator_points_;
}
std::vector<Eigen::VectorXd> Polyhedron::generatorRays() {
  if (dd_representation_dirty_) {
    updateDDRepresentation();
  }
  return generator_rays_;
}
bool Polyhedron::contains(Eigen::VectorXd point, double tolerance) {
  return (A_ * point - b_).maxCoeff() <= tolerance;
}

}


/**
 * Kalman Filter
 * Skeleton code for teaching
 * A3M33MKR
 * Czech Technical University
 * Faculty of Electrical Engineering
 * Intelligent and Mobile Robotics group
 *
 * Authors: Zdeněk Kasl, Karel Košnar kosnar@labe.felk.cvut.cz
 *
 * this code is inspired by tutorial on Kalman Filter by Greg Czerniak
 *
 * Licence: MIT (see LICENSE file)
 **/

#include<cmath>
#include<cassert>
#include<cstdlib>
#include<fstream>
#include<iostream>
#include <Eigen/Dense>

#include "gui/gui.h"
#include "systemSimulator/system.h"
using namespace imr;
using namespace gui;
using namespace Eigen;

Point KalmanFilter(Point measuredPosition);

//    float v0 = 149;
//    float alpha = (float) (M_PI / 4 - 0.007);
    float v0 = 150;
    float alpha = (float) (M_PI / 4);
    float dt = 0.1;
    float g = (float) (9.8 * dt);

    Matrix4f A,R,Sigma;
    Matrix<float,4,1> B;
    Matrix2f Q;
    Matrix<float,2,4> C;
    Vector4f X;

void help(char** argv)
{
    std::cout << "\nUsage of the program " << argv[0]+2 << std::endl
         << "Parameter [-h or -H] displays this message." <<std::endl
	 << " Parameter [-n or -N] number of simulation steps."
         << std::endl;
}

int main(int argc, char** argv)
{
    A <<    1,0,dt,0,
            0,1,0,dt,
            0,0,1,0,
            0,0,0,1;
    B <<    0,
            0,
            0,
            -1;
    C <<    1,0,0,0,
            0,1,0,0;
    X <<   0,
            0,
            cos(alpha) * v0,
            sin(alpha) * v0;
    R <<    3,0,0,0,
            0,3,0,0,
            0,0,3,0,
            0,0,0,3;
    Q <<    50,0,
            0,50;
    Sigma << 1,0,0,0,
            0,1,0,0,
            0,0,1,0,
            0,0,0,1;

    int nSteps = 1000;
    char *dataFile;

    // Parse all console parameters
    for (int i=0; i<argc; i++) {
        if (argv[i][0]=='-') {
            switch(argv[i][1]) {
                //> HELP
                case 'H' : case 'h' :
                    help(argv);
                    break;
		case 'N': case 'n':
                    assert(i+1 < argc);
                    assert(atoi(argv[i+1])>1);
                    nSteps = atoi(argv[i+1]);
		    break;
                default :
                    std::cout << "Parameter \033[1;31m" << argv[i] << "\033[0m is not valid!\n"
                        << "Use parameter -h or -H for help." << std::endl;
                    break;
            }
        }
    }
    // All parameters parsed

    Gui gui;
    System system;

    //> comment line below in order to let the program
    //> continue right away
//    gui.startInteractor();

    Point measurement;
    Point truth;
    Point kfPosition;

    for(int i=1; i<nSteps; i++) {
       system.makeStep();
       truth = system.getTruthPosition();
       measurement = system.getMeasurement();
       kfPosition = KalmanFilter(measurement);
       gui.setPoints(truth, measurement, kfPosition);

       //> comment line below in order to let the program
       //> continue right away
       if (i%40 == 0) gui.startInteractor();

    }
    gui.startInteractor();

    return EXIT_SUCCESS;
}

/// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//                               implement Kalman Filter here
/// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

Point  KalmanFilter(const Point measuredPosition)
{
    Point ret;
    Vector2f Z;
    Z <<    measuredPosition.x,
            measuredPosition.y;
    Matrix<float,4,1> input = g * B;
    Vector4f X_ = A * X + input;
    Matrix4f Sigma_ = A * Sigma * A.transpose() + R;
    Matrix<float,4,2> K =  Sigma_ * C.transpose() * (C * Sigma_ * C.transpose() + Q).inverse();
    X = X_ + 1*K * (Z - C * X_);

    ret.x = X(0);
    ret.y = X(1);
    return ret;
}

/// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
/// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>


/*
   Copyright (C) 2015-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

     http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/

// Free energy, energy, and specific heat of square lattice Ising model

#pragma once

#include <cmath>
#include <stdexcept>
#include <boost/math/constants/constants.hpp>
#include <boost/math/differentiation/autodiff.hpp>
#include <standards/simpson.hpp>

namespace ising {
namespace free_energy {
namespace square {

namespace {

template<typename T, typename FVAR>
struct functor {
  functor(T Jx, T Jy, FVAR beta) {
    using std::cosh; using std::sinh;
    c_ = 1 / (2 * boost::math::constants::pi<T>());
    chab_ = cosh(2 * beta * Jx) * cosh(2 * beta * Jy);
    k_ = 1.0 / (sinh(2 * beta * Jx) * sinh(2 * beta * Jy));
  }
  FVAR operator()(T t) const {
    using std::cos; using std::log; using std::sqrt;
    return c_ * log(chab_ + sqrt(1 + k_ * k_ - 2 * k_ * cos(2 * t)) / k_);
  }
  T c_;
  FVAR chab_, k_;
};

template<typename T, typename FVAR>
functor<T, FVAR> func(T Jx, T Jy, FVAR beta) { return functor<T, FVAR>(Jx, Jy, beta); }

}

template<typename T>
inline std::tuple<T, T, T> infinite(T Jx, T Jy, T beta) {
  typedef T real_t;
  using std::log;
  if (Jx <= 0 || Jy <= 0) throw(std::invalid_argument("Jx and Jy should be positive"));
  if (beta <= 0) throw(std::invalid_argument("beta should be positive"));
  const real_t pi = boost::math::constants::pi<real_t>();
  auto beta_fvar = boost::math::differentiation::make_fvar<real_t, 2>(beta);
  auto logZ = log(real_t(2)) / 2 + standards::simpson_1d(func(Jx, Jy, beta_fvar), real_t(0), pi, 8);
  real_t d2 = logZ.derivative(2);
  for (unsigned long n = 16; n < 1024; n *= 2) {
    logZ = log(real_t(2)) / 2 + standards::simpson_1d(func(Jx, Jy, beta_fvar), real_t(0), pi, n);
    if (beta * beta * abs((logZ.derivative(2) - d2) / logZ.derivative(0)) <
        2 * std::numeric_limits<real_t>::epsilon()) break;
    d2 = logZ.derivative(2);
  }
  real_t free_energy = - logZ.derivative(0) / beta;
  real_t energy = - logZ.derivative(1);
  real_t specific_heat = beta * beta * logZ.derivative(2);
  return std::make_tuple(free_energy, energy, specific_heat);
}

template<typename I, typename T>
inline std::tuple<T, T, T> finite(I Lx, I Ly, T Jx, T Jy, T beta) {
  typedef T real_t;
  using std::exp; using std::log;
  if (Lx <= 0 || Ly <= 0) throw(std::invalid_argument("Lx and Ly should be positive"));
  if (Jx <= 0 || Jy <= 0) throw(std::invalid_argument("Jx and Jy should be positive"));
  if (beta <= 0) throw(std::invalid_argument("beta should be positive"));
  const real_t pi = boost::math::constants::pi<real_t>();
  auto beta_fvar = boost::math::differentiation::make_fvar<real_t, 2>(beta);
  auto a = beta_fvar * Jx;
  auto b = beta_fvar * Jy;
  decltype(beta_fvar) lp0(0), lp1(0), lp2(0), lp3(0);
  for (std::size_t k = 0; k < 2 * Lx; ++k) {
    auto cosh_g = (cosh(2*a) * cosh(2*b) - cos(pi * k / Lx) * sinh(2*b)) / sinh(2*a);
    auto gamma_abs = log(cosh_g + sqrt(cosh_g * cosh_g - 1));
    if ((k & 1) == 1) {
      lp0 += (Ly * gamma_abs/2) + log(1 + exp(-(Ly*gamma_abs)));
      lp1 += (Ly * gamma_abs/2) + log(1 - exp(-(Ly*gamma_abs)));
    } else {
      lp2 += (Ly * gamma_abs/2) + log(1 + exp(-(Ly*gamma_abs)));
      lp3 += (Ly * gamma_abs/2) + log(1 - exp(-(Ly*gamma_abs)));
    }
  }
  auto logZ = -log(real_t(2)) / (Lx * Ly) + a + real_t(1)/2 * log(1 - exp(-4*a));
  if (sinh(2*a) * sinh(2*b) < real_t(1)) {
    logZ += (lp0 + log(1 + exp(lp1-lp0) + exp(lp2-lp0) - exp(lp3-lp0))) / (Lx * Ly);
  } else {
    logZ += (lp0 + log(1 + exp(lp1-lp0) + exp(lp2-lp0) + exp(lp3-lp0))) / (Lx * Ly);
  }
  real_t free_energy = - logZ.derivative(0) / beta;
  real_t energy = - logZ.derivative(1);
  real_t specific_heat = beta * beta * logZ.derivative(2);
  return std::make_tuple(free_energy, energy, specific_heat);
}

} // end namespace square
} // end namespace free_energy
} // end namespace ising


/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

////////////////////////////////////////////////////////////////////////////////
/// @file
/// Solution of the Euler Problem #3
/// (link).
////////////////////////////////////////////////////////////////////////////////

#include <stapl/utility/do_once.hpp>
#include <stapl/array.hpp>
#include <stapl/algorithm.hpp>
#include <boost/lexical_cast.hpp>

////////////////////////////////////////////////////////////////////////////////
/// @brief Returns truncated integer square root of given number.
/// @tparam IntType Integral type of the number.
////////////////////////////////////////////////////////////////////////////////
template <typename IntType>
inline IntType sqrt_int(IntType n)
{
  return static_cast<IntType>( std::sqrt(n) );
}

////////////////////////////////////////////////////////////////////////////////
/// @brief Functor that returns the larger of @p n and @p x/n that is both a
///        factor of @p x and a prime number (0 if neither satisfies these
///        conditions).
/// @tparam IntType Integral type of the dividend @p x.
////////////////////////////////////////////////////////////////////////////////
template <typename IntType>
struct largest_prime_factors_filter
{
  static_assert(std::is_integral<IntType>::value, "Integer required.");

  typedef IntType result_type;

  largest_prime_factors_filter(IntType x)
    : m_x(x)
  { }

  IntType operator()(IntType n)
  {
    if ((m_x % n) != 0)   // n is not a factor of x
      return 0;

    if (is_prime(m_x/n))  // x/n (which is larger than n) is prime
      return m_x/n;

    return is_prime(n) ? n : 0;
  }

  void define_type(stapl::typer& t)
  {
    t.member(m_x);
  }

private:
  IntType m_x;

  static bool is_prime(IntType n)
  {
    IntType sqrt_n = sqrt_int(n);

    for (IntType i = 2; i <= sqrt_n; ++i)
    {
      if ((n % i) == 0)
        return false;
    }

    return true;
  }
};

stapl::exit_code stapl_main(int argc, char** argv)
{
  using value_t = std::size_t;

  if (argc < 2)
  {
    stapl::do_once( [] {
      std::cout << "Run as: mpirun -n <ncpu> pe_3b <number>" << std::endl;
    });
    return EXIT_FAILURE;
  }

  stapl::counter<stapl::default_timer> exec_timer;
  exec_timer.start();

  // Read the dividend from command line.
  value_t num = boost::lexical_cast<value_t> (argv[1]);

  // Create a generator of consecutive values from 2 to sqrt(num).
  auto c_vw = stapl::counting_view(sqrt_int(num)-1, 2);

  // Apply the filter that effectively replaces each generated number by zero
  // if it is not prime factor of <num> and accumulate a maximum of these
  // numbers.
  value_t max_prime_factor = stapl::map_reduce(
      largest_prime_factors_filter<value_t>(num),
      stapl::max<value_t>(),
      c_vw);

  double t = exec_timer.stop();

  stapl::do_once( [&] {
    std::cout << "Computation finished (time taken: " << t << " s)."
              << std::endl
              << "Largest prime factor of " << num << " is: "
              << max_prime_factor << std::endl;
  });

  return EXIT_SUCCESS;
}


/**
 * @file
 * @brief NPDE homework ProjectionOntoGradients code
 * @author ?, Philippe Peter
 * @date December 2019
 * @copyright Developed at ETH Zurich
 */

#include "../projectionontogradients.h"

#include <gtest/gtest.h>
#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/fe/fe.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <memory>

namespace ProjectionOntoGradients::test {

TEST(ProjectionOntoGradients, ElementMatrixProvider) {
  // Building triangular test mesh
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);

  // implemented element matrix provider
  ElementMatrixProvider my_elem_mat_provider{};

  // For comparison
  lf::uscalfe::LinearFELaplaceElementMatrix lfe_elem_mat_provider{};

  // loop over cells and compute element matrices
  for (const lf::mesh::Entity *cell : mesh_p->Entities(0)) {
    Eigen::Matrix3d my_mat{my_elem_mat_provider.Eval(*cell)};
    lf::uscalfe::LinearFELaplaceElementMatrix::ElemMat lfe_mat{
        lfe_elem_mat_provider.Eval(*cell)};

    // compare element matrices:
    EXPECT_NEAR((lfe_mat.block<3, 3>(0, 0) - my_mat).norm(), 0.0, 1E-3);
  }
}

TEST(ProjectionOntoGradients, GradProjRhsProvider_1) {
  // Building triangular test mesh
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);

  // initialize functions f and v for which the linear form is evaluated
  auto f = [](Eigen::Vector2d x) { return Eigen::Vector2d(1.0, 2.0); };
  auto v = [](Eigen::Vector2d x) { return 2 * x(0) + x(1); };
  auto v_mf = lf::mesh::utils::MeshFunctionGlobal(v);

  // set up finite elements
  std::shared_ptr<const lf::uscalfe::UniformScalarFESpace<double>> fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  // construct element vector provider
  GradProjRhsProvider my_vec_provider(f);

  // assemble vector
  Eigen::VectorXd phi(fe_space->LocGlobMap().NumDofs());
  phi.setZero();
  lf::assemble::AssembleVectorLocally(0, fe_space->LocGlobMap(),
                                      my_vec_provider, phi);

  // project v onto the fe space
  auto v_vec = lf::fe::NodalProjection<double>(*fe_space, v_mf);

  // evaluate linear form on projected function:
  auto product = (v_vec.transpose() * phi).eval();
  EXPECT_NEAR(product(0, 0), 36, 1E-5);
}

TEST(ProjectionOntoGradients, GradProjRhsProvider_2) {
  // Building triangular test mesh
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);

  // initialize functions f and v for which the linear form is evaluated
  auto f = [](Eigen::Vector2d x) { return Eigen::Vector2d(x(1), x(0)); };
  auto v = [](Eigen::Vector2d x) { return 2 * x(0) + x(1); };
  auto v_mf = lf::mesh::utils::MeshFunctionGlobal(v);

  // set up finite elements
  std::shared_ptr<const lf::uscalfe::UniformScalarFESpace<double>> fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);

  // construct element vector provider
  GradProjRhsProvider my_vec_provider(f);

  // assemble vector
  Eigen::VectorXd phi(fe_space->LocGlobMap().NumDofs());
  phi.setZero();
  lf::assemble::AssembleVectorLocally(0, fe_space->LocGlobMap(),
                                      my_vec_provider, phi);

  // project v onto the fe space
  auto v_vec = lf::fe::NodalProjection<double>(*fe_space, v_mf);

  // evaluate linear form on projected function:
  auto product = (v_vec.transpose() * phi).eval();
  EXPECT_NEAR(product(0, 0), 40.5, 1E-5);
}

/* SAM_LISTING_BEGIN_1 */
TEST(ProjectionOntoGradients, div_free_test) {
  //====================
  // Your code goes here
  //====================
}
/* SAM_LISTING_END_1 */

TEST(ProjectionOntoGradients, exact_sol_test) {
  // I. Construct the test mesh
  //====================
  // Your code goes here
  //====================

  // II. Construct a linear finite element space on the test mesh
  //====================
  // Your code goes here
  //====================

  // III. Define a function which computes the index of the triangle in which
  // the coorindates of a given point are
  //====================
  // Your code goes here
  //====================

  // IV. Define the function f
  //====================
  // Your code goes here
  //====================

  // V. Define the tent function associated with the central node
  //====================
  // Your code goes here
  //====================

  // VI.Determine the coefficient vector of the tent function in
  // the FE space (perform a Nodal projection)
  //====================
  // Your code goes here
  //====================

  // VII. Test your implementation
  //====================
  // Your code goes here
  //====================
}

}  // namespace ProjectionOntoGradients::test


#include <CGAL/Simple_cartesian.h>
#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/Search_traits_3.h>
#include <CGAL/Search_traits_adapter.h>
#include <CGAL/Timer.h>
#include <vector>
#include <cmath>
#include <iostream>
#include <fstream>
#include <boost/iterator/counting_iterator.hpp>

typedef CGAL::Simple_cartesian<double> Kernel;

typedef Kernel::Point_3 Point_3;
typedef std::size_t Point;

//definition of the property map and get
//function as friend function to have access to
//private member
class My_point_property_map{
  const std::vector<Point_3>& points;
public:
  typedef Point_3 value_type;
  typedef const value_type& reference;
  typedef Point key_type;
  typedef boost::lvalue_property_map_tag category;

  My_point_property_map(const std::vector<Point_3>& pts):points(pts){}

  friend reference get(const My_point_property_map& ppmap,key_type i)
  {return ppmap.points[i];}
};
typedef CGAL::Search_traits_3<Kernel> Traits_base;
typedef CGAL::Search_traits_adapter<Point,My_point_property_map,Traits_base> Traits;

typedef CGAL::Orthogonal_k_neighbor_search<Traits> K_neighbor_search;
typedef K_neighbor_search::Tree       Tree;
typedef Tree::Splitter              Splitter;
typedef K_neighbor_search::Distance Distance;

int main() {
  const unsigned int N = 50;
  CGAL::Timer timer;
  timer.start();

  std::vector<Point_3> points, queries;
  Point_3 p;

  std::ifstream point_stream("points.xyz");
  while(point_stream >> p){
    points.push_back(p);

  }

  My_point_property_map ppmap(points);
  Distance tr_dist(ppmap);

  std::ifstream query_stream("queries.xyz");
  while(query_stream >> p ){
    queries.push_back(p);

  }
  timer.stop();
  std::cerr << "reading points took " << timer.time() << " sec." << std::endl;

  timer.reset();
  timer.start();
  Tree tree( boost::counting_iterator<std::size_t>(0),
             boost::counting_iterator<std::size_t>(points.size()),
             Splitter(),
             Traits(ppmap));
  tree.build();
  timer.stop();
  std::cerr << "tree construction took " << timer.time() << " sec." << std::endl;

  // Initialize the search structure, and search all N points

  double d = 0;
  timer.reset();
  timer.start();
  for(int i = 0; i < queries.size(); i++){
    K_neighbor_search search(tree, queries[i], 50, 0, true, tr_dist);

    // report the N nearest neighbors and their distance
    // This should sort all N points by increasing distance from origin
    for(K_neighbor_search::iterator it = search.begin(); it != search.end(); ++it){
      //std::cout << it->first << std::endl;
      d += get(ppmap,it->first).x();
    }
  }
  timer.stop();
  std::cerr << d << std::endl;
  std::cerr << queries.size() << " queries in " << timer.time() << " sec." << std::endl;

  return 0;
}


#pragma once
#ifndef __NVCC__
#include <armadillo>
#include <algorithm>
#endif

#ifdef __NVCC__
#define GPU __device__
#else
#define GPU
#endif

struct Squared_Error {
  template<typename U>
  GPU static inline U error(U target, U result) {
    return 1/2.0f * (target - result) * (target - result);
  }
  template<typename U, typename J>
  #ifdef __NVCC__
  __device__ static inline U error_dir(U target, J result) {
    return (target - result);
  }
  #else
  static inline auto error_dir(U target, J result) -> decltype(target-result) {
    return (target - result);
  }
  #endif
};

//Activation
#ifndef __NVCC__
using arma::exp;
#endif
struct Logistic {
  template<typename U>
  GPU static inline U activation(U k) {
    return 1 / (1 + exp(-k));
  }

  template<typename U>
  #ifdef __NVCC__
  __device__ static inline U activation_dir(U k) {
    return k * (1.0f - k);
  }
  #else
  static inline U activation_dir(U k) {
    return k % (1.0f - k);
  }
  #endif
};

struct Linear {
  template<typename U>
  GPU static inline U activation(U k) {
    return k;
  }

  #ifdef __NVCC__
  template<typename U>
  GPU static inline U activation_dir(U k) {
    return 1;
  }
  #else
  template<typename U>
  static inline U activation_dir(U k) {
    return k.ones();
  }
  #endif

};

#undef GPU


#include "resection.h"

#include <Eigen/SVD>
#include <iostream>
#include "sfm_utils.h"
#include "defines.h"

namespace {

    matrix34d canonicalizeP(const matrix34d &P)
    {
        matrix3d RR = P.get_minor<3, 3>(0, 0);
        vector3d tt;
        tt[0] = P(0, 3);
        tt[1] = P(1, 3);
        tt[2] = P(2, 3);

        if (cv::determinant(RR) < 0) {
            RR *= -1;
            tt *= -1;
        }

        double sc = 0;
        for (int i = 0; i < 9; i++) {
            sc += RR.val[i] * RR.val[i];
        }
        sc = std::sqrt(3 / sc);

        Eigen::MatrixXd RRe;
        copy(RR, RRe);
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(RRe, Eigen::ComputeFullU | Eigen::ComputeFullV);
        RRe = svd.matrixU() * svd.matrixV().transpose();
        copy(RRe, RR);

        tt *= sc;

        matrix34d result;
        for (int i = 0; i < 9; ++i) {
            result(i / 3, i % 3) = RR(i / 3, i % 3);
        }
        result(0, 3) = tt(0);
        result(1, 3) = tt(1);
        result(2, 3) = tt(2);

        return result;
    }

    // (см. Hartley & Zisserman p.178)
    cv::Matx34d estimateCameraMatrixDLT(const cv::Vec3d *Xs, const cv::Vec3d *xs, int count)
    {
        using mat = Eigen::MatrixXd;
        using vec = Eigen::VectorXd;

        mat A(2 * count, 12);

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

            double x = xs[i][0];
            double y = xs[i][1];
            double w = xs[i][2];

            double X = Xs[i][0];
            double Y = Xs[i][1];
            double Z = Xs[i][2];
            double W = 1.0;

            A.row(i * 2 + 0) << 0, 0, 0, 0, -w*X, -w*Y, -w*Z, -w*W, y*X, y*Y, y*Z, y*W;
            A.row(i * 2 + 1) << w*X, w*Y, w*Z, w*W, 0, 0, 0, 0, -x*X, -x*Y, -x*Z, -x*W;
        }

        Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::VectorXd null_space = svd.matrixV().col(11);

        matrix34d result;
        for (int i = 0; i < 12; ++i) {
            result(i / 4, i % 4) = null_space(i);
        }

        return canonicalizeP(result);
    }

    cv::Matx34d estimateCameraMatrixRANSAC(const phg::Calibration &calib, const std::vector<cv::Vec3d> &X, const std::vector<cv::Vec2d> &x, bool verbose)
    {
        if (X.size() != x.size()) {
            throw std::runtime_error("estimateCameraMatrixRANSAC: X.size() != x.size()");
        }

        const int n_points = X.size();

        // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters
        // будет отличаться от случая с гомографией
        const int n_trials = 10000;

        const double threshold_px = 3;

        const int n_samples = 6;
        uint64_t seed = 1;

        int best_support = 0;
        cv::Matx34d best_P;

        std::vector<int> sample;
        for (int i_trial = 0; i_trial < n_trials; ++i_trial) {
            phg::randomSample(sample, n_points, n_samples, &seed);

            cv::Vec3d ms0[n_samples];
            cv::Vec3d ms1[n_samples];
            for (int i = 0; i < n_samples; ++i) {
                ms0[i] = X[sample[i]];
                ms1[i] = calib.unproject(x[sample[i]]);
            }

            cv::Matx34d P = estimateCameraMatrixDLT(ms0, ms1, n_samples);

            int support = 0;
            for (int i = 0; i < n_points; ++i) {
                cv::Vec3d pt = calib.project(P * cv::Vec4d(X[i][0], X[i][1], X[i][2], 1.0));
                if (pt[2] == 0) {
                    continue;
                }
                cv::Vec2d px = {pt[0] / pt[2], pt[1] / pt[2]};
                if (cv::norm(px - x[i]) < threshold_px) {
                    ++support;
                }
            }

            if (support > best_support) {
                best_support = support;
                best_P = P;

                if (verbose) std::cout << "estimateCameraMatrixRANSAC : support: " << best_support << "/" << n_points << std::endl;

                if (best_support == n_points) {
                    break;
                }
            }
        }

        if (verbose) std::cout << "estimateCameraMatrixRANSAC : best support: " << best_support << "/" << n_points << std::endl;

        if (best_support == 0) {
            throw std::runtime_error("estimateCameraMatrixRANSAC : failed to estimate camera matrix");
        }

        return best_P;
    }

}

cv::Matx34d phg::findCameraMatrix(const Calibration &calib, const std::vector <cv::Vec3d> &X, const std::vector <cv::Vec2d> &x, bool verbose) {
    return estimateCameraMatrixRANSAC(calib, X, x, verbose);
}


#pragma once
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include "serializer.hpp"

namespace tomoto
{
	namespace math
	{
		template<typename _Ty = float>
		struct MultiNormalDistribution
		{
			static constexpr _Ty log2pi = (_Ty)1.83787706641;
			Eigen::Matrix<_Ty, -1, 1> mean;
			Eigen::Matrix<_Ty, -1, -1> cov, l;
			_Ty logDet = 0;

			MultiNormalDistribution(size_t k = 0) :
				mean{ Eigen::Matrix<_Ty, -1, 1>::Zero(k) },
				cov{ Eigen::Matrix<_Ty, -1, -1>::Identity(k, k) },
				l{ Eigen::Matrix<_Ty, -1, -1>::Identity(k, k) }
			{
			}

			_Ty getLL(const Eigen::Matrix<_Ty, -1, 1>& x) const
			{
				_Ty ll = -((x - mean).transpose() * cov.inverse() * (x - mean))[0] / 2;
				ll -= log2pi * mean.size() / 2 + logDet;
				return ll;
			}

			const Eigen::Matrix<_Ty, -1, -1>& getCovL() const
			{
				return l;
			}

			template<typename _List>
			static MultiNormalDistribution<_Ty> estimate(_List list, size_t len)
			{
				MultiNormalDistribution<_Ty> newDist;
				if (len)
				{
					newDist.mean = list(0);
					for (size_t i = 1; i < len; ++i) newDist.mean += list(i);
					newDist.mean /= len;
					newDist.cov = Eigen::Matrix<_Ty, -1, -1>::Identity(newDist.mean.size(), newDist.mean.size());
					for (size_t i = 0; i < len; ++i)
					{
						Eigen::Matrix<_Ty, -1, 1> o = list(i) - newDist.mean;
						newDist.cov += o * o.transpose();
					}
					if (len > 1) newDist.cov /= len - 1;
				}
				Eigen::MatrixXd l = newDist.cov.template cast<double>().llt().matrixL();
				newDist.l = l.template cast<float>();
				newDist.logDet = l.diagonal().array().log().sum();
				return newDist;
			}

			DEFINE_SERIALIZER_CALLBACK(onRead, mean, cov);
		private:
			void onRead()
			{
				l = cov.llt().matrixL();
				logDet = l.diagonal().array().log().sum();
			}
		};

	}
}


#ifndef POLYTOPE_H
#define POLYTOPE_H

#include <cassert>
#include <iostream>
#include <vector>

#include <Eigen/Dense>

namespace integrator_chains {

/** Basic half-space representation of polytopes.

   \ingroup integrator_chains */
class Polytope {
  public:
    Polytope( Eigen::MatrixXd incoming_H, Eigen::VectorXd incoming_K );
    Polytope( const Polytope &other );

    /** Check that defining matrices have consistent dimensions.

       The "defining matrices" are those involved in the inequality providing
       the half-space representation, i.e., H and K in {x | Hx <= K}.
       N.B., this check does not occur during instantiation of Polytope. */
    bool is_consistent() const;

    /** Is X contained in this polytope?

       The polytope is a closed set. No numerical tolerance is used. */
    bool is_in( Eigen::VectorXd X ) const;

    /** Intersect two polytopes that are defined in the same dimension space. */
    Polytope operator&( const Polytope &P2 );

    // Factory functions
    /** Create random Polytope in R^n using Eigen::MatrixXd::Random().

       \param n dimension of the containing space.

       Random matrices as provided by Eigen::MatrixXd::Random() are used to
       instantiate Polytope in half-space representation. The matrices have
       \c n+1 rows, corresponding to \c n+1 inequalities.
    */
    static Polytope * randomH( int n );

    /** Construct Polytope that is an axis-aligned rectangle.

       \param bounds an array of the form
       \code
       [x1_min, x1_max, x2_min, x2_max, ..., xn_min, xn_max],
       \endcode
       which defines a polytope in terms of intervals along each axis in R^n.
       The interval along the first axis is [x1_min, x1_max], the interval along
       the second axis is [x2_min, x2_max], etc. Thus the length of the given
       array is 2n.

       E.g., a unit square in R^2 can be created using
       \code
           Eigen::Vector4d bounds;
           bounds << 0, 1,
                     0, 1;
           Polytope *square = Polytope::box( bounds );
       \endcode
    */
    static Polytope * box( const Eigen::VectorXd &bounds );

    /** Output this polytope in JSON to given stream. */
    friend std::ostream & operator<<( std::ostream &out, const Polytope &P );

    /** Output this polytope in JSON excluding opening { and closing }.

       This method facilitates inheritance from Polytope without crowding the
       JSON representations of other classes. */
    void dumpJSONcore( std::ostream &out ) const;

  private:
    // { x \in R^n | Hx \leq K }
    Eigen::MatrixXd H;
    Eigen::VectorXd K;
};

std::ostream & operator<<( std::ostream &out, const Polytope &P )
{
    out << "{ ";
    P.dumpJSONcore( out );
    out << " }";
    return out;
}

void Polytope::dumpJSONcore( std::ostream &out ) const
{
    int i, j;
    out << "\"H\": [";
    for (i = 0; i < H.rows(); i++) {
        if (i > 0) {
            out << ", ";
        }
        out << "[";
        for (j = 0; j < H.cols(); j++) {
            if (j > 0)
                out << ", ";
            out << H(i,j);
        }
        out << "]";
    }
    out << "], \"K\": [";
    for (i = 0; i < K.rows(); i++) {
        if (i > 0) {
            out << ", ";
        }
        out << K(i);
    }
    out << "]";
}

Polytope * Polytope::randomH( int n )
{
    return new Polytope( Eigen::MatrixXd::Random( n+1, n ),
                         Eigen::VectorXd::Random( n+1 ) );
}

Polytope * Polytope::box( const Eigen::VectorXd &bounds )
{
    assert( bounds.size() % 2 == 0 );

    int n = bounds.size()/2;
    Eigen::MatrixXd H = Eigen::MatrixXd::Zero( 2*n, n );
    Eigen::VectorXd K( 2*n );
    for (int i = 0; i < n; i++) {
        H(2*i, i) = -1;
        K(2*i) = -bounds(2*i);
        H(2*i+1, i) = 1;
        K(2*i+1) = bounds(2*i+1);
    }
    return new Polytope( H, K );
}

bool Polytope::is_consistent() const
{
    if (H.rows() == K.size()) {
        return true;
    } else {
        return false;
    }
}

bool Polytope::is_in( Eigen::VectorXd X ) const
{
    assert( H.cols() == X.size() );

    if ((H*X-K).maxCoeff() <= 0) {
        return true;
    } else {
        return false;
    }
}

Polytope Polytope::operator&( const Polytope &P2 )
{
    assert( this->H.cols() == P2.H.cols() );

    Eigen::MatrixXd H( this->H.rows() + P2.H.rows(), this->H.cols() );
    Eigen::VectorXd K( this->H.rows() + P2.H.rows(), 1 );

    H.topRows( this->H.rows() ) = this->H;
    K.topRows( this->H.rows() ) = this->K;
    H.bottomRows( P2.H.rows() ) = P2.H;
    K.bottomRows( P2.H.rows() ) = P2.K;

    return Polytope( H, K );
}

Polytope::Polytope( Eigen::MatrixXd incoming_H, Eigen::VectorXd incoming_K )
    : H(incoming_H), K(incoming_K)
{ }

Polytope::Polytope( const Polytope &other )
    : H(other.H), K(other.K)
{ }

/** Extension of Polytope to have label (string).

   This class provides a basis for labeling trajectories. E.g., the labeling of
   the state can be a set of strings corresponding to the labels of polytopes
   containing the state.

   \ingroup integrator_chains */
class LabeledPolytope : public Polytope {
public:
    std::string label;

    LabeledPolytope( const Polytope &other );

    /** Polytope::box() but including a label. */
    static LabeledPolytope * box( const Eigen::VectorXd &bounds, std::string label="" );

    /** Output this labeled polytope in JSON to given stream. */
    friend std::ostream & operator<<( std::ostream &out, const LabeledPolytope &P );
};

LabeledPolytope::LabeledPolytope( const Polytope &other )
    : label(""), Polytope( other )
{ }

LabeledPolytope * LabeledPolytope::box( const Eigen::VectorXd &bounds, std::string label )
{
    Polytope *P = Polytope::box( bounds );
    LabeledPolytope *Q = new LabeledPolytope( *P );
    delete P;
    Q->label = label;
    return Q;
}

std::ostream & operator<<( std::ostream &out, const LabeledPolytope &P )
{
    out << "{ \"label\": \"" << P.label << "\", ";
    P.dumpJSONcore( out );
    out << " }";
    return out;
}

} // namespace integrator_chains

#endif  // ifndef POLYTOPE_H


#pragma once

#include "layer.hpp"
#include <Eigen/Dense>

namespace
{
	using namespace nn;

	uint32_t conv_size(uint32_t input, uint32_t kernel_width, uint32_t padding, uint32_t stride)
	{
		return (input - kernel_width + 2 * padding) / stride + 1;
	}

	layer::MatrixType conv_helper(const layer::MatrixType& input,
		const layer::MatrixType& kernel,
		uint32_t start_row, uint32_t start_col,
		uint32_t end_row, uint32_t end_col,
		uint32_t stride,
		bool normalize)
	{
		layer::MatrixType output = layer::MatrixType::Zero(end_row - start_row, end_col - start_col);
		const auto KSizeX = kernel.rows();
		const auto KSizeY = kernel.cols();

		for (auto row = start_row; row < end_row; row += stride)
			for (auto col = start_col; col < end_col; col += stride)
				output(row - start_row, col - start_col) = input.block(row - KSizeX / 2, col - KSizeY / 2, KSizeX, KSizeY).cwiseProduct(kernel).sum();

		if (normalize)
			return output / kernel.sum();
		else
			return output;
	}

	layer::MatrixType zero_pad(const layer::MatrixType& input, uint32_t size)
	{
		layer::MatrixType result = layer::MatrixType::Zero(input.rows() + 2 * size, input.cols() + 2 * size);
		result.block(size, size, input.rows(), input.cols()) = input;
		return result;
	}
}

namespace nn
{
	layer::MatrixType conv(const layer::MatrixType& input, const layer::MatrixType& kernel, uint32_t stride, bool zeroPad = false, bool normalize = false)
	{
		const auto kernel_half_width = kernel.cols() / 2;
		const auto kernel_half_height = kernel.rows() / 2;
		if (!zeroPad)
		{
			const auto result_width = conv_size(input.cols(), kernel.cols(), 0, stride);
			const auto result_height = conv_size(input.rows(), kernel.rows(), 0, stride);
			const auto conv_start_col = (input.cols() - result_width) / 2;
			const auto conv_start_row = (input.rows() - result_height) / 2;
			const auto conv_end_col = conv_start_col + result_width;
			const auto conv_end_row = conv_start_row + result_height;
			return conv_helper(input, kernel, conv_start_row, conv_start_col, conv_end_row, conv_end_col, stride, normalize);
		}
		else
		{
			return conv_helper(zero_pad(input, kernel_half_width),
				kernel, kernel_half_width, kernel_half_height,
				kernel_half_width + input.cols(),
				kernel_half_height + input.rows(),
				stride,
				normalize);
		}
	}

}


#include "vio/eigen_utils.h"
#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace vio {

Eigen::Vector3d rotro2eu(Eigen::Matrix3d R) {
  Eigen::Vector3d euler;
  euler[0] = atan2(R(2, 1), R(2, 2));
  euler[1] = -(atan2(R(2, 0), sqrt(1 - R(2, 0) * R(2, 0))));
  euler[2] = atan2(R(1, 0), R(0, 0));
  return euler;
}

Eigen::Matrix3d roteu2ro(Eigen::Vector3d eul) {
  double cr = cos(eul[0]);
  double sr = sin(eul[0]);  // roll
  double cp = cos(eul[1]);
  double sp = sin(eul[1]);  // pitch
  double ch = cos(eul[2]);
  double sh = sin(eul[2]);  // heading
  Eigen::Matrix3d dcm;
  dcm(0, 0) = cp * ch;
  dcm(0, 1) = (sp * sr * ch) - (cr * sh);
  dcm(0, 2) = (cr * sp * ch) + (sh * sr);

  dcm(1, 0) = cp * sh;
  dcm(1, 1) = (sr * sp * sh) + (cr * ch);
  dcm(1, 2) = (cr * sp * sh) - (sr * ch);

  dcm(2, 0) = -sp;
  dcm(2, 1) = sr * cp;
  dcm(2, 2) = cr * cp;
  return dcm;
}
// input: lat, long in radians, height is immaterial
// output: Ce2n
Eigen::Matrix3d llh2dcm(const Eigen::Vector3d llh) {
  double sL = sin(llh[0]);
  double cL = cos(llh[0]);
  double sl = sin(llh[1]);
  double cl = cos(llh[1]);

  Eigen::Matrix3d Ce2n;
  Ce2n << -sL * cl, -sL * sl, cL, -sl, cl, 0, -cL * cl, -cL * sl, -sL;
  return Ce2n;
}

Eigen::MatrixXd nullspace(const Eigen::MatrixXd &A) {
  Eigen::HouseholderQR<Eigen::MatrixXd> qr(A);
  // ColPivHouseholderQR<MatrixXd> qr(A); //don't use column pivoting because in
  // that case Q*R-A!=0
  Eigen::MatrixXd nullQ = qr.householderQ();

  int rows = A.rows(), cols = A.cols();
  assert(rows >
         cols);  // "Rows should be greater than columns in computing nullspace"
  nullQ = nullQ.block(0, cols, rows, rows - cols).eval();
  return nullQ;
}

void leftNullspaceAndColumnSpace(const Eigen::MatrixXd &A, Eigen::MatrixXd *Q2,
                                 Eigen::MatrixXd *Q1) {
  int rows = A.rows(), cols = A.cols();
  assert(rows > cols);  // "Rows should be greater than columns in computing
                        // left nullspace"
  Eigen::HouseholderQR<Eigen::MatrixXd> qr(A);
  // don't use column pivoting because in that case Q*R-A!=0
  Eigen::MatrixXd Q = qr.householderQ();

  Q2->resize(rows, rows - cols);
  *Q2 = Q.block(0, cols, rows, rows - cols);

  Q1->resize(rows, cols);
  *Q1 = Q.block(0, 0, rows, cols);
}

Eigen::Matrix<double, Eigen::Dynamic, 1> superdiagonal(
    const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> &M) {
  const int numElements = std::min(M.rows(), M.cols()) - 1;
  Eigen::Matrix<double, Eigen::Dynamic, 1> r(numElements, 1);
  for (int jack = 0; jack < numElements; ++jack) r[jack] = M(jack, jack + 1);
  return r;
}

Eigen::Matrix<double, Eigen::Dynamic, 1> subdiagonal(
    const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> &M) {
  const int numElements = std::min(M.rows(), M.cols()) - 1;
  Eigen::Matrix<double, Eigen::Dynamic, 1> r(numElements, 1);
  for (int jack = 0; jack < numElements; ++jack) r[jack] = M(jack + 1, jack);
  return r;
}

void reparameterize_AIDP(const Eigen::Matrix3d &Ri, const Eigen::Matrix3d &Rj,
                         const Eigen::Vector3d &abrhoi,
                         const Eigen::Vector3d &pi, const Eigen::Vector3d &pj,
                         Eigen::Vector3d &abrhoj,
                         Eigen::Matrix<double, 3, 9> *jacobian) {
  Eigen::Matrix<double, 4, 4> Tci2cj = Eigen::Matrix<double, 4, 4>::Identity();
  Tci2cj.topLeftCorner<3, 3>() = Rj.transpose() * Ri;
  Tci2cj.block<3, 1>(0, 3).noalias() = Rj.transpose() * (pi - pj);
  Eigen::Matrix<double, 4, 1> homogi;
  homogi << abrhoi.head<2>(), 1, abrhoi[2];
  double rhoj_drhoi = 1 / (Tci2cj.row(2) * homogi);  //\rho_j divided by \rho_i
  abrhoj.head<2>() = rhoj_drhoi * Tci2cj.topLeftCorner<2, 4>() * homogi;
  abrhoj[2] = abrhoi[2] * rhoj_drhoi;
  if (jacobian) {
    Eigen::Matrix3d lhs;
    lhs.setIdentity();
    lhs.col(2) = -abrhoj;
    //{\alpha, \beta, \rho}_i
    Eigen::Matrix<double, 3, 3> subrhs;
    subrhs << Tci2cj.topLeftCorner<3, 2>(), Tci2cj.block<3, 1>(0, 3);
    jacobian->topLeftCorner<3, 3>() = rhoj_drhoi * lhs * subrhs;
    (*jacobian)(2, 2) =
        rhoj_drhoi * rhoj_drhoi * Tci2cj.block<1, 3>(2, 0) * homogi.head<3>();
    //{pi, pj}
    Eigen::Matrix<double, 3, 6> rhs;
    rhs.topLeftCorner<3, 3>() = abrhoi[2] * Rj.transpose(),
                         rhs.block<3, 3>(0, 3) = -rhs.topLeftCorner<3, 3>();
    jacobian->block<3, 6>(0, 3) = rhoj_drhoi * lhs * rhs;
  }
}

void reparameterizeNumericalJacobian(const Eigen::Matrix3d &Ri,
                                     const Eigen::Matrix3d &Rj,
                                     const Eigen::Vector3d &abrhoi,
                                     const Eigen::Vector3d &pi,
                                     const Eigen::Vector3d &pj,
                                     Eigen::Vector3d &abrhoj,
                                     Eigen::Matrix<double, 3, 9> &jacobian) {
  // numerical differentation
  Eigen::Vector3d abrhojp;
  reparameterize_AIDP(Ri, Rj, abrhoi, pi, pj, abrhoj);
  double h = 1e-8;
  for (int jack = 0; jack < 3; ++jack) {
    Eigen::Vector3d abrhoip = abrhoi;
    abrhoip[jack] = abrhoi[jack] + h;
    reparameterize_AIDP(Ri, Rj, abrhoip, pi, pj, abrhojp);
    Eigen::Vector3d subJacobian = (abrhojp - abrhoj) / h;
    jacobian.col(jack) = subJacobian;
  }

  for (int jack = 0; jack < 3; ++jack) {
    Eigen::Vector3d pip = pi;
    pip[jack] = pi[jack] + h;
    reparameterize_AIDP(Ri, Rj, abrhoi, pip, pj, abrhojp);
    Eigen::Vector3d subJacobian = (abrhojp - abrhoj) / h;
    jacobian.col(jack + 3) = subJacobian;
  }

  for (int jack = 0; jack < 3; ++jack) {
    Eigen::Vector3d pjp = pj;
    pjp[jack] = pj[jack] + h;
    reparameterize_AIDP(Ri, Rj, abrhoi, pi, pjp, abrhojp);
    Eigen::Vector3d subJacobian = (abrhojp - abrhoj) / h;
    jacobian.col(jack + 6) = subJacobian;
  }
}

Eigen::Vector3d unskew3d(const Eigen::Matrix3d &Omega) {
  return 0.5 * Eigen::Vector3d(Omega(2, 1) - Omega(1, 2),
                               Omega(0, 2) - Omega(2, 0),
                               Omega(1, 0) - Omega(0, 1));
}

// https://github.com/ethz-asl/maplab_lightweight_filtering/blob/28417454164b08d5483754f0c0d250b84f2ce951/include/lightweight_filtering/Update.hpp
double getConditionNumberOfMatrix(const Eigen::MatrixXd &matrix) {
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrix);
  return svd.singularValues()(0) /
         svd.singularValues()(svd.singularValues().size() - 1);
}
}  // namespace vio