//=======================================================================
// Copyright 2007 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/ref.hpp>
#include <vector>

#include <boost/graph/planar_face_traversal.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>

using namespace boost;

// Some planar face traversal visitors that will
// print the vertices and edges on the faces

struct output_visitor : public planar_face_traversal_visitor
{
    void begin_face() { std::cout << "New face: "; }
    void end_face() { std::cout << std::endl; }
};

struct vertex_output_visitor : public output_visitor
{
    template < typename Vertex > void next_vertex(Vertex v)
    {
        std::cout << v << " ";
    }
};

struct edge_output_visitor : public output_visitor
{
    template < typename Edge > void next_edge(Edge e) { std::cout << e << " "; }
};

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

    typedef adjacency_list< vecS, vecS, undirectedS,
        property< vertex_index_t, int >, property< edge_index_t, int > >
        graph;

    // Create a graph - this is a biconnected, 3 x 3 grid.
    // It should have four small (four vertex/four edge) faces and
    // one large face that contains all but the interior vertex
    graph g(9);

    add_edge(0, 1, g);
    add_edge(1, 2, g);

    add_edge(3, 4, g);
    add_edge(4, 5, g);

    add_edge(6, 7, g);
    add_edge(7, 8, g);

    add_edge(0, 3, g);
    add_edge(3, 6, g);

    add_edge(1, 4, g);
    add_edge(4, 7, g);

    add_edge(2, 5, g);
    add_edge(5, 8, g);

    // Initialize the interior edge index
    property_map< graph, edge_index_t >::type e_index = get(edge_index, g);
    graph_traits< graph >::edges_size_type edge_count = 0;
    graph_traits< graph >::edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
        put(e_index, *ei, edge_count++);

    // Test for planarity - we know it is planar, we just want to
    // compute the planar embedding as a side-effect
    typedef std::vector< graph_traits< graph >::edge_descriptor > vec_t;
    std::vector< vec_t > embedding(num_vertices(g));
    if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
            boyer_myrvold_params::embedding = &embedding[0]))
        std::cout << "Input graph is planar" << std::endl;
    else
        std::cout << "Input graph is not planar" << std::endl;

    std::cout << std::endl << "Vertices on the faces: " << std::endl;
    vertex_output_visitor v_vis;
    planar_face_traversal(g, &embedding[0], v_vis);

    std::cout << std::endl << "Edges on the faces: " << std::endl;
    edge_output_visitor e_vis;
    planar_face_traversal(g, &embedding[0], e_vis);

    return 0;
}


//!
//! Contains the implementation of an adaptive proposal function
//!
//! \file infer/adaptive.hpp
//! \author Darren Shen
//! \date 2014
//! \license Affero General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

#pragma once

#include <random>
#include <functional>
#include <Eigen/Core>

namespace stateline
{
  namespace mcmc
  {

    //! A function to bounce the MCMC proposal off the hard boundaries.
    //! This allows the proposal to always move around without getting stuck at
    //! 'walls'
    //!
    //! \param val The proposed value
    //! \param min The minimum bound of theta
    //! \param max The maximum bound of theta
    //!  \returns The new bounced theta definitely in the bounds
    //!
    Eigen::VectorXd bouncyBounds(const Eigen::VectorXd& val,const Eigen::VectorXd& min, const Eigen::VectorXd& max)
    {
      Eigen::VectorXd delta = max - min;
      Eigen::VectorXd result = val;
      Eigen::Matrix<bool, Eigen::Dynamic, 1> tooBig = (val.array() > max.array());
      Eigen::Matrix<bool, Eigen::Dynamic, 1> tooSmall = (val.array() < min.array());
      for (uint i=0; i< result.size(); i++)
      {
        bool big = tooBig(i);
        bool small = tooSmall(i);
        if (big)
        {
          double overstep = val(i)-max(i);
          int nSteps = (int)(overstep /  delta(i));
          double stillToGo = overstep - nSteps*delta(i);
          if (nSteps % 2 == 0)
            result(i) = max(i) - stillToGo;
          else
            result(i) = min(i) + stillToGo;
        }
        if (small)
        {
          double understep = min(i) - val(i);
          int nSteps = (int)(understep / delta(i));
          double stillToGo = understep - nSteps*delta(i);
          if (nSteps % 2 == 0)
            result(i) = min(i) + stillToGo;
          else
            result(i) = max(i) - stillToGo;
        }
      }
      return result;
    }

    //! An adaptive Gaussian proposal function. It randomly varies each value in
    //! the state according to a Gaussian distribution whose variance changes
    //! depending on the acceptance ratio of a chain. It also bounces of the
    //! walls of the hard boundaries given so as not to get stuck in corners.
    //!
    //! \param state The current state of the chain
    //! \param sigma The standard deviation of the distribution (step size of the proposal)
    //! \param min The minimum bound of theta
    //! \param max The maximum bound of theta
    //! \returns The new proposed theta
    //!
    Eigen::VectorXd adaptiveGaussianProposal(const Eigen::VectorXd &state, double sigma,
        const Eigen::VectorXd& min, const Eigen::VectorXd& max)
    {
      // Random number generators
      static std::random_device rd;
      static std::mt19937 generator(rd());
      static std::normal_distribution<> rand; // Standard normal

      // Vary each paramater according to a Gaussian distribution
      Eigen::VectorXd proposal(state.rows());
      for (int i = 0; i < proposal.rows(); i++)
        proposal(i) = state(i) + rand(generator) * sigma;

      return bouncyBounds(proposal, min, max);
    };

  }
}


#include <iostream>
#include <igl/readPLY.h>
#include <igl/grad.h>
#include <igl/opengl/glfw/Viewer.h>
#include <igl/heat_geodesics.h>
#include <igl/triangle_triangle_adjacency.h>
#include <Eigen/Dense>
#include <cstdlib>
#include <random>
#include <unordered_map>
#include <chrono>

std::random_device rd;
std::mt19937 e2(rd());
std::uniform_real_distribution<> rand01(0, 1);

using Vec3 = Eigen::Vector3d;
Eigen::MatrixXd V;
Eigen::MatrixXi F;
Eigen::MatrixXi TT;
Eigen::SparseMatrix<double> G;
igl::HeatGeodesicsData<double> heat_data;
std::unordered_map<int, std::set<int>> neighbours;

void ComputeNeighbours() {
  for(int f = 0; f< F.rows(); f++) {
    const auto f0 = F(f, 0);
    const auto f1 = F(f, 1);
    const auto f2 = F(f, 2);

    neighbours[f0].insert(f1);
    neighbours[f0].insert(f2);
    neighbours[f1].insert(f0);
    neighbours[f1].insert(f2);
    neighbours[f2].insert(f0);
    neighbours[f2].insert(f1);
  }
}

bool is_in_ring(int f_a, int f_b, int ring) {
  std::vector<int> q;
  std::set<int> visited;

  q.push_back(f_b);
  for(int i=0; i<ring; i++) {
    if(q.size() == 0) {
      break;
    }

    std::vector<int> new_q;
    for(auto &f : q) {
      if(visited.find(f) != visited.end()) {
        continue;
      }

      visited.insert(f);
      int n0 = TT(f, 0);
      int n1 = TT(f, 1);
      int n2 = TT(f, 2);
      if(n0 == f_a || n1 == f_a || n2 == f_a) {
        return true;
      }
      new_q.push_back(n0);
      new_q.push_back(n1);
      new_q.push_back(n2);
    }
    q = std::move(new_q);
  }
  return false;
}

void sample_point(Vec3& out, Vec3& bary, int& f) {
  f = rand() % F.rows();
  const auto v0 = F(f, 0);
  const auto v1 = F(f, 1);
  const auto v2 = F(f, 2);

  const double b0 = rand01(e2);
  const double b1 = rand01(e2) * (1.0 - b0);
  const double b2 = 1.0 - b1 - b0;

  out = b0*V.row(v0) + b1*V.row(v1) + b2*V.row(v2);
  bary = {b0, b1, b2};
}

// doesn't consider TWO POINTS ON SAME TRIANGLE
void sample_point_close_to(int f_orig, Vec3& out, Vec3& bary, int& f) {
  const int hops = rand() % 5;
  f = f_orig;
  for(int i=0; i<hops || f==f_orig; i++) {
    int new_f = -1;
    while(new_f == -1) {
      new_f = TT(f, rand()%3);
    }
    f = new_f;
  }

  const auto v0 = F(f, 0);
  const auto v1 = F(f, 1);
  const auto v2 = F(f, 2);

  const double b0 = rand01(e2);
  const double b1 = rand01(e2) * (1.0 - b0);
  const double b2 = 1.0 - b1 - b0;

  out = b0*V.row(v0) + b1*V.row(v1) + b2*V.row(v2);
  bary = {b0, b1, b2};
}

const Eigen::VectorXd& all_geodesics_from_point(int v1) {
  static std::unordered_map<int, Eigen::VectorXd> cache_v;

  const auto entry = cache_v.find(v1);
  if(entry != cache_v.end()) {
    return entry->second;
  }

  Eigen::VectorXi gamma;
  Eigen::VectorXd D;
  gamma.resize(1); gamma << v1;
  igl::heat_geodesics_solve(heat_data, gamma, D);
  cache_v[v1] = std::move(D);

  return cache_v[v1];
}

