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

#include <string>
#include <iostream>
#include <sstream>
#include <algorithm>
#include <vector>
#include <map>
#include <time.h>

#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_stl.hpp>

#include <graphlab.hpp>
#include <graphlab/graph/distributed_graph.hpp>

//shared parameters
float gaussian_kernel_scale_parameter = 0.1;
float threshold_to_discard_small_similarities = 0.0;
size_t number_of_nearest_neighbors= 20;

//data point
struct vertex_data {
  std::vector<float> x;
  float D_ii;
  vertex_data():x(), D_ii(0.0) {};
  explicit vertex_data(const std::vector<float>& x_in) :
      x(x_in), D_ii(0.0) {}

  void save(graphlab::oarchive& oarc) const {
    oarc << x.size();
    for(size_t i=0;i<x.size();++i)
      oarc << x[i];
    oarc << D_ii;
  }

  void load(graphlab::iarchive& iarc) {
    size_t size = 0;
    iarc >> size;
    for(size_t i=0;i<size;++i){
      float temp = 0.0;
      iarc >> temp;
      x.push_back(temp);
    }
    iarc >> D_ii;
  }
};

//similarity
struct edge_data{
  float A_ij;
  bool nearest;
  edge_data() : A_ij(0.0), nearest(false){}
  void save(graphlab::oarchive& oarc) const {
    oarc << A_ij << nearest;
  }
  void load(graphlab::iarchive& iarc) {
    iarc >> A_ij >> nearest;
  }
};

typedef graphlab::distributed_graph<vertex_data, edge_data> graph_type;

//[vertex_id] [element1] [element2] [element3] ...
bool line_parser(graph_type& graph, const std::string& filename,
    const std::string& line) {
  if (line.empty()) return true;
  size_t id = 0;
  namespace qi = boost::spirit::qi;
  namespace ascii = boost::spirit::ascii;
  namespace phoenix = boost::phoenix;
  vertex_data vtx;
  const bool success = qi::phrase_parse
    (line.begin(), line.end(),
     //  Begin grammar
     (
      qi::ulong_[phoenix::ref(id) = qi::_1] >> -qi::char_(",") >>
      (qi::double_[phoenix::push_back(phoenix::ref(vtx.x), qi::_1)] % -qi::char_(",") )
      )
     ,
     //  End grammar
     ascii::space);
  if (!success) return false;
  graph.add_vertex(id, vtx);

  for(size_t i=1;i<id;++i){
    graph.add_edge(i, id);
  }

  return true;
}

// helper function to compute similarity between points
float similarity(const std::vector<float>& v1, const std::vector<float>& v2) {
  float ret = 0.0;
  for (size_t i = 0; i < v1.size(); ++i) {
    float tmp = v1[i] - v2[i];
    ret += tmp * tmp;
  }
  return exp(-ret / gaussian_kernel_scale_parameter);
}

//calculate similarities between data points
void calc_similarities(graph_type::edge_type& edata) {
  edata.data().A_ij = similarity(edata.source().data().x, edata.target().data().x);
}

//discard small similarities (Optional)
void discard_small_similarity(graph_type::edge_type& edata) {
  if(edata.data().A_ij < threshold_to_discard_small_similarities)
    edata.data().A_ij = 0.0;
}

//gather T-nearest neighbor (Optional)
struct top_t_similarity{
  std::vector<size_t> ids;
  std::vector<float> sims;
  top_t_similarity(): ids(number_of_nearest_neighbors, std::numeric_limits<size_t>::max()),
      sims(number_of_nearest_neighbors, -1.0){}
  top_t_similarity(size_t id, float sim): ids(number_of_nearest_neighbors, std::numeric_limits<size_t>::max()),
      sims(number_of_nearest_neighbors, -1.0){
    ids[0] = id;
    sims[0] = sim;
  }

  top_t_similarity& operator+=(const top_t_similarity& other){
    std::vector<size_t> new_ids;
    std::vector<float> new_sims;
    size_t pos1=0;
    size_t pos2=0;
    while(pos1+pos2 < number_of_nearest_neighbors){
      if(sims[pos1] >= other.sims[pos2]){
        new_ids.push_back(ids[pos1]);
        new_sims.push_back(sims[pos1]);
        pos1++;
      }else{
        new_ids.push_back(other.ids[pos2]);
        new_sims.push_back(other.sims[pos2]);
        pos2++;
      }
    }
    ids = new_ids;
    sims = new_sims;
    return *this;
  }

  void save(graphlab::oarchive& oarc) const {
    oarc << ids.size();
    for(size_t i=0;i<ids.size();++i)
      oarc << ids[i];
    for(size_t i=0;i<sims.size();++i)
      oarc << sims[i];
  }
  void load(graphlab::iarchive& iarc) {
    ids.clear();
    sims.clear();
    size_t size = 0;
    iarc >> size;
    for(size_t i=0;i<size;++i){
      size_t id = 0;
      iarc >> id;
      ids.push_back(id);
    }
    for(size_t i=0;i<size;++i){
      float sim = 0;
      iarc >> sim;
      sims.push_back(sim);
    }
  }
};

//get T-nearest neighbor and discard others (Optional)
class t_nearest: public graphlab::ivertex_program<graph_type,
  top_t_similarity>, public graphlab::IS_POD_TYPE {
private:
  float threshold;

public:
  t_nearest():threshold(0.0){}

  edge_dir_type gather_edges(icontext_type& context,
      const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }
  top_t_similarity gather(icontext_type& context, const vertex_type& vertex,
      edge_type& edge) const {
    if(edge.target().id() == vertex.id()){//in edge
      return top_t_similarity(edge.source().id(), edge.data().A_ij);
    }else{//out edge
      return top_t_similarity(edge.target().id(), edge.data().A_ij);
    }
  }

  //assign a cluster, considering the clusters of neighbors
  void apply(icontext_type& context, vertex_type& vertex,
      const gather_type& total) {
    threshold = total.sims[number_of_nearest_neighbors-1];
//    std::cout << vertex.id() << "\t" << total.ids[0] << "-" << total.sims[0] << ", "
//        << total.ids[1] << "-" << total.sims[1] << std::endl;
  }

  edge_dir_type scatter_edges(icontext_type& context,
      const vertex_type& vertex) const {
      return graphlab::ALL_EDGES;
  }
  void scatter(icontext_type& context, const vertex_type& vertex,
      edge_type& edge) const {
    if(edge.data().A_ij >= threshold)
      edge.data().nearest = true;
  }
};

//discard small similarities (Optional)
void make_other_similarities_zero(graph_type::edge_type& edata) {
  if(edata.data().nearest == false)
    edata.data().A_ij = 0.0;
}

//compute sums over rows and then take inverse square root
class calc_degrees: public graphlab::ivertex_program<graph_type,
    float>, public graphlab::IS_POD_TYPE {
public:
  //gather A_ij
  edge_dir_type gather_edges(icontext_type& context,
      const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }
  float gather(icontext_type& context, const vertex_type& vertex,
      edge_type& edge) const {
    return edge.data().A_ij;
  }

  //assign a cluster, considering the clusters of neighbors
  void apply(icontext_type& context, vertex_type& vertex,
      const gather_type& total) {
    vertex.data().D_ii = 1.0 / sqrt(total);
  }

  edge_dir_type scatter_edges(icontext_type& context,
      const vertex_type& vertex) const {
      return graphlab::NO_EDGES;
  }
  void scatter(icontext_type& context, const vertex_type& vertex,
      edge_type& edge) const {
  }
};

//multiply D^-1/2
void mult_D(graph_type::edge_type& edata) {
  edata.data().A_ij = edata.data().A_ij * edata.source().data().D_ii * edata.target().data().D_ii;
}

struct max_min_similarity{
  float max_sim;
  float min_sim;

  max_min_similarity(): max_sim(0.0), min_sim(0.0){}
  explicit max_min_similarity(float similarity): max_sim(similarity),
      min_sim(similarity){}

  max_min_similarity& operator+=(const max_min_similarity& other){
    if(max_sim < 1.0 && other.max_sim < 1.0){
      max_sim = std::max(max_sim, other.max_sim);
    }else if(other.max_sim < 1.0){
      max_sim = other.max_sim;
    }
    if(min_sim > 0.0 && other.min_sim > 0.0){
      min_sim = std::min(min_sim, other.min_sim);
    }else if(other.min_sim > 0.0){
      min_sim = other.min_sim;
    }
    return *this;
  }
  void save(graphlab::oarchive& oarc) const {
    oarc << max_sim << min_sim;
  }
  void load(graphlab::iarchive& iarc) {
    iarc >> max_sim >> min_sim;
  }
};
max_min_similarity absolute_edge_data(const graph_type::edge_type& edge) {
  return max_min_similarity(edge.data().A_ij);
}

struct max_vid{
  size_t vid;
  max_vid(): vid(0){}
  explicit max_vid(size_t in_vid): vid(in_vid){}

  max_vid& operator+=(const max_vid& other){
    vid = std::max(vid, other.vid);
    return *this;
  }
  void save(graphlab::oarchive& oarc) const {
    oarc << vid;
  }
  void load(graphlab::iarchive& iarc) {
    iarc >> vid;
  }
};
max_vid absolute_vertex_data(const graph_type::vertex_type& vertex) {
  return max_vid(vertex.id());
}

class graph_writer {
public:
  std::string save_vertex(graph_type::vertex_type v) {
    std::stringstream strm;
    size_t vid = v.id();
    if(vid == 0)
      return "";
    strm << vid << " " << vid << " 1.0\n";
    return strm.str();
  }

  std::string save_edge(graph_type::edge_type e) {
    const float& A_ij = e.data().A_ij;
    std::stringstream strm;
    if(A_ij > 0.0){
      strm << e.source().id() << " " << e.target().id() << " " <<
          A_ij << "\n";
      strm << e.target().id() << " " << e.source().id() << " " <<
          A_ij << "\n";
    }
    return strm.str();
  }
};

int main(int argc, char** argv) {
  std::cout << "construct graph Laplacian for spectral clustering.\n\n";

  //parse command line
  std::string datafile;
  graphlab::command_line_options clopts
    ("Constructing graph Laplacian for spectral clustering");
  clopts.attach_option("data", datafile,
                       "Input file. Each line hold a sample id followed by a white-space or "
                       "comma separated numeric vector. Id should start from 1");
  clopts.attach_option("sigma",  gaussian_kernel_scale_parameter,
                       "Scale parameter for Gaussian kernel.");
  clopts.attach_option("similarity-thres", threshold_to_discard_small_similarities,
                       "Threshold to discard small similarities. ");
  clopts.attach_option("t-nearest", number_of_nearest_neighbors,
                      "Number of nearest neighbors (=t). Will use only the t-nearest similarities "
                      "for each datapoint. If set at 0, will use all similarities.");
  if(!clopts.parse(argc, argv)) return EXIT_FAILURE;
  if (datafile == "") {
    std::cout << "--data is not optional\n";
    return EXIT_FAILURE;
  }
  gaussian_kernel_scale_parameter *= 2.0*gaussian_kernel_scale_parameter;

  //construct graph
  graphlab::mpi_tools::init(argc, argv);
  graphlab::distributed_control dc;
  graph_type graph(dc, clopts);
  graph.load(
      datafile,
      line_parser);
  graph.finalize();

  time_t start, end;
  time(&start);
  size_t data_num = graph.map_reduce_vertices<max_vid>(absolute_vertex_data).vid;

  //calculate similarities
  graph.transform_edges(calc_similarities);

  //show the max similarity less than 1 and the min similarity grater than 0
  max_min_similarity stat = graph.map_reduce_edges<max_min_similarity>(absolute_edge_data);
  dc.cout() << "max squared distance(min similarity): "
      << -log(stat.min_sim)*gaussian_kernel_scale_parameter
      << "(" << stat.min_sim << ")\n"
      << "min squared distance(max similarity):"
      << -log(stat.max_sim)*gaussian_kernel_scale_parameter
      << "(" << stat.max_sim << ")\n";

  //if t is set, use only t-nearest similarities
  if(number_of_nearest_neighbors > 0){
    if(number_of_nearest_neighbors > data_num-1)
      number_of_nearest_neighbors = data_num-1;
    dc.cout() << "use only the " << number_of_nearest_neighbors
        << "-nearest similarities for each datapoint\n";
    graphlab::omni_engine<t_nearest> engine_nearest(dc, graph, "sync", clopts);
    engine_nearest.signal_all();
    engine_nearest.start();
    graph.transform_edges(make_other_similarities_zero);
  }
  //if threshold is set, discard similarities less then the threshold
  if(threshold_to_discard_small_similarities > 0.0){
    dc.cout() << "discard small similarities less than "
        << threshold_to_discard_small_similarities << "\n";
    graph.transform_edges(discard_small_similarity);
  }

  //sum elements over rows (calculate the degree matrix D)
  graphlab::omni_engine<calc_degrees> engine(dc, graph, "sync", clopts);
  engine.signal_all();
  engine.start();
  //multiply D
  graph.transform_edges(mult_D);
  time(&end);

  dc.cout() << "graph calculation time is " << (end - start) << " sec\n";
  dc.cout() << "writing...\n";

  //write results
  const std::string outputname = datafile + ".glap";
  graph.save(
      outputname + "_diag",
      graph_writer(), false, //set to true if each output file is to be gzipped
      true, //whether vertices are saved
      false,1); //whether edges are saved
  graph.save(
      outputname + "_other",
      graph_writer(), false, //set to true if each output file is to be gzipped
      false, //whether vertices are saved
      true,1); //whether edges are saved

  //write the number of data
  const std::string datanum_filename = datafile + ".datanum";
  std::ofstream ofs(datanum_filename.c_str());
  if(!ofs) {
    std::cout << "can't create file for number of data" << std::endl;
    return EXIT_FAILURE;
  }
  ofs << data_num;

  graphlab::mpi_tools::finalize();

  return EXIT_SUCCESS;
}


