/*
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  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.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

#include <boost/unordered_set.hpp>
#include <graphlab.hpp>
#include <graphlab/ui/metrics_server.hpp>
#include <graphlab/macros_def.hpp>
/**
 *
 * In this program we implement the "hash-table" version of the
 * "edge-iterator" algorithm described in
 *
 *    T. Schank. Algorithmic Aspects of Triangle-Based Network Analysis.
 *    Phd in computer science, University Karlsruhe, 2007.
 *
 * The procedure is quite straightforward:
 *   - each vertex maintains a list of all of its neighbors in a hash table.
 *   - For each edge (u,v) in the graph, count the number of intersections
 *     of the neighbor set on u and the neighbor set on v.
 *   - We store the size of the intersection on the edge.
 *
 * This will count every triangle exactly 3 times. Summing across all the
 * edges and dividing by 3 gives the desired result.
 *
 * The preprocessing stage take O(|E|) time, and it has been shown that this
 * algorithm takes $O(|E|^(3/2))$ time.
 *
 * If we only require total counts, we can introduce a optimization that is
 * similar to the "forward" algorithm
 * described in thesis above. Instead of maintaining a complete list of all
 * neighbors, each vertex only maintains a list of all neighbors with
 * ID greater than itself. This implicitly generates a topological sort
 * of the graph.
 *
 * Then you can see that each triangle
 *
 * \verbatim

     A----->C
     |     ^
     |   /
     v /
     B

 * \endverbatim
 * Must be counted only once. (Only when processing edge AB, can one
 * observe that A and B have intersecting out-neighbor sets).
 *
 *
 * \note The implementation here is built to be easy to understand
 * and not necessarily optimal. In particular the unordered_set is slow
 * for small number of entries. There is a much more efficient
 * (and substantially more complicated) version in undirected_triangle_count.cpp
 */

/*
 * Each vertex maintains a list of all its neighbors.
 * and a final count for the number of triangles it is involved in
 */
struct vertex_data_type {
  vertex_data_type():num_triangles(0) { }
  // A list of all its neighbors
  boost::unordered_set<graphlab::vertex_id_type> vid_set;
  // The number of triangles this vertex is involved it.
  // only used if "per vertex counting" is used
  size_t num_triangles;

  void save(graphlab::oarchive &oarc) const {
    oarc << vid_set << num_triangles;
  }
  void load(graphlab::iarchive &iarc) {
    iarc >> vid_set >> num_triangles;
  }
};

/*
 * Each edge is simply a counter of triangles
 */
typedef size_t edge_data_type;

// To collect the set of neighbors, we need a message type which is
// basically a set of vertex IDs

bool PER_VERTEX_COUNT = false;

/*
 * This is the gathering type which accumulates an (unordered) set of
 * all neighboring vertices.
 * It is a simple wrapper around a boost::unordered_set with
 * an operator+= which simply performs a set union.
 *
 * This struct can be significantly accelerated for small sets.
 * Small collections of vertex IDs should not require the overhead
 * of the unordered_set.
 */
struct set_union_gather {
  boost::unordered_set<graphlab::vertex_id_type> vid_set;

  /*
   * Combining with another collection of vertices.
   * Union it into the current set.
   */
  set_union_gather& operator+=(const set_union_gather& other) {
    foreach(graphlab::vertex_id_type othervid, other.vid_set) {
      vid_set.insert(othervid);
    }
    return *this;
  }

  // serialize
  void save(graphlab::oarchive& oarc) const {
    oarc << vid_set;
  }

  // deserialize
  void load(graphlab::iarchive& iarc) {
    iarc >> vid_set;
  }
};

/*
 * Define the type of the graph
 */
typedef graphlab::distributed_graph<vertex_data_type,
                                    edge_data_type> graph_type;

/*
 * This class implements the triangle counting algorithm as described in
 * the header. On gather, we accumulate a set of all adjacent vertices.
 * If per_vertex output is not necessary, we can use the optimization
 * where each vertex only accumulates neighbors with greater vertex IDs.
 */
class triangle_count :
      public graphlab::ivertex_program<graph_type,
                                      set_union_gather>,
      /* I have no data. Just force it to POD */
      public graphlab::IS_POD_TYPE  {
public:
  // Gather on all edges
  edge_dir_type gather_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }

  /*
   * For each edge, figure out the ID of the "other" vertex
   * and accumulate a set of the neighborhood vertex IDs.
   */
  gather_type gather(icontext_type& context,
                     const vertex_type& vertex,
                     edge_type& edge) const {
    set_union_gather gather;
    // Insert the opposite end of the edge IF the opposite end has
    // ID greater than the current vertex
    // If we are getting per vertex counts, we need the entire neighborhood
    vertex_id_type otherid = edge.source().id() == vertex.id() ?
                             edge.target().id() : edge.source().id();
    if (PER_VERTEX_COUNT ||
        otherid > vertex.id()) gather.vid_set.insert(otherid);
    return gather;
  }

  /*
   * the gather result now contains the vertex IDs in the neighborhood.
   * store it on the vertex.
   */
  void apply(icontext_type& context, vertex_type& vertex,
             const gather_type& neighborhood) {
    vertex.data().vid_set = neighborhood.vid_set;
  } // end of apply

  /*
   * Scatter over all edges to compute the intersection.
   * I only need to touch each edge once, so if I scatter just on the
   * out edges, that is sufficient.
   */
  edge_dir_type scatter_edges(icontext_type& context,
                              const vertex_type& vertex) const {
    return graphlab::OUT_EDGES;
  }

  /*
   * Computes the size of the intersection of two unordered sets
   */
  static size_t count_set_intersect(
               const boost::unordered_set<vertex_id_type>& smaller_set,
               const boost::unordered_set<vertex_id_type>& larger_set) {
    size_t count = 0;
    foreach(vertex_id_type vid, smaller_set) {
      count += larger_set.count(vid);
    }
    return count;
  }

  /*
   * For each edge, count the intersection of the neighborhood of the
   * adjacent vertices. This is the number of triangles this edge is involved
   * in.
   */
  void scatter(icontext_type& context,
              const vertex_type& vertex,
              edge_type& edge) const {
    const vertex_data_type& srclist = edge.source().data();
    const vertex_data_type& targetlist = edge.target().data();
    if (srclist.vid_set.size() >= targetlist.vid_set.size()) {
      edge.data() = count_set_intersect(targetlist.vid_set, srclist.vid_set);
    }
    else {
      edge.data() = count_set_intersect(srclist.vid_set, targetlist.vid_set);
    }
  }
};

/*
 * This class is used in a second engine call if per vertex counts are needed.
 * The number of triangles a vertex is involved in can be computed easily
 * by summing over the number of triangles each adjacent edge is involved in
 * and dividing by 2.
 */
class get_per_vertex_count :
      public graphlab::ivertex_program<graph_type, size_t>,
      /* I have no data. Just force it to POD */
      public graphlab::IS_POD_TYPE  {
public:
  // Gather on all edges
  edge_dir_type gather_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }
  // We gather the number of triangles each edge is involved in
  size_t gather(icontext_type& context,
                     const vertex_type& vertex,
                     edge_type& edge) const {
    return edge.data();
  }

  /* the gather result is the total sum of the number of triangles
   * each adjacent edge is involved in . Dividing by 2 gives the
   * desired result.
   */
  void apply(icontext_type& context, vertex_type& vertex,
             const gather_type& num_triangles) {
    vertex.data().num_triangles = num_triangles / 2;
  }

  // No scatter
  edge_dir_type scatter_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::NO_EDGES;
  }

};

/* Used to sum over all the edges in the graph in a
 * map_reduce_edges call
 * to get the total number of triangles
 */
size_t get_edge_data(const graph_type::edge_type& e) {
  return e.data();
}

/*
 * A saver which saves a file where each line is a vid / # triangles pair
 */
struct save_triangle_count{
  std::string save_vertex(graph_type::vertex_type v) {
    return graphlab::tostr(v.id()) + "\t" +
           graphlab::tostr(v.data().num_triangles) + "\n";
  }
  std::string save_edge(graph_type::edge_type e) {
    return "";
  }
};

int main(int argc, char** argv) {
  std::cout << "This program counts the exact number of triangles in the "
            "provided graph.\n\n";

  graphlab::command_line_options clopts("Exact Triangle Counting. "
    "Given a graph, this program computes the total number of triangles "
    "in the graph. An option (per_vertex) is also provided which "
    "computes for each vertex, the number of triangles it is involved in."
    "The algorithm assumes that each undirected edge appears exactly once "
    "in the graph input. If edges may appear more than once, this procedure "
    "will over count.");
  std::string prefix, format;
  std::string per_vertex;
  clopts.attach_option("graph", prefix,
                       "Graph input. reads all graphs matching prefix*");
  clopts.attach_option("format", format,
                       "The graph format");
  clopts.attach_option("per_vertex", per_vertex,
                       "If not empty, will count the number of "
                       "triangles each vertex belongs to and "
                       "save to file with prefix \"[per_vertex]\". "
                       "The algorithm used is slightly different "
                       "and thus will be a little slower");

  if(!clopts.parse(argc, argv)) return EXIT_FAILURE;
  if (prefix == "") {
    std::cout << "--graph is not optional\n";
    clopts.print_description();
    return EXIT_FAILURE;
  }
  else if (format == "") {
    std::cout << "--format is not optional\n";
    clopts.print_description();
    return EXIT_FAILURE;
  }

  if (per_vertex != "") PER_VERTEX_COUNT = true;
  // Initialize control plane using mpi
  graphlab::mpi_tools::init(argc, argv);
  graphlab::distributed_control dc;

  graphlab::launch_metric_server();
  // load graph
  graph_type graph(dc, clopts);
  graph.load_format(prefix, format);
  graph.finalize();
  dc.cout() << "Number of vertices: " << graph.num_vertices() << std::endl
            << "Number of edges:    " << graph.num_edges() << std::endl;

  graphlab::timer ti;

  // create engine to count the number of triangles
  dc.cout() << "Counting Triangles..." << std::endl;
  graphlab::synchronous_engine<triangle_count> engine(dc, graph, clopts);
  engine.signal_all();
  engine.start();

  dc.cout() << "Counted in " << ti.current_time() << " seconds" << std::endl;

  if (PER_VERTEX_COUNT == false) {
    size_t count = graph.map_reduce_edges<size_t>(get_edge_data);
    dc.cout() << count << " Triangles"  << std::endl;
  }
  else {
    graphlab::synchronous_engine<get_per_vertex_count> engine(dc, graph, clopts);
    engine.signal_all();
    engine.start();
    graph.save(per_vertex,
            save_triangle_count(),
            false, /* no compression */
            true, /* save vertex */
            false, /* do not save edge */
            1); /* one file per machine */

  }

  graphlab::stop_metric_server();

  graphlab::mpi_tools::finalize();
  return EXIT_SUCCESS;
} // End of main


/* Copyright 2020 Oinam Romesh Meitei

   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.
 */

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <unsupported/Eigen/MatrixFunctions>
#include <cmath>
#include <vector>
#include <typeinfo>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <iomanip>

#include "getham.h"
#include "agradc.h"