const Eigen::VectorXd all_euclidean_from_point(const Vec3& src) {
  Eigen::VectorXd D;

  D = (V.array().rowwise() - src.transpose().array()).matrix().rowwise().norm();

  return D;
}

double geodesic_distance(int v1, int v2) {
  static std::unordered_map<int, Eigen::VectorXd> cache_v;
  if(v1 > v2) {
    std::swap(v1, v2);
  }

  const auto entry = cache_v.find(v1);
  if(entry != cache_v.end()) {
    return entry->second(v2);
  }

  Eigen::VectorXi gamma;
  Eigen::VectorXd D;
  gamma.resize(1); gamma << v1;
  igl::heat_geodesics_solve(heat_data, gamma, D);

  const double d = D(v2);
  cache_v[v1] = std::move(D);

  return d;
}

double geodesic_distance_from_face(int f, int v) {
  static std::unordered_map<int, Eigen::VectorXd> cache_f;
  const auto entry = cache_f.find(f);
  if(entry != cache_f.end()) {
    return entry->second(v);
  }

  Eigen::VectorXi gamma;
  Eigen::VectorXd D;
  gamma.resize(1); gamma << F(f, 0), F(f, 1), F(f, 2);
  igl::heat_geodesics_solve(heat_data, gamma, D);

  const double d = D(v);
  cache_f[f] = std::move(D);

  return d;
}

// consider 9 possible paths based on triangles and chose the shortest
double geodesic_distance_bo9(
        const Vec3& pt_a, const Vec3& bary_a, int f_a,
        const Vec3& pt_b, const Vec3& bary_b, int f_b,
        Vec3* grad_out=nullptr
        ) {
  if(f_a == f_b) {
    if(grad_out != nullptr) {
      *grad_out = pt_a - pt_b;
    }
    return (pt_a - pt_b).norm();
  }

  int best_i, best_j;
  double best_dist = 1e9;
  for(int i=0; i<3; i++) {
    // geodesic(==euclidean) distance between point a and face i
    const double d_ai = (pt_a - V.row(F(f_a, i)).transpose()).norm();

    for(int j=0; j<3; j++) {
      // geodesic distance between the two vertices on the mesh
      const double g_ij = geodesic_distance(F(f_a,i), F(f_b,j));

      // geodesic(==euclidean) distance between point b and face j
      const double d_bi = (pt_b - V.row(F(f_b, j)).transpose()).norm();

      // total geodesic distance
      const double g = d_ai + g_ij + d_bi;

      // std::cout << "(" << i << "," << j <<") " << d_ai << " + " << g_ij << " + " << d_bi << " == " << g << "\n";

      if(g < best_dist) {
        best_dist = g;
        best_i = i;
        best_j = j;
      }
    }
  }

  if(grad_out != nullptr) {
    // const auto& D = all_geodesics_from_point(F(f_b, best_j));
    const auto D = all_euclidean_from_point(pt_b);
    const Eigen::MatrixXd GD_all = Eigen::Map<const Eigen::MatrixXd>((G*D).eval().data(), F.rows(), 3);
    const Eigen::MatrixXd GD = GD_all.row(f_a);

    (*grad_out)[0] = GD(0, 0);
    (*grad_out)[1] = GD(0, 1);
    (*grad_out)[2] = GD(0, 2);
  }

  return best_dist;
}

double geodesic_distance_barydatar(
        const Vec3& pt_a, const Vec3& bary_a, int f_a,
        const Vec3& pt_b, const Vec3& bary_b, int f_b,
        Vec3 *grad_out
) {
  // ref: https://www.ncbi.nlm.nih.gov/pmc/articles/PMC3346950/
  if(f_a == f_b) {
    if(grad_out != nullptr) {
      *grad_out = pt_a - pt_b;
    }
    return (pt_a - pt_b).norm();
  }

  double res = 0.0;

  for(int i=0; i<3; i++) {
    const double g_i0 = geodesic_distance(F(f_a,i), F(f_b,0));
    const double g_i1 = geodesic_distance(F(f_a,i), F(f_b,1));
    const double g_i2 = geodesic_distance(F(f_a,i), F(f_b,2));
    const double g_iy = bary_b[0]*g_i0 + bary_b[1]*g_i1 + bary_b[2]*g_i2;
    res += bary_a[i] * g_iy;
  }

  if(grad_out != nullptr) {
    if(is_in_ring(f_a, f_b, 1)) {
      *grad_out = pt_a - pt_b;
    } else {
      *grad_out = Vec3(0.0, 0.0, 0.0);
      const auto D = all_euclidean_from_point(pt_b);
      const Eigen::MatrixXd GD_all = Eigen::Map<const Eigen::MatrixXd>((G*D).eval().data(), F.rows(), 3);

      Eigen::MatrixXd GD_all_pervertex;
      igl::per_vertex_normals(V, F, GD_all, GD_all_pervertex);

      for(int i=0; i<3; i++) {
        const Vec3 g = GD_all_pervertex.row(F(f_a, i));
        *grad_out += bary_a[i] * g;
      }
    }
  }

  return res;
}

double geodesic_distance_testmeshfimgrad(
        const Vec3& pt_a, const Vec3& bary_a, int f_a,
        const Vec3& pt_b, const Vec3& bary_b, int f_b,
        Vec3 *grad_out
) {
  if(grad_out != nullptr) {
    *grad_out = {0.0, 0.0, 0.0};

    const auto D = all_euclidean_from_point(pt_b);
    const auto grad_v = [&](int v) {
      Vec3 df = {0.0, 0.0, 0.0};
      for(int o : neighbours[v]) {
        const Vec3 pt_diff = V.row(o) - V.row(v);
        const double value_diff = D[o] - D[v];

        df[0] += value_diff / (pt_diff[0] + 0.0001);
        df[1] += value_diff / (pt_diff[1] + 0.0001);
        df[2] += value_diff / (pt_diff[2] + 0.0001);
      }
      df /= (double) neighbours[v].size();
      return df;
    };

    for(int i=0; i<3; i++) {
      *grad_out += bary_a[i] * grad_v(F(f_a, i));
    }

  }

  return (pt_a - pt_b).norm();
}

// barycentric, but my own interpretation?
double geodesic_distance_barykk(
  const Vec3& pt_a, const Vec3& bary_a, int f_a,
  const Vec3& pt_b, const Vec3& bary_b, int f_b
  ) {
  if(f_a == f_b) {
    return (pt_a - pt_b).norm();
  }

  const Vec3 distances = {
          geodesic_distance_from_face(f_b, F(f_a, 0)),
          geodesic_distance_from_face(f_b, F(f_a, 1)),
          geodesic_distance_from_face(f_b, F(f_a, 2)),
  };
  return distances.dot(bary_a);
}

int main() {
  srand(time(nullptr));

  const auto mesh_filepath = "/scratch/karthik/projects/ShapeWorks/Examples/Python/Output/plane/ply/plane_highres.ply";
  igl::readPLY(mesh_filepath, V, F);

  std::cerr << "Mesh:\n";
  std::cerr << "V: " << V.rows() << "x" << V.cols() << "\n";
  std::cerr << "F: " << F.rows() << "x" << F.cols() << "\n";
  std::cerr << "Bounds: [" << V.minCoeff() << "] -> [" << V.maxCoeff() << "]\n\n";

  igl::grad(V, F, G);
  igl::triangle_triangle_adjacency(F, TT);
  ComputeNeighbours();

  igl::heat_geodesics_precompute(V, F, heat_data);

  Vec3 pt_a, bary_a; int f_a;
  Vec3 pt_b, bary_b; int f_b;

  // compare geodesic distance
  for(int i=0; i<10000; i++) {
    sample_point(pt_a, bary_a, f_a);

    if(i < 5000) {
      sample_point_close_to(f_a, pt_b, bary_b, f_b);
    } else {
      sample_point(pt_b, bary_b, f_b);
    }

    Vec3 geo_grad;
    // const double geo = geodesic_distance_barykk(pt_a, bary_a, f_a, pt_b, bary_b, f_b);
    // const double geo = geodesic_distance_testmeshfimgrad(pt_a, bary_a, f_a, pt_b, bary_b, f_b, &geo_grad);
    // const double geo = geodesic_distance_bo9(pt_a, bary_a, f_a, pt_b, bary_b, f_b, &geo_grad);
    const double geo = geodesic_distance_barydatar(pt_a, bary_a, f_a, pt_b, bary_b, f_b, &geo_grad);

    /*
    using namespace std::chrono;
    const auto start = high_resolution_clock::now();
    const auto end = high_resolution_clock::now();
    const auto elapsed = duration_cast<nanoseconds>(end - start).count();
    std::cout << "time: " << elapsed <<"\n";
    continue;
    */

    const double euc = (pt_a - pt_b).norm();
    const Vec3 euc_grad = pt_a - pt_b;

    const double geo_angle = std::atan2(geo_grad.y(), geo_grad.x());
    const double euc_angle = std::atan2(euc_grad.y(), euc_grad.x());
    const double geo_mag = geo_grad.norm();
    const double euc_mag = euc_grad.norm();

    std::cout << "distance: " << geo <<       " | " << euc <<       " (diff: " << geo - euc <<             ") ";
    std::cout << "| angle:  " << geo_angle << " | " << euc_angle << " (diff: " << geo_angle - euc_angle << ") ";
    std::cout << "|   mag:  " << geo_mag   << " | " << euc_mag   << " (diff: " << geo_mag   - euc_mag   << ") ";
    std::cout << "\n";
  }

  return 0;

  Eigen::MatrixXd T1(2, 3);
  T1.row(0) = pt_a;
  T1.row(1) = pt_b;

  Eigen::MatrixXd T2(1, 3); T2 << 1.0, 0.0, 0.0;
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(V, F);
  viewer.data().set_points(T1, T2);
  // viewer.data().show_lines = false;
  // viewer.data().line_width = 2.0;
  viewer.launch();

  return 0;
}