#include <Engine/MeshEdit/Parameterize_ASAP.h>
#include <Eigen/Sparse>

using namespace Ubpa;
using namespace std;

Parameterize_ASAP::Parameterize_ASAP(Ptr<TriMesh> triMesh)
	:Paramaterize_Basic::Paramaterize_Basic(triMesh)
{
	nP = nV = 0;
}

Parameterize_ASAP::~Parameterize_ASAP()
{
	Clear();
}

void Parameterize_ASAP::Parameterization()
{
	clock_t time = clock();
	nP = heMesh->NumPolygons();
	nV = heMesh->NumVertices();
	Local_Flatten();
	Init_Matrix();
	Solve();
	cout << "Time:" <<  clock()- time << endl;
}

coff_map Parameterize_ASAP::Map_Add(coff_map map1, coff_map map2)
{
	coff_map result = map1;
	for (auto iter2 = map2.begin(); iter2 != map2.end(); iter2++)
	{
		if (!result.count(iter2->first))
			result[iter2->first] = iter2->second;
		else
			result[iter2->first] += iter2->second;
	}
	return result;
}

double Parameterize_ASAP::Map_Derivation(coff_map map_, size_t elem)
{
	return map_[elem];
}

coff_map Parameterize_ASAP::Map_Multiply(coff_map map_, double coef)
{
	coff_map result;
	for (auto iter = map_.begin(); iter != map_.end(); iter++)
	{
		result.insert(coff_map::value_type(iter->first, iter->second * coef));
	}
	return result;
}

std::vector<coff_map> Parameterize_ASAP::Get_elem_map(size_t t, size_t i)
{
	std::vector<coff_map> elem_vec;
	coff_map elem_x, elem_y;
	double C1 = 0;
	coff_map C2, C3;
	for (size_t k = 0; k < 3; k++)
	{
		C1 += cotangent_list[t][k] * (delta_x_list[t][k] * delta_x_list[t][k] + delta_y_list[t][k] * delta_y_list[t][k]);
		C2.insert(coff_map::value_type(vertices_index_hemesh[t][k], cotangent_list[t][k] * delta_x_list[t][k]));	// x
		C2.insert(coff_map::value_type(vertices_index_hemesh[t][k] + nV, cotangent_list[t][k] * delta_y_list[t][k]));	//	y
		C2.insert(coff_map::value_type(vertices_index_hemesh[t][(k+1)%3], -cotangent_list[t][k] * delta_x_list[t][k]));	// x
		C2.insert(coff_map::value_type(vertices_index_hemesh[t][(k+1)%3] + nV, -cotangent_list[t][k] * delta_y_list[t][k]));	//	y

		C3.insert(coff_map::value_type(vertices_index_hemesh[t][k], cotangent_list[t][k] * delta_y_list[t][k]));	// x
		C3.insert(coff_map::value_type(vertices_index_hemesh[t][k] + nV, -cotangent_list[t][k] * delta_x_list[t][k]));	//	y
		C3.insert(coff_map::value_type(vertices_index_hemesh[t][(k + 1) % 3], -cotangent_list[t][k] * delta_y_list[t][k]));	// x
		C3.insert(coff_map::value_type(vertices_index_hemesh[t][(k + 1) % 3] + nV, cotangent_list[t][k] * delta_x_list[t][k]));	//	y
	}
	elem_x.insert(coff_map::value_type(vertices_index_hemesh[t][i], 1));
	elem_x.insert(coff_map::value_type(vertices_index_hemesh[t][(i+1)%3], -1));
	elem_y.insert(coff_map::value_type(vertices_index_hemesh[t][i], 1));
	elem_y.insert(coff_map::value_type(vertices_index_hemesh[t][(i + 1) % 3], -1));

	elem_x = Map_Add(elem_x, Map_Multiply(C2, -delta_x_list[t][i] / C1));
	elem_x = Map_Add(elem_x, Map_Multiply(C3, -delta_y_list[t][i] / C1));
	elem_y = Map_Add(elem_y, Map_Multiply(C2, -delta_y_list[t][i] / C1));
	elem_y = Map_Add(elem_y, Map_Multiply(C3, delta_x_list[t][i] / C1));

	elem_vec.push_back(elem_x);
	elem_vec.push_back(elem_y);
	for (size_t t = 0; t < 2; t++)
	{
		for (auto it = elem_vec[t].begin(); it != elem_vec[t].end(); it++)
			cout << "(" << it->first << ", " << it->second << ") ";
		cout << endl;
	}

	return elem_vec;
}

void Parameterize_ASAP::Init_Matrix()
{
	vector<Eigen::Triplet<double>> Lij;
	for (size_t n = 0; n < nV; n++)
	{
		coff_map coefn[2];
		for (size_t t = 0; t < nP; t++)
		{
			for (size_t i = 0; i < 3; i++)
			{
				vector<coff_map> elem_map = Get_elem_map(t, i);
				for (size_t k = 0; k < 2; k++)
				{
					coff_map elem = elem_map[k];
					elem = Map_Multiply(Map_Multiply(elem, Map_Derivation(elem, vertices_index_hemesh[t][i]*(k+1))),cotangent_list[t][i]);
					coefn[k] = Map_Add(coefn[k], elem);
				}
			}
		}
		for (size_t k = 0; k < 2; k++)
		{
			for (auto iter = coefn[k].begin(); iter != coefn[k].end(); iter++)
				if((n<2||(n>=nV&&n<nV+2))&&n==iter->first)
					Lij.push_back(Eigen::Triplet<double>(n*(k+1), iter->first, 1));
				else
					Lij.push_back(Eigen::Triplet<double>(n*(k+1), iter->first, iter->second));
		}
	}
	Coef_matrix.resize(2 * nV, 2 * nV);
	Coef_matrix.setZero();
	b_vector.resize(nV * 2);
	b_vector.setZero();
	b_vector(0) = heMesh->Vertices()[0]->pos[0];
	b_vector(0) = heMesh->Vertices()[0]->pos[0];
	b_vector(nV) = heMesh->Vertices()[0]->pos[1];
	b_vector(nV+1) = heMesh->Vertices()[0]->pos[1];

	Coef_matrix.setFromTriplets(Lij.begin(), Lij.end());
	cout << Coef_matrix;

}

void Parameterize_ASAP::Solve()
{
	Eigen::VectorXd X(2 * nV + 4 * nP);

	cout << "Solving Equation..." << endl;
	solver.compute(Coef_matrix);
	X = solver.solve(b_vector);
	cout << "Solving complete" << endl;

	for (size_t i = 0; i < nV; i++)
	{
		texcoords.push_back(pointf2(X(i), X(i+nV)));
		heMesh->Vertices()[i]->pos.at(0) = X(i);
		heMesh->Vertices()[i]->pos.at(1) = X(i + nV);
		heMesh->Vertices()[i]->pos.at(2) = 0;
	}
}

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

size_t Parameterize_ASAP::Get_a_index(size_t t)
{
	return 2 * nV + t;
}

size_t Parameterize_ASAP::Get_b_index(size_t t)
{
	return 2 * nV + nP + t;
}

veci2 Parameterize_ASAP::Get_u_index(size_t index)
{
	return veci2(index * 2, index * 2 + 1);
}

bool Parameterize_ASAP::map_update(size_t index, coff_map& map)
{
	if (index > 2)//	not fixed
	{
		if (!map.count(index-3))
		{
			map.insert(coff_map::value_type(index - 3, 0));
			map.insert(coff_map::value_type(index + nV - 6, 0));
		}
		return true;
	}
	else
		return false;	//	fixed
}

std::vector<std::vector<double>> Parameterize_ASAP::Get_delta_list(size_t index)
{
	if (index)
		return delta_y_list;
	else
		return delta_x_list;
}


/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// check memory allocation in some method
#define EIGEN_RUNTIME_NO_MALLOC

// includes
// std
#include <iostream>

// boost
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE PTransformd test
#include <boost/math/constants/constants.hpp>
#include <boost/test/unit_test.hpp>

// SpaceVecAlg
#include <SpaceVecAlg/Conversions.h>

namespace sva
{
static constexpr double PI = boost::math::constants::pi<double>();
}

BOOST_AUTO_TEST_CASE(ConversionsHomogeneous)
{
  using namespace Eigen;
  using namespace sva;

  Matrix4d hom = Matrix4d::Zero();
  hom(0, 1) = -1;
  hom(1, 0) = +1;
  hom(2, 2) = +1;
  hom(3, 3) = +1;
  PTransformd pt = conversions::fromHomogeneous(hom, conversions::RightHanded);

  const Vector4d vec(4, 3, 2, 1);
  Matrix4d homVec = Matrix4d::Identity();
  homVec.block<4, 1>(0, 3) = vec;
  PTransformd ptVec = conversions::fromHomogeneous(homVec, conversions::RightHanded);

  Matrix4d hom2 = conversions::toHomogeneous(pt, conversions::RightHanded);

  const PTransformd rotatedPT = ptVec * pt;
  const Vector4d rotated = hom * vec;
  const Vector4d rotated2 = hom2 * vec;

  BOOST_CHECK_EQUAL(rotated, rotated2);
  BOOST_CHECK_EQUAL((rotated.block<3, 1>(0, 0)), rotatedPT.translation());
}

BOOST_AUTO_TEST_CASE(ConversionsEigenTransform)
{
  using namespace Eigen;
  using namespace sva;

  PTransformd pt(sva::RotX(sva::PI / 2) * sva::RotZ(sva::PI / 4), Eigen::Vector3d(1., 2., 3.));
  conversions::affine3_t<double> et = conversions::toAffine(pt, conversions::RightHanded);
  PTransformd pt2 = conversions::fromAffine(et, conversions::RightHanded);

  BOOST_CHECK_SMALL(sva::transformError(pt, pt2).vector().norm(), 1e-12);

  conversions::affine3_t<double> etL = conversions::toAffine(pt, conversions::LeftHanded);
  PTransformd pt3 = conversions::fromAffine(etL, conversions::LeftHanded);

  BOOST_CHECK_SMALL(sva::transformError(pt, pt3).vector().norm(), 1e-12);

  Eigen::Vector3d vec(5, 7, 12);

  sva::PTransformd transformed = sva::PTransformd(vec) * pt;
  Eigen::Vector3d affineTransformed = et * vec;

  std::cout << transformed.translation().transpose() << std::endl;
  std::cout << affineTransformed.transpose() << std::endl;

  BOOST_CHECK_EQUAL(transformed.translation(), affineTransformed);
}


#include "gtest/gtest.h"

#include <cmath>
#include <iostream>
#include <numeric>
#include <cassert>
#include <omp.h>

#include <dataset.h>
#include <Eigen/Dense>

#include "logging.h"

#include "dbscan_vp.h"

namespace {
static const std::string CURRENT_TDIR( CURRENT_TEST_DIR );
}

using namespace clustering;

TEST( DBSCAN_VP, TwoClusters )
{
    Dataset::Ptr dset = Dataset::create();
    dset->load_csv( CURRENT_TDIR + "/csv/vptree01.csv" );

    DBSCAN_VP::Ptr dbs = boost::make_shared< DBSCAN_VP >( dset );
    dbs->fit();
    dbs->predict( 0.01, 5 );

    const DBSCAN_VP::Labels& l = dbs->get_labels();

    for ( size_t i = 0; i < l.size(); ++i ) {
        LOG( INFO ) << "Element = " << i << " cluster = " << l[i];
        if ( i < 5 ) {
            EXPECT_EQ( l[i], 0 );
        } else {
            EXPECT_EQ( l[i], 1 );
        }
    }
}