gradc grad_ana(
	       std::vector<double> &tlist,
	       std::vector<std::complex<double> > &ini_vec,
	       pulsec pobj,
	       std::vector< std::vector< Eigen::SparseMatrix
	       <double,0,ptrdiff_t> > > hdrive,
	       std::vector< std::complex<double> > dsham,
	       std::vector< int> &states,
	       Eigen::MatrixXcd &cHam){

  Eigen::SparseMatrix<std::complex<double> > hamdr;
  Eigen::SparseMatrix<std::complex<double> > hamdR;
  std::complex<double> hcoef; //, hcoefc;

  int i,j;
  int dsham_len = dsham.size();
  int tlen = tlist.size();
  int nstate = states.size();
  int &nqubit = pobj.nqubit;
  double energy;

  std::complex<double> psit_g;

  Eigen::SparseMatrix<std::complex<double> >
    matexp_(dsham_len, dsham_len);

  Eigen::SparseMatrix<std::complex<double> > hD, Hd_;
  Eigen::MatrixXcd H1_, P_;
  Eigen::VectorXcd psi_;
  Eigen::VectorXcd states_(nstate);

  std::vector< Eigen::VectorXcd > O_(nqubit,
				     Eigen::VectorXcd (dsham_len));

  std::vector< Eigen::VectorXcd > g1__(nqubit,
				       Eigen::VectorXcd (dsham_len));

  std::vector< Eigen::VectorXcd > g1_(nqubit,
				       Eigen::VectorXcd (nstate));

  Eigen::Map<Eigen::VectorXcd> ket_(ini_vec.data(), ini_vec.size());
  std::vector< Eigen::SparseMatrix<std::complex<double> > > hamd(nqubit);
  std::vector<std::vector<double> > t_grad(nqubit,
					   std::vector<double> (tlen));

  double tau = tlist[tlen-1] / tlen;
  std::complex<double> im(0.0,-tau);
  std::complex<double> imp(0.0,tau);

  for (int t=0; t<tlen; t++){

    hD = getham2(tlist[t], t, pobj, hdrive, dsham, dsham_len, matexp_, hamd, hamdr, hamdR, hcoef);
    H1_ = im * Eigen::MatrixXcd(hD);
    ket_ = H1_.exp() * ket_;
  }

  for (i=0; i<nqubit; i++){
    g1__[i] = hamd[i] * ket_;
  }

  for (i=0; i<nstate; i++){
    states_[i] = ket_[states[i]];
  }
  double nrm =  states_.norm();

  for (int i = 0; i<nqubit; i++){
    for (int j=0; j<nstate; j++){
      g1_[i][j] = g1__[i][states[j]];
    }
  }

  psi_ = cHam * states_;

  std::complex<double> energy_ = states_.conjugate().transpose() * psi_;
  energy = energy_.real();

  psi_ = psi_.conjugate();
  Eigen::Transpose<Eigen::VectorXcd> psi1_ = psi_.transpose();
  P_ = Eigen::MatrixXcd::Identity(dsham_len, dsham_len);

  double tau1 = 2.0 * tau;

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

    psit_g = psi1_ * g1_[i];
    t_grad[i][tlen-1] = tau1 * psit_g.imag();
  }

  for (int idx=tlen-1; idx > 0; idx--){
    Hd_ = getham3( tlist[idx], idx, pobj, hdrive, dsham, dsham_len, matexp_, hamdr, hamdR, hcoef);
    hD = getham2(tlist[idx-1], idx-1, pobj, hdrive, dsham, dsham_len,
		 matexp_, hamd, hamdr, hamdR, hcoef);

    H1_ = (imp*Eigen::MatrixXcd(Hd_)).exp();
    ket_ = H1_ * ket_;
    P_ = P_ * H1_.conjugate().transpose();

    for (i=0; i<nqubit; i++){
      O_[i] = P_ * hamd[i] * ket_;
    }

    for (i=0; i<nqubit; i++){
      for (j=0; j<nstate; j++){
	g1_[i][j] = O_[i][states[j]];
      }
    }

    for (i=0; i<nqubit; i++){
      psit_g = psi1_ * g1_[i];
      t_grad[i][idx-1] = tau1 * psit_g.imag();
    }
  }
  gradc g1(energy, nrm, t_grad);

  return g1;
}

gradc grad_ana_normalized(
                               std::vector<double> &tlist,
                               std::vector<std::complex<double> > &ini_vec,
                               pulsec pobj,
                               std::vector< std::vector< Eigen::SparseMatrix
			       <double,0,ptrdiff_t> > > hdrive,
                               std::vector< std::complex<double> > dsham,
			       std::vector< int> &states,
			       Eigen::MatrixXcd &cHam){

  Eigen::SparseMatrix<std::complex<double> > hamdr;
  Eigen::SparseMatrix<std::complex<double> > hamdR;
  std::complex<double> hcoef;

  int i,j;
  int dsham_len = dsham.size();
  int tlen = tlist.size();
  int nstate = states.size();
  int &nqubit = pobj.nqubit;
  double energy,nrm2, nrm4;

  std::complex<double> psit_g,psit_gN;

  Eigen::SparseMatrix<std::complex<double> >
    matexp_(dsham_len, dsham_len);

  Eigen::SparseMatrix<std::complex<double> > hD, Hd_;
  Eigen::MatrixXcd H1_, P_;
  Eigen::VectorXcd psi_,psi_N;
  Eigen::VectorXcd states_(nstate);

  std::vector< Eigen::VectorXcd > O_(nqubit,
				     Eigen::VectorXcd (dsham_len));

  std::vector< Eigen::VectorXcd > g1__(nqubit,
				       Eigen::VectorXcd (dsham_len));

  std::vector< Eigen::VectorXcd > g1_(nqubit,
				       Eigen::VectorXcd (nstate));

  Eigen::Map<Eigen::VectorXcd> ket_(ini_vec.data(), ini_vec.size());
  std::vector< Eigen::SparseMatrix<std::complex<double> > > hamd(nqubit);
  std::vector<std::vector<double> > t_grad(nqubit,
					   std::vector<double> (tlen));

  double tau = tlist[tlen-1] / tlen;
  std::complex<double> im(0.0,-tau);
  std::complex<double> imp(0.0,tau);

  for (int t=0; t<tlen; t++){

    hD = getham2(tlist[t], t, pobj, hdrive, dsham, dsham_len, matexp_, hamd, hamdr, hamdR, hcoef);
    H1_ = im * Eigen::MatrixXcd(hD);
    ket_ = H1_.exp() * ket_;
  }

  for (i=0; i<nqubit; i++){
    g1__[i] = hamd[i] * ket_;
  }

  for (i=0; i<nstate; i++){
    states_[i] = ket_[states[i]];
  }
  double nrm =  states_.norm();
  nrm2 = nrm*nrm;
  nrm4 = nrm2*nrm2;

  for (int i = 0; i<nqubit; i++){
    for (int j=0; j<nstate; j++){
      g1_[i][j] = g1__[i][states[j]];
    }
  }

  psi_ = cHam * states_;
  psi_N = states_;

  std::complex<double> energy_ = states_.conjugate().transpose() * psi_;
  energy = energy_.real();

  psi_ = psi_.conjugate();
  Eigen::Transpose<Eigen::VectorXcd> psi1_ = psi_.transpose();
  psi_N = psi_N.conjugate();
  Eigen::Transpose<Eigen::VectorXcd> psi1_N = psi_N.transpose();
  P_ = Eigen::MatrixXcd::Identity(dsham_len, dsham_len);
  double tau1 = 2.0 * tau;
  nrm4 = tau1/nrm4;

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

    psit_g = psi1_ * g1_[i];
    psit_gN = psi1_N * g1_[i];
    t_grad[i][tlen-1] = (psit_g.imag()*nrm2 - energy*psit_gN.imag())*nrm4;
  }

  for (int idx=tlen-1; idx > 0; idx--){
    Hd_ = getham3( tlist[idx], idx, pobj, hdrive, dsham, dsham_len, matexp_, hamdr, hamdR, hcoef);
    hD = getham2(tlist[idx-1], idx-1, pobj, hdrive, dsham, dsham_len,
		 matexp_, hamd, hamdr, hamdR, hcoef);

    H1_ = (imp*Eigen::MatrixXcd(Hd_)).exp();
    ket_ = H1_ * ket_;
    P_ = P_ * H1_.conjugate().transpose();

    for (i=0; i<nqubit; i++){
      O_[i] = P_ * hamd[i] * ket_;
    }

    for (i=0; i<nqubit; i++){
      for (j=0; j<nstate; j++){
	g1_[i][j] = O_[i][states[j]];
      }
    }

    for (i=0; i<nqubit; i++){
      psit_g = psi1_ * g1_[i];
      psit_gN = psi1_N * g1_[i];
      t_grad[i][idx-1] = (psit_g.imag()*nrm2 - energy*psit_gN.imag())*nrm4;
    }
  }
  energy = energy/nrm2;
  gradc g1(energy, nrm, t_grad);

  return g1;
}


#include "RefElement.h"
#include "basis/Functions.h"
#include "basis/Nodal.h"
#include "quadrules/AutoRule.h"
#include "tensor/EigenMap.h"
#include "tensor/Reshape.h"
#include "tensor/TensorBase.h"
#include "util/Combinatorics.h"
#include "util/Enumerate.h"
#include "util/MultiIndex.h"

#include <Eigen/Core>
#include <Eigen/LU>

#include <cassert>
#include <cstddef>

namespace tndm {

template <std::size_t D> Managed<Matrix<double>> ModalRefElement<D>::massMatrix() const {
    auto rule = simplexQuadratureRule<D>(2 * this->degree());
    std::ptrdiff_t nbf = this->numBasisFunctions();
    Managed<Matrix<double>> M({nbf, nbf}, this->alignment());
    auto E = evaluateBasisAt(rule.points(), {0, 1});
    for (std::ptrdiff_t i = 0; i < M.shape(0); ++i) {
        for (std::ptrdiff_t j = 0; j < M.shape(1); ++j) {
            M(i, j) = 0.0;
            // The basis is orthogonal, therefore we only need to compute the diagonal
            if (i == j) {
                for (std::size_t q = 0; q < rule.size(); ++q) {
                    M(i, j) += rule.weights()[q] * E(i, q) * E(j, q);
                }
            }
        }
    }

    return M;
}

template <std::size_t D> Managed<Matrix<double>> ModalRefElement<D>::inverseMassMatrix() const {
    auto Minv = massMatrix();
    for (std::ptrdiff_t i = 0; i < Minv.shape(0); ++i) {
        Minv(i, i) = 1.0 / Minv(i, i);
    }
    return Minv;
}

template <std::size_t D>
Managed<Matrix<double>>
ModalRefElement<D>::evaluateBasisAt(std::vector<std::array<double, D>> const& points,
                                    std::array<unsigned, 2> const& permutation) const {
    using index_t = Matrix<double>::index_t;
    auto shape =
        permute(permutation, make_index<index_t>(this->numBasisFunctions(), points.size()));
    Managed<Matrix<double>> E(shape, this->alignment());
    for (std::size_t p = 0; p < points.size(); ++p) {
        for (auto&& [bf, j] : enumerate(AllIntegerSums<D>(this->degree()))) {
            auto index = permute(permutation, make_index<index_t>(bf, p));
            E(index) = DubinerP(j, points[p]);
        }
    }
    return E;
}

template <std::size_t D>
Managed<Tensor<double, 3u>>
ModalRefElement<D>::evaluateGradientAt(std::vector<std::array<double, D>> const& points,
                                       std::array<unsigned, 3> const& permutation) const {
    using index_t = Matrix<double>::index_t;
    auto shape =
        permute(permutation, make_index<index_t>(this->numBasisFunctions(), D, points.size()));
    Managed<Tensor<double, 3u>> grad(shape, this->alignment());
    for (std::size_t p = 0; p < points.size(); ++p) {
        for (auto&& [bf, j] : enumerate(AllIntegerSums<D>(this->degree()))) {
            auto dphi = gradDubinerP(j, points[p]);
            for (std::size_t d = 0; d < D; ++d) {
                auto index = permute(permutation, make_index<index_t>(bf, d, p));
                grad(index) = dphi[d];
            }
        }
    }
    return grad;
}

template <std::size_t D>
NodalRefElement<D>::NodalRefElement(unsigned degree, NodesFactory<D> const& nodesFactory,
                                    std::size_t alignment)
    : RefElement<D>(degree, alignment), refNodes_(nodesFactory(degree)) {
    assert(this->numBasisFunctions() == refNodes_.size());
    vandermonde_ = Vandermonde(this->degree(), refNodes_);
    vandermondeInv_ = vandermonde_.inverse();
}

template <std::size_t D> Managed<Matrix<double>> NodalRefElement<D>::massMatrix() const {
    std::ptrdiff_t nbf = this->numBasisFunctions();
    Managed<Matrix<double>> M({nbf, nbf}, this->alignment());

    Managed<Matrix<double>> modalM =
        ModalRefElement<D>(this->degree(), this->alignment()).massMatrix();
    EigenMap(M) = vandermondeInv_.transpose() * EigenMap(modalM) * vandermondeInv_;

    return M;
}

template <std::size_t D> Managed<Matrix<double>> NodalRefElement<D>::inverseMassMatrix() const {
    std::ptrdiff_t nbf = this->numBasisFunctions();
    Managed<Matrix<double>> Minv({nbf, nbf}, this->alignment());

    Managed<Matrix<double>> modalMinv =
        ModalRefElement<D>(this->degree(), this->alignment()).inverseMassMatrix();
    EigenMap(Minv) = vandermonde_ * EigenMap(modalMinv) * vandermonde_.transpose();

    return Minv;
}

template <std::size_t D>
Managed<Matrix<double>>
NodalRefElement<D>::evaluateBasisAt(std::vector<std::array<double, D>> const& points,
                                    std::array<unsigned, 2> const& permutation) const {
    Managed<Matrix<double>> E =
        ModalRefElement<D>(this->degree(), this->alignment()).evaluateBasisAt(points, permutation);
    auto Emap = EigenMap(E);
    if (permutation[0] == 0 && permutation[1] == 1) {
        Emap = vandermondeInv_.transpose() * Emap;
    } else if (permutation[0] == 1 && permutation[1] == 0) {
        Emap = Emap * vandermondeInv_;
    } else {
        assert(false);
    }
    return E;
}

template <std::size_t D>
Managed<Tensor<double, 3u>>
NodalRefElement<D>::evaluateGradientAt(std::vector<std::array<double, D>> const& points,
                                       std::array<unsigned, 3> const& permutation) const {
    Managed<Tensor<double, 3u>> gradE = ModalRefElement<D>(this->degree(), this->alignment())
                                            .evaluateGradientAt(points, permutation);

    assert(vandermondeInv_.cols() == vandermondeInv_.rows());
    // 0,1,2 F_idq = V_ji E_jdq => F_i(dq) = V^T E_j(dq)
    // 0,2,1 F_iqd = V_ji E_jqd => F_i(qd) = V^T E_j(qd)
    if (permutation[0] == 0) {
        assert((permutation[1] == 1 && permutation[2] == 2) ||
               (permutation[1] == 2 && permutation[2] == 1));
        auto mat = reshape(gradE, vandermondeInv_.rows(), D * points.size());
        EigenMap(mat) = vandermondeInv_.transpose() * EigenMap(mat);
    }
    // 1,2,0 F_dqi = V_ji E_dqj => F_(dq)i = E_(dq)j V
    // 2,1,0 F_qdi = V_ji E_qdj => F_(qd)i = E_(qd)j V
    else if (permutation[2] == 0) {
        assert((permutation[0] == 1 && permutation[1] == 2) ||
               (permutation[0] == 2 && permutation[1] == 1));
        auto mat = reshape(gradE, D * points.size(), vandermondeInv_.rows());
        EigenMap(mat) = EigenMap(mat) * vandermondeInv_;
    }
    // 1,0,2 F_diq = V_ji E_djq => F_di[q] = E_dj[q] V
    // 2,0,1 F_qid = V_ji E_qjd => F_qi[d] = E_qj[d] V
    else {
        assert((permutation[0] == 1 && permutation[2] == 2) ||
               (permutation[0] == 2 && permutation[2] == 1));
        for (decltype(gradE)::index_t i = 0; i < gradE.shape(2); ++i) {
            auto mat = gradE.subtensor(slice{}, slice{}, i);
            EigenMap(mat) = EigenMap(mat) * vandermondeInv_;
        }
    }

    return gradE;
}

template class ModalRefElement<1ul>;
template class ModalRefElement<2ul>;
template class ModalRefElement<3ul>;
template class NodalRefElement<1ul>;
template class NodalRefElement<2ul>;
template class NodalRefElement<3ul>;

} // namespace tndm