/**
 * @author     : Zhao Chonyyao (cyzhao@zju.edu.cn)
 * @date       : 2021-04-30
 * @description: embedded elasticity finite element method problem
 * @version    : 1.0
 */
#include <memory>
#include <iomanip>
#include <boost/property_tree/ptree.hpp>

#include "Common/error.h"

// TODO: possible bad idea of having dependence to model in problem module
#include "Model/fem/elas_energy.h"
#include "Model/fem/mass_matrix.h"

#include "Problem/energy/basic_energy.h"
#include "Io/io.h"
#include "Geometry/extract_surface.imp"
#include "Geometry/interpolate.h"
#include "libigl/include/igl/readOBJ.h"

#include "embedded_elas_fem_problem.h"

namespace PhysIKA {
using namespace std;
using namespace Eigen;
using namespace igl;

template <typename T>
using MAT = Eigen::Matrix<T, -1, -1>;
template <typename T>
using VEC = Eigen::Matrix<T, -1, 1>;

template <typename T>
embedded_elas_problem_builder<T>::embedded_elas_problem_builder(const T* x, const boost::property_tree::ptree& pt)
    : pt_(pt)
{
    //TODO: need to check exception
    using Vector3T               = Eigen::Matrix<T, 3, 1>;
    const string filename        = pt.get<string>("filename");
    const string filename_coarse = pt.get<string>("filename_coarse");
    cout << "filename " << filename << "\n filename_coarse " << filename_coarse << endl;
    MAT<T>   nods;
    MatrixXi cells;

    MAT<T>   nods_coarse;
    MatrixXi cells_coarse;

    string type = pt.get<string>("type", "tet");
    if (type == "vox")
        type = "hex";
    string type_coarse = pt.get<string>("type_coarse", type);
    if (type_coarse == "vox")
        type_coarse = "hex";
    cout << "mesh type is " << type << endl;
    cout << "coarse type is " << type_coarse << endl;
    if (type_coarse == "tet")
    {
        IF_ERR(exit, mesh_read_from_vtk<T, 4>(filename_coarse.c_str(), nods_coarse, cells_coarse));
    }
    else if (type_coarse == "hex")
    {
        exit_if(mesh_read_from_vtk<T, 8>(filename_coarse.c_str(), nods_coarse, cells_coarse));
    }
    else if (type_coarse == "hybrid")
    {
        cout << "read hybrid mesh " << filename << endl;
        exit_if(mesh_read_from_vtk<T, 8>(filename_coarse.c_str(), nods_coarse));
    }
    else
    {
        // error_msg("type:<%s> is not supported.", type.c_str());
    }
    cout << "number of cells is " << cells_coarse.cols() << endl;
#if 1
    if (type_coarse == "hybrid")
    {
        Vector3T nods_min = nods_coarse.col(0);
        Vector3T nods_max = nods_coarse.col(1);
        for (size_t i = 0; i < 3; ++i)
        {
            nods_min(i) = nods_coarse.row(i).minCoeff();
            nods_max(i) = nods_coarse.row(i).maxCoeff();
        }
        Matrix<T, 3, 8> nods_coarsest = Matrix<T, 3, 8>::Ones();
        //set z
        nods_coarsest.block<1, 4>(2, 0) *= nods_min(2);
        nods_coarsest.block<1, 4>(2, 4) *= nods_max(2);
        //set y
        Vector4i y_min{ { 2, 3, 6, 7 } };
        Vector4i y_max{ { 0, 1, 4, 5 } };
        nods_coarsest(1, y_min) *= nods_min(1);
        nods_coarsest(1, y_max) *= nods_max(1);
        //set x
        Vector4i x_min{ { 0, 3, 4, 7 } };
        Vector4i x_max{ { 1, 2, 5, 6 } };
        nods_coarsest(0, x_min) *= nods_min(0);
        nods_coarsest(0, x_max) *= nods_max(0);

        nods_coarse = nods_coarsest;

        cout << nods_coarse << endl;
        cells_coarse.resize(8, 1);
        cells_coarse.col(0).setLinSpaced(8, 0, 7);
        Eigen::MatrixXi hexs2tets = hex_2_tet(cells_coarse);
        cells_coarse              = hexs2tets;

        cout << cells_coarse << endl;
        type_coarse = "tet";
    }
#endif
    if (filename.rfind(".obj") != string::npos)
    {
        readOBJ(filename.c_str(), nods, cells);
        nods.transposeInPlace();
        cells.transposeInPlace();
    }
    else
    {
        //TODO: need to check file reading error
        if (type == "tet")
        {
            IF_ERR(exit, mesh_read_from_vtk<T, 4>(filename.c_str(), nods, cells));
        }
        else if (type == "hex")
        {
            exit_if(mesh_read_from_vtk<T, 8>(filename.c_str(), nods, cells));
        }
        //else if (type == "hybrid") {
        //	cout << "read hybrid mesh " << filename << endl;
        //	exit_if(mesh_read_from_vtk<T, 8>(filename.c_str(), nods));
        //

        //}
        else
        {
            // error_msg("type:<%s> is not supported.", type.c_str());
        }
    }

    if (cells.size() == 0)
        cells.resize(4, 0);
    if (cells_coarse.size() == 0)
        cells_coarse.resize(4, 0);
    interp_pts_in_tets<T, 3>(nods, cells, nods_coarse, fine_to_coarse_coef_);
    //interp_pts_in_point_cloud<T, 3>(nods_coarse, nods, coarse_to_fine_coef_);

    if (type_coarse == "tet")
        interp_pts_in_tets<T, 3>(nods_coarse, cells_coarse, nods, coarse_to_fine_coef_);
    else
    {
        //interp_pts_in_point_cloud<T, 3>(nods_coarse, nods, coarse_to_fine_coef_);
        Eigen::MatrixXi hexs2tets = hex_2_tet(cells_coarse);
        cout << "size of hexs2tets " << hexs2tets.rows() << " " << hexs2tets.cols() << endl;
        interp_pts_in_tets<T, 3>(nods_coarse, hexs2tets, nods, coarse_to_fine_coef_);
    }

    const size_t num_nods = nods_coarse.cols();
    cout << "V" << nods_coarse.rows() << " " << nods_coarse.cols() << endl
         << "T " << cells_coarse.rows() << " " << cells_coarse.cols() << endl;
    if (x != nullptr)
    {
        nods        = Map<const MAT<T>>(x, nods.rows(), nods.cols());
        nods_coarse = nods * fine_to_coarse_coef_;
    }

    cout << "Boundary Box :\n"
         << nods_coarse.rowwise().minCoeff() << endl
         << nods_coarse.rowwise().maxCoeff() << endl;

    REST_  = nods;
    cells_ = cells;
    /*Matrix<T, -1, -1> nods_temp = nods_coarse;*/
    fine_verts_num_ = REST_.cols();

    auto         phy_paras = pt.get_child("physics");
    const T      rho       = phy_paras.get<T>("rho", 20);
    const T      Young     = phy_paras.get<T>("Young", 2000.0);
    const T      poi       = phy_paras.get<T>("poi", 0.3);
    const T      gravity   = phy_paras.get<T>("gravity", 9.8);
    const T      dt        = phy_paras.get<T>("dt", 0.01);
    const T      w_pos     = phy_paras.get<T>("w_pos", 1e6);
    const size_t num_frame = phy_paras.get<size_t>("num_frames", 100);

    //read fixed points
    vector<size_t> cons(0);
    const string   cons_file_path = pt.get<string>("cons", "");
    /*if(cons_file_path != "")
    IF_ERR(exit, read_fixed_verts_from_csv(cons_file_path.c_str(), cons));*/
    cout << "constrint " << cons.size() << " points" << endl;

    //calc mass vector
    Matrix<T, -1, 1> mass_vec(num_nods);
    // calc_mass_vector<T>(nods, cells, rho, mass_vec);
    if (type_coarse == "tet")
        mass_calculator<T, 3, 4, 1, 1, basis_func, quadrature>(nods_coarse, cells_coarse, rho, mass_vec);
    else if (type_coarse == "hex")
        mass_calculator<T, 3, 8, 1, 2, basis_func, quadrature>(nods_coarse, cells_coarse, rho, mass_vec);

    cout << "build energy" << endl;
    int ELAS = 0;
    int GRAV = 1;
    int KIN  = 2;
    int POS  = 3;
    if (pt_.get<string>("solver_type") == "explicit")
        POS = 2;

    ebf_.resize(POS + 1);
    {
        const string csttt_type = phy_paras.get<string>("csttt", "linear");

        gen_elas_energy_intf<T>(type_coarse, csttt_type, nods_coarse, cells_coarse, Young, poi, ebf_[ELAS], &elas_intf_);
        /* nods_coarse = nods_temp;*/
        // to lowercase.
        char axis  = pt.get<char>("grav_axis", 'y') | 0x20;
        ebf_[GRAV] = make_shared<gravity_energy<T, 3>>(num_nods, 1, gravity, mass_vec, axis);
        kinetic_   = make_shared<momentum<T, 3>>(nods_coarse.data(), num_nods, mass_vec, dt);

        if (pt_.get<string>("solver_type") == "implicit")
            ebf_[KIN] = kinetic_;

        ebf_[POS] = make_shared<position_constraint<T, 3>>(nods_coarse.data(), num_nods, w_pos, cons);
    }
    cout << "set up energy done." << endl;

    //set constraint

    enum constraint_type
    {
        COLL
    };
    cbf_.resize(COLL + 1);
    collider_  = nullptr;
    cbf_[COLL] = collider_;

    shared_ptr<Problem<T, 3>> pb      = make_shared<Problem<T, 3>>(ebf_[0], nullptr);
    auto                      dat_str = make_shared<dat_str_core<T, 3>>(pb->Nx() / 3, pt.get<bool>("hes_is_const", false));
    compute_hes_pattern(pb->energy_, dat_str);
    ebf_[0]->Hes(nods_coarse.data(), dat_str);
    SparseMatrix<T> K = dat_str->get_hes();

    embedded_interp_ = make_shared<embedded_interpolate<T>>(nods_coarse, coarse_to_fine_coef_, fine_to_coarse_coef_, K, 0.586803 / 2);

    if (pt_.get<string>("solver_type") == "explicit")
    {
        Map<Matrix<T, -1, 1>> position(nods_coarse.data(), nods_coarse.size());
        semi_implicit_ = make_shared<semi_implicit<T>>(dt, mass_vec, position);
    }
    cout << "init problem done." << endl;
}

template <typename T>
std::shared_ptr<Problem<T, 3>> embedded_elas_problem_builder<T>::build_problem() const
{
    cout << "assemble energy" << endl;
    shared_ptr<Functional<T, 3>> energy;
    try
    {
        energy = build_energy_t<T, 3>(ebf_);
    }
    catch (std::exception& e)
    {
        cerr << e.what() << endl;
        exit(EXIT_FAILURE);
    }

    shared_ptr<Constraint<T>> constraint;
    cout << "assemble constraint" << endl;
    bool all_null = true;
    for (auto& c : cbf_)
        if (c != nullptr)
            all_null = false;
    if (all_null)
    {
        constraint = nullptr;
        cout << "WARNGING: No hard constraints." << endl;
    }
    else
    {
        try
        {
            constraint = build_constraint_t<T>(cbf_);
        }
        catch (std::exception& e)
        {
            cerr << e.what() << endl;
            exit(EXIT_FAILURE);
        }
    }
    exit_if(constraint != nullptr && energy->Nx() != constraint->Nx(), "energy and constraint has different dimension.");
    return make_shared<Problem<T, 3>>(energy, constraint);
}

template <typename T>
int embedded_elas_problem_builder<T>::update_problem(const T* x, const T* v)
{
    embedded_interp_->update_verts(x, fine_verts_num_);
    const Eigen::Matrix<T, -1, -1>& verts = embedded_interp_->get_verts();

    IF_ERR(return, kinetic_->update_location_and_velocity(verts.data(), v));
    if (collider_ != nullptr)
        IF_ERR(return, collider_->update(verts.data()));
    return 0;
}

template class embedded_elas_problem_builder<double>;

template class embedded_elas_problem_builder<float>;

}  // namespace PhysIKA