TEST( DBSCAN_VP, OneCluster )
{
    Dataset::Ptr dset = Dataset::create();
    dset->load_csv( CURRENT_TDIR + "/csv/vptree02.csv" );

    DBSCAN_VP::Ptr dbs = boost::make_shared< DBSCAN_VP >( dset );
    dbs->fit();
    dbs->predict( 0.01, 5 );

    const DBSCAN_VP::Labels& l = dbs->get_labels();

    for ( size_t i = 0; i < l.size(); ++i ) {
        LOG( INFO ) << "Element = " << i << " cluster = " << l[i];
        if ( i < 6 ) {
            EXPECT_EQ( l[i], 0 );
        } else {
            EXPECT_EQ( l[i], -1 );
        }
    }
}

TEST( DBSCAN_VP, NoClusters )
{
    Dataset::Ptr dset = Dataset::create();
    dset->load_csv( CURRENT_TDIR + "/csv/vptree03.csv" );

    DBSCAN_VP::Ptr dbs = boost::make_shared< DBSCAN_VP >( dset );
    dbs->fit();
    dbs->predict( 0.01, 2 );

    const DBSCAN_VP::Labels& l = dbs->get_labels();

    for ( size_t i = 0; i < l.size(); ++i ) {
        LOG( INFO ) << "Element = " << i << " cluster = " << l[i];
        EXPECT_EQ( l[i], -1 );
    }
}

TEST( DBSCAN_VP, Iris )
{
    Dataset::Ptr dset = Dataset::create();
    dset->load_csv( CURRENT_TDIR + "/csv/iris.data.txt" );

    DBSCAN_VP::Ptr dbs = boost::make_shared< DBSCAN_VP >( dset );

    dbs->fit();
    dbs->predict( 0.4, 5 );

    const DBSCAN_VP::Labels& l = dbs->get_labels();

    for ( size_t i = 0; i < l.size(); ++i ) {
        LOG( INFO ) << "Element = " << i << " cluster = " << l[i];
    }
}