#include "mex.h"
#include <math.h>
#include <Eigen/Dense>

using namespace std;

void constraints(mxArray* c_out, mxArray* ceq_out, mxArray* dc_out, mxArray* dceq_out, const Eigen::MatrixXd& x, size_t nc, size_t nceq, size_t nv, size_t nsteps)
{
  Eigen::MatrixXd steps = x.block(0, 0, 6, nsteps);
  Eigen::MatrixXd rel_steps = x.block(6, 0, 6, nsteps);

  Eigen::MatrixXd c(nc, 1);
  c = Eigen::MatrixXd::Zero(nc, 1);
  Eigen::VectorXd ceq(nceq);
  ceq = Eigen::VectorXd::Zero(nceq);
  Eigen::MatrixXd dc(nv, nsteps);
  dc = Eigen::MatrixXd::Zero(nv, nc);
  Eigen::MatrixXd dceq(nv, nceq);
  dceq = Eigen::MatrixXd::Zero(nv, nceq);
  Eigen::Matrix2d R;
  Eigen::Vector2d dxy;
  Eigen::Vector2d proj;
  Eigen::Vector2d u;
  Eigen::Vector2d al;

  // ceq.segment(0,2) = steps.block(0,0,2,1) - rel_steps.block(0,0,2,1);
  // dceq.block(0,0,2,2) << 1, 0, 0, 1;
  // dceq.block(6,0,2,2) << -1, 0, 0, -1;

  int j;
  int x1_ndx;
  int dx_ndx;
  int x2_ndx;
  int con_ndx;
  int con_dndx;
  double dx, dy, si, co;

  for (j = 2; j <= nsteps; j++) {
    con_ndx = (j-1)*2;
    con_dndx = 2;
    si = sin(steps(5,j-2));
    co = cos(steps(5,j-2));
    R << co, -si, si, co;
    dxy = R * rel_steps.block(0,j-1,2,1);
    proj = steps.block(0,j-2,2,1) + dxy;
    ceq.segment(con_ndx, con_dndx) = steps.block(0,j-1,2,1) - proj;
    x1_ndx = (j-2)*12;
    dx_ndx = (j-1)*12+6;
    x2_ndx = (j-1)*12;
    dceq.block(x2_ndx,con_ndx,2,con_dndx) << 1, 0, 0, 1;
    dceq.block(x1_ndx,con_ndx,2,con_dndx) << -1, 0, 0, -1;
    dx = rel_steps(0,j-1);
    dy = rel_steps(1,j-1);
    dceq.block(x1_ndx+5,con_ndx,1,con_dndx) << dx*si + dy*co, -dx*co + dy*si;
    dceq.block(dx_ndx,con_ndx,1,con_dndx) << -co, -si;
    dceq.block(dx_ndx+1,con_ndx,1,con_dndx) << si, -co;
  }

  memcpy(mxGetPr(c_out), c.data(), sizeof(double)*c.rows()*c.cols());
  memcpy(mxGetPr(dc_out), dc.data(), sizeof(double)*dc.rows()*dc.cols());
  memcpy(mxGetPr(ceq_out), ceq.data(), sizeof(double)*ceq.rows()*ceq.cols());
  memcpy(mxGetPr(dceq_out), dceq.data(), sizeof(double)*dceq.rows()*dceq.cols());

  return;
}

void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[] )
{
  // [c_mex, ceq_mex, dc_mex, dceq_mex] = stepCollocationConstraintsMex(x);

  size_t nv = mxGetM(prhs[0]);
  size_t nsteps = nv / 12;
  size_t nceq = 2 * nsteps;
  size_t nc = 0;

  Eigen::MatrixXd x = Eigen::Map<Eigen::MatrixXd>(mxGetPr(prhs[0]), 12, nsteps);

  /* Create matrices for the return arguments. */
  plhs[0] = mxCreateDoubleMatrix((mwSize)nc, (mwSize)1, mxREAL); // c
  plhs[1] = mxCreateDoubleMatrix((mwSize)nceq, (mwSize)1, mxREAL); // ceq
  plhs[2] = mxCreateDoubleMatrix((mwSize)nv, (mwSize)nc, mxREAL); // dc
  plhs[3] = mxCreateDoubleMatrix((mwSize)nv, (mwSize)nceq, mxREAL); // dceq

  /* Assign pointers to each output. */
  mxArray* c = plhs[0];
  mxArray* ceq = plhs[1];
  mxArray* dc = plhs[2];
  mxArray* dceq = plhs[3];

  constraints(c, ceq, dc, dceq, x, nc, nceq, nv, nsteps);
}


/*
Copyright (c) 2015 - 2016, Tianwei Shen
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* 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.

* Neither the name of libvot 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.

*/

/** \file svt.cpp
 *	\brief singular value threshold (immature)
 */
#include <iostream>
#include <vector>
#include <sstream>
#include <Eigen/Dense>
#include <random>

#include "utils/io_utils.h"

using namespace std;
using namespace Eigen;

float Fnorm(Eigen::MatrixXf &f)
{
	float norm = 0;
	for (int i = 0; i < f.rows(); i++) {
		for (int j = 0; j < f.cols(); j++) {
			norm += f(i, j) * f(i, j);
		}
	}
	return sqrt(norm);
}

double Fnorm(Eigen::MatrixXd &d)
{
	double norm = 0;
	for (int i = 0; i < d.rows(); i++) {
		for (int j = 0; j < d.cols(); j++) {
			norm += d(i, j) * d(i, j);
		}
	}
	return sqrt(norm);
}

// sample_set.size() = d.rows();
Eigen::MatrixXd MatrixProjection(Eigen::MatrixXd &d, std::vector<std::vector<int> > sample_set)
{
	Eigen::MatrixXd pd = Eigen::MatrixXd::Constant(d.rows(), d.cols(), 0);
	for (int i = 0; i < d.rows(); i++) {
		for (int j = 0; j < sample_set[i].size(); j++) {
			pd(i, sample_set[i][j]) = d(i, sample_set[i][j]);
		}
	}
	return pd;
}

int main(int argc, char ** argv)
{
	if (argc != 2) {
		cout << "Usage: " << argv[0] << " <mat_file>\n";
		exit(-1);
	}
	const char *mat_file = argv[1];

	// here we assume that the input matrix is a square matrix
	vector<string> mat_string;
	tw::IO::ExtractLines(mat_file, mat_string);
	int mat_size = mat_string.size();
	MatrixXd input_mat = MatrixXd::Constant(mat_size, mat_size, 0);
	for (int i = 0; i < mat_size; i++) {
		stringstream ss;
		ss << mat_string[i];
		float temp;
		for (int j = 0; j < mat_size; j++) {
			ss >> temp;
			input_mat(i, j) = temp;
		}
	}

	MatrixXd sample_mat = MatrixXd::Constant(mat_size, mat_size, 0);
	const float sample_ratio = 0.4;
	const int sample_size = mat_size * sample_ratio;
	vector<vector<int> > sample_set;
	sample_set.resize(mat_size);

	// sample without replacement
	default_random_engine e(0);
	uniform_int_distribution<int> uni_rand(0, mat_size-1);
	for (int i = 0; i < mat_size; i++) {
		for (int j = 0; j < sample_size; j++) {
			int curr_sample = uni_rand(e);
			int k;
			for (k = 0; k < j; k++) {
				if (curr_sample == sample_set[i][k]) {
					j--; k = -1;
					break;
				}
			}
			if (k != -1) {
				sample_set[i].push_back(curr_sample);
			}
		}
	}

	for (int i = 0; i < mat_size; i++) {
		for (int j = 0; j < sample_set[i].size(); j++) {
			sample_mat(i, sample_set[i][j]) = input_mat(i, sample_set[i][j]);
		}
	}

	cout << sample_mat << endl;

	// singular value thresholding
	float step_size = 1.5;//1.2 * mat_size / sample_ratio;
	float tau = 5 * mat_size;
	int k0 = tau / (step_size * Fnorm(sample_mat)) + 1;
	int max_iter = 200;
	MatrixXd Y = k0 * step_size * sample_mat;
	MatrixXd X;
	double stop_threshold = 1e-4;
	for (int i = 0; i < max_iter; i++) {
		JacobiSVD<MatrixXd> svd(Y, ComputeThinU | ComputeThinV);
		MatrixXd U = svd.matrixU();
		MatrixXd V = svd.matrixV();
		VectorXd singular_vector = svd.singularValues();
		MatrixXd S = MatrixXd::Constant(mat_size, mat_size, 0);
		for (int i = 0; i < mat_size; i++) {
			if (singular_vector[i] > tau) {
				S(i, i) = singular_vector[i] - tau;
			}
		}
		X = U * S * V.transpose();

		// compute error
		MatrixXd residual_mat = X - sample_mat;
		MatrixXd residual_proj = MatrixProjection(residual_mat, sample_set);
		double error = Fnorm(residual_proj);
		double sample_mat_norm = Fnorm(sample_mat);
		if (error / sample_mat_norm < stop_threshold) {
			cout << "break at iter " << i << endl;
			break;
		}

		// refresh Y
		for (int i = 0; i < sample_mat.rows(); i++) {
			for (int j = 0; j < sample_set[i].size(); j++) {
				Y(i, sample_set[i][j]) = Y(i, sample_set[i][j]) + step_size * (sample_mat(i, sample_set[i][j]) - X(i, sample_set[i][j]));
			}
		}
	}

	// compute completion error
	float completion_error = 0;
	float max_error = 0;
	float error_bound = 10;
	int within_bound_count = 0;
	for (int i = 0; i < mat_size; i++) {
		for (int j = 0; j < mat_size; j++) {
			completion_error += (X(i, j) - input_mat(i, j)) * (X(i, j) - input_mat(i, j));
			if (max_error < abs(X(i, j) - input_mat(i, j)) ) {
				max_error = abs(X(i, j) - input_mat(i, j));
			}
			if (abs(X(i, j) - input_mat(i, j)) < error_bound) {
				within_bound_count++;
			}
		}
	}
	MatrixXd error_mat = X - input_mat;
	float relative_error = Fnorm(error_mat) / Fnorm(input_mat);
	cout << "completion_error " << completion_error << endl;
	cout << "max_error " << max_error << endl;
	cout << "within_bound_count " << within_bound_count << endl;
	cout << "relative_error " << relative_error << endl;

	return 0;
}


#include <set>
using namespace std;

#include "Denoising.h"

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/IterativeLinearSolvers>

typedef Eigen::MatrixXd Matrix;
typedef Eigen::SparseMatrix<double> SpMat;
typedef Eigen::Triplet<double> Triplet;