////////////////////////////////////////////////////////////////////////////////
//  Copyright (c) 2021 Andreas Wagner.
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
////////////////////////////////////////////////////////////////////////////////

#ifndef TUMORMODELS_UPWINDING_FORMULAS_LINEARIZED_FLOW_HPP
#define TUMORMODELS_UPWINDING_FORMULAS_LINEARIZED_FLOW_HPP

#include <Eigen/Dense>
#include <cassert>

#include "vessel_formulas.hpp"

namespace macrocirculation {

namespace linearized {

/*! @brief Calculates the upwinding at an inner boundary.
 *
 * @param alpha The \f$ \sqrt{\frac{C}{L}} \f$ factor for the linearized characteristics.
 * @param p_l   The pressure at the boundary of the left cell.
 * @param q_l   The flux at the boundary of the left cell.
 * @param p_r   The pressure at the boundary of the right cell.
 * @param q_r   The flux at the boundary of the right cell.
 * @param p_up  The upwinded pressure.
 * @param q_up  The upwinded flux.
 */
inline void inner_boundary(double alpha, double p_l, double q_l, double p_r, double q_r, double &p_up, double &q_up) {
  q_up = 0.5 * (q_l + q_r + alpha * (p_l - p_r));
  p_up = (alpha * p_l + q_l  - q_up) / alpha;
}

/*! @brief Calculates the upwinding at an nfurcation.
 *
 * @param p         The boundary pressure values of each vessel at a common vertex.
 * @param q         The boundary flux values of each vessel at a common vertex.
 * @param params    The physical edge parameters near the nfurcation.
 * @param sigma     The vessel normals (+1 if the vessel points to the vertex, -1 if not).
 * @param p_up      The upwinded pressure values.
 * @param q_up      The upwinded flux values.
 */
inline void nfurcation_boundary(const std::vector<double> &p,
                                const std::vector<double> &q,
                                const std::vector<VesselParameters> &params,
                                const std::vector<double> &sigma,
                                std::vector<double> &p_up,
                                std::vector<double> &q_up) {
  // all vectors need to have the same size:
  assert(p.size() == q.size());
  assert(p_up.size() == p.size());
  assert(q_up.size() == p.size());
  assert(params.size() == p.size());
  assert(sigma.size() == p.size());

  const size_t N = p.size();

  // dof-ordering: (p_1, q_1, p_2, q_2, ... p_N, q_N)
  Eigen::MatrixXd mat(p.size() + q.size(), p.size() + q.size());
  Eigen::VectorXd rhs(p.size() + q.size());

  mat.setZero();

  // vector containing the \f$ \sqrt{ \frac{C}{L} } \f$ factors:
  std::vector<double> alpha;
  for (auto &param : params) {
    alpha.push_back(std::sqrt(linear::get_C(param) / linear::get_L(param)));
  }

  // constrain the upwinded fluxes to zero:
  for (int k = 0; k < N; k += 1) {
    mat(0, 2 * k + 1) = sigma[k];
  }
  rhs(0) = 0;

  // the characteristics should be equal
  for (int k = 0; k < N; k += 1) {
    mat(1 + k, 2 * k) = 0.5 * alpha[k] * sigma[k];
    mat(1 + k, 2 * k + 1) = 0.5;
    rhs(1 + k) = 0.5 * alpha[k] * sigma[k] * p[k] + 0.5 * q[k];
  }

  // the pressures should be equal
  for (int k = 1; k < N; k += 1) {
    mat(N + k, 2 * k) = 1.;
    mat(N + k, 2 * (k - 1)) = -1.;
    rhs(N + k) = 0.;
  }

  Eigen::VectorXd result = mat.fullPivLu().solve(rhs);

  for (int k = 0; k < N; k += 1) {
    p_up[k] = result[2 * k];
    q_up[k] = result[2 * k + 1];
  }
}

} // namespace linearized

} // namespace macrocirculation

#endif //TUMORMODELS_UPWINDING_FORMULAS_LINEARIZED_FLOW_HPP


#ifndef STAN_MATH_PRIM_FUN_MODIFIED_BESSEL_FIRST_KIND_HPP
#define STAN_MATH_PRIM_FUN_MODIFIED_BESSEL_FIRST_KIND_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <boost/math/special_functions/bessel.hpp>

namespace stan {
namespace math {

/**
 *
   \f[
   \mbox{modified\_bessel\_first\_kind}(v, z) =
   \begin{cases}
     I_v(z) & \mbox{if } -\infty\leq z \leq \infty \\[6pt]
     \textrm{error} & \mbox{if } z = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{modified\_bessel\_first\_kind}(v, z)}{\partial z} =
   \begin{cases}
     \frac{\partial\, I_v(z)}{\partial z} & \mbox{if } -\infty\leq z\leq \infty
 \\[6pt] \textrm{error} & \mbox{if } z = \textrm{NaN} \end{cases} \f]

   \f[
     {I_v}(z) = \left(\frac{1}{2}z\right)^v\sum_{k=0}^\infty
 \frac{\left(\frac{1}{4}z^2\right)^k}{k!\Gamma(v+k+1)} \f]

     \f[
     \frac{\partial \, I_v(z)}{\partial z} = I_{v-1}(z)-\frac{v}{z}I_v(z)
     \f]
 *
 */
template <typename T2>
inline T2 modified_bessel_first_kind(int v, const T2 z) {
  check_not_nan("modified_bessel_first_kind", "z", z);

  return boost::math::cyl_bessel_i(v, z);
}

}  // namespace math
}  // namespace stan

#endif


// Copyright (c) 2018 Evan S Weinberg
// Test bounds on the residual of sequential solves

#include <iostream>
#include <iomanip>
#include <cmath>
#include <string>
#include <sstream>
#include <complex>
#include <random>

// Borrow dense matrix eigenvalue routines.
#include <Eigen/Dense>

#include "blas/generic_vector.h"

#include "square_wilson.h"
#include "inverters/generic_bicgstab.h"