TEST( DBSCAN_VP, IrisAnalyze )
{
    Dataset::Ptr dset = Dataset::create();
    dset->load_csv( CURRENT_TDIR + "/csv/iris.data.txt" );

    DBSCAN_VP::Ptr dbs = boost::make_shared< DBSCAN_VP >( dset );

    dbs->fit();
    const auto r = dbs->predict_eps( 3u );

    for ( size_t i = 0; i < r.size(); ++i ) {
        std::cout << ( i + 1 ) << "," << r[i] << std::endl;
    }
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2020 Digvijay Janartha, Hamirpur, India.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

#include <geometry_test_common.hpp>

#include <boost/core/ignore_unused.hpp>
#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/algorithms/append.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/concepts/linestring_concept.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/io/dsv/write.hpp>

#include <test_common/test_point.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

#ifdef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
#include <initializer_list>
#endif//BOOST_NO_CXX11_HDR_INITIALIZER_LIST

template <typename P>
bg::model::linestring<P> create_linestring()
{
    bg::model::linestring<P> l1;
    P p1;
    bg::assign_values(p1, 1, 2, 3);
    bg::append(l1, p1);
    return l1;
}

template <typename L, typename T>
void check_linestring(L& to_check, T x, T y, T z)
{
    BOOST_CHECK_EQUAL(bg::get<0>(to_check[0]), x);
    BOOST_CHECK_EQUAL(bg::get<1>(to_check[0]), y);
    BOOST_CHECK_EQUAL(bg::get<2>(to_check[0]), z);
}

template <typename P>
void test_default_constructor()
{
    bg::model::linestring<P> l1(create_linestring<P>());
    check_linestring(l1, 1, 2, 3);
}

template <typename P>
void test_copy_constructor()
{
    bg::model::linestring<P> l1 = create_linestring<P>();
    check_linestring(l1, 1, 2, 3);
}

template <typename P>
void test_copy_assignment()
{
    bg::model::linestring<P> l1(create_linestring<P>()), l2;
    l2 = l1;
    check_linestring(l2, 1, 2, 3);
}

template <typename P>
void test_concept()
{
    typedef bg::model::linestring<P> L;

    BOOST_CONCEPT_ASSERT( (bg::concepts::ConstLinestring<L>) );
    BOOST_CONCEPT_ASSERT( (bg::concepts::Linestring<L>) );

    typedef typename bg::coordinate_type<L>::type T;
    typedef typename bg::point_type<L>::type LP;
    boost::ignore_unused<T, LP>();
}

template <typename P>
void test_all()
{
    test_default_constructor<P>();
    test_copy_constructor<P>();
    test_copy_assignment<P>();
    test_concept<P>();
}

template <typename P>
void test_custom_linestring(std::initializer_list<P> IL)
{
    bg::model::linestring<P> l1(IL);
    std::ostringstream out;
    out << bg::dsv(l1);
    BOOST_CHECK_EQUAL(out.str(), "((1, 2), (2, 3), (3, 4))");
}

template <typename P>
void test_custom()
{
#ifdef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
    std::initializer_list<P> IL = {P(1, 2), P(2, 3), P(3, 4)};
    test_custom_linestring<P>(IL);
#endif//BOOST_NO_CXX11_HDR_INITIALIZER_LIST
}

template <typename CS>
void test_cs()
{
    test_all<bg::model::point<int, 3, CS> >();
    test_all<bg::model::point<float, 3, CS> >();
    test_all<bg::model::point<double, 3, CS> >();

    test_custom<bg::model::point<double, 2, CS> >();
}

int test_main(int, char* [])
{
    test_cs<bg::cs::cartesian>();
    test_cs<bg::cs::spherical<bg::degree> >();
    test_cs<bg::cs::spherical_equatorial<bg::degree> >();
    test_cs<bg::cs::geographic<bg::degree> >();

    test_custom<bg::model::d2::point_xy<double> >();

    return 0;
}


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operations.hpp
 *
 * \brief Include all supported operations.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 *
 * <hr/>
 *
 * Copyright (c) 2009, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATIONS_HPP
#define BOOST_NUMERIC_UBLASX_OPERATIONS_HPP

#include <boost/numeric/ublasx/operation/abs.hpp>
#include <boost/numeric/ublasx/operation/all.hpp>
#include <boost/numeric/ublasx/operation/any.hpp>
#include <boost/numeric/ublasx/operation/arithmetic_ops.hpp>
#include <boost/numeric/ublasx/operation/balance.hpp>
#include <boost/numeric/ublasx/operation/begin.hpp>
#include <boost/numeric/ublasx/operation/cat.hpp>
#include <boost/numeric/ublasx/operation/cholesky.hpp>
#include <boost/numeric/ublasx/operation/cond.hpp>
#include <boost/numeric/ublasx/operation/cumsum.hpp>
#include <boost/numeric/ublasx/operation/diag.hpp>
#include <boost/numeric/ublasx/operation/dot.hpp>
#include <boost/numeric/ublasx/operation/eigen.hpp>
#include <boost/numeric/ublasx/operation/empty.hpp>
#include <boost/numeric/ublasx/operation/end.hpp>
#include <boost/numeric/ublasx/operation/eps.hpp>
#include <boost/numeric/ublasx/operation/exp.hpp>
//#include <boost/numeric/ublasx/operation/expm.hpp>
#include <boost/numeric/ublasx/operation/eye.hpp>
#include <boost/numeric/ublasx/operation/find.hpp>
#include <boost/numeric/ublasx/operation/for_each.hpp>
#include <boost/numeric/ublasx/operation/hilb.hpp>
#include <boost/numeric/ublasx/operation/hold.hpp>
#include <boost/numeric/ublasx/operation/illcond.hpp>
#include <boost/numeric/ublasx/operation/inv.hpp>
#include <boost/numeric/ublasx/operation/isfinite.hpp>
#include <boost/numeric/ublasx/operation/isinf.hpp>
#include <boost/numeric/ublasx/operation/linspace.hpp>
#include <boost/numeric/ublasx/operation/log10.hpp>
#include <boost/numeric/ublasx/operation/log2.hpp>
#include <boost/numeric/ublasx/operation/log.hpp>
#include <boost/numeric/ublasx/operation/logspace.hpp>
#include <boost/numeric/ublasx/operation/lsq.hpp>
#include <boost/numeric/ublasx/operation/lu.hpp>
#include <boost/numeric/ublasx/operation/max.hpp>
#include <boost/numeric/ublasx/operation/min.hpp>
#include <boost/numeric/ublasx/operation/mldivide.hpp>
#include <boost/numeric/ublasx/operation/mpow.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/pow.hpp>
#include <boost/numeric/ublasx/operation/pow2.hpp>
#include <boost/numeric/ublasx/operation/ql.hpp>
#include <boost/numeric/ublasx/operation/qr.hpp>
#include <boost/numeric/ublasx/operation/qz.hpp>
#include <boost/numeric/ublasx/operation/rank.hpp>
#include <boost/numeric/ublasx/operation/rcond.hpp>
#include <boost/numeric/ublasx/operation/realmin.hpp>
#include <boost/numeric/ublasx/operation/relational_ops.hpp>
#include <boost/numeric/ublasx/operation/rep.hpp>
#include <boost/numeric/ublasx/operation/reshape.hpp>
#include <boost/numeric/ublasx/operation/rot90.hpp>
#include <boost/numeric/ublasx/operation/round.hpp>
#include <boost/numeric/ublasx/operation/seq.hpp>
#include <boost/numeric/ublasx/operation/sign.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <boost/numeric/ublasx/operation/sqr.hpp>
#include <boost/numeric/ublasx/operation/sqrt.hpp>
#include <boost/numeric/ublasx/operation/sum.hpp>
#include <boost/numeric/ublasx/operation/svd.hpp>
#include <boost/numeric/ublasx/operation/tanh.hpp>
#include <boost/numeric/ublasx/operation/trace.hpp>
#include <boost/numeric/ublasx/operation/transform.hpp>
#include <boost/numeric/ublasx/operation/tril.hpp>
#include <boost/numeric/ublasx/operation/triu.hpp>
#include <boost/numeric/ublasx/operation/which.hpp>

#endif // BOOST_NUMERIC_UBLASX_OPERATIONS_HPP


/*
 * wave_equation.cpp
 *
 *  Created on: 11.07.2017
 *      Author: thies
 */

#include <base/AdaptiveMesh.h>
#include <base/ConstantMesh.h>
#include <base/DiscretizedFunction.h>
#include <base/SpaceTimeMesh.h>
#include <base/Util.h>
#include <deal.II/base/exceptions.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/numbers.h>
#include <deal.II/base/point.h>
#include <deal.II/base/quadrature.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/timer.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria.h>
#include <forward/L2RightHandSide.h>
#include <forward/WaveEquation.h>
#include <gtest/gtest.h>
#include <norms/L2L2.h>
#include <stddef.h>
#include <iostream>
#include <memory>
#include <vector>

namespace {

using namespace dealii;
using namespace wavepi::forward;
using namespace wavepi::base;
using namespace wavepi;

template<int dim>
class TestF: public LightFunction<dim> {
public:
   virtual ~TestF() = default;

   virtual double evaluate(const Point<dim> &p, const double t) const {
      if ((t <= 1) && (p.norm() < 0.5))
         return std::sin(t * 2 * numbers::PI);
      else
         return 0.0;
   }
};

template<int dim>
class TestG: public LightFunction<dim> {
public:
   virtual ~TestG() = default;

   virtual double evaluate(const Point<dim> &p, const double t) const {
      Point<dim> pc = Point<dim>::unit_vector(0);
      pc *= 0.5;

      if (std::abs(this->get_time() - 1.0) < 0.5 && (p.distance(pc) < 0.5))
         return std::sin(this->get_time() * 1 * numbers::PI);
      else
         return 0.0;
   }
};

template<int dim>
class TestH: public LightFunction<dim> {
public:
   virtual ~TestH() = default;

   virtual double evaluate(const Point<dim> &p, const double t) const {
      return p.norm() * this->get_time();
   }
};

template<int dim>
double rho(const Point<dim> &p, double t) {
   return p.norm() + t + 1.0;
}

template<int dim>
double c_squared(const Point<dim> &p, double t) {
   double tmp = p.norm() * t + 1.0;

   return tmp * tmp;
}

template<int dim>
class TestC: public LightFunction<dim> {
public:
   virtual ~TestC() = default;

   virtual double evaluate(const Point<dim> &p, const double t) const {
      return 1.0 / (rho(p, t) * c_squared(p, t));
   }
};

template<int dim>
class TestRho: public LightFunction<dim> {
public:
   virtual ~TestRho() = default;

   virtual double evaluate(const Point<dim> &p, const double t) const {
      return rho(p, t);
   }
};

template<int dim>
class TestNu: public LightFunction<dim> {
public:
   virtual ~TestNu() = default;

   virtual double evaluate(const Point<dim> &p, const double t) const {
      // do not just change this, reference_test_nu needs this
      return p.norm() * t;
   }
};

template<int dim>
class RhsTestNu: public LightFunction<dim> {
public:
   virtual ~RhsTestNu() = default;

   virtual double evaluate(const Point<dim> &p, const double t) const {
      // do not just change this, reference_test_nu needs this
      return nu.evaluate(p, t) * v->evaluate(p, t);
   }

   RhsTestNu(const std::shared_ptr<LightFunction<dim>> &v)
         : v(v) {
      AssertThrow(v, ExcNotInitialized());
   }

private:
   std::shared_ptr<LightFunction<dim>> v;  // pointer to u'
   TestNu<dim> nu;
};

template<int dim>
class TestQ: public LightFunction<dim> {
public:
   virtual ~TestQ() = default;

   virtual double evaluate(const Point<dim> &p, const double t) const override {
      return p.norm() < 0.5 ? std::sin(t / 2 * 2 * numbers::PI) : 0.0;
   }

   static const Point<dim> q_position;
};

template<>
const Point<1> TestQ<1>::q_position = Point<1>(-1.0);
template<>
const Point<2> TestQ<2>::q_position = Point<2>(-1.0, 0.5);
template<>
const Point<3> TestQ<3>::q_position = Point<3>(-1.0, 0.5, 0.0);

template<int dim>
class DiscretizedFunctionDisguise: public LightFunction<dim> {
public:
   DiscretizedFunctionDisguise(std::shared_ptr<DiscretizedFunction<dim>> base)
         : base(base) {
   }
   virtual ~DiscretizedFunctionDisguise() = default;

   virtual double evaluate(const Point<dim> &p, const double t) const {
      return base->value(p, t);
   }
private:
   std::shared_ptr<DiscretizedFunction<dim>> base;
};

// checks, whether the matrix assembly of discretized parameters works correct
// (by supplying DiscretizedFunctions and DiscretizedFunctionDisguises)
template<int dim>
void run_discretized_test(int fe_order, int quad_order, int refines) {
   Timer timer;

   auto triangulation = std::make_shared<Triangulation<dim>>();
   GridGenerator::hyper_cube(*triangulation, -1, 1);
   Util::set_all_boundary_ids(*triangulation, 0);
   triangulation->refine_global(refines);

   double t_start = 0.0, t_end = 2.0, dt = t_end / 64.0;
   std::vector<double> times;

   for (size_t i = 0; t_start + i * dt <= t_end; i++)
      times.push_back(t_start + i * dt);

   FE_Q<dim> fe(fe_order);
   Quadrature<dim> quad = QGauss<dim>(quad_order);  // exact in poly degree 2n-1 (needed: fe_dim^3)

   std::shared_ptr<SpaceTimeMesh<dim>> mesh = std::make_shared<ConstantMesh<dim>>(times, fe, quad, triangulation);

   deallog << std::endl << "----------  n_dofs / timestep: " << mesh->get_dof_handler(0)->n_dofs();
   deallog << ", n_steps: " << times.size() << "  ----------" << std::endl;

   WaveEquation<dim> wave_eq(mesh);

   /* continuous */

   wave_eq.set_param_rho(std::make_shared<TestRho<dim>>());
   wave_eq.set_param_c(std::make_shared<TestC<dim>>());
   wave_eq.set_param_q(std::make_shared<TestQ<dim>>());
   wave_eq.set_param_nu(std::make_shared<TestNu<dim>>());

   timer.restart();
   DiscretizedFunction<dim> sol_cont = wave_eq.run(
         std::make_shared<L2RightHandSide<dim>>(std::make_shared<TestF<dim>>()), WaveEquation<dim>::Forward);
   sol_cont.set_norm(std::make_shared<norms::L2L2<dim>>());

   timer.stop();
   deallog << "continuous params: " << std::fixed << timer.wall_time() << " s of wall time" << std::endl;
   EXPECT_GT(sol_cont.norm(), 0.0);

   /* discretized */

   TestC<dim> c;
   auto c_disc = std::make_shared<DiscretizedFunction<dim>>(mesh, c);
   wave_eq.set_param_c(c_disc);

   TestRho<dim> rho;
   auto rho_disc = std::make_shared<DiscretizedFunction<dim>>(mesh, rho);
   wave_eq.set_param_rho(rho_disc);

   TestQ<dim> q;
   auto q_disc = std::make_shared<DiscretizedFunction<dim>>(mesh, q);
   wave_eq.set_param_q(q_disc);

   TestNu<dim> nu;
   auto nu_disc = std::make_shared<DiscretizedFunction<dim>>(mesh, nu);
   wave_eq.set_param_nu(nu_disc);

   TestF<dim> f;
   auto f_disc = std::make_shared<DiscretizedFunction<dim>>(mesh, f);

   timer.restart();
   DiscretizedFunction<dim> sol_disc = wave_eq.run(std::make_shared<L2RightHandSide<dim>>(f_disc),
         WaveEquation<dim>::Forward);
   sol_disc.set_norm(std::make_shared<norms::L2L2<dim>>());
   timer.stop();
   deallog << "all discretized: " << std::fixed << timer.wall_time() << " s of wall time" << std::endl;
   EXPECT_GT(sol_disc.norm(), 0.0);

   /* discretized, q disguised */

   auto c_disguised = std::make_shared<DiscretizedFunctionDisguise<dim>>(c_disc);
   auto rho_disguised = std::make_shared<DiscretizedFunctionDisguise<dim>>(rho_disc);
   auto q_disguised = std::make_shared<DiscretizedFunctionDisguise<dim>>(q_disc);
   auto nu_disguised = std::make_shared<DiscretizedFunctionDisguise<dim>>(nu_disc);
   auto f_disguised = std::make_shared<DiscretizedFunctionDisguise<dim>>(f_disc);

   wave_eq.set_param_q(q_disguised);

   timer.restart();
   DiscretizedFunction<dim> sol_disc_except_q = wave_eq.run(std::make_shared<L2RightHandSide<dim>>(f_disc),
         WaveEquation<dim>::Forward);
   sol_disc_except_q.set_norm(std::make_shared<norms::L2L2<dim>>());
   timer.stop();
   deallog << "all discretized, q disguised: " << std::fixed << timer.wall_time() << " s of wall time" << std::endl;
   EXPECT_GT(sol_disc_except_q.norm(), 0.0);

   /* discretized, a disguised */

   wave_eq.set_param_rho(rho_disguised);
   wave_eq.set_param_q(q_disc);

   timer.restart();
   DiscretizedFunction<dim> sol_disc_except_a = wave_eq.run(std::make_shared<L2RightHandSide<dim>>(f_disc),
         WaveEquation<dim>::Forward);
   sol_disc_except_a.set_norm(std::make_shared<norms::L2L2<dim>>());
   timer.stop();
   deallog << "all discretized, a disguised: " << std::fixed << timer.wall_time() << " s of wall time" << std::endl;
   EXPECT_GT(sol_disc_except_a.norm(), 0.0);

   /* disguised */

   wave_eq.set_param_nu(nu_disguised);
   wave_eq.set_param_q(q_disguised);
   wave_eq.set_param_rho(rho_disguised);
   wave_eq.set_param_c(c_disguised);

   timer.restart();
   DiscretizedFunction<dim> sol_disguised = wave_eq.run(std::make_shared<L2RightHandSide<dim>>(f_disguised),
         WaveEquation<dim>::Forward);
   sol_disguised.set_norm(std::make_shared<norms::L2L2<dim>>());
   timer.stop();
   deallog << "all discretized and disguised as continuous: " << std::fixed << timer.wall_time() << " s of wall time"
         << std::endl << std::endl;
   EXPECT_GT(sol_disguised.norm(), 0.0);

   /* results */
   DiscretizedFunction<dim> tmp(sol_disc_except_q);
   tmp -= sol_disguised;
   double err_disguised_vs_disc_except_q = tmp.norm() / sol_disguised.norm();

   deallog << "rel. error between disguised discrete and discrete (q disguised): " << std::scientific
         << err_disguised_vs_disc_except_q << std::endl;
   EXPECT_LT(err_disguised_vs_disc_except_q, 1e-6);

   tmp = sol_disc_except_a;
   tmp -= sol_disguised;
   double err_disguised_vs_disc_except_a = tmp.norm() / sol_disguised.norm();

   deallog << "rel. error between disguised discrete and discrete (a disguised): " << std::scientific
         << err_disguised_vs_disc_except_a << std::endl;
   EXPECT_LT(err_disguised_vs_disc_except_a, 1e-6);

   tmp = sol_disc;
   tmp -= sol_disguised;
   double err_disguised_vs_disc = tmp.norm() / sol_disguised.norm();

   deallog << "rel. error between disguised discrete and full discrete: " << std::scientific << err_disguised_vs_disc
         << std::endl << std::endl;
   EXPECT_LT(err_disguised_vs_disc, 1e-6);
}

// product of sines in space to have dirichlet b.c. in [0,pi], times a sum of sine and cosine in time.
// its time derivative is the same function with C[1] = C[0]*norm(k), C[0] = -C[1]*norm(k)
template<int dim>
class SeparationAnsatz: public LightFunction<dim> {
public:
   virtual ~SeparationAnsatz() = default;

   virtual double evaluate(const Point<dim> &p, const double t) const {
      double res = 1;

      for (size_t i = 0; i < dim; i++)
         res *= std::sin(k[i] * p[i]);

      res *= constants[0] * std::sin(std::sqrt(k.square()) * t) + constants[1] * std::cos(std::sqrt(k.square()) * t);
      return res;
   }

   SeparationAnsatz(Point<dim, int> k, Point<2> constants)
         : k(k), constants(constants) {
   }

private:
   Point<dim, int> k;
   Point<2> constants;
};

template<int dim>
void run_reference_test(std::shared_ptr<SpaceTimeMesh<dim>> mesh, Point<dim, int> k, Point<2> constants, bool expect =
      true, bool save = false) {
   deallog << std::endl << "----------  n_dofs(0): " << mesh->get_dof_handler(0)->n_dofs();
   deallog << ", n_steps: " << mesh->get_times().size() << "  ----------" << std::endl;

   WaveEquation<dim> wave_eq(mesh);

   Point<2> derivative_constants;
   derivative_constants[0] = -constants[1] * std::sqrt(k.square());
   derivative_constants[1] = constants[0] * std::sqrt(k.square());

   auto u = std::make_shared<SeparationAnsatz<dim>>(k, constants);
   auto v = std::make_shared<SeparationAnsatz<dim>>(k, derivative_constants);

   wave_eq.set_initial_values_u(u);
   wave_eq.set_initial_values_v(v);

   DiscretizedFunction<dim> solu = wave_eq.run(
         std::make_shared<L2RightHandSide<dim>>(std::make_shared<Functions::ZeroFunction<dim>>(1)),
         WaveEquation<dim>::Forward);
   DiscretizedFunction<dim> solv = solu.derivative();
   solu.throw_away_derivative();

   solu.set_norm(std::make_shared<norms::L2L2<dim>>());
   solv.set_norm(std::make_shared<norms::L2L2<dim>>());

   DiscretizedFunction<dim> refu(mesh, *u);
   DiscretizedFunction<dim> refv(mesh, *v);

   refu.set_norm(std::make_shared<norms::L2L2<dim>>());
   refv.set_norm(std::make_shared<norms::L2L2<dim>>());

   DiscretizedFunction<dim> tmp(solu);
   tmp -= refu;
   double err_u = tmp.norm() / refu.norm();

   tmp = solv;
   tmp -= refv;
   double err_v = tmp.norm() / refv.norm();

   if (expect) {
      EXPECT_LT(err_u, 1e-1);
      EXPECT_LT(err_v, 1e-1);
   }

   if (save) {
      solu.write_pvd("./", "solu", "u");
      refu.write_pvd("./", "refu", "uref");

      DiscretizedFunction<dim> tmp(solu);
      tmp -= refu;
      tmp.write_pvd("./", "diff", "udiff");
   }

   deallog << std::scientific << "forward : rerr(u) = " << err_u << ", rerr(v) = " << err_v << std::endl;

   solu = wave_eq.run(std::make_shared<L2RightHandSide<dim>>(std::make_shared<Functions::ZeroFunction<dim>>(1)),
         WaveEquation<dim>::Backward);
   solv = solu.derivative();
   solu.throw_away_derivative();

   solu.set_norm(std::make_shared<norms::L2L2<dim>>());
   solv.set_norm(std::make_shared<norms::L2L2<dim>>());

   tmp = solu;
   tmp -= refu;
   err_u = tmp.norm() / refu.norm();

   tmp = solv;
   tmp -= refv;
   err_v = tmp.norm() / refv.norm();

   if (expect) {
      EXPECT_LT(err_u, 1e-1);
      EXPECT_LT(err_v, 1e-1);
   }

   deallog << std::scientific << "backward: rerr(u) = " << err_u << ", rerr(v) = " << err_v << std::endl << std::endl;
}
}  // namespace

template<int dim>
void run_reference_test_constant(int fe_order, int quad_order, int refines, Point<dim, int> k, Point<2> constants,
      double t_end, int steps, bool expect = true, bool save = false) {
   auto triangulation = std::make_shared<Triangulation<dim>>();
   GridGenerator::hyper_cube(*triangulation, 0.0, numbers::PI);
   Util::set_all_boundary_ids(*triangulation, 0);
   triangulation->refine_global(refines);

   double t_start = 0.0, dt = t_end / steps;
   std::vector<double> times;

   for (size_t i = 0; t_start + i * dt <= t_end; i++)
      times.push_back(t_start + i * dt);

   FE_Q<dim> fe(fe_order);
   Quadrature<dim> quad = QGauss<dim>(quad_order);  // exact in poly degree 2n-1 (needed: fe_dim^3)

   std::shared_ptr<SpaceTimeMesh<dim>> mesh = std::make_shared<ConstantMesh<dim>>(times, fe, quad, triangulation);

   run_reference_test<dim>(mesh, k, constants, expect, save);
}

template<int dim>
void run_reference_test_nu(int fe_order, int quad_order, int refines, Point<dim, int> k, Point<2> constants,
      double t_end, int steps, bool expect = true, bool save = false) {
   auto triangulation = std::make_shared<Triangulation<dim>>();
   GridGenerator::hyper_cube(*triangulation, 0.0, numbers::PI);
   Util::set_all_boundary_ids(*triangulation, 0);
   triangulation->refine_global(refines);

   double t_start = 0.0, dt = t_end / steps;
   std::vector<double> times;

   for (size_t i = 0; t_start + i * dt <= t_end; i++)
      times.push_back(t_start + i * dt);

   FE_Q<dim> fe(fe_order);
   Quadrature<dim> quad = QGauss<dim>(quad_order);  // exact in poly degree 2n-1 (needed: fe_dim^3)

   std::shared_ptr<SpaceTimeMesh<dim>> mesh = std::make_shared<ConstantMesh<dim>>(times, fe, quad, triangulation);
   deallog << std::endl << "----------  n_dofs(0): " << mesh->get_dof_handler(0)->n_dofs();
   deallog << ", n_steps: " << mesh->get_times().size() << "  ----------" << std::endl;

   Point<2> derivative_constants;
   derivative_constants[0] = -constants[1] * std::sqrt(k.square());
   derivative_constants[1] = constants[0] * std::sqrt(k.square());

   auto u = std::make_shared<SeparationAnsatz<dim>>(k, constants);
   auto v = std::make_shared<SeparationAnsatz<dim>>(k, derivative_constants);

   WaveEquation<dim> wave_eq(mesh);
   wave_eq.set_param_nu(std::make_shared<TestNu<dim>>());

   wave_eq.set_initial_values_u(u);
   wave_eq.set_initial_values_v(v);

   auto x = std::make_shared<RhsTestNu<dim>>(v);

   DiscretizedFunction<dim> solu = wave_eq.run(std::make_shared<L2RightHandSide<dim>>(x), WaveEquation<dim>::Forward);
   DiscretizedFunction<dim> solv = solu.derivative();
   solu.throw_away_derivative();

   solu.set_norm(std::make_shared<norms::L2L2<dim>>());
   solv.set_norm(std::make_shared<norms::L2L2<dim>>());

   DiscretizedFunction<dim> refu(mesh, *u);
   DiscretizedFunction<dim> refv(mesh, *v);

   refu.set_norm(std::make_shared<norms::L2L2<dim>>());
   refv.set_norm(std::make_shared<norms::L2L2<dim>>());

   DiscretizedFunction<dim> tmp(solu);
   tmp -= refu;
   double err_u = tmp.norm() / refu.norm();

   tmp = solv;
   tmp -= refv;
   double err_v = tmp.norm() / refv.norm();

   if (expect) {
      EXPECT_LT(err_u, 1e-1);
      EXPECT_LT(err_v, 1e-1);
   }

   if (save) {
      solu.write_pvd("./", "solu", "u");
      refu.write_pvd("./", "refu", "uref");

      DiscretizedFunction<dim> tmp(solu);
      tmp -= refu;
      tmp.write_pvd("./", "diff", "udiff");
   }

   deallog << std::scientific << "forward : rerr(u) = " << err_u << ", rerr(v) = " << err_v << std::endl;

   // backward does not work for nu > 0
   /*
    wave_eq.set_run_direction(WaveEquation<dim>::Backward);
    solu = wave_eq.run();
    solv = solu.derivative();
    solu.throw_away_derivative();

    solu.set_norm(Norm::L2L2);
    solv.set_norm(Norm::L2L2);

    tmp = solu;
    tmp -= refu;
    err_u = tmp.norm() / refu.norm();

    tmp = solv;
    tmp -= refv;
    err_v = tmp.norm() / refv.norm();

    if (expect) {
    EXPECT_LT(err_u, 1e-1);
    EXPECT_LT(err_v, 1e-1);
    }

    deallog << std::scientific << "backward: rerr(u) = " << err_u << ", rerr(v) = " << err_v << std::endl << std::endl;
    */
}

template<int dim>
void run_reference_test_adaptive(int fe_order, int quad_order, int refines, Point<dim, int> k, Point<2> constants,
      double t_end, int steps, bool expect = true, bool save = false) {
   auto triangulation = std::make_shared<Triangulation<dim>>();
   GridGenerator::hyper_cube(*triangulation, 0.0, numbers::PI);
   Util::set_all_boundary_ids(*triangulation, 0);
   triangulation->refine_global(refines);

   double t_start = 0.0, dt = t_end / steps;
   std::vector<double> times;

   for (size_t i = 0; t_start + i * dt <= t_end; i++)
      times.push_back(t_start + i * dt);

   FE_Q<dim> fe(fe_order);
   Quadrature<dim> quad = QGauss<dim>(quad_order);  // exact in poly degree 2n-1 (needed: fe_dim^3)

   auto mesh = std::make_shared<AdaptiveMesh<dim>>(times, fe, quad, triangulation);

   // flag some cells for refinement, and refine them in some step
   for (auto cell : triangulation->active_cell_iterators())
      if (cell->center()[0] > numbers::PI / 2) cell->set_refine_flag();

   std::vector<bool> ref;
   std::vector<bool> coa;

   triangulation->save_refine_flags(ref);
   triangulation->save_coarsen_flags(coa);

   for (auto cell : triangulation->active_cell_iterators())
      cell->clear_refine_flag();

   std::vector<Patch> patches = mesh->get_forward_patches();
   patches[steps / 4].emplace_back(ref, coa);
   mesh->set_forward_patches(patches);

   mesh->get_dof_handler(0);

   run_reference_test<dim>(mesh, k, constants, expect, save);
}

template<int dim>
void run_reference_test_refined(int fe_order, int quad_order, int refines, Point<dim, int> k, Point<2> constants,
      double t_end, int steps, bool expect = true, bool save = false) {
   auto triangulation = std::make_shared<Triangulation<dim>>();
   GridGenerator::hyper_cube(*triangulation, 0.0, numbers::PI);
   Util::set_all_boundary_ids(*triangulation, 0);
   triangulation->refine_global(refines);

   // flag some cells for refinement and refine them
   for (auto cell : triangulation->active_cell_iterators())
      if (cell->center()[1] > numbers::PI / 2) cell->set_refine_flag();

   triangulation->execute_coarsening_and_refinement();

   double t_start = 0.0, dt = t_end / steps;
   std::vector<double> times;

   for (size_t i = 0; t_start + i * dt <= t_end; i++)
      times.push_back(t_start + i * dt);

   FE_Q<dim> fe(fe_order);
   Quadrature<dim> quad = QGauss<dim>(quad_order);  // exact in poly degree 2n-1 (needed: fe_dim^3)

   auto mesh = std::make_shared<ConstantMesh<dim>>(times, fe, quad, triangulation);

   run_reference_test<dim>(mesh, k, constants, expect, save);
}

TEST(WaveEquation, DiscretizedParameters1DFE1) {
   run_discretized_test<1>(1, 3, 8);
}

TEST(WaveEquation, DiscretizedParameters1DFE2) {
   run_discretized_test<1>(2, 4, 8);
}

TEST(WaveEquation, DiscretizedParameters2DFE1) {
   run_discretized_test<2>(1, 3, 3);
}

TEST(WaveEquation, DiscretizedParameters2DFE2) {
   run_discretized_test<2>(2, 4, 3);
}

TEST(WaveEquation, DiscretizedParameters3DFE1) {
   run_discretized_test<3>(1, 3, 1);
}

TEST(WaveEquation, ReferenceTest1DFE1) {
   for (int steps = 128; steps <= 1024; steps *= 2)
      run_reference_test_constant<1>(1, 3, 10, Point<1, int>(2), Point<2>(1.0, 1.5), 2 * numbers::PI, steps,
            steps >= 64);

   for (int refine = 7; refine >= 1; refine--)
      run_reference_test_constant<1>(1, 3, refine, Point<1, int>(2), Point<2>(1.0, 1.5), 2 * numbers::PI, 1024, false);
}

TEST(WaveEquation, ReferenceTest1DFE2) {
   for (int steps = 16; steps <= 128; steps *= 2)
      run_reference_test_constant<1>(2, 4, 7, Point<1, int>(2), Point<2>(1.0, 1.5), 2 * numbers::PI, steps,
            steps >= 64);

   for (int refine = 6; refine >= 1; refine--)
      run_reference_test_constant<1>(2, 4, refine, Point<1, int>(2), Point<2>(1.0, 1.5), 2 * numbers::PI, 128, false);
}

TEST(WaveEquation, ReferenceTest2DFE1) {
   for (int steps = 16; steps <= 256; steps *= 2)
      run_reference_test_constant<2>(1, 3, 5, Point<2, int>(1, 2), Point<2>(1.0, 1.5), 2 * numbers::PI, steps,
            steps >= 64);

   for (int refine = 5; refine >= 1; refine--)
      run_reference_test_constant<2>(1, 3, refine, Point<2, int>(1, 2), Point<2>(1.0, 1.5), 2 * numbers::PI, 256,
            false);
}

TEST(WaveEquation, ReferenceTestAdaptive2DFE1) {
   for (int steps = 16; steps <= 256; steps *= 2)
      run_reference_test_adaptive<2>(1, 3, 5, Point<2, int>(1, 2), Point<2>(1.0, 1.5), 2 * numbers::PI, steps,
            steps >= 64, false);

   for (int refine = 5; refine >= 1; refine--)
      run_reference_test_adaptive<2>(1, 3, refine, Point<2, int>(1, 2), Point<2>(1.0, 1.5), 2 * numbers::PI, 256,
            false);
}

TEST(WaveEquation, ReferenceTestRefined2DFE1) {
   for (int steps = 16; steps <= 256; steps *= 2)
      run_reference_test_refined<2>(1, 3, 5, Point<2, int>(1, 2), Point<2>(1.0, 1.5), 2 * numbers::PI, steps,
            steps >= 64, false);

   for (int refine = 5; refine >= 1; refine--)
      run_reference_test_refined<2>(1, 3, refine, Point<2, int>(1, 2), Point<2>(1.0, 1.5), 2 * numbers::PI, 256, false);
}

TEST(WaveEquation, ReferenceTest3DFE1) {
   for (int steps = 8; steps <= 32; steps *= 2)
      run_reference_test_constant<3>(1, 3, 3, Point<3, int>(1, 2, 3), Point<2>(0.7, 1.2), 2 * numbers::PI, steps,
            steps >= 32);

   for (int refine = 2; refine >= 0; refine--)
      run_reference_test_constant<3>(1, 3, refine, Point<3, int>(1, 2, 3), Point<2>(0.7, 1.2), 2 * numbers::PI, 32,
            false);
}

TEST(WaveEquation, ReferenceTestNu1DFE1) {
   for (int steps = 128; steps <= 1024; steps *= 2)
      run_reference_test_nu<1>(1, 3, 10, Point<1, int>(2), Point<2>(1.0, 1.5), 2 * numbers::PI, steps, steps >= 64);

   for (int refine = 7; refine >= 1; refine--)
      run_reference_test_nu<1>(1, 3, refine, Point<1, int>(2), Point<2>(1.0, 1.5), 2 * numbers::PI, 1024, false);
}

TEST(WaveEquation, ReferenceTestNu1DFE2) {
   for (int steps = 16; steps <= 128; steps *= 2)
      run_reference_test_nu<1>(2, 4, 7, Point<1, int>(2), Point<2>(1.0, 1.5), 2 * numbers::PI, steps, steps >= 64);

   for (int refine = 6; refine >= 1; refine--)
      run_reference_test_nu<1>(2, 4, refine, Point<1, int>(2), Point<2>(1.0, 1.5), 2 * numbers::PI, 128, false);
}

TEST(WaveEquation, ReferenceTestNu2DFE1) {
   for (int steps = 16; steps <= 512; steps *= 2)
      run_reference_test_nu<2>(1, 3, 6, Point<2, int>(1, 2), Point<2>(1.0, 1.5), 2 * numbers::PI, steps, steps >= 64);

   for (int refine = 5; refine >= 1; refine--)
      run_reference_test_nu<2>(1, 3, refine, Point<2, int>(1, 2), Point<2>(1.0, 1.5), 2 * numbers::PI, 256, false);
}

TEST(WaveEquation, ReferenceTestNu3DFE1) {
   for (int steps = 8; steps <= 32; steps *= 2)
      run_reference_test_nu<3>(1, 3, 3, Point<3, int>(1, 2, 3), Point<2>(0.7, 1.2), 2 * numbers::PI, steps,
            steps >= 32);

   for (int refine = 2; refine >= 0; refine--)
      run_reference_test_nu<3>(1, 3, refine, Point<3, int>(1, 2, 3), Point<2>(0.7, 1.2), 2 * numbers::PI, 32, false);
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE ToTEnergyConverterFactoryTest

#include "../ToTEnergyConverterFactory.h"
#include "JPetLoggerInclude.h"
#include <boost/test/unit_test.hpp>
using namespace jpet_common_tools;
using namespace tot_energy_converter;

/// Returns Time-over-threshold for given deposited energy
/// the current parametrization is par1 + par2 * eDep
/// Returned value in ps, and eDep is given in keV.
double getToT1(double eDep, double par1 = -91958, double par2 = 19341)
{
  if (eDep < 0)
    return 0;
  double value = par1 + eDep * par2;
  return value;
}

BOOST_AUTO_TEST_SUITE(ToTEnergyConverterFactoryTestSuite)

BOOST_AUTO_TEST_CASE(test1)
{
  std::string formula1 = "pol1";
  std::vector<double> params1 = {-91958, 19341};
  std::vector<double> limits1 = {0, 100};

  std::string formula2 = "[0] + [1] * TMath::Log(x)";
  std::vector<double> params2 = {1, -2};
  std::vector<double> limits2 = {2, 100};

  std::map<std::string, boost::any> options = {
    {"ToTEnergyConverterFactory_Energy2ToTParameters_std::vector<double>", params1},
    {"ToTEnergyConverterFactory_Energy2ToTFunction_std::string", formula1},
    {"ToTEnergyConverterFactory_Energy2ToTFunctionLimits_std::vector<double>", limits1},
    {"ToTEnergyConverterFactory_ToT2EnergyParameters_std::vector<double>", params2},
    {"ToTEnergyConverterFactory_ToT2EnergyFunction_std::string", formula2},
    {"ToTEnergyConverterFactory_ToT2EnergyFunctionLimits_std::vector<double>", limits2}
  };

  ToTEnergyConverterFactory fact;
  fact.loadConverterOptions(options);
  auto conv = fact.getToTConverter();
  BOOST_CHECK_CLOSE(conv(0), getToT1(0), 0.1);
  BOOST_CHECK_CLOSE(conv(1), getToT1(1), 0.1);
  BOOST_CHECK_CLOSE(conv(10), getToT1(10), 0.1);
  BOOST_CHECK_CLOSE(conv(59.5), getToT1(59.5), 0.1);
  BOOST_CHECK_CLOSE(conv(99.9), getToT1(99.9), 0.1);

  auto conv2 = fact.getEnergyConverter();

  TF1 funcTest("funcTest", "[0] + [1] * TMath::Log(x)", 2, 100);
  funcTest.SetParameters(1, -2);
  BOOST_CHECK_CLOSE(conv2(3), funcTest.Eval(3), 0.1);
  BOOST_CHECK_CLOSE(conv2(5.5), funcTest.Eval(5.5), 0.1);
  BOOST_CHECK_CLOSE(conv2(45.25), funcTest.Eval(45.25), 0.1);
  BOOST_CHECK_CLOSE(conv2(91), funcTest.Eval(91), 0.1);
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_DETAIL_GENERIC_F_LOG_KERNEL_HPP_INCLUDED
#define BOOST_SIMD_ARCH_DETAIL_GENERIC_F_LOG_KERNEL_HPP_INCLUDED

#include <boost/simd/function/fast.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>
#include <boost/simd/function/simd/frexp.hpp>
#include <boost/simd/function/simd/is_less.hpp>
#include <boost/simd/function/simd/tofloat.hpp>
#include <boost/simd/function/simd/seladd.hpp>
#include <boost/simd/function/simd/minusone.hpp>
#include <boost/simd/function/simd/sqr.hpp>
#include <boost/simd/function/simd/multiplies.hpp>

#include <boost/simd/constant/sqrt_2o_2.hpp>
#include <boost/simd/constant/mone.hpp>

#include <boost/simd/arch/detail/scalar/horner.hpp>
#include <boost/simd/logical.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace detail
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  template < class A0,
             class Style ,
             class base_A0 = bd::scalar_of_t<A0>
           >
  struct kernel{};

  template < class A0, class style>
  struct kernel< A0, style, float>
  {
    using i_t = bd::as_integer_t<A0, signed>;
    using s_t = bd::scalar_of_t<A0>;

    static BOOST_FORCEINLINE void log(const A0& a0,
                           A0& fe,
                           A0& x,
                           A0& x2,
                           A0& y) BOOST_NOEXCEPT
    {
      i_t e;
      x = fast_(frexp)(a0, e);
      auto xltsqrthf = (x < Sqrt_2o_2<A0>());
      fe = seladd(xltsqrthf, tofloat(e), Mone<A0>());
      x =  minusone(seladd(xltsqrthf, x, x));
      x2 = sqr(x);
      // performances informations using this kernel for nt2::log
      // exhaustive and bench tests with g++-4.7 sse4.2 or scalar give:
      // at most 0.5 ulp  for input in [0, 3.40282e+38]
      // 2130706656 values computed.
      // 2127648316 values (99.86%)  within 0.0 ULPs
      //    3058340 values (0.14%)   within 0.5 ULPs
      // bench produces  8.9 cycles/value (simd) 34.5 cycles/value (scalar) full computation
      // bench produces  7.1 cycles/value (simd) 32.2 cycles/value (scalar) with NO_DENORMALS, NO_INVALIDS etc.
      y =  horner< BOOST_SIMD_HORNER_COEFF_T( s_t
                                     , 8
                                     , (0xbda5dff0, //     -8.0993533e-02
                                        0x3e0229f9, //      1.2711324e-01
                                        0xbe04d6b7, //     -1.2972532e-01
                                        0x3e116e80, //      1.4202309e-01
                                        0xbe2a6aa0, //     -1.6642237e-01
                                        0x3e4cd0a3, //      2.0001464e-01
                                        0xbe800064, //     -2.5000298e-01
                                        0x3eaaaaa9  //      3.3333328e-01
                                       )
                                      )>(x)*x*x2;
    }
  };
} } }