bool operator<(const Vector3d& v1, const Vector3d& v2) {
    if(v1[0] != v2[0]) return v1[0] < v2[0];
    if(v1[1] != v2[1]) return v1[1] < v2[1];
    return v1[2] < v1[2];
}

bool operator>(const Vector3d& v1, const Vector3d& v2) {
    if(v1[0] != v2[0]) return v1[0] > v2[0];
    if(v1[1] != v2[1]) return v1[1] > v2[1];
    return v1[2] > v1[2];
}

double genrand_gauss(double mu, double sigma) {
    double z = sqrt(-2.0 * log(genrand_real2())) * sin(2.0 * M_PI * genrand_real2());
    return mu + sigma * z;
}

void addNoise(Mesh& mesh) {
    init_genrand((unsigned long)time(NULL));
    OpenMesh::VPropHandleT<Mesh::Point> vprop;
    mesh.add_property(vprop);

    Mesh::VertexIter v_it;
    for(v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); ++v_it) {
        double vx = genrand_gauss(0.0, 0.001);
        double vy = genrand_gauss(0.0, 0.001);
        double vz = genrand_gauss(0.0, 0.001);
        mesh.property(vprop, *v_it) = mesh.point(*v_it) + Mesh::Point(vx, vy, vz);
        mesh.set_point(*v_it, mesh.property(vprop, *v_it));
    }
    mesh.remove_property(vprop);
}

void denoise(Mesh& mesh) {
    // Prepare coefficient matrices R and D.
    Mesh::EdgeIter e_it;
    Mesh::FaceVertexIter fv_it;

    const int nVert = static_cast<int>(mesh.n_vertices());
    const int nEdge = static_cast<int>(mesh.n_edges());

    vector<Triplet> tripR;
    vector<Triplet> tripD;

    int ie = 0;
    for(e_it = mesh.edges_begin(); e_it != mesh.edges_end(); ++e_it) {
        Mesh::HalfedgeHandle heh = mesh.halfedge_handle(*e_it, 0);

        Mesh::VertexHandle vh1 = mesh.to_vertex_handle(heh);
        Mesh::VertexHandle vh3 = mesh.from_vertex_handle(heh);
        Mesh::VertexHandle vh2 = mesh.opposite_vh(heh);
        Mesh::VertexHandle vh4 = mesh.opposite_he_opposite_vh(heh);

        int i1 = vh1.idx();
        int i2 = vh2.idx();
        int i3 = vh3.idx();
        int i4 = vh4.idx();
        if(i1 == -1 || i2 == -1 || i3 == -1 || i4 == -1) {
            continue;
        }

        OpenMesh::Vec3f p1 = mesh.point(vh1);
        OpenMesh::Vec3f p2 = mesh.point(vh2);
        OpenMesh::Vec3f p3 = mesh.point(vh3);
        OpenMesh::Vec3f p4 = mesh.point(vh4);

        double S123 = 0.5 * OpenMesh::cross(p2 - p1, p2 - p3).norm();
        double S134 = 0.5 * OpenMesh::cross(p4 - p1, p4 - p3).norm();
        double l13  = (p1 - p3).norm();

        double coef1 = (S123 * OpenMesh::dot(p4 - p3, p3 - p1) + S134 * OpenMesh::dot(p1 - p3, p3 - p2)) / (l13 * l13 * (S123 + S134));
        double coef2 = S134 / (S123 + S134);
        double coef3 = (S123 * OpenMesh::dot(p3 - p1, p1 - p4) + S134 * OpenMesh::dot(p2 - p1, p1 - p3)) / (l13 * l13 * (S123 + S134));
        double coef4 = S123 / (S123 + S134);

        tripR.push_back(Triplet(ie, i1, coef1));
        tripR.push_back(Triplet(ie, i2, coef2));
        tripR.push_back(Triplet(ie, i3, coef3));
        tripR.push_back(Triplet(ie, i4, coef4));

        tripD.push_back(Triplet(ie, i1, 1.0));
        tripD.push_back(Triplet(ie, i2, -1.0));
        tripD.push_back(Triplet(ie, i3, 1.0));
        tripD.push_back(Triplet(ie, i4, -1.0));
        ie++;
    }

    SpMat R(nEdge, nVert);
    SpMat D(nEdge, nVert);
    R.setFromTriplets(tripR.begin(), tripR.end());
    D.setFromTriplets(tripD.begin(), tripD.end());

    // Store vertex positions in the Eigen::Vector
    Matrix pInit(nVert, 3);
    Matrix pVec(nVert, 3);
    Mesh::VertexIter v_it;
    int ip = 0;
    for(v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); ++v_it) {
        Mesh::Point p = mesh.point(*v_it);
        pInit(ip, 0) = p[0];
        pInit(ip, 1) = p[1];
        pInit(ip, 2) = p[2];

        pVec(ip, 0) = p[0];
        pVec(ip, 1) = p[1];
        pVec(ip, 2) = p[2];
        ip++;
    }

    // Prepare sparse matrix.
    vector<Triplet> tripI(nVert);
    for(int i=0; i<nVert; i++) {
        tripI[i] = Triplet(i, i, 1.0);
    }
    SpMat I(nVert, nVert);
    I.setFromTriplets(tripI.begin(), tripI.end());

    // Solve linear problem.
    double lambda = 1.0e-2;
    double alpha  = 0.1;
    double beta   = 1.0e-3;
    double bmax   = 1.0;
    double mu     = 2.0;

    // Matrix pVec = pInit;
    Matrix dlt(nEdge, 3);
    while(beta < bmax) {
        // solve sub-problem for delta (shrinkage operator)
        Matrix y = D * pVec;
        for(int i=0; i<nEdge; i++) {
            dlt(i, 0) = 0.0;
            for(int d=0; d<3; d++) {
                dlt(i, 0) += y(i, d) * y(i, d);
            }

            if(dlt(i, 0) < lambda / beta) {
                dlt(i, 0) = 0.0;
            }

            dlt(i, 1) = dlt(i, 0);
            dlt(i, 2) = dlt(i, 0);
        }

        // solve sub-problem for points (linear system)
        SpMat  A = I + alpha * R.transpose() * R + beta * D.transpose() * D;
        Matrix b = pInit + beta * D.transpose() * dlt;

        Eigen::ConjugateGradient<SpMat> solver;
        solver.compute(A);
        pVec = solver.solve(b);
        if(solver.info() != Eigen::Success) {
            printf("Solve linear system failed.\n");
        }

        // update parameters
        printf(".");
        beta  *= mu;
        alpha *= 0.5;
    }
    printf("\n");

    // Update vertex information_OBJMESH_H_
    OpenMesh::VPropHandleT<Mesh::Point> vprop;
    mesh.add_property(vprop);
    ip = 0;
    for(v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); ++v_it) {
        mesh.property(vprop, *v_it) = Mesh::Point(pVec(ip, 0), pVec(ip, 1), pVec(ip, 2));
        mesh.set_point(*v_it, mesh.property(vprop, *v_it));
        ip++;
    }
    mesh.remove_property(vprop);
}


#ifndef URASTER_HPP
#define URASTER_HPP

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/LU>	//for .inverse().  Probably not needed
#include <vector>
#include <array>
#include <memory>
#include <functional>

namespace uraster {

	class Pixel {
		public:
			Eigen::Vector4f color;
			float& depth() {
				return color[3];
			}
			Pixel():color(0.0f,0.0f,0.0f,-1e10f) {
			}
	};

	bool _wireframe = false;

	//This is the framebuffer class.  It's a part of namespace uraster because the uraster needs to have a well-defined image class to render to.
	//It is templated because the output type need not be only colors, could contain anything (like a stencil buffer or depth buffer or gbuffer for deferred rendering)
	template<class PixelType>
		class Framebuffer {
			protected:
				std::vector<PixelType> data;
			public:
				const std::size_t width;
				const std::size_t height;
				//constructor initializes the array
				Framebuffer(std::size_t w,std::size_t h):
					data(w*h),
					width(w),height(h) {
					}
				//2D pixel access
				PixelType& operator()(std::size_t x,std::size_t y) {
					return data[y*width+x];
				}
				//const version
				const PixelType& operator()(std::size_t x,std::size_t y) const {
					return data[y*width+x];
				}
				void clear(const PixelType& pt=PixelType()) {
					std::fill(data.begin(),data.end(),pt);
				}
		};

	//This function runs the vertex shader on all the vertices, producing the varyings that will be interpolated by the rasterizer.
	//VertexVsIn can be anything, VertexVsOut MUST have a position() method that returns a 4D vector, and it must have an overloaded *= and += operator for the interpolation
	//The right way to think of VertexVsOut is that it is the class you write containing the varying outputs from the vertex shader.
	template<class VertexVsIn,class VertexVsOut,class VertShader>
		void run_vertex_shader(const VertexVsIn* b,const VertexVsIn* e,VertexVsOut* o,
				VertShader vertex_shader) {
			std::size_t n=e-b;
#pragma omp parallel for
			for(std::size_t i=0; i<n; i++) {
				o[i]=vertex_shader(b[i]);
			}
		}
	struct BarycentricTransform {
		private:
			Eigen::Vector2f offset;
			Eigen::Matrix2f Ti;
		public:
			BarycentricTransform(const Eigen::Vector2f& s1,const Eigen::Vector2f& s2,const Eigen::Vector2f& s3):
				offset(s3) {
					Eigen::Matrix2f T;
					T << (s1-s3),(s2-s3);
					Ti=T.inverse();
				}
			Eigen::Vector3f operator()(const Eigen::Vector2f& v) const {
				Eigen::Vector2f b;
				b=Ti*(v-offset);
				return Eigen::Vector3f(b[0],b[1],1.0f-b[0]-b[1]);
			}
	};

	void line(int x0, int y0, int x1, int y1, uraster::Framebuffer<uraster::Pixel>& fp, Eigen::Array2i ul, Eigen::Array2i lr)
	{
		int dx =  abs(x1-x0), sx = x0<x1 ? 1 : -1;
		int dy = -abs(y1-y0), sy = y0<y1 ? 1 : -1;
		int err = dx+dy, e2; /* error value e_xy */
		uraster::Pixel p;
		p.color = Eigen::Vector4f(1, 1, 1, 1);

		while(1) {
			if (x0 < ul[0]) x0 = ul[0];
			if (x0 >= lr[0]) x0 = lr[0]-1;
			if (y0 < ul[1]) y0 = ul[1];
			if (y0 >= lr[1]) y0 = lr[1]-1;
			fp(x0, y0) = p;
			//fp(x0+1, y0) = p;
			//fp(x0, y0+1) = p;
			//fp(x0+1, y0+1) = p;
			//fp(x0-1, y0) = p;
			//fp(x0, y0-1) = p;
			//fp(x0-1, y0-1) = p;
			if (x0==x1 && y0==y1) break;
			e2 = 2*err;
			if (e2 > dy) { err += dy; x0 += sx; } /* e_xy+e_x > 0 */
			if (e2 < dx) { err += dx; y0 += sy; } /* e_xy+e_y < 0 */
		}
	}