using namespace std;
using namespace Eigen;

typedef Matrix<std::complex<double>, Dynamic, Dynamic, ColMajor> cMatrix;

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

  complex<double> *gauge_links;

  // Set output precision to be long.
  cout << setprecision(10);

  // Inversion info
  inversion_info invif;

  // RNG related things.
  std::mt19937 generator (1337u); // RNG, 1337u is the seed.
  double inv_variance = 6.0; // inverse of variance for gaussian non-compact U(1) links.

  // Basic information about the lattice.
  int length = 24;
  double m_sq = 0.001;

  // max iter
  int max_iter = 100000;

  // Set tolerances below.I'll nonetheless type the proof up tomorrow, but in any case, I'm feeling good

  // Some start-up.
  int volume = length*length;
  int wilson_volume = 2*volume;

  // Create a random compact U(1) link.
  gauge_links = allocate_vector<complex<double>>(2*length*length);
  gaussian_real(gauge_links, 2*length*length, generator, 1.0/inv_variance);
  polar(gauge_links, 2*length*length);

  // Structure which gets passed to the function.
  laplace_gauged_struct lapstr_gauged;
  lapstr_gauged.length = length;
  lapstr_gauged.m_sq = m_sq;
  lapstr_gauged.gauge_links = gauge_links;

  ///////////////////////////////////////////////////
  // GET THE CONDITION NUMBER OF THE WILSON MATRIX //
  ///////////////////////////////////////////////////

  // Need to get the sqrt of the largest and smallest eigenvalue of the normal op

  double condition_number = 0;

  {
    complex<double> *rhs_cplx;
    complex<double> *inter_cplx;

    // Vectors.
    rhs_cplx = allocate_vector<complex<double>>(wilson_volume);
    inter_cplx = allocate_vector<complex<double>>(wilson_volume);

    // Zero out the vector.
    zero_vector(rhs_cplx, 2*length*length);
    zero_vector(inter_cplx, 2*length*length);

    // Allocate a sufficiently gigantic matrix.
    cMatrix mat_cplx = cMatrix::Zero(wilson_volume, wilson_volume);

    // Form matrix elements. This is where it's important that
    // dMatrix and cMatrix are column major.
    // I should probably make this safer by using a "Map".
    for (int i = 0; i < wilson_volume; i++)
    {
      // Set a point on the rhs for a matrix element.
      zero_vector(rhs_cplx, wilson_volume);
      rhs_cplx[i] = 1.0;
      zero_vector(inter_cplx, wilson_volume);
      square_wilson_gauged(inter_cplx, rhs_cplx, &lapstr_gauged);

      // Where we put the result of the matrix element.
      complex<double>* mptr = &(mat_cplx(i*wilson_volume));

      square_wilson_dagger_gauged(mptr, inter_cplx, &lapstr_gauged);
    }

    SelfAdjointEigenSolver<cMatrix> eigsolve_cplx(volume);
    eigsolve_cplx.compute(mat_cplx);

    double largest_eval = real(eigsolve_cplx.eigenvalues()(wilson_volume-1));
    double smallest_eval = real(eigsolve_cplx.eigenvalues()(0));
    std::cout << " Largest singular value: " << sqrt(largest_eval) << "\n";
    std::cout << "Smallest singular value: " << sqrt(smallest_eval) << "\n";

    condition_number = sqrt(largest_eval/smallest_eval);
    std::cout << "       Condition number: " << condition_number << "\n";

    deallocate_vector(&rhs_cplx);
    deallocate_vector(&inter_cplx);
  }

  ///////////////////////////////////
  // PERFORM SEQUENTIAL INVERSIONS //
  ///////////////////////////////////

  // truly desired tolerance.
  double eps = 1e-10;

  // tolerances
  //double eps_1 = 1e-3;
  //double eps_2 = 1e-14;

  // bound
  double eps_1 = eps/2;
  double eps_2 = eps/(2*condition_number);

  {
    // Get a rhs b.
    complex<double>* b;
    b = allocate_vector<complex<double>>(wilson_volume);
    gaussian_real(b, wilson_volume, generator, 1.0/inv_variance);

    double bnorm = sqrt(norm2sq(b, wilson_volume));

    // We want to solve M^dag M x = b.
    // Define Y == M^dag, X == M
    // Solve Y X x = b sequentially.
    // First, define y == X x and solve Y y = b.
    std::cout << "\nSolve Y y = b\n";
    complex<double>* y;
    y = allocate_vector<complex<double>>(wilson_volume);
    zero_vector(y, wilson_volume);

    invif = minv_vector_bicgstab(y, b, wilson_volume, max_iter, eps_1, square_wilson_dagger_gauged, &lapstr_gauged);
    if (invif.success == true)
    {
      printf("  Algorithm %s took %d iterations to reach a tolerance of %.8e.\n", invif.name.c_str(), invif.iter, sqrt(invif.resSq)/bnorm);
    }

    // Next, solve M x = y
    double ynorm = sqrt(norm2sq(y, wilson_volume));
    std::cout << "\nSolve X x = y\n";
    complex<double>* x;
    x = allocate_vector<complex<double>>(wilson_volume);
    zero_vector(x, wilson_volume);

    invif = minv_vector_bicgstab(x, y, wilson_volume, max_iter, eps_2, square_wilson_gauged, &lapstr_gauged);
    if (invif.success == true)
    {
      printf("  Algorithm %s took %d iterations to reach a tolerance of %.8e.\n", invif.name.c_str(), invif.iter, sqrt(invif.resSq)/ynorm);
    }

    // Do the verify.
    complex<double>* tmp;
    tmp = allocate_vector<complex<double>>(wilson_volume);
    zero_vector(tmp, wilson_volume);

    complex<double>* check;
    check = allocate_vector<complex<double>>(wilson_volume);
    zero_vector(check, wilson_volume);

    // Apply M^dag M
    square_wilson_gauged(tmp, x, &lapstr_gauged);
    square_wilson_dagger_gauged(check, tmp, &lapstr_gauged);
    double rel_res = sqrt(diffnorm2sq(check, b, wilson_volume))/bnorm;
    printf("\nThe normal op tolerance is %.8e.\n", rel_res);

    // Check the equality.
    printf("\nThis should be below %.8e.\n", eps_1 + condition_number*eps_2);

    deallocate_vector(&b);
    deallocate_vector(&y);
    deallocate_vector(&x);
    deallocate_vector(&tmp);
    deallocate_vector(&check);

  }

  //////////////
  // CLEAN UP //
  //////////////

  // Free the lattice.
  //delete[] lattice;

  deallocate_vector(&gauge_links);
  return 0;
}


#pragma once

#include <Eigen/Dense>

namespace boltzmann {

/**
 * @brief Barycentric interpolation formula
 *
 * @param wi
 * @param xi
 */
template <typename NUMERIC = double>
class LagrangePolynomial
{
 public:
  typedef NUMERIC numeric_t;
  typedef Eigen::Matrix<numeric_t, -1, 1> vector_t;

 public:
  /**
   * @brief comput barycentric
   *
   * @param wi   weights from barycentric interpolation formula
   * @param xi   abscissas
   */
  static void compute_weights(vector_t &wi, const vector_t &xi);
  /**
   *
   *
   * @param y    output
   * @param x    evaluation points
   * @param xi   abscissas
   * @param yi   interpolation values
   * @param wi   weights (obtained via compute weights)
   */
  static void evaluate(
      vector_t &y, const vector_t &x, const vector_t &xi, const vector_t &yi, const vector_t &wi);
};

// --------------------------------------------------------------------------------
template <typename NUMERIC>
void
LagrangePolynomial<NUMERIC>::compute_weights(vector_t &wi, const vector_t &xi)
{
  const int N = xi.size();

  assert(wi.size() == xi.size());

  for (int i = 0; i < N; ++i) {
    numeric_t v = 1;
    for (int j = 0; j < N; ++j) {
      if (j != i) v *= (xi[j] - xi[i]);
    }
    wi[i] = 1 / v;
  }
}

// --------------------------------------------------------------------------------
template <typename NUMERIC>
void
LagrangePolynomial<NUMERIC>::evaluate(
    vector_t &y, const vector_t &x, const vector_t &xi, const vector_t &yi, const vector_t &wi)
{
  const numeric_t tol(1e-15);

  vector_t dist(xi.size());

  // number of grid points
  const int N = xi.size();

  // number of evaluation nodes
  const int n = x.size();
  assert(y.size() == x.size());

  // iterate over evaluation points in x(j)
  for (int j = 0; j < n; ++j) {
    // compute distance
    bool done = false;
    for (int k = 0; k < N; ++k) {
      dist(k) = x(j) - xi(k);
      if (std::abs(dist(k)) < tol) {
        y(j) = yi(k);
        done = true;
        break;
      }
    }
    if (done) continue;

    // compute sum
    double denom = 0;
    double numer = 0;

    for (int k = 0; k < N; ++k) {
      double v = (wi[k] / dist[k]);
      denom += v;
      numer += v * yi[k];
    }
    y(j) = numer / denom;
  }
}

}  // end namespace boltzmann


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT license.
//
// gDLS*: Generalized Pose-and-Scale Estimation Given Scale and Gravity Priors
//
// Victor Fragoso, Joseph DeGol, Gang Hua.
// Proc. of the IEEE/CVF Conf. on Computer Vision and Pattern Recognition 2020.
//
// Please contact the author of this library if you have any questions.
// Author: Victor Fragoso (victor.fragoso@microsoft.com)