#endif


#include <ros/ros.h>
#include <math.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h>
#include <tf2_ros/transform_broadcaster.h>
#include <std_msgs/Empty.h>
#include "quadrotor_msgs/PositionCommand.h"
#include "quadrotor_msgs/PolynomialTrajectory.h"

using std::vector;

const int  _DIM_x = 0;
const int  _DIM_y = 1;
const int  _DIM_z = 2;

enum ServerState{INIT, TRAJ, HOVER} _state = INIT;
ros::Publisher _cmd_pub, _cmd_vis_pub, _track_err_trig_pub;
Eigen::Vector3d _curr_posi;
Eigen::Vector3d _goal_point;
quadrotor_msgs::PositionCommand _cmd, _last_cmd;
Eigen::Vector3d _initial_pos;
Eigen::Quaterniond _initial_q;
bool _first_odom = true;

// configuration for polynomial trajectory
int _n_segment = 0;
int _traj_id = 0;
uint32_t _traj_flag = 0;
Eigen::VectorXd _time;
Eigen::MatrixXd _coef[3];
vector<int> _order;
double _mag_coeff;
ros::Time _final_time = ros::TIME_MIN;
ros::Time _start_time = ros::TIME_MAX;
double _start_yaw = 0.0, _final_yaw = 0.0;
bool _receive_traj = false;
double _real_yaw;
Eigen::Quaterniond _q;
Eigen::Vector3d _euler;