	//This function takes in 3 varyings vertices from the vertex shader that make up a triangle,
	//rasterizes the triangle and runs the fragment shader on each resulting pixel.
	template<class PixelOut,class VertexVsOut,class FragShader>
		void rasterize_triangle(Framebuffer<PixelOut>& fb,const std::array<VertexVsOut,3>& verts,FragShader fragment_shader) {
			std::array<Eigen::Vector4f,3> points {{verts[0].position(),verts[1].position(),verts[2].position()}};
			//Do the perspective divide by w to get screen space coordinates.
			std::array<Eigen::Vector4f,3> epoints {{points[0]/points[0][3],points[1]/points[1][3],points[2]/points[2][3]}};
			auto ss1=epoints[0].head<2>().array(),ss2=epoints[1].head<2>().array(),ss3=epoints[2].head<2>().array();

			//calculate the bounding box of the triangle in screen space floating point.
			Eigen::Array2f bb_ul=ss1.min(ss2).min(ss3);
			Eigen::Array2f bb_lr=ss1.max(ss2).max(ss3);
			Eigen::Array2i isz(fb.width,fb.height);

			//convert bounding box to fixed point.
			//move bounding box from (-1.0,1.0)->(0,imgdim)
			Eigen::Array2i ibb_ul=((bb_ul*0.5f+0.5f)*isz.cast<float>()).cast<int>();
			Eigen::Array2i ibb_lr=((bb_lr*0.5f+0.5f)*isz.cast<float>()).cast<int>();
			ibb_lr+=1;	//add one pixel of coverage

			//clamp the bounding box to the framebuffer size if necessary
			ibb_ul=ibb_ul.max(Eigen::Array2i(0,0));
			ibb_lr=ibb_lr.min(isz);

			BarycentricTransform bt(ss1.matrix(),ss2.matrix(),ss3.matrix());

				static uraster::Framebuffer<uraster::Pixel> tp(fb.width, fb.height); // mask pixels to draw
			if (_wireframe) {

			int x001 = 0, x010 = 0, x100 = 0;
			int y001 = 0, y010 = 0, y100 = 0;
			Eigen::Vector2f ex_001(1.0, -1.0), ex_010(1.0, -1.0), ex_100(1.0, -1.0);
			// find the extremities in barycentric coordinates of each corner of the
			// vertex in viewport coordinates

			bool visible = false, set_1 = false, set_2 = false, set_3 = false;
			// tell the fragment shader to only draw these in $color
			for(int y=ibb_ul[1]; y<ibb_lr[1]; y++) {
				for(int x=ibb_ul[0]; x<ibb_lr[0]; x++) {
					Eigen::Vector2f ssc(x, y);
					ssc.array()/=isz.cast<float>();	//move pixel to relative coordinates
					ssc.array()-=0.5f;
					ssc.array()*=2.0f;

					//Compute barycentric coordinates of the pixel center
					Eigen::Vector3f bary=bt(ssc);

					//if the pixel has valid barycentric coordinates, the pixel is in the triangle
					if(!((bary.array() <= 1.0f).all() && (bary.array() >= 0.0f).all())) {
						continue;
					}
					visible = true;

					if (bary[0] + bary[1] < ex_001[0] || bary[2] > ex_001[1]) {
					//if (bary[2] > ex_001[1]) {
						ex_001[0] = bary[0] + bary[1];
						ex_001[1] = bary[2];
						x001 = x;
						y001 = y;
						set_1 = true;
					}
					if (bary[0] + bary[2] < ex_010[0] || (bary[1] > ex_010[1])) {
					//if (bary[1] > ex_010[1]) {
						ex_010[0] = bary[0] + bary[2];
						ex_010[1] = bary[1];
						x010 = x;
						y010 = y;
						set_2 = true;
					}
					if (bary[1] + bary[2] < ex_100[0] || (bary[0] > ex_100[1])) {
					//if (bary[0] > ex_100[1]) {
						ex_100[0] = bary[1] + bary[2];
						ex_100[1] = bary[0];
						x100 = x;
						y100 = y;
						set_3 = true;
					}
				}
			}

				tp.clear();
				if (visible && set_1 && set_2 && set_3) {
					line(x001, y001, x010, y010, tp, ibb_ul, ibb_lr);
					line(x010, y010, x100, y100, tp, ibb_ul, ibb_lr);
					line(x100, y100, x001, y001, tp, ibb_ul, ibb_lr);
				}
					}
				//

				//std::cout << coords.size() << " ";

				// calculate all display coordinates of projected edges of the triangle

				//std::cout << ex_001 << std::endl << ex_010 << std::endl << ex_100 << std::endl;
				//exit(1);
				auto a = verts[2].p - verts[1].p;
				auto b = verts[1].p - verts[0].p;
				Eigen::Vector3f c(a[0], a[1], a[2]);
				Eigen::Vector3f d(b[0], b[1], b[2]);
				auto normal = c.cross(d).normalized();

				//for all the pixels in the bounding box
				for(int y=ibb_ul[1]; y<ibb_lr[1]; y++) {
					for(int x=ibb_ul[0]; x<ibb_lr[0]; x++) {
						Eigen::Vector2f ssc(x,y);
						ssc.array()/=isz.cast<float>();	//move pixel to relative coordinates
						ssc.array()-=0.5f;
						ssc.array()*=2.0f;

						//Compute barycentric coordinates of the pixel center
						Eigen::Vector3f bary=bt(ssc);

						//if the pixel has valid barycentric coordinates, the pixel is in the triangle
						if(!((bary.array() < 1.0f).all() && (bary.array() > 0.0f).all())) {
							continue;
						}
						float d=bary[0]*epoints[0][2]+bary[1]*epoints[1][2]+bary[2]*epoints[2][2];
						//Reference the current pixel at that coordinate
						PixelOut& po=fb(x,y);
						// if the interpolated depth passes the depth test
						if(po.depth() >= d || d > 1.0) {
							continue;
						}
						//interpolate varying parameters
						VertexVsOut v;
						for(int i=0; i<3; i++) {
							VertexVsOut vt=verts[i];
							vt*=bary[i];
							v+=vt;
						}
						if (_wireframe) {
						if (tp(x, y).color[0] == 1) {
							v.p[3] = 0.0;
						} else {
							v.p[3] = 0.5;
						}
						}
						v.p[0] = normal[0];
						v.p[1] = normal[1];
						v.p[2] = normal[2];
						//call the fragment shader
						po=fragment_shader(v);
						po.depth()=d; //write the depth buffer
					}
				}
					}

					//This function rasterizes a set of triangles determined by an index buffer and a buffer of output verts.
					template<class PixelOut,class VertexVsOut,class FragShader>
						void rasterize(Framebuffer<PixelOut>& fb,const std::size_t* ib,const std::size_t* ie,const VertexVsOut* verts,
								FragShader fragment_shader) {
							std::size_t n=ie-ib;
#pragma omp parallel for
							for(std::size_t i=0; i<n; i+=3) {
								const std::size_t* ti=ib+i;
								std::array<VertexVsOut,3> tri {{verts[ti[0]],verts[ti[1]],verts[ti[2]]}};
								rasterize_triangle(fb,tri,fragment_shader);
							}
						}

					//This function does a draw call from an indexed buffer
					template<class PixelOut,class VertexVsOut,class VertexVsIn,class VertShader, class FragShader>
						void draw(Framebuffer<PixelOut>& fb,
								const VertexVsIn* vertexbuffer_b,const VertexVsIn* vertexbuffer_e,
								const std::size_t* indexbuffer_b,const std::size_t* indexbuffer_e,
								VertexVsOut* vcache_b,VertexVsOut* vcache_e,
								VertShader vertex_shader,
								FragShader fragment_shader, bool wireframe = false) {
							std::unique_ptr<VertexVsOut[]> vc;
							_wireframe = wireframe;
							if(vcache_b==NULL || (vcache_e-vcache_b) != (vertexbuffer_e-vertexbuffer_b)) {
								vcache_b=new VertexVsOut[(vertexbuffer_e-vertexbuffer_b)];
								vc.reset(vcache_b);
							}
							run_vertex_shader(vertexbuffer_b,vertexbuffer_e,vcache_b,vertex_shader);
							rasterize(fb,indexbuffer_b,indexbuffer_e,vcache_b,fragment_shader);
						}

					struct VertVsOut {
						Eigen::Vector4f p;
						Eigen::Vector3f color;

						VertVsOut():
							p(0.0f,0.0f,0.0f,0.0f),color(0.0f,0.0f,0.0f) {
							}
						const Eigen::Vector4f& position() const {
							return p;
						}
						VertVsOut& operator+=(const VertVsOut& tp) {
							p+=tp.p;
							color+=tp.color;
							return *this;
						}
						VertVsOut& operator*=(const float& f) {
							p*=f;
							color*=f;
							return *this;
						}
					};

					//VertVsOut example_vertex_shader(const Eigen::Vector3f& vin,const Eigen::Matrix4f& mvp,float t) {
					//	VertVsOut vout;
					//	vout.p=mvp*Eigen::Vector4f(vin[0],vin[1],vin[2],1.0f);
					//	//vout.p[3]=1.0f;
					//	vout.color=Eigen::Vector3f(1.0f,static_cast <float> (rand()) / static_cast <float> (RAND_MAX),0.0f);
					//	return vout;
					//}

					//Pixel example_fragment_shader(const VertVsOut& fsin) {
					//	Pixel p;
					//	p.color.head<3>()=fsin.color;
					//	return p;
					//}
					// example end

				}

#endif


#include "rotation.h"
#include "himan_common.h"
#include <Eigen/Geometry>

using namespace Eigen;

template <typename T>
void himan::geoutil::rotate(himan::geoutil::position<T>& p, const himan::geoutil::rotation<T>& r)
{
	// Map data structures to Eigen library objects
	Map<Matrix<T, 3, 1>> P(p.Data());
	Map<const Quaternion<T>> QR(r.Data());

	// Create a corresponding quaternion of the input position vector
	Quaternion<T> QP;
	QP.w() = 0;
	QP.vec() = P;

	// Apply spatial rotation through quaternion products
	Quaternion<T> rotatedP = QR * QP * QR.inverse();
	P = rotatedP.vec();
}
template void himan::geoutil::rotate<float>(himan::geoutil::position<float>&, const himan::geoutil::rotation<float>&);
template void himan::geoutil::rotate<double>(himan::geoutil::position<double>&,
                                             const himan::geoutil::rotation<double>&);

template <typename T>
himan::geoutil::position<T> himan::geoutil::rotate(const himan::geoutil::position<T>& p,
                                                   const himan::geoutil::rotation<T>& r)
{
	position<T> ret(p);
	rotate(ret, r);
	return ret;
}
template himan::geoutil::position<float> himan::geoutil::rotate<float>(const himan::geoutil::position<float>&,
                                                                       const himan::geoutil::rotation<float>&);
template himan::geoutil::position<double> himan::geoutil::rotate<double>(const himan::geoutil::position<double>&,
                                                                         const himan::geoutil::rotation<double>&);

template <typename T>
himan::geoutil::rotation<T> himan::geoutil::rotation<T>::FromRotLatLon(const T& latOfSouthPole, const T& lonOfSouthPole,
                                                                       const T& angleOfRot)
{
	himan::geoutil::rotation<T> ret;

	// Map data structures to Eigen library objects
	Map<Quaternion<T>> QRot(ret.Data());

	// Create a rotation quaternion from product of a series of rotations about principle axis
	QRot = AngleAxis<T>(lonOfSouthPole, Matrix<T, 3, 1>::UnitZ()) *
	       AngleAxis<T>(-(T(M_PI / 2.0) + latOfSouthPole), Matrix<T, 3, 1>::UnitY()) *
	       AngleAxis<T>(-angleOfRot, Matrix<T, 3, 1>::UnitZ());

	return ret;
}
template himan::geoutil::rotation<float> himan::geoutil::rotation<float>::FromRotLatLon(const float& latOfSouthPole,
                                                                                const float& lonOfSouthPole,
                                                                                const float& angleOfRot);
template himan::geoutil::rotation<double> himan::geoutil::rotation<double>::FromRotLatLon(const double& latOfSouthPole,
                                                                                const double& lonOfSouthPole,
                                                                                const double& angleOfRot);

template <typename T>
himan::geoutil::rotation<T> himan::geoutil::rotation<T>::ToRotLatLon(const T& latOfSouthPole, const T& lonOfSouthPole,
                                                                     const T& angleOfRot)
{
	himan::geoutil::rotation<T> ret;
	// Map data structures to Eigen library objects
	Map<Quaternion<T>> QRot(ret.Data());

	// Create a rotation quaternion from product of a series of rotations about principle axis
	QRot = AngleAxis<T>(angleOfRot, Matrix<T, 3, 1>::UnitZ()) *
	       AngleAxis<T>(T(M_PI / 2.0) + latOfSouthPole, Matrix<T, 3, 1>::UnitY()) *
	       AngleAxis<T>(-lonOfSouthPole, Matrix<T, 3, 1>::UnitZ());

	return ret;
}
template himan::geoutil::rotation<float> himan::geoutil::rotation<float>::ToRotLatLon(const float& latOfSouthPole,
                                                                                const float& lonOfSouthPole,
                                                                                const float& angleOfRot);
template himan::geoutil::rotation<double> himan::geoutil::rotation<double>::ToRotLatLon(const double& latOfSouthPole,
                                                                                const double& lonOfSouthPole,
                                                                                const double& angleOfRot);


#ifndef _BAYES_FILTER__MODELS
#define _BAYES_FILTER__MODELS

/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2002 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id: models.hpp 634 2010-08-15 16:39:44Z mistevens $
 */

/*
 * Predict and Observe models
 *  These models extend, adapt and simplify the fundamental Bayesian filter models
 *  Simple : Simplify model construction and use
 *  General: Generalise a model so it include properties of more then one model
 *  Adapted: Adapt one model type into another
 */
#include <boost/function.hpp>