#include "gdls_star/gdls_star.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <glog/logging.h>

#include <cmath>
#include <complex>
#include <utility>
#include <vector>

#include "math/alignment.h"
#include "math/utils.h"
#include "upnp/build_upnp_action_matrix_using_symmetry.h"

namespace msft {
namespace {
// Useful aliases.
using Matrix3x10d = Eigen::Matrix<double, 3, 10>;
using Matrix10d = Eigen::Matrix<double, 10, 10>;
using Matrix8cd = Eigen::Matrix<std::complex<double>, 8, 8>;
using Matrix8d = Eigen::Matrix<double, 8, 8>;
using RowVector10d = Eigen::Matrix<double, 1, 10>;
using Vector10d = Eigen::Matrix<double, 10, 1>;

using CostParameters = GdlsStar::CostParameters;
using Input = GdlsStar::Input;
using Priors = GdlsStar::Priors;
using Solution = GdlsStar::Solution;

constexpr int kNumMaxRotationsExploitingSymmetry = 8;

// Computes the skew-symmetric matrix to compute the cross product.
inline Eigen::Matrix3d
ComputeSkewSymmetricMatrix(const Eigen::Vector3d& vector) {
  Eigen::Matrix3d skew_symmetric_matrix;
  skew_symmetric_matrix.setZero();
  skew_symmetric_matrix(0, 1) = -vector.z();
  skew_symmetric_matrix(1, 0) = vector.z();
  skew_symmetric_matrix(0, 2) = vector.y();
  skew_symmetric_matrix(2, 0) = -vector.y();
  skew_symmetric_matrix(1, 2) = -vector.x();
  skew_symmetric_matrix(2, 1) = vector.x();
  return skew_symmetric_matrix;
}

// This function arranges a 3D point into a 3x10 matrix so that we can
// rotate a point using the rotation expressed as a function of the monomials.
// See Eq. xxiv of the gDLS* supplemental material.
Matrix3x10d LeftMultiply(const Eigen::Vector3d& point) {
  Matrix3x10d phi_mat;
  // Row 0.
  phi_mat(0, 0) = point.x();
  phi_mat(0, 1) = point.x();
  phi_mat(0, 2) = -point.x();
  phi_mat(0, 3) = -point.x();
  phi_mat(0, 4) = 0.0;
  phi_mat(0, 5) = 2 * point.z();
  phi_mat(0, 6) = -2 * point.y();
  phi_mat(0, 7) = 2 * point.y();
  phi_mat(0, 8) = 2 * point.z();
  phi_mat(0, 9) = 0.0;

  // Row 1.
  phi_mat(1, 0) = point.y();
  phi_mat(1, 1) = -point.y();
  phi_mat(1, 2) = point.y();
  phi_mat(1, 3) = -point.y();
  phi_mat(1, 4) = -2.0 * point.z();
  phi_mat(1, 5) = 0.0;
  phi_mat(1, 6) = 2 * point.x();
  phi_mat(1, 7) = 2 * point.x();
  phi_mat(1, 8) = 0.0;
  phi_mat(1, 9) = 2 * point.z();

  // Row 3.
  phi_mat(2, 0) = point.z();
  phi_mat(2, 1) = -point.z();
  phi_mat(2, 2) = -point.z();
  phi_mat(2, 3) = point.z();
  phi_mat(2, 4) = 2.0 * point.y();
  phi_mat(2, 5) = -2.0 * point.x();
  phi_mat(2, 6) = 0.0;
  phi_mat(2, 7) = 0.0;
  phi_mat(2, 8) = 2.0 * point.x();
  phi_mat(2, 9) = 2.0 * point.y();
  return phi_mat;
}

// Check that the input to gDLS* is valid. It checks the inputs have the same
// sizes and that the prior scales are valid.
inline void IsInputDatumValid(const Input& input) {
  CHECK_EQ(input.ray_origins.size(), input.ray_directions.size());
  CHECK_EQ(input.ray_origins.size(), input.world_points.size());
  CHECK_GE(input.priors.scale_penalty, 0.0);
  CHECK_GE(input.priors.gravity_penalty, 0.0);
}

Eigen::Matrix4d ComputeHMatrix(
    const std::vector<Eigen::Vector3d>& ray_origins,
    const std::vector<Eigen::Vector3d>& ray_directions,
    const double scale_penalty_factor) {
  Eigen::Matrix4d h_inverse = Eigen::Matrix4d::Zero();
  for (size_t i = 0; i < ray_directions.size(); ++i) {
    // Computing the scalar in the top-left corner of matrix H^-1.
    h_inverse(0, 0) +=
        ray_origins[i].squaredNorm() -
        ray_origins[i].dot(ray_directions[i]) *
        ray_origins[i].dot(ray_directions[i]);

    // Computing the 3x1 row-vector of the top-right part of matrix H^-1.
    const Eigen::Vector3d temp_term =
        ray_origins[i].dot(ray_directions[i]) * ray_directions[i] -
        ray_origins[i];
    h_inverse.block<3, 1>(1, 0) += temp_term;

    // Computing the 1x3 vector of the bottom-left part of matrix H^-1.
    h_inverse.block<1, 3>(0, 1) += temp_term.transpose();

    // Bottom right 3x3 block of matrix H^-1.
    h_inverse.block<3, 3>(1, 1) +=
        Eigen::Matrix3d::Identity() -
        (ray_directions[i] * ray_directions[i].transpose());
  }

  // Add the scale penalty term to the first entry of h_inverse.
  h_inverse(0, 0) += scale_penalty_factor;

  const Eigen::Matrix4d h_matrix = h_inverse.inverse();

  return h_matrix;
}

// Compute the the matrix F in Eq. xiv i the supplemental material of gDLS*.
// This matrix is F = -B * H.
Eigen::MatrixXd ComputeFMat(
    const std::vector<Eigen::Vector3d>& ray_origins,
    const std::vector<Eigen::Vector3d>& ray_directions,
    const Eigen::Matrix4d& h_matrix) {
  const int num_points = ray_origins.size();
  Eigen::MatrixXd f_mat(num_points, 4);
  Eigen::RowVector4d helper_vec;
  for (size_t i = 0; i < num_points; ++i) {
    // Scalar term: -r_i' c_i.
    helper_vec[0] = -ray_directions[i].dot(ray_origins[i]);
    // Vector term: r'.
    helper_vec.tail(3) = ray_directions[i].transpose();
    // Computing i-th row of matrix F.
    f_mat.block(i, 0, 1, 4) = helper_vec * h_matrix;
  }
  return f_mat;
}

void ComputeScaleAndTranslationFactors(const Input& input,
                                       const Eigen::Matrix4d& h_matrix,
                                       RowVector10d* scale_factor,
                                       Matrix3x10d* translation_factor) {
  // Useful aliases.
  const std::vector<Eigen::Vector3d>& ray_origins = input.ray_origins;
  const std::vector<Eigen::Vector3d>& ray_directions = input.ray_directions;
  const std::vector<Eigen::Vector3d>& world_points = input.world_points;
  const size_t num_correspondences = ray_directions.size();
  Eigen::Matrix<double, 4, 10> sv_helper = Eigen::Matrix<double, 4, 10>::Zero();
  for (size_t i = 0; i < num_correspondences; ++i) {
    const Matrix3x10d left_multiply_matrix = LeftMultiply(world_points[i]);
    // Scale factor.
    sv_helper.row(0) +=
        (ray_origins[i].transpose() - ray_origins[i].dot(ray_directions[i]) *
         ray_directions[i].transpose()) * left_multiply_matrix;

    // Translation factor.
    sv_helper.block<3, 10>(1, 0) +=
        (ray_directions[i] * ray_directions[i].transpose() -
         Eigen::Matrix3d::Identity()) * left_multiply_matrix;
  }

  sv_helper = h_matrix * sv_helper;
  *scale_factor = sv_helper.row(0);
  *translation_factor = sv_helper.block<3, 10>(1, 0);
}

// Computes vector k_i from the scale-constrained gDLS.
inline Eigen::Vector3d ComputeLinearTermPerPoint(
    const int point_idx,
    const Eigen::Matrix4d& h_matrix,
    const Eigen::MatrixXd& f_matrix,
    const Eigen::Vector3d& ray_origin,
    const Eigen::Vector3d& ray_direction,
    const Priors& priors) {
  // Scale linear term.
  const Eigen::Vector3d scalar_linear_term =
      f_matrix(point_idx, 0) * ray_direction - h_matrix.col(0).tail(3) +
      h_matrix(0, 0) * ray_origin;

  // Compute total linear term.
  const Eigen::Vector3d linear_term =
      priors.scale_prior * priors.scale_penalty * scalar_linear_term;
  return linear_term;
}

Matrix10d
ComputeQuadraticPenaltyMatrixFromGravityRegularizer(const Input& input,
                                                    double penalty) {
  // Compute the left-multiply matrix for gravity index.
  const Matrix3x10d world_gravity_dir_matrix =
      LeftMultiply(input.priors.world_down_direction.normalized());
  const Eigen::Vector3d query_gravity_dir =
      input.priors.query_down_direction.normalized();
  const Eigen::Matrix3d query_down_dir_mat =
      ComputeSkewSymmetricMatrix(query_gravity_dir);
  // M = penalty * L(g_I)' * Cross(g_Q)^T * Cross(g_Q) L(g_I),
  // L() is the left-multiply function, and Cross() is the skew-symmetric
  // matrix for cross product. Here g indicates gravity direction according
  // to Q (query) and I (reference).
  const Matrix10d penalty_matrix =
      penalty * world_gravity_dir_matrix.transpose() *
      query_down_dir_mat.transpose() *
      query_down_dir_mat * world_gravity_dir_matrix;
  return penalty_matrix;
}

inline Matrix10d ComputeQuadraticPenaltiesFromScaleConstraint(
    const double scale_penalty,
    const RowVector10d& scale_factor) {
  return scale_penalty * scale_factor.transpose() * scale_factor;
}

inline Vector10d ComputeLinearPenaltiesFromScaleConstraint(
    const Priors& priors,
    const Eigen::Matrix4d& h_matrix,
    const RowVector10d& scale_factor) {
  const double& scale_prior = priors.scale_prior;
  const double& scale_penalty = priors.scale_penalty;
  const double scalar =
      scale_penalty * scale_prior * h_matrix(0, 0) - scale_prior;
  return scale_penalty * scalar * scale_factor;
}

inline double ComputeConstantTermFromScaleConstraint(
    const Priors& priors,
    const Eigen::Matrix4d& h_matrix) {
  const double& scale_penalty = priors.scale_penalty;
  const double& scale_prior = priors.scale_prior;
  const double scale_term =
      scale_penalty * scale_prior * h_matrix(0, 0) - scale_prior;
  const double term = scale_term * scale_term;
  return scale_penalty * term;
}

// The observed pattern is that duplicate rotations appear consequtively in the
// vector, i.e., rotation[i] == rotation[i + 1] is common.
std::vector<Eigen::Quaterniond> RemoveDuplicateRotations(
    const std::vector<Eigen::Quaterniond>& candidate_rotations) {
  const double kAngleThreshold = DegToRad(0.1);
  std::vector<Eigen::Quaterniond> rotations;
  rotations.reserve(candidate_rotations.size());

  // If no rotations then return empty vector.
  if (candidate_rotations.empty()) {
    return rotations;
  }

  for (int i = 0; i < candidate_rotations.size(); ++i) {
    bool duplicate_rotation = false;
    const Eigen::Quaterniond& candidate_rotation = candidate_rotations[i];
    for (int j = rotations.size() - 1; j >= 0; --j) {
      if (candidate_rotation.angularDistance(rotations[j]) < kAngleThreshold) {
        duplicate_rotation = true;
        break;
      }
    }
    if (!duplicate_rotation) {
      rotations.push_back(candidate_rotation);
    }
  }
  return rotations;
}

// Constructs the vector s as indicated in Eq. 13 of gDLS* main paper.
inline Vector10d ComputeRotationVector(const Eigen::Quaterniond& rotation) {
  Vector10d rotation_vector;
  // Set the values of the rotation vector.
  rotation_vector[0] = rotation.w() * rotation.w();
  rotation_vector[1] = rotation.x() * rotation.x();
  rotation_vector[2] = rotation.y() * rotation.y();
  rotation_vector[3] = rotation.z() * rotation.z();
  rotation_vector[4] = rotation.w() * rotation.x();
  rotation_vector[5] = rotation.w() * rotation.y();
  rotation_vector[6] = rotation.w() * rotation.z();
  rotation_vector[7] = rotation.x() * rotation.y();
  rotation_vector[8] = rotation.x() * rotation.z();
  rotation_vector[9] = rotation.y() * rotation.z();
  return rotation_vector;
}

std::vector<Eigen::Vector3d> ComputeScalesAndTranslations(
    const std::vector<Eigen::Quaterniond>& rotations,
    const Matrix3x10d& translation_factor,
    const Input& input,
    const Eigen::Matrix4d& h_matrix,
    const RowVector10d& scale_factor,
    std::vector<double>* scales) {
  // Solve for translation as a function of rotation.
  std::vector<Eigen::Vector3d> translations;
  translations.reserve(rotations.size());
  CHECK_NOTNULL(scales)->reserve(rotations.size());
  const double& scale_penalty = input.priors.scale_penalty;
  for (const Eigen::Quaterniond& rotation : rotations) {
    const Vector10d rotationVector = ComputeRotationVector(rotation);
    translations.emplace_back(
        translation_factor * rotationVector +
        scale_penalty * input.priors.scale_prior * h_matrix.col(0).tail(3));
    scales->emplace_back(
        scale_factor * rotationVector +
        scale_penalty * input.priors.scale_prior * h_matrix(0, 0));
  }
  return translations;
}

void DiscardBadSolutions(const Input& input_datum, Solution* solution) {
  // Useful aliases.
  std::vector<Eigen::Quaterniond>& solution_rotations = solution->rotations;
  std::vector<Eigen::Vector3d>& solution_translations = solution->translations;
  std::vector<double>& solution_scales = solution->scales;
  CHECK_EQ(solution_rotations.size(), solution_translations.size());
  CHECK_EQ(solution_rotations.size(), solution_scales.size());
  std::vector<Eigen::Quaterniond> final_rotations;
  std::vector<Eigen::Vector3d> final_translations;
  std::vector<double> final_scales;
  final_rotations.reserve(solution_rotations.size());
  final_translations.reserve(solution_translations.size());
  final_scales.reserve(solution_scales.size());

  const std::vector<Eigen::Vector3d>& world_points = input_datum.world_points;
  const std::vector<Eigen::Vector3d>& ray_origins = input_datum.ray_origins;
  const std::vector<Eigen::Vector3d>& ray_directions =
      input_datum.ray_directions;

  // For every computed solution, check that points are in front of camera.
  for (int i = 0; i < solution_rotations.size(); ++i) {
    const Eigen::Quaterniond& soln_rotation = solution_rotations[i];
    const Eigen::Vector3d& soln_translation = solution_translations[i];
    const double scale = solution_scales[i];

    // Check that all points are in front of the camera. Discard the solution
    // if this is not the case.
    bool all_points_in_front_of_camera = true;

    for (int j = 0; j < world_points.size(); ++j) {
      const Eigen::Vector3d transformed_point =
          soln_rotation * world_points[j] + soln_translation -
          scale * ray_origins[j];

      // Find the rotation that puts the image ray at unit Z direction.
      const Eigen::Quaterniond unrot =
          Eigen::Quaterniond::FromTwoVectors(ray_directions[j],
                                             Eigen::Vector3d::UnitZ());

      // Rotate the transformed point and check if the z coordinate is
      // negative. This will indicate if the point is projected behind the
      // camera.
      const Eigen::Vector3d rotated_projection = unrot * transformed_point;
      if (rotated_projection.z() < 0) {
        all_points_in_front_of_camera = false;
        break;
      }
    }

    if (all_points_in_front_of_camera) {
      final_rotations.emplace_back(soln_rotation);
      final_translations.emplace_back(soln_translation);
      final_scales.push_back(scale);
    }
  }

  // Set the final solutions.
  std::swap(solution_rotations, final_rotations);
  std::swap(solution_translations, final_translations);
  std::swap(solution_scales, final_scales);
}

}  // namespace

void GdlsStar::ComputeHelperMatrices(const Input& input) {
  // Compute helper matrices.
  // Compute H matrix from.
  helper_matrices_.h_matrix =
      ComputeHMatrix(input.ray_origins,
                     input.ray_directions,
                     input.priors.scale_penalty);

  // Compute the F matrix, they are needed for the translation prior part.
  helper_matrices_.f_matrix = ComputeFMat(input.ray_origins,
                                          input.ray_directions,
                                          helper_matrices_.h_matrix);

  // Compute scale and translation factors.
  ComputeScaleAndTranslationFactors(input,
                                    helper_matrices_.h_matrix,
                                    &helper_matrices_.scale_factor,
                                    &helper_matrices_.translation_factor);
}

void GdlsStar::ComputeLeastSquaresCostParameters(const Input& input) {
  // Useful aliases.
  const std::vector<Eigen::Vector3d>& ray_origins = input.ray_origins;
  const std::vector<Eigen::Vector3d>& ray_directions = input.ray_directions;
  const std::vector<Eigen::Vector3d>& world_points = input.world_points;
  const size_t num_correspondences = ray_directions.size();

  cost_params_.quadratic_penalty_mat.setZero();
  cost_params_.linear_penalty_vector.setZero();
  cost_params_.gamma = 0.0;
  for (int i = 0; i < num_correspondences; ++i) {
    // Compute the quadratic term per point.
    const Matrix3x10d left_multiply_matrix = LeftMultiply(world_points[i]);
    const Matrix3x10d cost_coeff_term =
        (ray_directions[i] * ray_directions[i].transpose() -
         Eigen::Matrix3d::Identity()) *
        (LeftMultiply(world_points[i]) -
         ray_origins[i] * helper_matrices_.scale_factor +
         helper_matrices_.translation_factor);
    cost_params_.quadratic_penalty_mat +=
        cost_coeff_term.transpose() * cost_coeff_term;

    // Compute the linear term per point as calculated in Eq. 22 of scale and
    // gravity constrained gdls.
    const Eigen::Vector3d scalar_penalty_vector =
        ComputeLinearTermPerPoint(i,
                                  helper_matrices_.h_matrix,
                                  helper_matrices_.f_matrix,
                                  ray_origins[i],
                                  ray_directions[i],
                                  input.priors);
    cost_params_.linear_penalty_vector +=
        scalar_penalty_vector.transpose() * cost_coeff_term;

    // Compute the constant term per point.
    cost_params_.gamma += scalar_penalty_vector.dot(scalar_penalty_vector);
  }

  // Add penalties for gravity constraints.
  if (input.priors.gravity_penalty > 0.0) {
    cost_params_.quadratic_penalty_mat +=
        ComputeQuadraticPenaltyMatrixFromGravityRegularizer(
            input, input.priors.gravity_penalty);
  }

  // Add penalties for scale constraints.
  if (input.priors.scale_penalty > 0.0) {
    cost_params_.quadratic_penalty_mat +=
        ComputeQuadraticPenaltiesFromScaleConstraint(
            input.priors.scale_penalty,
            helper_matrices_.scale_factor);
    cost_params_.linear_penalty_vector +=
        ComputeLinearPenaltiesFromScaleConstraint(
            input.priors,
            helper_matrices_.h_matrix,
            helper_matrices_.scale_factor);
    cost_params_.gamma +=
        ComputeConstantTermFromScaleConstraint(
            input.priors,
            helper_matrices_.h_matrix);
  }
}

CostParameters GdlsStar::ComputeCostParameters(const Input& input) {
  // Validate input.
  IsInputDatumValid(input);

  // Compute all the helper matrices.
  ComputeHelperMatrices(input);

  // Compute the least squares cost parameters.
  ComputeLeastSquaresCostParameters(input);

  return cost_params_;
}

std::vector<Eigen::Quaterniond> GdlsStar::EstimateRotations() {
  std::vector<Eigen::Quaterniond> rotations(kNumMaxRotationsExploitingSymmetry);
  // Build action matrix.
  const Matrix8d action_matrix = theia::BuildActionMatrixUsingSymmetry(
      cost_params_.quadratic_penalty_mat,
      cost_params_.linear_penalty_vector,
      &template_matrix_);

  const Eigen::EigenSolver<Matrix8d> eigen_solver(action_matrix);
  const Matrix8cd eigen_vectors = eigen_solver.eigenvectors();

  for (int i = 0; i < rotations.size(); ++i) {
    // Complex solutions can be good, in particular when the number of
    // correspondences is really low. To use these complex solutions, we simply
    // ignore their imaginary part.
    rotations[i] = Eigen::Quaterniond(eigen_vectors(4, i).real(),
                                      eigen_vectors(5, i).real(),
                                      eigen_vectors(6, i).real(),
                                      eigen_vectors(7, i).real()).normalized();
  }

  return RemoveDuplicateRotations(rotations);
}

bool GdlsStar::EstimateSimilarityTransformation(const Input& input,
                                                Solution* solution) {
  CHECK_NOTNULL(solution)->rotations.clear();
  solution->translations.clear();
  solution->scales.clear();

  // Construct cost parameters.
  ComputeCostParameters(input);

  // Estimate rotations.
  solution->rotations = EstimateRotations();

  // Compute translations and scales.
  solution->translations =
      ComputeScalesAndTranslations(solution->rotations,
                                   helper_matrices_.translation_factor,
                                   input,
                                   helper_matrices_.h_matrix,
                                   helper_matrices_.scale_factor,
                                   &solution->scales);

  // Discard solutions that do not have the points in front of the camera.
  DiscardBadSolutions(input, solution);
  return !solution->rotations.empty();
}

}  // namespace msft