//yaw control
double _yaw_error, _last_yaw_error, _cum_yaw_error, _slop_yaw_error;
geometry_msgs::Point _yaw;

//so3 ctrl
double pos_gain[3] = { 5.7, 5.7, 6.2 };
double vel_gain[3] = { 3.4, 3.4, 4.0 };

/*
  * use Qingjiushao's method to calculate the value of f(t) = p0 + p1*t^1 + ... + pn*t^n
  * coeff[i]: pn, pn-1, pn-2, ..., p1, p0
  * order: n
  */
inline double calPosFromCoeff(double t, const Eigen::VectorXd &coeff, const int& order)
{
  double f = coeff[0];
  for (int i=0; i<order; ++i)
  {
    f = t * f + coeff[i+1];
  }
  return f;
}

inline double calVelFromCoeff(double t, const Eigen::VectorXd &coeff, const int& order)
{
  double f = order * coeff[0];
  for (int i=0; i<order-1; ++i)
  {
    f = t * f + (order - i - 1) * coeff[i+1];
  }
  return f;
}

inline double calAccFromCoeff(double t, const Eigen::VectorXd &coeff, const int& order)
{
  double f = order * (order-1) * coeff[0];
  for (int i=0; i<order - 2; ++i)
  {
    f = t * f + (order - i - 1) * (order - i - 2) * coeff[i+1];
  }
  return f;
}

inline void calPVAFromCoeff(Eigen::Vector3d& pos, Eigen::Vector3d& vel, Eigen::Vector3d& acc,
                            const Eigen::VectorXd &x_coeff, const Eigen::VectorXd &y_coeff, const Eigen::VectorXd &z_coeff,
                            double t, const int& order)
{
  pos[0] = calPosFromCoeff(t, x_coeff, order);
  pos[1] = calPosFromCoeff(t, y_coeff, order);
  pos[2] = calPosFromCoeff(t, z_coeff, order);
  vel[0] = calVelFromCoeff(t, x_coeff, order);
  vel[1] = calVelFromCoeff(t, y_coeff, order);
  vel[2] = calVelFromCoeff(t, z_coeff, order);
  acc[0] = calAccFromCoeff(t, x_coeff, order);
  acc[1] = calAccFromCoeff(t, y_coeff, order);
  acc[2] = calAccFromCoeff(t, z_coeff, order);
}

Eigen::Vector3d getRPY(const Eigen::Quaterniond &quat)
{
    double rotMat[3][3] = {(1 - 2 * (quat.y() * quat.y() + quat.z() * quat.z())), 2 * (quat.x() * quat.y() - quat.w() * quat.z()), 2 * (quat.x() * quat.z() + quat.w() * quat.y()),
                           2 * (quat.x() * quat.y() + quat.w() * quat.z()), (1 - 2 * (quat.x() * quat.x() + quat.z() * quat.z())), 2 * (quat.y() * quat.z() - quat.w() * quat.x()),
                           2 * (quat.x() * quat.z() - quat.w() * quat.y()), 2 * (quat.y() * quat.z() + quat.w() * quat.x()), (1 - 2 * (quat.x() * quat.x() + quat.y() * quat.y()))};

    double yaw, pitch, roll;
    if (rotMat[2][0] != 1 && rotMat[2][0] != -1)
    {
        double yaw1, yaw2, pitch1, pitch2, roll1, roll2;
        pitch1 = -asin(rotMat[2][0]);
        pitch2 = M_PI - pitch1;
        double cos_pitch1 = cos(pitch1);
        double cos_pitch2 = cos(pitch2);
        roll1 = atan2(rotMat[2][1] / cos_pitch1, rotMat[2][2] / cos_pitch1);
        roll2 = atan2(rotMat[2][1] / cos_pitch2, rotMat[2][2] / cos_pitch2);
        yaw1 = atan2(rotMat[1][0] / cos_pitch1, rotMat[0][0] / cos_pitch1);
        yaw2 = atan2(rotMat[1][0] / cos_pitch2, rotMat[0][0] / cos_pitch2);
        if (fabs(pitch1) <= fabs(pitch2))
        {
            yaw = yaw1;
            pitch = pitch1;
            roll = roll1;
        }
        else
        {
            yaw = yaw2;
            pitch = pitch2;
            roll = roll2;
        }
    }
    else if (rotMat[2][0] == 1)
    {
        yaw = 0;
        pitch = M_PI / 2;
        roll = yaw + atan2(rotMat[0][1], rotMat[0][2]);
    }
    else
    {
        yaw = 0;
        pitch = -M_PI / 2;
        roll = -yaw + atan2(-rotMat[0][1], -rotMat[0][2]);
    }

    return Eigen::Vector3d(roll, pitch, yaw);
}