/* Filter namespace */
namespace Bayesian_filter
{

typedef boost::function1<const FM::Vec&, const FM::Vec&> State_function;
// A generalised function of state. Compatible with predict and observe models

class Simple_additive_predict_model : public Additive_predict_model
// Additive predict model initialised from function and model matricies
{
	State_function ff;
public:
	Simple_additive_predict_model (State_function f_init, const FM::Matrix& G_init, const FM::Vec& q_init);
	// Precondition: G, q are conformantly dimensioned (not checked)

	// No default assignment operator

	virtual const FM::Vec& f(const FM::Vec& x) const
	{	return ff(x);
	}
};

class Simple_linrz_predict_model : public Linrz_predict_model
// Linrz predict model initialised from function and model matrices
{
	State_function ff;
public:
	Simple_linrz_predict_model (State_function f_init, const FM::Matrix& Fx_init, const FM::Matrix& G_init, const FM::Vec& q_init);
	// Precondition: Fx, G, q are conformantly dimensioned (not checked)

	// No default assignment operator

	virtual const FM::Vec& f(const FM::Vec& x) const
	{	return ff(x);
	}
};

class Simple_linear_predict_model : public Linear_predict_model
// Linear predict model initialised from model matricies
{
public:
	Simple_linear_predict_model (const FM::Matrix& Fx_init, const FM::Matrix& G_init, const FM::Vec& q_init);
	// Precondition: Fx, q and G are conformantly dimensioned (not checked)
};

class Simple_linrz_correlated_observe_model : public Linrz_correlated_observe_model
// Linrz observe model initialised from function and model matrices
{
	State_function ff;
public:
	Simple_linrz_correlated_observe_model (State_function f_init, const FM::Matrix& Hx_init, const FM::SymMatrix& Z_init);
	// Precondition: Hx, Z are conformantly dimensioned (not checked)
	// No default assignment operator

	virtual const FM::Vec& h(const FM::Vec& x) const
	{	return ff(x);
	}
};

class Simple_linrz_uncorrelated_observe_model : public Linrz_uncorrelated_observe_model
// Linrz observe model initialised from function and model matrices
{
	State_function ff;
public:
	Simple_linrz_uncorrelated_observe_model (State_function f_init, const FM::Matrix& Hx_init, const FM::Vec& Zv_init);
	// Precondition: Hx, Zv are conformantly dimensioned (not checked)
	// No default assignment operator

	virtual const FM::Vec& h(const FM::Vec& x) const
	{	return ff(x);
	}
};

class Simple_linear_correlated_observe_model : public Linear_correlated_observe_model
// Linear observe model initialised from model matrices
{
public:
	Simple_linear_correlated_observe_model (const FM::Matrix& Hx_init, const FM::SymMatrix& Z_init);
	// Precondition: Hx, Z are conformantly dimensioned (not checked)
};

class Simple_linear_uncorrelated_observe_model : public Linear_uncorrelated_observe_model
// Linear observe model initialised from model matrices
{
public:
	Simple_linear_uncorrelated_observe_model (const FM::Matrix& Hx_init, const FM::Vec& Zv_init);
	// Precondition: Hx, Zv are conformantly dimensioned (not checked)
};

/*
 * Model Adaptors: Constructed with a reference to another model
 */

class Adapted_Correlated_additive_observe_model : public Correlated_additive_observe_model
/*
 * Adapt Uncorrelated_additive_observe_model to an equivalent
 * Correlated_additive_observe_model_adaptor
 */
{
public:
	Adapted_Correlated_additive_observe_model (Uncorrelated_additive_observe_model& adapt);
	const FM::Vec& h(const FM::Vec& x) const
	{
		return unc.h(x);
	}
	inline void normalise (FM::Vec& z_denorm, const FM::Vec& z_from) const
	{
		unc.normalise (z_denorm, z_from);
	};
private:
	Uncorrelated_additive_observe_model& unc;
};

class Adapted_Linrz_correlated_observe_model : public Linrz_correlated_observe_model
/*
 * Adapt Linrz_uncorrelated_observe_model to an equivalent
 * Linrz_correlated_observe_model
 */
{
public:
	Adapted_Linrz_correlated_observe_model (Linrz_uncorrelated_observe_model& adapt);
	const FM::Vec& h(const FM::Vec& x) const
	{
		return unc.h(x);
	}
	inline void normalise (FM::Vec& z_denorm, const FM::Vec& z_from) const
	{
		unc.normalise (z_denorm, z_from);
	};
protected:
	Linrz_uncorrelated_observe_model& unc;
};

/*
 * Generalised Models: generalise a model so it include properties of more then one model.
 */

// General Linearised Uncorrelated Additive and Likelihood observe model
class General_LzUnAd_observe_model : public Linrz_uncorrelated_observe_model, public Likelihood_observe_model
{
public:
	General_LzUnAd_observe_model (std::size_t x_size, std::size_t z_size) :
		Linrz_uncorrelated_observe_model(x_size, z_size),
		Likelihood_observe_model(z_size),
		li(z_size)
	{}
	virtual Float L(const FM::Vec& x) const
	// Definition of likelihood for additive noise model given zz
	{	return li.L(*this, z, h(x));
	}
	virtual void Lz (const FM::Vec& zz)
	// Fix the observation zz about which to evaluate the Likelihood function
	// Zv is also fixed
	{	Likelihood_observe_model::z = zz;
		li.Lz(*this);
	}
private:
	friend class General_LiUnAd_observe_model;
	struct Likelihood_uncorrelated
	{
		Likelihood_uncorrelated(std::size_t z_size) :
			zInnov(z_size), Zv_inv(z_size)
		{	zset = false;
		}
		mutable FM::Vec zInnov;	// Normailised innovation, temporary for L(x)
		FM::Vec Zv_inv;			// Inverse Noise Covariance given zz
		Float logdetZ;			// log(det(Z))
		bool zset;
		Float L(const Uncorrelated_additive_observe_model& model, const FM::Vec& z, const FM::Vec& zp) const;
		// Definition of likelihood for additive noise model given zz
		void Lz(const Uncorrelated_additive_observe_model& model);
	};
	Likelihood_uncorrelated li;
};

// General Linear Uncorrelated Additive and Likelihood observe model
class General_LiUnAd_observe_model : public Linear_uncorrelated_observe_model, public Likelihood_observe_model
{
public:
	General_LiUnAd_observe_model (std::size_t x_size, std::size_t z_size) :
		Linear_uncorrelated_observe_model(x_size, z_size),
		Likelihood_observe_model(z_size),
		li(z_size)
	{}
	virtual Float L(const FM::Vec& x) const
	// Definition of likelihood for additive noise model given zz
	{	return li.L(*this, z, h(x));
	}
	virtual void Lz (const FM::Vec& zz)
	// Fix the observation zz about which to evaluate the Likelihood function
	// Zv is also fixed
	{	Likelihood_observe_model::z = zz;
		li.Lz(*this);
	}

private:
	General_LzUnAd_observe_model::Likelihood_uncorrelated li;
};

// General Linearised Correlated Additive and Likelihood observe model
class General_LzCoAd_observe_model : public Linrz_correlated_observe_model, public Likelihood_observe_model
{
public:
	General_LzCoAd_observe_model (std::size_t x_size, std::size_t z_size) :
		Linrz_correlated_observe_model(x_size, z_size),
		Likelihood_observe_model(z_size),
		li(z_size)
	{}
	virtual Float L(const FM::Vec& x) const
	// Definition of likelihood for additive noise model given zz
	{	return li.L(*this, z, h(x));
	}
	virtual void Lz (const FM::Vec& zz)
	// Fix the observation zz about which to evaluate the Likelihood function
	// Zv is also fixed
	{	Likelihood_observe_model::z = zz;
		li.Lz(*this);
	}

private:
	friend class General_LiCoAd_observe_model;
	struct Likelihood_correlated
	{
		Likelihood_correlated(std::size_t z_size) :
			zInnov(z_size), Z_inv(z_size,z_size)
		{	zset = false;
		}
		mutable FM::Vec zInnov;	// Normalised innovation, temporary for L(x)
		FM::SymMatrix Z_inv;	// Inverse Noise Covariance
		Float logdetZ;			// log(det(Z)
		bool zset;
		static Float scaled_vector_square(const FM::Vec& v, const FM::SymMatrix& V);
		Float L(const Correlated_additive_observe_model& model, const FM::Vec& z, const FM::Vec& zp) const;
		// Definition of likelihood for additive noise model given zz
		void Lz(const Correlated_additive_observe_model& model);
	};
	Likelihood_correlated li;
};

// General Linear Correlated Additive and Likelihood observe model
class General_LiCoAd_observe_model : public Linear_correlated_observe_model, public Likelihood_observe_model
{
public:
	General_LiCoAd_observe_model (std::size_t x_size, std::size_t z_size) :
		Linear_correlated_observe_model(x_size, z_size),
		Likelihood_observe_model(z_size),
		li(z_size)
	{}
	virtual Float L(const FM::Vec& x) const
	// Definition of likelihood for additive noise model given zz
	{	return li.L(*this, z, h(x));
	}
	virtual void Lz (const FM::Vec& zz)
	// Fix the observation zz about which to evaluate the Likelihood function
	// Zv is also fixed
	{	Likelihood_observe_model::z = zz;
		li.Lz(*this);
	}

private:
	General_LzCoAd_observe_model::Likelihood_correlated li;
};

}// namespace

#endif


#include <k52/optimization/hleborodov_rosenbrock_method.h>

#ifdef BUILD_WITH_MPI

#include <boost/mpi.hpp>
#include <k52/parallel/mpi/constants.h>

#endif

#include <cmath>
#include <stdexcept>
#include <algorithm>
#include <fstream>
#include <iostream>

#include <k52/optimization/params/i_continuous_parameters.h>

using ::std::vector;

namespace
{
const double kArgumentsIncreaseValue = 1.5;
const double kArgumentsDecreaseValue = -0.5;
}

namespace k52
{
namespace optimization
{

HleborodovRosenbrockMethod::HleborodovRosenbrockMethod(double precision, size_t max_iteration_number,
    double first_step, double max_step)
    : precision_(precision)
    , arguments_increase_(kArgumentsIncreaseValue)
    , arguments_decrease_(kArgumentsDecreaseValue)
    , first_step_(first_step)
    , max_step_(max_step)
    , max_iteration_number_(max_iteration_number)
{
}

HleborodovRosenbrockMethod* HleborodovRosenbrockMethod::Clone() const
{
    return new HleborodovRosenbrockMethod(precision_, max_iteration_number_, first_step_, max_step_);
}

std::string HleborodovRosenbrockMethod::get_name() const
{
    return "Hleborodov Rosenbrock Method";
}

#ifdef BUILD_WITH_MPI
void HleborodovRosenbrockMethod::Send(boost::mpi::communicator* communicator, int target) const
{
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, precision_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, first_step_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, max_step_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, max_iteration_number_);
}

void HleborodovRosenbrockMethod::Receive(boost::mpi::communicator* communicator, int source)
{
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, precision_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, first_step_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, max_step_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, max_iteration_number_);
}
#endif

vector<double> HleborodovRosenbrockMethod::FindOptimalParameters(const vector<double>& initial_parameters)
{
    vector<double> arguments = initial_parameters;
    dimension_ = arguments.size();
    vector<double> next_step_arguments(arguments);

    InitializeBasisAndSteps();

    for (size_t i = 1; i <= max_iteration_number_; i++)
    {
        // Research of next arguments, that minimize function
        MakeStep(next_step_arguments);

        if (IsExitCriteriaFulfilled(next_step_arguments, arguments))
        {
            break;
        }

        // Check number of iterations
        if (i == max_iteration_number_)
        {
            std::cout << " Minimun has not found";
        }

        // Make new basis by Gramm-Shmidt method
        CreateNewBasis();
        arguments = next_step_arguments;
    }

    return next_step_arguments;
}

void HleborodovRosenbrockMethod::InitializeBasisAndSteps()
{
    steps_array_ = vector<double> (dimension_,first_step_);

    basis_ = vector< vector<double> > (dimension_, vector<double> (dimension_, 0));

    // Initialize basis is Decart coordinate system
    for(size_t i = 0; i<dimension_; i++)
    {
        basis_[i][i] = 1;
    }
}

bool HleborodovRosenbrockMethod::IsExitCriteriaFulfilled(
    const vector<double>& arguments,
    const vector<double>& previous_step_arguments)
{
    bool escape = true;
    // check arguments increases to escape
    for(size_t j = 0; j<dimension_; j++)
    {
        if(std::abs(arguments[j] - previous_step_arguments[j]) > precision_)
        {
            escape = false;
        }
    }
    return escape;
}

void HleborodovRosenbrockMethod::MakeStep(vector<double> &arguments)
{
    double start_function_value = CountObjectiveFunctionValueToMinimize(arguments);

    vector<double> tested_arguments(arguments);
    vector<double> tested_arguments2(arguments);

    /// Sort steps_array_ by descending for beter method speed
    std::sort(steps_array_.begin(), steps_array_.end(), std::greater<double>());

    for(size_t i = 0; i<dimension_; i++)
    {
        // make test step to minimize function in current direction
        // make it by each of coordinates
        MakeStepPerCoordinate(i, &tested_arguments);

        double counted_value = CountObjectiveFunctionValueToMinimize(tested_arguments);

        if(start_function_value > counted_value)
        {
            steps_array_[i] *= arguments_increase_;
        }
        else
        {
            steps_array_[i] *= arguments_decrease_;
        }

        CorrectStep(i);

        MakeStepPerCoordinate(i, &tested_arguments2);

        tested_arguments = arguments;
    }

    arguments = tested_arguments2;
}