#include <mmtbx/error.h>
#include <iotbx/pdb/hierarchy.h>
#include <scitbx/vec3.h>
#include <boost/optional.hpp>
#include <limits>
#include <cstring>

#define HB_Q1 0.42
#define HB_Q2 0.2
#define HB_F 332
#define HB_F_Q1_Q2 27.888

namespace mmtbx { namespace secondary_structure { namespace dssp {
  using namespace iotbx::pdb::hierarchy;

  inline double hbond_energy (
      scitbx::vec3<double> c_xyz,
      scitbx::vec3<double> h_xyz,
      scitbx::vec3<double> o_xyz,
      scitbx::vec3<double> n_xyz) {
    double r_ON = (o_xyz - n_xyz).length();
    double r_CH = (c_xyz - h_xyz).length();
    double r_OH = (o_xyz - h_xyz).length();
    double r_CN = (c_xyz - n_xyz).length();
    double E = HB_F_Q1_Q2 * (1./r_ON + 1./r_CH - 1./r_OH - 1./r_CN);
    return E;
  }

  // given a backbone nitrogen atom, calculate the position of the attached
  // hydrogen atom.
  boost::optional< scitbx::vec3<double> >
  get_n_h_position (
    atom const& N,
    double nh_bond_length=1.01)
  {
    bool have_caN = false;
    bool have_cN = false;
    scitbx::vec3<double> N_xyz = N.data->xyz;
    scitbx::vec3<double> caN_xyz(0,0,0);
    scitbx::vec3<double> cN_xyz(0,0,0);
    // find CA attached to N
    boost::optional<atom_group> ag_N = N.parent();
    MMTBX_ASSERT(ag_N);
    unsigned n_ats = ag_N->atoms_size();
    std::vector<atom> agN_atoms = ag_N->atoms();
    for(unsigned i_at=0;i_at<n_ats;i_at++) {
      atom const& a = agN_atoms[i_at];
      if (std::strcmp(a.data->name.elems, " CA ") == 0) {
        caN_xyz = scitbx::vec3<double>(a.data->xyz);
        have_caN = true;
        break;
      }
    }
    if (! have_caN) {
      return boost::optional< scitbx::vec3<double> >();
    }
    // find previous C attached to N
    boost::optional<residue_group> rg_N = ag_N->parent();
    boost::optional<chain> chn_N = rg_N->parent();
    unsigned n_rgs = chn_N->residue_groups_size();
    std::vector<residue_group> rgs = chn_N->residue_groups();
    residue_group prev_rg;
    bool have_prev_rg = false;
    for (unsigned i_rg = 0; i_rg < n_rgs; i_rg++) {
      if (rgs[i_rg].memory_id() == rg_N->memory_id()) {
        break;
      } else {
        prev_rg = rgs[i_rg];
        have_prev_rg = true;
      }
    }
    if (! have_prev_rg) {
      return boost::optional< scitbx::vec3<double> >();
    }
    unsigned n_ags = prev_rg.atom_groups_size();
    std::vector<atom_group> const& ags = prev_rg.atom_groups();
    for (unsigned i_ag = 0; i_ag < n_ags; i_ag++) {
      if (ags[i_ag].data->altloc.elems[0] == ag_N->data->altloc.elems[0]) {
        n_ats = ags[i_ag].atoms_size();
        std::vector<atom> prev_atoms = ags[i_ag].atoms();
        for (unsigned i_at = 0; i_at < n_ats; i_at++) {
          atom const& a = prev_atoms[i_at];
          if (std::strcmp(a.data->name.elems, " C  ") == 0) {
            cN_xyz = scitbx::vec3<double>(a.data->xyz);
            have_cN = true;
            break;
          }
        }
        break;
      }
    }
    if (! have_cN) {
      return boost::optional< scitbx::vec3<double> >();
    }
    scitbx::vec3<double> midpoint = (caN_xyz + cN_xyz) / 2;
    scitbx::vec3<double> vec_nm = N_xyz - midpoint;
    scitbx::vec3<double>hN_xyz = N_xyz + (vec_nm.normalize() * nh_bond_length);
    return boost::optional< scitbx::vec3<double> >(hN_xyz);
  }