void polyTrajCallback(const quadrotor_msgs::PolynomialTrajectory& traj)
{
  if (traj.action == quadrotor_msgs::PolynomialTrajectory::ACTION_ADD)
  {
    ROS_WARN("[SERVER] Loading the trajectory.");
    if ((int)traj.trajectory_id < _traj_id) return ;

    _state = TRAJ;
    _traj_flag = quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_READY;
    _traj_id = traj.trajectory_id;
    _n_segment = traj.num_segment;
    _final_time = _start_time = traj.header.stamp;
    _time.resize(_n_segment);

    _order.clear();
    for (int idx = 0; idx < _n_segment; ++idx)
    {
      _final_time += ros::Duration(traj.time[idx]);
      _time(idx) = traj.time[idx];
      _order.push_back(traj.order[idx]);
    }
    _start_yaw = traj.start_yaw;
    _final_yaw = traj.final_yaw;
    _mag_coeff = traj.mag_coeff;

    int max_order = *max_element( begin( _order ), end( _order ) );
    _coef[_DIM_x] = Eigen::MatrixXd::Zero(max_order + 1, _n_segment);
    _coef[_DIM_y] = Eigen::MatrixXd::Zero(max_order + 1, _n_segment);
    _coef[_DIM_z] = Eigen::MatrixXd::Zero(max_order + 1, _n_segment);

    //ROS_WARN("stack the coefficients");
    int shift = 0;
    for (int idx = 0; idx < _n_segment; ++idx)
    {
      int order = traj.order[idx];
      for (int j = 0; j < (order + 1); ++j)
      {
        _coef[_DIM_x](j, idx) = traj.coef_x[shift + j];
        _coef[_DIM_y](j, idx) = traj.coef_y[shift + j];
        _coef[_DIM_z](j, idx) = traj.coef_z[shift + j];
      }
      shift += (order + 1);
    }

    //compute the goal
    _goal_point[0] = calPosFromCoeff(_time(_n_segment-1), _coef[_DIM_x].col(_n_segment-1), _order[_n_segment-1]);
    _goal_point[1] = calPosFromCoeff(_time(_n_segment-1), _coef[_DIM_y].col(_n_segment-1), _order[_n_segment-1]);
    _goal_point[2] = calPosFromCoeff(_time(_n_segment-1), _coef[_DIM_z].col(_n_segment-1), _order[_n_segment-1]);

    _receive_traj = true;
  }
  else if (traj.action == quadrotor_msgs::PolynomialTrajectory::ACTION_ABORT)
  {
    ROS_WARN("[SERVER] Aborting the trajectory! EMERGENCY STOP!!");
    _state = HOVER;
    _traj_flag = quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_COMPLETED;
  }
  else if (traj.action == quadrotor_msgs::PolynomialTrajectory::ACTION_WARN_IMPOSSIBLE)
  {
    _state = HOVER;
    _traj_flag = quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_IMPOSSIBLE;
  }
}

void odomCallbck(const nav_msgs::OdometryConstPtr odom)
{
  _curr_posi[0] = odom->pose.pose.position.x;
  _curr_posi[1] = odom->pose.pose.position.y;
  _curr_posi[2] = odom->pose.pose.position.z;
  if (_first_odom)
  {
    _first_odom = false;
    _initial_pos[0] = odom->pose.pose.position.x;
    _initial_pos[1] = odom->pose.pose.position.y;
    _initial_pos[2] = odom->pose.pose.position.z;
    _initial_q.w() = odom->pose.pose.orientation.w;
    _initial_q.x() = odom->pose.pose.orientation.x;
    _initial_q.y() = odom->pose.pose.orientation.y;
    _initial_q.z() = odom->pose.pose.orientation.z;
  }

  /* no publishing before receive traj */
//  if (!_receive_traj)
//    return;

  // #1. check if it is right state
  if (_state == INIT)
  {
    _cmd.header = odom->header;

    _cmd.position.x = _initial_pos[0];
    _cmd.position.y = _initial_pos[1];
    _cmd.position.z = _initial_pos[2];

    _cmd.velocity.x = 0.0;
    _cmd.velocity.y = 0.0;
    _cmd.velocity.z = 0.0;

    _cmd.acceleration.x = 0.0;
    _cmd.acceleration.y = 0.0;
    _cmd.acceleration.z = 0.0;

  }
  if (_state == HOVER)
  {
    _cmd.header = odom  ->header;
    _cmd.trajectory_flag = quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_COMPLETED;

    _cmd.position = _last_cmd.position;

    _cmd.velocity.x = 0.0;
    _cmd.velocity.y = 0.0;
    _cmd.velocity.z = 0.0;

    _cmd.acceleration.x = 0.0;
    _cmd.acceleration.y = 0.0;
    _cmd.acceleration.z = 0.0;

    _cmd.yaw_dot = 0.0;
    _cmd.yaw = _last_cmd.yaw;
  }
  // #2. locate the trajectory segment
  if (_state == TRAJ)
  {
    //(odom freq >= cmd timer freq) has to be satisfied or it will cause several diffferent cmd in the same time stamp

    double t = std::max(0.0, (ros::Time::now() - _start_time).toSec());
    if (t >= (_final_time - _start_time).toSec() - 0.02)
      _state = HOVER;

    // #3. calculate the desired states
    //ROS_WARN("[SERVER] the time : %.3lf\n, n = %d, m = %d", t, _n_order, _n_segment);
    for (int idx = 0; idx < _n_segment; ++idx)
    {
      if (t > _time[idx] && idx + 1 < _n_segment)
      {
        t -= _time[idx];
      }
      else
      {
        _cmd.header = odom->header;

        int cur_order = _order[idx];
        Eigen::Vector3d pos(0.0, 0.0, 0.0), vel(0.0, 0.0, 0.0), acc(0.0, 0.0, 0.0);
        calPVAFromCoeff(pos, vel, acc, _coef[_DIM_x].col(idx), _coef[_DIM_y].col(idx), _coef[_DIM_z].col(idx), t, cur_order);
        _cmd.position.x = pos[0];
        _cmd.position.y = pos[1];
        _cmd.position.z = pos[2];
        _cmd.velocity.x = vel[0];
        _cmd.velocity.y = vel[1];
        _cmd.velocity.z = vel[2];
        _cmd.acceleration.x = acc[0];
        _cmd.acceleration.y = acc[1];
        _cmd.acceleration.z = acc[2];

		//use look_forward yaw planning
        double look_forward_time = std::min(1/vel.norm(), std::max(0.0, (_final_time - _start_time).toSec() - t - 0.02));
        if (look_forward_time == 0)
        {
          _cmd.yaw = _last_cmd.yaw;
        }
        else
        {
          Eigen::Vector3d pos_lf(0.0, 0.0, 0.0), vel_lf(0.0, 0.0, 0.0), acc_lf(0.0, 0.0, 0.0);
          calPVAFromCoeff(pos_lf, vel_lf, acc_lf, _coef[_DIM_x].col(idx), _coef[_DIM_y].col(idx), _coef[_DIM_z].col(idx), t + look_forward_time, cur_order);
          //use look_forward yaw planning
          _cmd.yaw = atan2(pos_lf[1] - pos[1], pos_lf[0] - pos[0]);
          //use tangent direction for yaw planning
          //_cmd.yaw = atan2(_cmd.velocity.y, _cmd.velocity.x); //(-pi, pi]
          double d_yaw = _cmd.yaw - _last_cmd.yaw;
          if (d_yaw >= M_PI)
          {
            d_yaw -= M_PI;
            d_yaw -= M_PI;
          }
          if (d_yaw <= -M_PI)
          {
            d_yaw += M_PI;
            d_yaw += M_PI;
          }
          double d_yaw_abs = fabs(d_yaw);
          if (d_yaw_abs >= 0.02)
            _cmd.yaw = _last_cmd.yaw + d_yaw / d_yaw_abs * 0.02;
        }
        //or use tangent direction for yaw planning

        _last_cmd = _cmd;
        break;
      }
    }
  }
  // #4. just publish
  _cmd_pub.publish(_cmd);
  Eigen::Vector3d desire_pos(_cmd.position.x, _cmd.position.y, _cmd.position.z);

  static tf2_ros::TransformBroadcaster br_map_ego_desired;
  geometry_msgs::TransformStamped transformStamped;
  transformStamped.header.stamp = odom->header.stamp;
  transformStamped.header.frame_id = "map";
  transformStamped.child_frame_id = "ego_desired";
  transformStamped.transform.translation.x = _cmd.position.x;
  transformStamped.transform.translation.y = _cmd.position.y;
  transformStamped.transform.translation.z = _cmd.position.z;
  Eigen::AngleAxisd aa(_cmd.yaw, Eigen::Vector3d::UnitZ());
  Eigen::Quaterniond d_q(aa);
  transformStamped.transform.rotation.x = d_q.x();
  transformStamped.transform.rotation.y = d_q.y();
  transformStamped.transform.rotation.z = d_q.z();
  transformStamped.transform.rotation.w = d_q.w();
  br_map_ego_desired.sendTransform(transformStamped);

  if ((_curr_posi - desire_pos).norm() >= 1.0)
  {
    std_msgs::Empty empty_msg;
    _track_err_trig_pub.publish(empty_msg);
  }

  if ((_curr_posi - _goal_point).norm() <= 0.01)
    _state = HOVER;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "traj_server");
  ros::NodeHandle node;

  ros::Subscriber poly_sub = node.subscribe("planning/poly_traj", 1, polyTrajCallback);
  ros::Subscriber odom_sub = node.subscribe("/curr_state_sub_topic", 50, odomCallbck, ros::TransportHints().tcpNoDelay());
  //ros::Timer cmd_timer = node.createTimer(ros::Duration(0.01), cmdCallback);
  _cmd_pub = node.advertise<quadrotor_msgs::PositionCommand>("/position_cmd", 50);
  _cmd_vis_pub = node.advertise<visualization_msgs::Marker>("planning/position_cmd_vis", 10);
  _track_err_trig_pub = node.advertise<std_msgs::Empty>("/trig/tracking_err", 1);

  /* control parameter for so3 control*/
  _cmd.kx[0] = pos_gain[0];
  _cmd.kx[1] = pos_gain[1];
  _cmd.kx[2] = pos_gain[2];

  _cmd.kv[0] = vel_gain[0];
  _cmd.kv[1] = vel_gain[1];
  _cmd.kv[2] = vel_gain[2];

  ros::Duration(1.0).sleep();
  ROS_WARN("[Traj server]: ready.");
  ros::spin();
  return 0;
}


/*
    Boost Competency Test - GSoC 2020
    digu_J - Digvijay Janartha
    NIT Hamirpur - INDIA
*/

#include <algorithm>
#include <iostream>
#include <iterator>
#include <utility>
#include <vector>
#include <random>
#include <chrono>

#include <boost/geometry/geometry.hpp>

#include "../includes/concave_hull_k_nearest_neighbours_optimized.hpp"

namespace bg = boost::geometry;
using bg::dsv;

typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;
typedef bg::model::multi_point<point_t> mpoint_t;
typedef bg::model::polygon<point_t> polygon_t;
typedef bg::model::segment<point_t> segment_t;

std::mt19937_64 rang(std::chrono::high_resolution_clock::now().time_since_epoch().count());

int my_rand(int l, int r)
{
    if (l > r)
    {
        std::swap(l, r);
    }
	std::uniform_int_distribution <int> uid(l, r);
	return uid(rang);
}

bool test(mpoint_t &input, mpoint_t &output)
{
    // First test is to check if all the points must lie inside or on the concave hull.
    polygon_t p;
    for (int i = 0; i < boost::size(output); ++i)
    {
        bg::append(p, output[i]);
    }
    for (int i = 0; i < boost::size(input); ++i)
    {
        if (!(bg::covered_by(input[i], p)))
        {
            std::cout << dsv(input[i]) << "\n";
            return false;
        }
    }

    // Second test is to check if no two edges of the concave hull intersects each other.
    for (int i = 0; i < int(boost::size(output)) - 1; ++i)
    {
        segment_t cur(output[i], output[i + 1]);
        for (int j = 0; j < int(boost::size(output)) - 1; ++j)
        {
            if (bg::equals(output[i], output[j]) or bg::equals(output[i], output[j + 1]))
            {
                continue;
            }
            if (bg::equals(output[i + 1], output[j]) or bg::equals(output[i + 1], output[j + 1]))
            {
                continue;
            }
            segment_t check(output[j], output[j + 1]);
            if (bg::intersects(check, cur))
            {
                std::cout << dsv(check) << "\n";
                std::cout << dsv(cur) << "\n";
                return false;
            }
        }
    }
    return true;
}

int main()
{
    std::cout << std::fixed << std::setprecision(0);
    #ifdef HOME
        freopen("input.txt", "r", stdin);
        freopen("output.txt", "w", stdout);
    #endif

    for (int tt = 1; tt <= 50; ++tt) // 50 random test cases
    {
        mpoint_t mpt1, hull1, hull2;

        int points = 30, k = 3;
        for (int i = 0; i < points; ++i)
        {
            int x = my_rand(0, 100);
            int y = my_rand(0, 100);
            bg::append(mpt1, point_t(x, y));
        }

        algo3::ConcaveHullKNN(mpt1, hull2, k);
        assert(test(mpt1, hull2) == true);
    }

    return 0;
}