void HleborodovRosenbrockMethod::MakeStepPerCoordinate(int basis_index, vector<double>* arguments)
{
    for(size_t j = 0; j<dimension_; j++)
    {
        (*arguments)[j] += steps_array_[basis_index]*basis_[basis_index][j];
    }
}

void HleborodovRosenbrockMethod::CreateNewBasis()
{
    vector< vector<double> > matrix_of_coeff(dimension_, vector<double> (dimension_, 0));
    vector< vector<double> > unnormalized_basis(dimension_, vector<double> (dimension_, 0));

    for(size_t j = 0; j<dimension_; j++)
    {
        for(size_t i = j; i<dimension_; i++)
        {
            for(size_t k = 0; k<dimension_; k++)
            {
                matrix_of_coeff[j][k] += steps_array_[i]*basis_[i][k];
            }
        }
    }

    for(size_t i = 0; i<dimension_; i++)
    {
        for(size_t j = 0; j<dimension_; j++)
        {
            unnormalized_basis[i][j] = matrix_of_coeff[i][j];
        }
        basis_[0][i] = matrix_of_coeff[0][i];
    }

    NormalizeVector(basis_[0]);
    double temp;

    for(size_t j = 1; j<dimension_; j++)
    {
        for(size_t i = 0; i<j; i++)
        {
            temp = ScalarComposition(matrix_of_coeff[j],basis_[i]);
            for(size_t k = 0; k<dimension_; k++)
            {
                unnormalized_basis[j][k] -= temp*basis_[i][k];
            }
        }

        for(size_t k = 0; k<dimension_; k++)
        {
            basis_[j][k] = unnormalized_basis[j][k];
        }
        NormalizeVector(basis_[j]);
    }
}

void HleborodovRosenbrockMethod::NormalizeVector(vector<double> &target_vector)
{
    double temp = 0;
    for(size_t i = 0; i<dimension_; i++)
    {
        temp += target_vector[i]*target_vector[i];
    }
    temp = sqrt(temp);
    for(size_t i = 0; i<dimension_; i++)
    {
        if(temp != 0)
        {
            target_vector[i] /= temp;
        }
    }
}

double HleborodovRosenbrockMethod::ScalarComposition(
    const vector<double>& first_vector,
    const vector<double>& second_vector)
{
    double scalar_composition = 0;
    for(size_t i = 0; i<dimension_; i++)
    {
        scalar_composition += first_vector[i]*second_vector[i];
    }
    return scalar_composition;
}

void HleborodovRosenbrockMethod::CorrectStep(size_t step_index)
{
    if( std::abs(steps_array_[step_index]) > max_step_)
    {
        steps_array_[step_index] =
            max_step_*steps_array_[step_index]/std::abs(steps_array_[step_index]);
    }
}

}/* namespace optimization */
}/* namespace k52 */


#include <iostream>
#include <cmath>
#include <boost/make_shared.hpp>
#include <problem/solver.h>
#include <communicators/mpicom.h>
#include <communicators/localcom.h>
#include <problem/subtasks.h>
#include <fstream>
#include <getopt.h>
#include <cstring>
#include <omp.h>

Solver::Solver(int& argc, char**& argv)
    : xSplit_(0)
    , ySplit_(0)
    , normOfDifference_(0)
    , maxDifference_(0)
    , iterations_(0)
    , elapsedTime_(0)
    , dump_(false)
{
    int opt;
    while (-1 != (opt = getopt(argc, argv, "m:n:d")))
    {
        switch (opt)
        {
        case 'm':
            xSplit_ = std::atoi(optarg);
            break;
        case 'n':
            ySplit_ = std::atoi(optarg);
            break;
        case 'd':
            dump_ = true;
            break;
        default:
            throw std::runtime_error("invalid arg");
            break;
        }
    }
    if (xSplit_ < 3 || ySplit_ < 3)
    {
        throw std::runtime_error("set nonzero splits");
    }

#ifdef CMC_OPENMPI
    mpi_.reset(new MpiHolder(&argc, &argv));
    com_ = boost::make_shared<MpiCommunicator>(xSplit_, ySplit_);
#else
    com_ = boost::make_shared<LocalCommunicator>();
#endif
}

void
Solver::SetProblem(Problem p, double eps)
{
    SetSubtask(
        p.domain,
        xSplit_, ySplit_,
        com_->CountSubtask(Vertical), com_->CountSubtask(Horizontal),
        com_->GetSubtask(Vertical),   com_->GetSubtask(Horizontal)
    );
    m_ = boost::make_shared<FiniteElementMethod>(com_, p, xSplit_, ySplit_);
    eps_ = eps;
}

void
Solver::Solve()
{
    double startTime = MPI_Wtime();

    Matrix w = m_->FirstApproximation();
    do
    {
        Matrix next = m_->EvaluateNext(w);
        normOfDifference_ = m_->NormOfDifference(w, next);
        w = next;
        iterations_++;
    } while (normOfDifference_ > eps_);

    double endTime = MPI_Wtime();

    elapsedTime_ = endTime - startTime;

    Matrix etalon = m_->Etalon();
    double localError = 0;
    for (int i = 0; i < w.GetRows(); i++)
    {
        for (int j = 0; j < w.GetCols(); j++)
        {
            double diff = std::fabs(w(i, j) - etalon(i, j));
            if (diff > localError)
            {
                localError = diff;
            }
        }
    }
    maxDifference_ = com_->UpdateError(localError);
    solution_ = w.Unwrapped();
}

void
Solver::PrintReport()
{
    if (dump_)
    {
        int row = com_->GetSubtask(Vertical);
        int col = com_->GetSubtask(Horizontal);
        DumpSolution(row, col);
        DumpEtalon(row, col);
    }

    if (!com_->IsMaster())
    {
        return;
    }

    std::cout << "Max difference: " << maxDifference_ << std::endl;
    std::cout << "Norm:           " << normOfDifference_ << std::endl;
    std::cout << "Iterations:     " << iterations_ << std::endl;
    std::cout << "Elapsed time:   " << elapsedTime_ << std::endl;
}

void
Solver::DumpSolution(int row, int col) const
{
    char filename[32];
    std::sprintf(filename, "solution_%d-%d", row, col);
    std::ofstream f(filename);
    for (int i = 0; i < solution_.GetRows(); i++)
    {
        for (int j = 0; j < solution_.GetCols(); j++)
        {
            f << i << ',' << j << ',' << solution_(i, j) << std::endl;
        }
    }
}

void
Solver::DumpEtalon(int row, int col) const
{
    char filename[32];
    std::sprintf(filename, "etalon_%d-%d", row, col);
    std::ofstream f(filename);

    const Matrix etalon = m_->Etalon().Unwrapped();
    for (int i = 0; i < etalon.GetRows(); i++)
    {
        for (int j = 0; j < etalon.GetCols(); j++)
        {
            f << i << ',' << j << ',' << etalon(i, j) << std::endl;
        }
    }
}


#ifndef itkLaplaceEquationSolverImageFilter_hxx_included
#define itkLaplaceEquationSolverImageFilter_hxx_included

#include "itkLaplaceEquationSolverImageFilter.h"

#include "itkImageRegionConstIteratorWithIndex.h"

#include <algorithm>
#include <ctime>
#include <limits>

#include <Eigen/SparseCholesky>

namespace itk
{

template< typename TInputImage, typename TOutputImage >
inline void
LaplaceEquationSolverImageFilter< TInputImage, TOutputImage >
::UpdateAB( size_t i, InputIndexType voxelIndex, OutputPixelType invDxDx,
            TripletListType & tripletList, VectorType & b,
            OffsetValueType solutionIndex )
{
  bool labelFound = false;
  InputPixelType label = this->GetInput()->GetPixel( voxelIndex );
  if ( label == m_SolutionLabel )
    {
    Eigen::Triplet< OutputPixelType > triplet( i, solutionIndex, invDxDx );
    tripletList.push_back( triplet );
    labelFound = true;
    }
  else if ( label == m_NeumannBoundaryConditionLabel )
    {
    Eigen::Triplet< OutputPixelType > triplet( i, i, invDxDx );
    tripletList.push_back( triplet );
    labelFound = true;
    }
  else
    {
    typename DirichletMapType::iterator iter =
      m_DirichletBoundaryConditionMap.find( label );
    if ( iter != m_DirichletBoundaryConditionMap.end() )
      {
      b[i] -= iter->second * invDxDx;
      labelFound = true;
      }
    }
  if ( !labelFound )
    {
    itkExceptionMacro( << "Unknown label value "
                       << label << " at index " << voxelIndex );
    }
}

template< typename TInputImage, typename TOutputImage >
LaplaceEquationSolverImageFilter< TInputImage, TOutputImage >
::LaplaceEquationSolverImageFilter()
{
  m_SolutionLabel = 11;
  m_NeumannBoundaryConditionLabel = 6;
}

template< typename TInputImage, typename TOutputImage >
void
LaplaceEquationSolverImageFilter< TInputImage, TOutputImage >
::ResetDirichletBoundaryConditions()
{
  m_DirichletBoundaryConditionMap.reset();

  this->Modified();
}

template< typename TInputImage, typename TOutputImage >
void
LaplaceEquationSolverImageFilter< TInputImage, TOutputImage >
::AddDirichletBoundaryCondition( InputPixelType boundaryConditionLabel,
                                 OutputPixelType boundaryConditionValue )
{
  m_DirichletBoundaryConditionMap[ boundaryConditionLabel ] =
    boundaryConditionValue;

  this->Modified();
}

template< typename TInputImage, typename TOutputImage >
size_t
LaplaceEquationSolverImageFilter< TInputImage, TOutputImage >
::GetNumberOfDirichletBoundaryConditions() const
{
  return m_DirichletBoundaryConditionMap.size();
}

template< typename TInputImage, typename TOutputImage >
void
LaplaceEquationSolverImageFilter< TInputImage, TOutputImage >
::GenerateData()
{
  typename TInputImage::ConstPointer input = this->GetInput();
  typename TOutputImage::Pointer output = this->GetOutput();

  // Allocate the output and initialize to NaN
  this->AllocateOutputs();
  output->FillBuffer( std::numeric_limits< OutputPixelType >::quiet_NaN() );

  // Do Laplace solution by iterative solver using 6 neighborhood
  InputIndexType minIndex = input->GetBufferedRegion().GetIndex();
  InputIndexType maxIndex = input->GetBufferedRegion().GetUpperIndex();

  // First get all the solution domain indices. We do this, otherwise
  // the matrices to solve will be far too large to fit into memory.
  std::vector< InputIndexType > solutionIndices;

  typedef typename TInputImage::OffsetValueType OffsetValueType;
  std::vector< OffsetValueType > linearIndex2UnknownIndex(
    input->GetPixelContainer()->Size(), 0 );

  long counter = 0;
  typedef ImageRegionConstIteratorWithIndex< TInputImage > IteratorWithIndexType;
  IteratorWithIndexType fullRegionIter( input.GetPointer(), input->GetBufferedRegion() );
  for ( fullRegionIter.GoToBegin(); !fullRegionIter.IsAtEnd(); ++fullRegionIter )
    {
    InputPixelType pixelValue = fullRegionIter.Get();
    InputIndexType index = fullRegionIter.GetIndex();
    if ( pixelValue == m_SolutionLabel )
      {
      solutionIndices.push_back( index );
      linearIndex2UnknownIndex[ input->ComputeOffset( fullRegionIter.GetIndex() ) ] = counter;
      ++counter;
      }
    }

  itkDebugMacro( << "Done with noting indices" );

  size_t numberOfUnknowns = solutionIndices.size();

  itkDebugMacro( << "Number of unknowns: " << numberOfUnknowns );

  // Set up the linear system by going through all the indices of the
  // solution domain and creating the sparse matrix A and the right
  // vector b
  typedef Eigen::SparseMatrix< OutputPixelType, Eigen::ColMajor > MatrixType;
  MatrixType A( numberOfUnknowns, numberOfUnknowns );

  // Set up the b vector
  typedef Eigen::Matrix< OutputPixelType, Eigen::Dynamic, 1 > VectorType;
  VectorType b( numberOfUnknowns );
  b.fill( 0.0 );

  // Set the diagonal elements
  OutputPixelType dx = input->GetSpacing()[0];
  OutputPixelType dy = input->GetSpacing()[1];
  OutputPixelType dz = input->GetSpacing()[2];

  OutputPixelType invDxDx = 1.0 / (dx * dx);
  OutputPixelType invDyDy = 1.0 / (dy * dy);
  OutputPixelType invDzDz = 1.0 / (dz * dz);

  itkDebugMacro( << "Done setting diagonal elements of A" );

  // Triplets to feed into A via
  // Eigen::SparseMatrix<>::setFromTriplets.  Note that triplets with
  // the same i, j indices will be summed in the sparse matrix.
  typedef Eigen::Triplet< OutputPixelType > Triplet;
  std::vector< Triplet > tripletList;
  tripletList.reserve( 7 * numberOfUnknowns );

  // This is somewhat of a goofy way to set up A. It is nearly a
  // direct translation from MATLAB code. Setting the elements based
  // on a traditional stencil approach might be faster and use less
  // memory.
  for ( size_t i = 0; i < numberOfUnknowns; ++i )
    {
    tripletList.push_back( Triplet( i, i, -2.0 * ( invDxDx + invDyDy + invDzDz ) ) );
    }

  for ( size_t i = 0; i < numberOfUnknowns; ++i )
    {
    InputIndexType index = solutionIndices[i];

    // +x, -x
    InputIndexType indexXP = index;
    indexXP[0] = std::min( maxIndex[0], index[0] + 1 );
    InputIndexType indexXM = index;
    indexXM[0] = std::max( minIndex[0], index[0] - 1 );

    // +y, -y
    InputIndexType indexYP = index;
    indexYP[1] = std::min( maxIndex[1], index[1] + 1 );
    InputIndexType indexYM = index;
    indexYM[1] = std::max( minIndex[1], index[1] - 1 );

    // +z, -z
    InputIndexType indexZP = index;
    indexZP[2] = std::min( maxIndex[2], index[2] + 1 );
    InputIndexType indexZM = index;
    indexZM[2] = std::max( minIndex[2], index[2] - 1 );

    OffsetValueType linearIndex;
    linearIndex = input->ComputeOffset( indexXP );
    this->UpdateAB( i, indexXP, invDxDx, tripletList, b,
                    linearIndex2UnknownIndex[ linearIndex ] );
    linearIndex = input->ComputeOffset( indexXM );
    this->UpdateAB( i, indexXM, invDxDx, tripletList, b,
                    linearIndex2UnknownIndex[ linearIndex ] );
    linearIndex = input->ComputeOffset( indexYP );
    this->UpdateAB( i, indexYP, invDyDy, tripletList, b,
                    linearIndex2UnknownIndex[ linearIndex ] );
    linearIndex = input->ComputeOffset( indexYM );
    this->UpdateAB( i, indexYM, invDyDy, tripletList, b,
                    linearIndex2UnknownIndex[ linearIndex ] );
    linearIndex = input->ComputeOffset( indexZP );
    this->UpdateAB( i, indexZP, invDzDz, tripletList, b,
                    linearIndex2UnknownIndex[ linearIndex ] );
    linearIndex = input->ComputeOffset( indexZM );
    this->UpdateAB( i, indexZM, invDzDz, tripletList, b,
                    linearIndex2UnknownIndex[ linearIndex ] );
    }

  itkDebugMacro( << "Done building linear systems" );

  // Fill A
  A.setFromTriplets( tripletList.begin(), tripletList.end() );
  tripletList.clear();

  // Compress the matrix before handing it off to the solver
  A.makeCompressed();

  itkDebugMacro( << "Done compressing" );

  std::time_t startTime;
  std::time( &startTime );

  // Tried several solvers on one large image to find the fastest:
  // ConjugateGradient  - 111 seconds
  // SimplicialCholesky - 204 seconds
  // SimplicialLDLT     - 203 seconds
  // SimplicalLLT       - error
  Eigen::ConjugateGradient< MatrixType > solver;
  solver.compute( A );
  VectorType x = solver.solve( b );

  itkDebugMacro( << "Done solving" );

  std::time_t endTime;
  std::time( &endTime );

  double elapsedSeconds = std::difftime( endTime, startTime );
  (void) elapsedSeconds; // To avoid warning when compiled in release mode
  itkDebugMacro( << "Execution took " << elapsedSeconds << " seconds." );

  if ( solver.info() != Eigen::Success )
    {
    itkExceptionMacro( << "Failed to solve linear system." );
    }

  // Now copy the output vector to the output image
  for ( size_t i = 0; i < solutionIndices.size(); ++i )
    {
    output->SetPixel( solutionIndices[i], x[i] );
    }

  // Now set the values for the Dirichlet boundary conditions
  // in the output image.
  ImageRegionConstIterator< TInputImage > iterIn( input.GetPointer(),
                                                     input->GetBufferedRegion() );
  ImageRegionIterator< TOutputImage > iterOut( output.GetPointer(),
                                                  output->GetBufferedRegion() );

  while ( !iterIn.IsAtEnd() && !iterOut.IsAtEnd() )
    {
    typename DirichletMapType::iterator iter = m_DirichletBoundaryConditionMap.find( iterIn.Get() );
    if ( iter != m_DirichletBoundaryConditionMap.end() )
      {
      iterOut.Set( iter->second );
      }
    ++iterIn;
    ++iterOut;
    }
}

} // end namespace itk