  // given carbonyl oxygen and amino nitrogen atoms, calculate the hydrogen
  // bond energy between them.
  boost::optional<double>
  get_o_n_hbond_energy (
    atom const& O,
    atom const& N,
    double nh_bond_length=1.01)
  {
    scitbx::vec3<double> N_xyz = N.data->xyz;
    scitbx::vec3<double> O_xyz = O.data->xyz;
    bool have_cO = false;
    scitbx::vec3<double> cO_xyz(0,0,0);
    boost::optional< scitbx::vec3<double> > hN_xyz;
    // find C attached to O
    boost::optional<atom_group> ag_O = O.parent();
    MMTBX_ASSERT(ag_O);
    unsigned n_ats = ag_O->atoms_size();
    std::vector<atom> const& atoms = ag_O->atoms();
    for(unsigned i_at=0;i_at<n_ats;i_at++) {
      atom const& a = atoms[i_at];
      if (std::strcmp(a.data->name.elems, " C  ") == 0) {
        cO_xyz = scitbx::vec3<double>(a.data->xyz);
        have_cO = true;
        break;
      }
    }
    if (! have_cO) {
      return boost::optional<double>();
    }
    double rCN = (cO_xyz - N_xyz).length_sq();
    if (rCN > 49.0) {
      return boost::optional<double>();
    }
    // now calculate the hydrogen position
    hN_xyz = get_n_h_position(N, nh_bond_length);
    if (! hN_xyz) {
      return boost::optional<double>();
    }
    // finally we can calculate the energy
    return hbond_energy(cO_xyz, *hN_xyz, O_xyz, N_xyz);
  }

}}} // namespace mmtbx::secondary_structure::dssp