#include <boost/math/complex/asinh.hpp>


#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <bits/stdc++.h>
#include <ros/ros.h>
#include <math.h>
#include <stdio.h>
#include <dwm1001/anchor.h>
#include <sensor_msgs/Imu.h>
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/TwistStamped.h"
#include <tf/transform_broadcaster.h>
#define PI (double)acos(-1.0)

int flag=0;
double X,P,Q;
int set_=0;
double theta_meas,theta_meas_1,r,K,P_p,X_p,error_1;
int set_p=0;
double T,m_last_range_time,u,X_e,M,error,yaw_imu_prev;
double x_1,x_2,y_1,y_2,z_1,z_2;

Eigen::Quaternionf q;
Eigen::Quaternionf q_;
Eigen::MatrixXf R_mat(3,3);
Eigen::MatrixXf rpy(3,1);
geometry_msgs::PoseStamped pose;
double error_threshold;
double l;
Eigen::Quaternionf q_imu;
Eigen::MatrixXf rpy_imu(3,1);
double yaw_imu;

void setState(double Y)
{
    X = Y;
}
void setCovariance(double S)
{
    P = S;
}

void Initialize()
{

    boost::shared_ptr<sensor_msgs::Imu const> final_msg;
    ros::Duration one_second(0.5);
    final_msg = ros::topic::waitForMessage<sensor_msgs::Imu>("/mavros/imu/data",one_second);
    if(final_msg != NULL){
    q = Eigen::Quaternionf(final_msg->orientation.w, final_msg->orientation.x, final_msg->orientation.y, final_msg->orientation.z);
    R_mat= q.toRotationMatrix();
    rpy = q.toRotationMatrix().eulerAngles(2, 1, 0);
    ROS_WARN("yaw initialized %f",rpy(2,0)*180/PI);
    X = rpy(2,0)-PI ;
    }
    P = 1;

}

void correction_step()
{
    //l = 0.2;
    theta_meas_1 = (x_2 - x_1) ;
    if(theta_meas_1>l)
    theta_meas_1 = l;
    //l = std::sqrt( std::pow( x_1 - x_2, 2) + std::pow( y_1 - y_2, 2) + std::pow( z_1 - z_2, 2) ) ;
    theta_meas = (((theta_meas_1/l)*-90.0) + 90.0);
    if(theta_meas > 90)
        theta_meas = (theta_meas )*PI/180;
    else
        theta_meas = theta_meas*PI/180;

    r = std::pow(r,2) ;
    //ROS_WARN("runing");
    K = P / (P + r );
    P_p = ( 1 - K ) * P;
    X_p = X + K * (theta_meas - X);
    // decide to take the range info or not.
    flag = 0;
    //ROS_WARN("measured distance %f, angle %f",theta_meas_1,theta_meas*180/PI);
    error_1 = std::fabs(X_p - X);
    if(error_1 < error_threshold){
        setState(X_p);
        setCovariance(P_p);
        set_= 0;
        return ;
    } else {

        ROS_WARN("Update too large: predicted %f measured-- %f with length %f", X , theta_meas,l);
        ROS_WARN("measurement setup %f with %f",x_1,x_2);
        set_=set_+1;
        if(set_>10)
            Initialize();
        return ;
    }

}

void prediction_step(const geometry_msgs::TwistStamped::ConstPtr& msg)
{
    T = msg->header.stamp.toSec() - m_last_range_time;
    if(T>1){
        T = 1;
    } else if(T<0){
        T = 0.02;
    }

    u = msg->twist.angular.z;
    X_e = X + T * u;
    M = P + Q;
    m_last_range_time = msg->header.stamp.toSec();
    error = std::fabs(X-X_e);
    //ROS_WARN("predicted %f",X_e);
    if(error < error_threshold){
        //ROS_WARN("\n sucess too large: %f", error);
        set_p=0;
        setState(X_e);
        setCovariance(M);
        return ;
    } else {

        ROS_WARN("\n Estimate too large: %f", error);
        set_p=set_p+1;
        if(set_p>10)
            Initialize();
        return ;
    }

}

void tag1_cb(const dwm1001::anchor::ConstPtr& msg)
{
    x_1 = msg->x;
    y_1 = msg->y;
    z_1 = msg->z;
    if(flag==1)
        correction_step();
}

void tag2_cb(const dwm1001::anchor::ConstPtr& msg)
{
    x_2 = msg->x;
    y_2 = msg->y;
    z_2 = msg->z;
    flag=1;
}

void imu_cb(const sensor_msgs::Imu::ConstPtr& msg)
{
    //conerts the acceleration from body frame to earth frame(NED)
    q_imu = Eigen::Quaternionf(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);

    //Eigen::MatrixXf rpy(3,1);
    rpy_imu = q_imu.toRotationMatrix().eulerAngles(0, 1, 2);
    //acc_ang.header.stamp = ros::Time::now();
    //acc_ang.vector.x = rpy_imu(0,0)*(180.0/3.14159265358979);
    //acc_ang.vector.y = rpy_imu(1,0)*(180.0/3.14159265358979);
    yaw_imu = rpy_imu(2,0);

}

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

    ros::init(argc,argv,"yaw");
    ros::NodeHandle nh;
    //Initializinging the parameters
    Initialize();
    nh.getParam("yaw/tag_r", r);
    nh.getParam("yaw/imu_q", Q);
    nh.getParam("yaw/distance", l);
    nh.getParam("yaw/error_threshold", error_threshold);
    Q = Q*Q;
    tf::TransformBroadcaster br;
    tf::Transform transform;

    //Subscriber and Publisher for the data. remapped in the launch file to the topic required
    ros::Subscriber anchor_1 = nh.subscribe("tag_1",10,tag1_cb);
    ros::Subscriber anchor_2 = nh.subscribe("tag_2",10,tag2_cb);
    ros::Subscriber velocity_sb = nh.subscribe("velocity",10,prediction_step);
    ros::Subscriber imu_sb = nh.subscribe("imu",10,imu_cb);

    ros::Publisher fused_theta = nh.advertise<geometry_msgs::PoseStamped>("yaw_angle", 10);
    transform.setOrigin( tf::Vector3(0.0, 0.0, 1.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
    ros::Rate loop_rate(30);
    while(ros::ok()){

        pose.header.stamp = ros::Time::now();
        pose.header.frame_id = "uwb";
    if(isnan(X)==1)
            Initialize();
        //pose.pose.position.x= X*180/PI;
        if(abs(yaw_imu_prev - yaw_imu) > 170*PI/180)
            yaw_imu = yaw_imu_prev;
        //pose.pose.position.y= abs(yaw_imu*180/PI-16);
        yaw_imu_prev = yaw_imu;
        pose.pose.position.z= 0;
	//ROS_WARN("yaw %f compared %f",X*180/PI,yaw_imu*180/PI-16);
        q_ = Eigen::AngleAxisf(0.0, Eigen::Vector3f::UnitX())*Eigen::AngleAxisf(0.0, Eigen::Vector3f::UnitY())*Eigen::AngleAxisf(X, Eigen::Vector3f::UnitZ());
        pose.pose.orientation.w = q_.w();
        pose.pose.orientation.x = q_.x();
        pose.pose.orientation.y = q_.y();
        pose.pose.orientation.z = q_.z();

        fused_theta.publish(pose);
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "local_origin", "uwb"));
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}


#include <unordered_set>
#include <tuple>

#include <Eigen/Core>

#include "vision.hpp"
#include "world.hpp"

namespace cuauv {
namespace fishbowl {

camera::camera(world* w, Eigen::Quaterniond* pq, Eigen::Vector3d* px, const Eigen::Quaterniond& q, const Eigen::Vector3d& x, double f)
    : pq(pq)
    , px(px)
    , q(q)
    , x(x)
    , f(f)
    , w(w)
    , C(Eigen::Matrix<double, 3, 4>::Zero())
{
    step();
}

void camera::step()
{
    C.block<3, 3>(0, 0) = (q.conjugate() * pq->conjugate()).matrix();
    C.col(3) = C.block<3, 3>(0, 0) * (-(*px) - ((*pq) * x));
}

std::tuple<double, double, double, double> camera::query(entity_id id)
{
    const entity& e = w->get_entity(id);
    const Eigen::Vector3d tx = e.x;
    // target position homogenous
    Eigen::Vector4d txh(tx[0], tx[1], tx[2], 1);
    // target position in the camera frame
    Eigen::Vector3d txc = C * txh;

    if (txc[0] == 0) {
        return std::make_tuple(0, 0, 0, 0);
    }

    txc[1] *= f/txc[0];
    txc[2] *= f/txc[0];

    return std::make_tuple(txc[1], txc[2], txc[0] > 0 ? e.get_r() * f/txc[0] : 0, txc[0]);
}

} // namespace fishbowl
} // namespace cuauv


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

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

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

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

#ifndef LATTICE_TYPES_HPP
#define LATTICE_TYPES_HPP

#include <string>
#include <Eigen/Dense>

namespace lattice {

enum boundary_t { open, periodic };

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> basis_t;

// set of spanning vectors
typedef Eigen::Matrix<long, Eigen::Dynamic, Eigen::Dynamic> span_t;

// extent vector = diagonal spanning matrix
typedef Eigen::Matrix<long, Eigen::Dynamic, 1> extent_t;

// coordinate vector
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> coordinate_t;

// unitcell offset
typedef Eigen::Matrix<long, Eigen::Dynamic, 1> offset_t;

} // end namespace lattice

#endif


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file multi_sensor_gaussian_filter_test.cpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#include <gtest/gtest.h>
#include "../typecast.hpp"

#include <Eigen/Dense>

#include "gaussian_filter_test_suite.hpp"
#include <fl/util/meta.hpp>
#include <fl/filter/gaussian/quadrature/unscented_quadrature.hpp>
#include <fl/filter/gaussian/robust_multi_sensor_gaussian_filter.hpp>
#include <fl/model/sensor/linear_cauchy_sensor.hpp>
#include <fl/model/sensor/body_tail_sensor.hpp>
#include <fl/model/sensor/linear_gaussian_sensor.hpp>

using namespace fl;

template <
    int StateDimension,
    int ObsrvDimension,
    int Count,               // Local observation model count
    int FilterIterations
>
struct RobustMultiSensorGfTestConfiguration
{
    enum : signed int
    {
        StateDim = StateDimension,
        InputDim = 1,
        ObsrvDim = ObsrvDimension,
        Iterations = FilterIterations
    };

    template <typename ModelFactory>
    struct FilterDefinition
    {
        enum : signed int
        {
            // compile time size (positive for static and -1 for dynamic)
            Size  = ExpandSizes<Count, ModelFactory::Sizes>::Value
        };

        // ================================================================== //
        // == Define Process Model                                         == //
        // ================================================================== //
        typedef typename ModelFactory::LinearTransition Transition;

        // ================================================================== //
        // == Define Body Tail Observation Model                           == //
        // ================================================================== //
        typedef typename ModelFactory::LinearObservation::Obsrv Obsrv;
        typedef typename ModelFactory::LinearObservation::State State;

        typedef fl::LinearCauchySensor<Obsrv, State> CauchyModel;

        typedef fl::BodyTailSensor<
                    typename ModelFactory::LinearObservation,
                    CauchyModel
                > BodyTailSensor;

        typedef BodyTailSensor LocalSensor;

        // ================================================================== //
        // == Define Joint Body Tail Observation Model                     == //
        // ================================================================== //
        typedef JointSensor<
                    MultipleOf<LocalSensor, Size>
                > JointSensor;

        // ================================================================== //
        // == Define Integration Quadrature                                == //
        // ================================================================== //
        typedef UnscentedQuadrature Quadrature;

        // ================================================================== //
        // == Define the filter                                            == //
        // ================================================================== //
        typedef RobustMultiSensorGaussianFilter<
                    Transition, JointSensor, Quadrature
                > Type;
    };

    template <typename ModelFactory>
    static typename FilterDefinition<ModelFactory>::Type
    create_filter(ModelFactory&& factory)
    {
        typedef FilterDefinition<ModelFactory> Definition;

        typedef typename Definition::Type Filter;
        typedef typename Definition::CauchyModel CauchyModel;
        typedef typename Definition::BodyTailSensor BodyTailSensor;
        typedef typename Definition::JointSensor JointSensor;

        auto body_model = factory.create_sensor();
        auto tail_model = CauchyModel();
        tail_model.noise_covariance(tail_model.noise_covariance() * 10.);

        return Filter(
            factory.create_linear_state_model(),
            JointSensor(BodyTailSensor(body_model, tail_model, 0.1), Count),
            typename Definition::Quadrature());
    }
};

typedef ::testing::Types<
            StaticTest<RobustMultiSensorGfTestConfiguration<6, 3, 48, 30>>
        > TestTypes;

INSTANTIATE_TYPED_TEST_CASE_P(RobustMultiSensorGaussianFilterTest,
                              GaussianFilterTest,
                              TestTypes);