#endif


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <array>
#include <boost/math/quaternion.hpp>
#include <cmath>
#include <limits>
#include <pup.h>
#include <string>
#include <vector>

#include "DataStructures/DataVector.hpp"
#include "Domain/FunctionsOfTime/FunctionOfTime.hpp"
#include "Domain/FunctionsOfTime/FunctionOfTimeHelpers.hpp"
#include "Domain/FunctionsOfTime/PiecewisePolynomial.hpp"
#include "Parallel/CharmPupable.hpp"
#include "Utilities/Gsl.hpp"

namespace domain::FunctionsOfTime {
/// \ingroup ComputationalDomainGroup
/// \brief A FunctionOfTime that stores quaternions for the rotation map
///
/// \details This FunctionOfTime stores quaternions that will be used in the
/// time-dependent rotation map as well as the orbital angular velocity that
/// will be controlled by the rotation control sytem. To get the quaternion, an
/// ODE is solved of the form \f$ \dot{q} = \frac{1}{2} q \times \omega \f$
/// where \f$ \omega \f$ is the orbital angular velocity which is stored
/// internally as the derivative of an angle `PiecewisePolynomial`, and
/// \f$ \times \f$ here is quaternion multiplication.
///
/// Different from a `PiecewisePolynomial`, only the quaternion
/// itself is stored, not any of the derivatives because the derivatives must be
/// calculated from the solved ODE at every function call. Because
/// derivatives of the quaternion are not stored, the template parameter
/// `MaxDeriv` refers to both the max derivative of the stored angle
/// PiecewisePolynomial and the max derivative returned by the
/// QuaternionFunctionOfTime. The `update` function is then just a wrapper
/// around the internal `PiecewisePolynomial::update` function with the addition
/// that it then updates the stored quaternions as well.
///
/// The angle PiecewisePolynomial is accessible through the `angle_func`,
/// `angle_func_and_deriv`, and `angle_func_and_2_derivs` functions which
/// correspond to the function calls of a normal PiecewisePolynomial except
/// without the `angle_` prefix.
///
/// It is encouraged to use `quat_func` and `angle_func` when you want the
/// specific values of the functions to avoid ambiguity in what you are
/// calling. However, the original three `func` functions inherited from the
/// FunctionOfTime base class are necessary because the maps use the generic
/// `func` functions, thus they return the quaternion and its derivatives (which
/// are needed for the map). This is all to keep the symmetry of naming
/// `angle_func` and `quat_func` so that function calls won't be ambiguous.
template <size_t MaxDeriv>
class QuaternionFunctionOfTime : public FunctionOfTime {
 public:
  QuaternionFunctionOfTime() = default;
  QuaternionFunctionOfTime(
      double t, std::array<DataVector, 1> initial_quat_func,
      std::array<DataVector, MaxDeriv + 1> initial_angle_func,
      double expiration_time);

  ~QuaternionFunctionOfTime() override = default;
  QuaternionFunctionOfTime(QuaternionFunctionOfTime&&) = default;
  QuaternionFunctionOfTime& operator=(QuaternionFunctionOfTime&&) = default;
  QuaternionFunctionOfTime(const QuaternionFunctionOfTime&) = default;
  QuaternionFunctionOfTime& operator=(const QuaternionFunctionOfTime&) =
      default;

  // LCOV_EXCL_START
  explicit QuaternionFunctionOfTime(CkMigrateMessage* /*unused*/) {}
  // LCOV_EXCL_STOP

  auto get_clone() const -> std::unique_ptr<FunctionOfTime> override;

  // clang-tidy: google-runtime-references
  // clang-tidy: cppcoreguidelines-owning-memory,-warnings-as-errors
  WRAPPED_PUPable_decl_template(QuaternionFunctionOfTime<MaxDeriv>);  // NOLINT

  void reset_expiration_time(const double next_expiration_time) override {
    angle_f_of_t_.reset_expiration_time(next_expiration_time);
  }

  /// Returns domain of validity for the function of time
  std::array<double, 2> time_bounds() const override {
    return angle_f_of_t_.time_bounds();
  }

  /// Updates the `MaxDeriv`th derivative of the angle piecewisepolynomial at
  /// the given time, then updates the stored quaternions.
  ///
  /// `updated_max_deriv` is a datavector of the `MaxDeriv`s for each component.
  /// `next_expiration_time` is the next expiration time.
  void update(double time_of_update, DataVector updated_max_deriv,
              double next_expiration_time) override;

  // NOLINTNEXTLINE(google-runtime-references)
  void pup(PUP::er& p) override;

  /// Returns the quaternion at an arbitrary time `t`.
  std::array<DataVector, 1> func(const double t) const override {
    return quat_func(t);
  }

  /// Returns the quaternion and its first derivative at an arbitrary time `t`.
  std::array<DataVector, 2> func_and_deriv(const double t) const override {
    return quat_func_and_deriv(t);
  }

  /// Returns the quaternion and the first two derivatives at an arbitrary
  /// time `t`.
  std::array<DataVector, 3> func_and_2_derivs(const double t) const override {
    return quat_func_and_2_derivs(t);
  }

  /// Returns the quaternion at an arbitrary time `t`.
  std::array<DataVector, 1> quat_func(double t) const;

  /// Returns the quaternion and its first derivative at an arbitrary time `t`.
  std::array<DataVector, 2> quat_func_and_deriv(double t) const;

  /// Returns the quaternion and the first two derivatives at an arbitrary
  /// time `t`.
  std::array<DataVector, 3> quat_func_and_2_derivs(double t) const;

  /// Returns stored angle at an arbitrary time `t`.
  std::array<DataVector, 1> angle_func(const double t) const {
    return angle_f_of_t_.func(t);
  }

  /// Returns stored angle and its first derivative (omega) at an arbitrary time
  /// `t`.
  std::array<DataVector, 2> angle_func_and_deriv(const double t) const {
    return angle_f_of_t_.func_and_deriv(t);
  }

  /// Returns stored angle and the first two derivatives at an arbitrary
  /// time `t`.
  std::array<DataVector, 3> angle_func_and_2_derivs(const double t) const {
    return angle_f_of_t_.func_and_2_derivs(t);
  }

 private:
  template <size_t LocalMaxDeriv>
  friend bool operator==(  // NOLINT(readability-redundant-declaration)
      const QuaternionFunctionOfTime<LocalMaxDeriv>& lhs,
      const QuaternionFunctionOfTime<LocalMaxDeriv>& rhs);

  template <size_t LocalMaxDeriv>
  friend std::ostream& operator<<(  // NOLINT(readability-redundant-declaration)
      std::ostream& os,
      const QuaternionFunctionOfTime<LocalMaxDeriv>& quaternion_f_of_t);

  std::vector<FunctionOfTimeHelpers::StoredInfo<1, false>>
      stored_quaternions_and_times_;

  domain::FunctionsOfTime::PiecewisePolynomial<MaxDeriv> angle_f_of_t_;

  /// Integrates the ODE \f$ \dot{q} = \frac{1}{2} q \times \omega \f$ from time
  /// `t0` to time `t`. On input, `quaternion_to_integrate` is the initial
  /// quaternion at time `t0` and on output, it stores the result at time `t`
  void solve_quaternion_ode(
      gsl::not_null<boost::math::quaternion<double>*> quaternion_to_integrate,
      double t0, double t) const;

  /// Updates the `std::vector<StoredInfo>` to have the same number of stored
  /// quaternions as the `angle_f_of_t_ptr` has stored angles. This is necessary
  /// to ensure we can solve the ODE at any time `t`
  void update_stored_info();

  /// Does common operations to all the `func` functions such as updating stored
  /// info, solving the ODE, and returning the normalized quaternion as a boost
  /// quaternion for easy calculations
  boost::math::quaternion<double> setup_func(double t) const;
};

template <size_t MaxDeriv>
bool operator!=(const QuaternionFunctionOfTime<MaxDeriv>& lhs,
                const QuaternionFunctionOfTime<MaxDeriv>& rhs);

/// \cond
template <size_t MaxDeriv>
PUP::able::PUP_ID QuaternionFunctionOfTime<MaxDeriv>::my_PUP_ID = 0;  // NOLINT
/// \endcond
}  // namespace domain::FunctionsOfTime