#pragma once
#include <vector>
#include <string>
#include <utility>

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

#include <opencv2/opencv.hpp>
#include <opencv2/videoio.hpp>
#define win64
//#define linux
#define normalization
//#define posit
#define EPSILON 1e-3

const int G_land_num = 74;
const int G_train_pic_id_num = 3300;
const int G_nShape = 47;
const int G_nVerts = 11510;
const int G_nFaces = 11540;
const int G_test_num = 77;
const int G_iden_num = 77;
const int G_inner_land_num = 59;
const int G_line_num = 50;
const int G_jaw_land_num = 20;

const float G_rand_rot_border = 0.1;
const float G_rand_tslt_border = 10;
const float G_rand_s_border = 10;
const float G_rand_f_border = 100;

const int G_rnd_rot = 5;
const int G_rnd_tslt = 5;
const int G_rnd_exp = 15;
const int G_rnd_user = 5;
const int G_rnd_camr = 5;
const int G_trn_factor = 35;

const int G_target_type_size = G_nShape -1 + 2 + 3 + 2 * G_land_num;
const int G_dde_K = 50;

const float pi = acos(-1);
const float G_rand_angle_border = 15.0* pi / 180;

const int G_tslt_num = 2;
const int G_angle_num = 3;

struct Target_type {
	Eigen::VectorXf exp;
	Eigen::RowVector3f tslt;
	//Eigen::Matrix3f rot;
	Eigen::Vector3f angle;
	Eigen::MatrixX2f dis;

};

struct DataPoint
{
	cv::Mat image;
	//cv::Rect face_rect;
	std::vector<cv::Point2d> landmarks;
	//std::vector<cv::Point2d> init_shape;
	Target_type shape;
	Eigen::VectorXf user;
	Eigen::RowVector2f center;
	Eigen::MatrixX2f land_2d;
#ifdef posit
	float f;
#endif // posit
#ifdef normalization
	Eigen::MatrixX3f s;
#endif

	Eigen::VectorXi land_cor;
};

float cal_3d_vtx(
	Eigen::MatrixXf &bldshps,
	Eigen::VectorXf &user, Eigen::VectorXf &exp, int vtx_idx, int axis);

//void recal_dis(DataPoint &data, Eigen::MatrixXf &bldshps);
void recal_dis_ang(DataPoint &data, Eigen::MatrixXf &bldshps);

void cal_mesh(DataPoint &data, Eigen::MatrixXf &bldshps, Eigen::MatrixX3f &mesh);
void cal_3d_land(DataPoint &data, Eigen::MatrixXf &bldshps, Eigen::MatrixX3f &land_3d);

void test_load_mesh(std::vector <DataPoint> &data, Eigen::MatrixXf &bldshps, int idx, std::string path);
void test_load_3d_land(std::vector <DataPoint> &data, Eigen::MatrixXf &bldshps, int idx, std::string path);

void draw_point(cv::Mat& img, cv::Point2f fp, cv::Scalar color);

void draw_delaunay(cv::Mat& img, cv::Subdiv2D& subdiv, cv::Scalar delaunay_color);
void draw_voronoi(cv::Mat& img, cv::Subdiv2D& subdiv);

void test_del_tri(Eigen::MatrixX2f &points, cv::Mat& img, cv::Subdiv2D subdiv);

void cal_del_tri(
	Eigen::MatrixX2f &points, cv::Mat& img, std::vector<cv::Vec6f> &triangleList);

void cal_del_tri(
	const std::vector<cv::Point2d> &points, cv::Rect &rect,
	std::vector<cv::Vec6f> &triangleList, Eigen::MatrixX3i &tri_idx);
//void test_del_tri(
//	std::vector < std::vector <cv::Mat_<uchar> > >& imgs,
//	iden *ide, std::vector<std::pair<int, std::pair <int, int> > > &del_tri);

double dis_cv_pt(cv::Point2d pointO, cv::Point2d pointA);
double cal_cv_area(cv::Point2d point0, cv::Point2d point1, cv::Point2d point2);

//void update_2d_land(DataPoint &data, Eigen::MatrixXf &bldshps);
//void update_2d_land_0ide(DataPoint &data, Eigen::MatrixXf &exp_r_t_all_matrix);
void update_2d_land_ang_0ide(DataPoint &data, Eigen::MatrixXf &exp_r_t_all_matrix);

void cal_2d_land_i_ang_0ide(
	std::vector<cv::Point2d> &ans, const Target_type &data, Eigen::MatrixXf &exp_r_t_all_matrix,
	const DataPoint &data_dp);
//void cal_2d_land_i(
//	std::vector<cv::Point2d> &ans, const Target_type &data, Eigen::MatrixXf &bldshps, DataPoint &ini_data);
Target_type shape_difference(const Target_type &s1, const Target_type &s2);
Target_type shape_adjustment(Target_type &shape, Target_type &offset);

void show_image(cv::Mat img, cv::Rect rect, std::vector<cv::Point2d> landmarks);
void show_image_0rect(cv::Mat img, std::vector<cv::Point2d> landmarks);
void show_image_land_2d(cv::Mat img, Eigen::MatrixX2f &land);

//void cal_2d_land_i_0dis(
//	std::vector<cv::Point2d> &ans, Eigen::MatrixXf &bldshps, DataPoint &data);
void cal_2d_land_i_0dis_ang(
	std::vector<cv::Point2d> &ans, Eigen::MatrixXf &bldshps, DataPoint &data);

void rect_scale(cv::Rect &rect, double scale);

void save_for_debug(DataPoint &temp, std::string name);

void print_datapoint(DataPoint &data);
void print_target(Target_type &data);
void save_datapoint(DataPoint &temp, std::string save_name);

void cal_exp_r_t_all_matrix(
	Eigen::MatrixXf &bldshps, DataPoint &data, Eigen::MatrixXf &result);

void target2vector(Target_type &data, Eigen::VectorXf &ans);
void vector2target(Eigen::VectorXf &data, Target_type &ans);

float cal_3d_vtx_0ide(
	Eigen::MatrixXf &exp_r_t_all_matrix, Eigen::VectorXf &exp, int vtx_idx, int axis);

//Eigen::RowVector3f get_uler_angle(Eigen::Matrix3f R);
//Eigen::Matrix3f get_r_from_angle(float angle, int axis);
//Eigen::Matrix3f get_r_from_angle(const Eigen::Vector3f &angle);

Eigen::Vector3f get_uler_angle_zyx(Eigen::Matrix3f R);
Eigen::Matrix3f get_r_from_angle_zyx(const Eigen::Vector3f &angle);


/*============================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center (DKFZ)
All rights reserved.

Use of this source code is governed by a 3-clause BSD license that can be
found in the LICENSE file.

============================================================================*/

//#include <boost>
#include <chrono>
#include <mitkCommon.h>

#include "mitkPALinearSpectralUnmixingFilter.h"
#include "mitkPASpectralUnmixingFilterBase.h"
#include "mitkPASpectralUnmixingFilterVigra.h"
#include "mitkPASpectralUnmixingSO2.h"

#include <mitkCommandLineParser.h>
#include <mitkException.h>
#include <mitkIOUtil.h>
#include <mitkUIDGenerator.h>

#include <itksys/SystemTools.hxx>

#include "mitkPreferenceListReaderOptionsFunctor.h"

struct InputParameters
{
  std::string inputPath;
  std::string outputPath;
  int numberOfInputs;
};

InputParameters parseInput(int argc, char *argv[])
{
  MITK_INFO << "Parsing arguments...";
  mitkCommandLineParser parser;

  parser.setCategory("MITK-Photoacoustics");
  parser.setTitle("Mitk Spectral Unmixing App");
  parser.setDescription("Batch processing for spectral unmixing.");
  parser.setContributor("Computer Assisted Medical Interventions, DKFZ");

  parser.setArgumentPrefix("--", "-");

  parser.beginGroup("Required parameters");
  parser.addArgument("inputPath",
                     "i",
                     mitkCommandLineParser::Directory,
                     "Input folder (directory)",
                     "input folder",
                     us::Any(),
                     false, false, false, mitkCommandLineParser::Input);
  parser.addArgument("outputPath",
                     "o",
                     mitkCommandLineParser::Directory,
                     "Input save folder (directory)",
                     "input save folder",
                     us::Any(),
                     false, false, false, mitkCommandLineParser::Output);
  parser.addArgument("numberOfInputs",
                     "n",
                     mitkCommandLineParser::Int,
                     "Number of Input files",
                     "number of inputs",
                     us::Any(),
                     false);
  parser.endGroup();

  InputParameters input;

  std::map<std::string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
  if (argc == 0)
    exit(-1);

  for (int i = 0; i < argc; ++i)
  {
    MITK_INFO << argv[i];
  }

  if (parsedArgs.count("inputPath"))
  {
    input.inputPath = us::any_cast<std::string>(parsedArgs["inputPath"]);
  }
  else
  {
    MITK_ERROR << "Error: No inputPath";
    mitkThrow() << "Error: No inputPath";
  }

  if (parsedArgs.count("outputPath"))
  {
    input.outputPath = us::any_cast<std::string>(parsedArgs["outputPath"]);
  }
  else
  {
    MITK_ERROR << "Error: No outputPath";
    mitkThrow() << "Error: No outputPath";
  }
  if (parsedArgs.count("numberOfInputs"))
  {
    input.numberOfInputs = us::any_cast<int>(parsedArgs["numberOfInputs"]);
  }
  else
  {
    MITK_ERROR << "Error: No number of Inputs";
    mitkThrow() << "Error: No number of Inputs";
  }
  MITK_INFO << "Parsing arguments...[Done]";
  return input;
}

mitk::pa::SpectralUnmixingFilterBase::Pointer GetFilterInstance(std::string algorithm)
{
  mitk::pa::SpectralUnmixingFilterBase::Pointer spectralUnmixingFilter;

  if (algorithm == "QR")
  {
    spectralUnmixingFilter = mitk::pa::LinearSpectralUnmixingFilter::New();
    dynamic_cast<mitk::pa::LinearSpectralUnmixingFilter *>(spectralUnmixingFilter.GetPointer())
      ->SetAlgorithm(mitk::pa::LinearSpectralUnmixingFilter::AlgortihmType::HOUSEHOLDERQR);
  }

  else if (algorithm == "SVD")
  {
    spectralUnmixingFilter = mitk::pa::LinearSpectralUnmixingFilter::New();
    dynamic_cast<mitk::pa::LinearSpectralUnmixingFilter *>(spectralUnmixingFilter.GetPointer())
      ->SetAlgorithm(mitk::pa::LinearSpectralUnmixingFilter::AlgortihmType::JACOBISVD);
  }

  else if (algorithm == "LU")
  {
    spectralUnmixingFilter = mitk::pa::LinearSpectralUnmixingFilter::New();
    dynamic_cast<mitk::pa::LinearSpectralUnmixingFilter *>(spectralUnmixingFilter.GetPointer())
      ->SetAlgorithm(mitk::pa::LinearSpectralUnmixingFilter::AlgortihmType::FULLPIVLU);
  }

  else if (algorithm == "NNLS")
  {
    spectralUnmixingFilter = mitk::pa::SpectralUnmixingFilterVigra::New();
    dynamic_cast<mitk::pa::SpectralUnmixingFilterVigra *>(spectralUnmixingFilter.GetPointer())
      ->SetAlgorithm(mitk::pa::SpectralUnmixingFilterVigra::VigraAlgortihmType::LARS);
  }

  else if (algorithm == "WLS")
  {
    spectralUnmixingFilter = mitk::pa::SpectralUnmixingFilterVigra::New();
    dynamic_cast<mitk::pa::SpectralUnmixingFilterVigra *>(spectralUnmixingFilter.GetPointer())
      ->SetAlgorithm(mitk::pa::SpectralUnmixingFilterVigra::VigraAlgortihmType::WEIGHTED);

    /*std::vector<int> weigthVec = {39, 45, 47};

    for (int i = 0; i < 3; ++i)
    {
      dynamic_cast<mitk::pa::SpectralUnmixingFilterVigra *>(spectralUnmixingFilter.GetPointer())
        ->AddWeight(weigthVec[i]);
    }*/
  }
  return spectralUnmixingFilter;
}

void add_weight(int weights, mitk::pa::SpectralUnmixingFilterBase::Pointer m_SpectralUnmixingFilter)
{
  std::vector<int> weigthVec = { 30, 32, 33, 35, 37, 38, 40, 41, 43, 44, 45, 46, 47, 47,
    47, 47, 47, 46, 46, 45, 44, 44, 43, 42, 42, 41 };

  for (int i = 0; i < weights; ++i)
  {
    dynamic_cast<mitk::pa::SpectralUnmixingFilterVigra *>(m_SpectralUnmixingFilter.GetPointer())
      ->AddWeight(weigthVec[i]);
  }
}

int main(int argc, char *argv[])
{
  auto input = parseInput(argc, argv);

  std::string inputDir = input.inputPath;
  std::string outputDir = input.outputPath;
  unsigned int N = input.numberOfInputs;

/*
  //maybee try with "itk system tools"

  //auto test = itksys::SystemTools::GetFilenameName(argv[0]).c_str();

  //MITK_INFO << "test: " << test;

  /  +++ temporary solution BEGIN +++
  std::vector<std::string> files;
  std::string file;
  for (int i = 1; i < 34; ++i)
  {

    if (i < 10)
    {
      file = "E:/NHDATA/sdmas_beamformed/merged/static-oxy_sdmas_00" + std::to_string(i) + "_merged.nrrd";
    }
    else
    {
      file = "E:/NHDATA/sdmas_beamformed/merged/static-oxy_sdmas_0" + std::to_string(i) + "_merged.nrrd";
    }
    files.push_back(file);
  }
  /  +++ temporary solution END +++

  std::vector<std::string> files;
  std::string file;
  for (int i = 0; i < 7; ++i)
  {
    file = "E:/NHCAMI/cami-experimental/PAI/spectralUnmixing/inSilico/paImages/selection/noiselevel1_rep1000_wavelength_selction_data_" +
      std::to_string(i) + ".nrrd";
    files.push_back(file);
  }
  std::vector<std::string> files;
  std::string file;
  file = "E:/NHCAMI/cami-experimental/PAI/spectralUnmixing/inSilico/paImages/selection/noiselevel1_rep1000_wavelength_selction_data.nrrd";
  files.push_back(file);*/

  std::vector<std::string> algorithms = { "QR", "LU", "SVD", "NNLS", "WLS" };
  int repetition = 6000;

  for (unsigned alg = 0; alg < 5; ++alg)
  {
    ofstream myerrorfile;
    myerrorfile.open("E:/NHDATA/time/time_evaluation_" + std::to_string(repetition)+"_" + algorithms[alg] + "_new02.txt");

    int ctr = 0;
    for(int i = 2; i < 27; ++i)
    {
      myerrorfile << std::to_string(i) + "\t";
      std::string file;
      if (i < 10)
        file = "E:/NHDATA/time/input/time_0" + std::to_string(i) + ".nrrd";
      else
        file = "E:/NHDATA/time/input/time_" + std::to_string(i) + ".nrrd";

      auto m_inputImage = mitk::IOUtil::Load<mitk::Image>(file);

      MITK_INFO << "File: " << i;

      for (int j = 0; j < repetition; ++j)
        {
           std::chrono::steady_clock::time_point _start;
           _start = std::chrono::steady_clock::now();

          mitk::pa::SpectralUnmixingFilterBase::Pointer m_SpectralUnmixingFilter = GetFilterInstance(algorithms[alg]);
          m_SpectralUnmixingFilter->SetInput(m_inputImage);
          m_SpectralUnmixingFilter->AddOutputs(2);
          m_SpectralUnmixingFilter->Verbose(false);
          m_SpectralUnmixingFilter->RelativeError(false);
          m_SpectralUnmixingFilter->AddChromophore(mitk::pa::PropertyCalculator::ChromophoreType::OXYGENATED);
          m_SpectralUnmixingFilter->AddChromophore(mitk::pa::PropertyCalculator::ChromophoreType::DEOXYGENATED);

          for (int wl = 0; wl < i; ++wl)
          {
            m_SpectralUnmixingFilter->AddWavelength(700 + wl * 10);
          }

          if (alg == 4)
          {
            add_weight(i, m_SpectralUnmixingFilter);
          }

          m_SpectralUnmixingFilter->Update();

          auto output1 = m_SpectralUnmixingFilter->GetOutput(0);
          auto output2 = m_SpectralUnmixingFilter->GetOutput(1);

          m_SpectralUnmixingFilter = nullptr;

          std::chrono::steady_clock::time_point _end(std::chrono::steady_clock::now());
          myerrorfile << std::chrono::duration_cast<std::chrono::duration<double>>(_end - _start).count() << "\t";

        /*std::string unmixingOutputHbO2 = "E:/NHDATA/time/output/time_" + std::to_string(i) + ".nrrd";
        std::string unmixingOutputHb = "E:/NHDATA/time/output/time_" + std::to_string(i) + ".nrrd";
        mitk::IOUtil::Save(output1, unmixingOutputHbO2);
        mitk::IOUtil::Save(output2, unmixingOutputHb);

        //auto m_sO2 = mitk::pa::SpectralUnmixingSO2::New();
        //m_sO2->Verbose(false);
        //auto output1 = m_SpectralUnmixingFilter->GetOutput(0);
        //auto output2 = m_SpectralUnmixingFilter->GetOutput(1);

        //std::string unmixingOutputHbO2 ="E:/NHDATA/time/input/time_" + std::to_string(i) + ".nrrd";
        //std::string unmixingOutputHb = outputDir + "/SUOutput/" + "Hb_" + algorithms[alg] + "_" + str_ctr + ".nrrd";
        //mitk::IOUtil::Save(output1, unmixingOutputHbO2);
        //mitk::IOUtil::Save(output2, unmixingOutputHb);

        //m_sO2->SetInput(0, output1);
        //m_sO2->SetInput(1, output2);

        //m_sO2->Update();

        //mitk::Image::Pointer sO2 = m_sO2->GetOutput(0);
        //sO2->SetSpacing(output1->GetGeometry()->GetSpacing());

        //std::string outputSo2 = outputDir + "/So2/" + algorithms[alg] + "/So2_" + algorithms[alg] + "_" + str_ctr + ".nrrd";
        //std::string outputSo2 = outputDir + "/" + algorithms[alg] + "_sel_" + str_ctr + ".nrrd";
        //std::string outputSo2 = outputDir + "/" + algorithms[alg] + "_sel.nrrd";
        //mitk::IOUtil::Save(sO2, outputSo2);

        //std::string outputSo2 = "E:/NHDATA/time/output/time_" + std::to_string(i) + algorithms[alg] + ".nrrd";
        //mitk::IOUtil::Save(sO2, outputSo2);*/
        }
      myerrorfile << "\n";
    }
  myerrorfile.close();
  }
  MITK_INFO << "Spectral Unmixing DONE";
}


#include <v4r/recognition/model_only_source.h>

#include <boost/graph/connected_components.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/undirected_graph.hpp>

template<typename Full3DPointT, typename PointInT>
void
computeFacesImpl (typename v4r::Model<PointInT> & model)
{
    float voxel_resolution = 0.005f;
    float seed_resolution = 0.05f;
    typename pcl::SupervoxelClustering<PointInT> super (voxel_resolution, seed_resolution, false);
    super.setInputCloud (model.assembled_);
    super.setColorImportance (0.f);
    super.setSpatialImportance (1.f);
    super.setNormalImportance (2.f);
    super.setNormalCloud(model.normals_assembled_);
    std::map <uint32_t, typename pcl::Supervoxel<PointInT>::Ptr > supervoxel_clusters;
    pcl::console::print_highlight ("Extracting supervoxels!\n");
    super.extract (supervoxel_clusters);
    //super.refineSupervoxels(5, supervoxel_clusters);
    pcl::console::print_info ("Found %d supervoxels\n", supervoxel_clusters.size ());

    pcl::PointCloud<pcl::PointXYZL>::Ptr supervoxels_labels_cloud = super.getLabeledCloud();
    uint32_t max_label = super.getMaxLabel();

    pcl::PointCloud<pcl::PointNormal>::Ptr sv_normal_cloud = super.makeSupervoxelNormalCloud (supervoxel_clusters);

    std::vector<int> label_to_idx;
    label_to_idx.resize(max_label + 1, -1);
    typename std::map <uint32_t, typename pcl::Supervoxel<PointInT>::Ptr>::iterator sv_itr,sv_itr_end;
    sv_itr = supervoxel_clusters.begin ();
    sv_itr_end = supervoxel_clusters.end ();
    int idx=0;
    for ( ; sv_itr != sv_itr_end; ++sv_itr, idx++)
    {
      label_to_idx[sv_itr->first] = idx;
    }

    std::vector< std::vector<bool> > adjacent;
    adjacent.resize(supervoxel_clusters.size());
    for(size_t i=0; i < (supervoxel_clusters.size()); i++)
        adjacent[i].resize(supervoxel_clusters.size(), false);

    std::multimap<uint32_t, uint32_t> supervoxel_adjacency;
    super.getSupervoxelAdjacency (supervoxel_adjacency);
    //To make a graph of the supervoxel adjacency, we need to iterate through the supervoxel adjacency multimap
    std::multimap<uint32_t,uint32_t>::iterator label_itr = supervoxel_adjacency.begin ();
    std::cout << "super voxel adjacency size:" << supervoxel_adjacency.size() << std::endl;
    for ( ; label_itr != supervoxel_adjacency.end (); )
    {
      //First get the label
      uint32_t supervoxel_label = label_itr->first;
      Eigen::Vector3f normal_super_voxel = sv_normal_cloud->points[label_to_idx[supervoxel_label]].getNormalVector3fMap();
      normal_super_voxel.normalize();
      //Now we need to iterate through the adjacent supervoxels and make a point cloud of them
      std::multimap<uint32_t,uint32_t>::iterator adjacent_itr = supervoxel_adjacency.equal_range (supervoxel_label).first;
      for ( ; adjacent_itr!=supervoxel_adjacency.equal_range (supervoxel_label).second; ++adjacent_itr)
      {
        Eigen::Vector3f normal_neighbor_supervoxel = sv_normal_cloud->points[label_to_idx[adjacent_itr->second]].getNormalVector3fMap();
        normal_neighbor_supervoxel.normalize();

        if(normal_super_voxel.dot(normal_neighbor_supervoxel) > 0.9f)
        {
            adjacent[label_to_idx[supervoxel_label]][label_to_idx[adjacent_itr->second]] = true;
        }
      }

      //Move iterator forward to next label
      label_itr = supervoxel_adjacency.upper_bound (supervoxel_label);
    }

    typedef boost::adjacency_matrix<boost::undirectedS, int> Graph;
    Graph G(supervoxel_clusters.size());
    for(size_t i=0; i < supervoxel_clusters.size(); i++)
    {
        for(size_t j=(i+1); j < supervoxel_clusters.size(); j++)
        {
            if(adjacent[i][j])
                boost::add_edge(i, j, G);
        }
    }

    std::vector<int> components (boost::num_vertices (G));
    int n_cc = static_cast<int> (boost::connected_components (G, &components[0]));
    std::cout << "Number of connected components..." << n_cc << std::endl;

    std::vector<int> cc_sizes;
    std::vector<std::vector<int> > ccs;
    std::vector<uint32_t> original_labels_to_merged;
    original_labels_to_merged.resize(supervoxel_clusters.size());

    ccs.resize(n_cc);
    cc_sizes.resize (n_cc, 0);
    typename boost::graph_traits<Graph>::vertex_iterator vertexIt, vertexEnd;
    boost::tie (vertexIt, vertexEnd) = vertices (G);
    for (; vertexIt != vertexEnd; ++vertexIt)
    {
      int c = components[*vertexIt];
      cc_sizes[c]++;
      ccs[c].push_back(*vertexIt);
      original_labels_to_merged[*vertexIt] = c;
    }

    for(size_t i=0; i < supervoxels_labels_cloud->points.size(); i++)
    {
        //std::cout << supervoxels_labels_cloud->points[i].label << " " << label_to_idx.size() << " " << original_labels_to_merged.size() << " " << label_to_idx[supervoxels_labels_cloud->points[i].label] << std::endl;
        if(label_to_idx[supervoxels_labels_cloud->points[i].label] < 0)
            continue;

        supervoxels_labels_cloud->points[i].label = original_labels_to_merged[label_to_idx[supervoxels_labels_cloud->points[i].label]];
    }

    model.faces_cloud_labels_ = supervoxels_labels_cloud;

    //    for(size_t i=0; i < cc_sizes.size(); i++)
    //    {
    //        std::cout << "cc size:" << cc_sizes[i] << std::endl;
    //    }

    //    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr supervoxels_rgb(new pcl::PointCloud<pcl::PointXYZRGBA>);
    //    supervoxels_rgb = super.getColoredCloud();

    //    pcl::visualization::PCLVisualizer vis("model smooth surfaces");
    //    int v1,v2, v3;
    //    vis.createViewPort(0,0,0.33,1.0,v1);
    //    vis.createViewPort(0.33, 0, 0.66, 1, v2);
    //    vis.createViewPort(0.66, 0, 1, 1, v3);
    //    vis.addPointCloud<PointInT>(model.assembled_, "model", v1);
    //    vis.addPointCloudNormals<pcl::PointNormal> (sv_normal_cloud,1,0.05f, "supervoxel_normals", v2);
    //    vis.addPointCloud(supervoxels_rgb, "labels", v2);

    //    pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZL> handler_labels(supervoxels_labels_cloud, "label");
    //    vis.addPointCloud(supervoxels_labels_cloud, handler_labels, "labels_", v3);
    //    vis.spin();
}

template< >
void
computeFacesImpl <pcl::PointXYZ, pcl::PointXYZ>
    (v4r::Model<pcl::PointXYZ> & model)
{
    (void) model;
    PCL_WARN("Not implemented for pcl::PointXYZ... this function would be available for PCL1.7.2 or higher\n");
}

template<typename Full3DPointT, typename PointInT>
void
v4r::ModelOnlySource<Full3DPointT, PointInT>::computeFaces (ModelT & model)
{
    //boost::shared_ptr< typename v4r::Model<PointInT> > m = model;
    computeFacesImpl<Full3DPointT, PointInT>(model);
}

template class V4R_EXPORTS v4r::ModelOnlySource<pcl::PointXYZRGBNormal, pcl::PointXYZRGB>;
template class V4R_EXPORTS v4r::ModelOnlySource<pcl::PointXYZ, pcl::PointXYZ>;


/* ForceConstants.cpp
 * -*- coding: utf-8 -*-
 *
 */

#include <boost/test/unit_test.hpp>

// system includes =============================================================

#include <vector>
#include <memory>
#include <Eigen/Dense>

// local includes ==============================================================

#include "../src/TrajectoryAnalyzer.hpp"
#include "../src/TrajectoryFile.hpp"
#include "../src/ProteinFile.hpp"
#include "../src/Protein.hpp"
#include "../src/Trajectory.hpp"
#include "../src/ProteinSegment.hpp"
#include "../src/DCD.hpp"

#include "utils/test.hpp"
#include "utils/log.hpp"

BOOST_AUTO_TEST_SUITE(compare_force_constants)

BOOST_AUTO_TEST_CASE(kk_matrix_global){
    TEST_MESSAGE("kk_matrix_global");

    std::vector<std::shared_ptr<TrajectoryFile>> dcd_files = {std::make_shared<DCD>("test/resources/mb_ca.dcd")};
    auto pdb = std::make_shared<PDB>("test/resources/integration/mb_ca.pdb");
    double temperature   = 120.0;
    int    ensemble_size = 2000;

    Trajectory trajectory(dcd_files);
    Protein protein(pdb);
    std::vector<std::shared_ptr<ProteinSegment>> protein_segments{
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 1, 151, COMPLETE_PROTEIN))};

    TrajectoryAnalyzer analyzer;
    analyzer.analyze(trajectory, protein_segments, temperature, ensemble_size);

    Eigen::MatrixXd force_constant = protein_segments[0]->force_constant();

    // average absolute error: 1.14614e-06
    // most bad values for (i,i+1)
    compare_force_constant_matrix(
                force_constant,
                "test/resources/forceconst/mb_ca_forceconst_kkhex.csv",
                10e-5,
                true,
                true);

    compare_force_constant_matrix(
                force_constant,
                "test/resources/forceconst/mb_ca_forceconst_kk.csv",
                10e-5,
                false,
                true);
}

BOOST_AUTO_TEST_CASE(kk_matrix_segments){
    TEST_MESSAGE("kk_matrix_segments");

    std::vector<std::shared_ptr<TrajectoryFile>> dcd_files = {std::make_shared<DCD>("test/resources/mb_ca.dcd")};
    auto pdb = std::make_shared<PDB>("test/resources/integration/mb_ca.pdb");
    double temperature = 120.0;
    int    ensemble_size = 2000;

    Trajectory trajectory(dcd_files);
    Protein protein(pdb);

    std::vector<std::pair<int,int>> segments = {{1,20},{18,37},{35,54},{52,71},{69,88},{86,105},{103,122},{120,139},{132,151}};
    std::vector<std::shared_ptr<ProteinSegment>> protein_segments{
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 1, 20, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 18, 37, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 35, 54, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 52, 71, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 69, 88, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 86, 105, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 103, 122, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 120, 139, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 132, 151, COMPLETE_PROTEIN))
    };

    TrajectoryAnalyzer analyzer;
    analyzer.analyze(trajectory, protein_segments, temperature, ensemble_size);

    for (size_t i=0; i<9; ++i)
    {

        Eigen::MatrixXd force_constants = protein_segments.at(i)->force_constant();
        std::stringstream ss;
        ss << "test/resources/forceconst/mb_ca_forceconst_segment0" << i+1 << "_kkhex.csv";
        compare_force_constant_matrix(force_constants, ss.str(), 10e-7, true, true);
    }
}

BOOST_AUTO_TEST_CASE(kk_matrix_helices){
    TEST_MESSAGE("kk_matrix_helices");

    std::vector<std::shared_ptr<TrajectoryFile>> dcd_files = {std::make_shared<DCD>("test/resources/mb_ca.dcd")};
    auto pdb = std::make_shared<PDB>("test/resources/integration/mb_ca.pdb");
    double temperature   = 120.0;
    int    ensemble_size = 2000;

    Trajectory trajectory(dcd_files);
    Protein protein(pdb);

    std::vector<std::pair<int,int>> helices = {{4,17},{21,35},{37,40},{52,56},{59,77},{83,95},{101,118},{125,149}};
    std::vector<std::shared_ptr<ProteinSegment>> protein_segments{
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 4,17, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 21,35, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 37,40, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 52,56, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 59,77, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 83,95, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 101,118, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 125,149, COMPLETE_PROTEIN)),
    };

    TrajectoryAnalyzer analyzer;
    analyzer.analyze(trajectory, protein_segments, temperature, ensemble_size);

    for (size_t i=0; i<8; ++i)
    {

        Eigen::MatrixXd force_constants = protein_segments.at(i)->force_constant();
        std::stringstream ss;
        ss << "test/resources/forceconst/mb_ca_forceconst_helix0" << i+1 << "_kkhex.csv";
        compare_force_constant_matrix(force_constants, ss.str(), 10e-7, true, true);
    };
}

BOOST_AUTO_TEST_SUITE_END()

// vim: tabstop=8 expandtab shiftwidth=4 softtabstop=4 fenc=utf-8


#include <boost/random/mersenne_twister.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/random/uniform_real_distribution.hpp>

#include <include/grassmann_pca.hpp>
#include <include/private/boost_ublas_row_iterator.hpp>

// generating a matrix of random numbers, each row will contain
// a data vector.
template <class matrix_t>
void generate_matrix(const int nb_elements, const int dimension, matrix_t& mat_data)
{
  static boost::random::mt19937 rng;
  boost::random::uniform_real_distribution<double> dist(-1000, 1000);

  mat_data.resize(nb_elements, dimension);
  for(int i = 0; i < nb_elements; i++)
  {
    for(int j = 0; j < dimension; j++)
    {
      mat_data(i, j) = dist(rng);
    }
  }
}

void example_grassmann_pca_impl()
{
  using namespace grassmann_averages_pca;
  using namespace grassmann_averages_pca::details::ublas_helpers;
  namespace ub = boost::numeric::ublas;

  typedef ub::vector<double> data_t;                         // type of the vectors
  typedef ub::matrix<double> matrix_t;                       // type of the structure holding the data

  typedef grassmann_pca< data_t > grassmann_pca_t;           // the type of the structure for the computation of the grassmann averages pca
                                                             // data_t tells which kind of structure will be used for internal computations
                                                             // and will be received from the data iterators.

  typedef row_iter<const matrix_t> const_row_iter_t;         // iterator on the data, deferencing one
                                                             // of these iterator should yield a structure
                                                             // convertible to data_t. This iterator iterates
                                                             // over the rows of a given matrix.

  const int dimensions = 5;                   // each vector is of dimension 5
  const int max_dimension_to_compute = 3;     // we want only the first 3 basis-vectors
  const int max_iterations = 1000;            // at most 1000 iterations before giving up for the current dimension

  // generating the data points
  matrix_t mat_data;
  generate_matrix(10000, dimensions, mat_data);

  // configure the first points
  const double initial_point[] = {0.2097, 0.3959, 0.5626, 0.2334, 0.6545}; // some dummy initial point
  data_t vec_initial_point(dimensions);
  std::copy(initial_point, initial_point + dimensions, vec_initial_point.begin());
  std::vector< data_t > v_init_points(max_dimension_to_compute, vec_initial_point);

  // allocate the output
  std::vector<data_t> basis_vectors(max_dimension_to_compute);

  // the instance
  grassmann_pca_t instance;

  // some configuration
  if(!instance.set_nb_processors(4))              // using 4 processors
  {
    std::cerr << "Error while setting the number of threads" << std::endl;
    return;
  }

  if(!instance.set_max_chunk_size(1000))          // using max chunk size of 1000
  {
    std::cerr << "Error while setting the chunk size" << std::endl;
    return;
  }

  if(!instance.set_nb_steps_pca(3))               // using 3 PCA steps
  {
    std::cerr << "Error while setting the number of PCA steps" << std::endl;
    return;
  }

  bool return_calue = instance.batch_process(
    max_iterations,
    max_dimension_to_compute,
    const_row_iter_t(mat_data, 0),
    const_row_iter_t(mat_data, mat_data.size1()),
    basis_vectors.begin(),
    &v_init_points);

  if(!return_calue)
  {
    std::cerr << "Error during the computation of the GrassmannAveragesPCA" << std::endl;
  }
}


#pragma once

//template<typename S>
//Real GetShapeBoundingSphereRadiusSquared(const S &s){
//}

#include <Meta/entity.hpp>
#include <boost/utility/enable_if.hpp>

template<typename S>
typename boost::enable_if<GenericSim::Meta::IsEntity<S>,const Real>::type
GetShapeBoundingSphereRadiusSquared(const S &s){
	return GetShapeBoundingSphereRadiusSquared_Impl::ByEntityConcept( s, typename GenericSim::Meta::GetEntityConcept<S>::Type() );
}

#include <boost/qvm/v.hpp>
#include <cmath>
namespace GetShapeBoundingSphereRadiusSquared_Impl{
	template<typename E>
	Real ByEntityConcept( const E &e, EC::Box ){
		return boost::qvm::mag2(e.at_e<EC::Box::Size>())*Real(0.25);
	}

	template<typename E>
	Real ByEntityConcept( const E &e, EC::Sphere ){
		return pow(e.at_e<EC::Sphere::Radius>(),2);
	}

	template<typename E>
	Real ByEntityConcept( const E &e, EC::Cylinder ){
		return pow(e.at_e<EC::Cylinder::Length>(),2)*Real(0.25)+pow(e.at_e<EC::Cylinder::Radius>(),2);
	}
}


#include <iostream>
#include <cmath>
#include <complex>
#include <boost/numeric/mtl/mtl.hpp>

template <typename T>
struct Magnitude
{
    typedef T type;
};

template <typename T>
struct Magnitude<std::complex<T> >
{
    typedef T type;
};

template <typename T, typename P>
struct Magnitude<mtl::dense_vector<T, P> >
{
    typedef typename Magnitude<T>::type type;
};

template <typename T, typename P>
struct Magnitude<mtl::dense2D<T, P> >
{
    typedef typename Magnitude<T>::type type;
};

template <typename T, typename P>
struct Magnitude<mtl::compressed2D<T, P> >
{
    typedef typename Magnitude<T>::type type;
};

template <typename T>
T inline min_magnitude(const T& x, const T& y)
{
    using std::abs;
    typename Magnitude<T>::type ax= abs(x), ay= abs(y);
    return ax < ay ? ax : ay;
}

template <typename T>
class cc
{
    //typedef typename Magnitude<T>::type  my_mag;
};

template <typename T>
struct my_mag2
{
    typedef typename Magnitude<T>::type type;
};

int main (int argc, char* argv[])
{
    using namespace std;
    cc<short> oo;

    // my_mag2<short>::type sm;

    std::cout << min_magnitude(3.5, -2.3) << '\n';
    double               d1= 3., d2= 4.;
    cout << "min |d1, d2| = " << min_magnitude(d1, d2) << '\n';

    std::complex<float> c1(3.5), c2(-2.3, 1.0);
    cout << min_magnitude(c1, c2) << '\n';

    Magnitude<double>::type x;

    return 0 ;

}


#ifndef QUADRIC_H
#define QUADRIC_H

#include <Core/RaCore.hpp>
#include <Core/Math/LinearAlgebra.hpp>

#include <Eigen/Eigenvalues>

namespace Ra
{
    namespace Core
    {

        /// A representation for quadratic equation as
        /// v^T A v + 2 b^T v + c
        ///
        /// Reminder on ProgressiveMesh
        /// ---------------------------
        ///
        ///     Calculate the Quadric of the edge Qe = sum(Qf) with f all the faces adjacent
        ///     to e
        ///
        ///     Calculate vmin = Ae^(-1) be                with Qe = (Ae, be, ce)
        ///
        ///     Calculate the error on the edge
        ///         error = vmin^T Ae vmin + 2 be^T vmin + ce       with Qe = (Ae, be, ce)

        //template<int _DIM = 3>
        template<int _DIM>
        class Quadric
        {
        public:
            static constexpr int DIM = _DIM;
            using Matrix = Eigen::Matrix<Scalar, DIM, DIM>;
            using Vector = Eigen::Matrix<Scalar, DIM, 1>;

            /// Construct an initialized quadric.
            Quadric();

            /// Construct and initialize a quadric.
            Quadric(const Matrix &a, const Vector &b, double c) : m_a(a), m_b(b), m_c(c) {}

            /// Construct a quadric from an other.
            Quadric(const Quadric<DIM> &q);

            /// Create a quadric from a normal vector n and the scalar which
            /// represent the distance from the plane of normal n to the origin
            /// \see compute
            Quadric(const Vector& n, Scalar ndotp);

            ~Quadric();

            /// Getters and setters

            inline const Matrix& getA() const;
            inline void setA(const Matrix &a);

            inline const Vector& getB() const;
            inline void setB(const Vector &b);

            inline const double & getC() const;
            inline void setC(const double &c);

            /// Create a quadric from a normal vector n and the scalar which
            /// represent the distance from the plane of normal n to the origin
            /// \deprecated Use constructor instead
            void compute(const Vector& n, double ndotp);

            /// Computes eigen values and vectors of matrix A
            inline typename Eigen::EigenSolver<Matrix3>::EigenvalueType computeEigenValuesA();
            inline typename Eigen::EigenSolver<Matrix3>::EigenvectorsType computeEigenVectorsA();

            /// Operators

            inline Quadric operator+(const Quadric& q) const;
            inline Quadric operator*(const Scalar scal);

            inline Quadric& operator+=(const Quadric& q);
            inline Quadric& operator*=(Scalar scal);

        private:

            Matrix m_a;
            Vector m_b;
            double m_c;
        };

    }

}

#include <Core/Math/Quadric.inl>

#endif // QUADRIC_H


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

  @copyright 2016 NumScale SAS
  @copyright 2016 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_FUNCTION_CEIL_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_CEIL_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!
  @ingroup group-arithmetic
    Function object implementing ceil capabilities

   Computes the ceil of its parameter.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = ceil(x);
    @endcode

    computes the smallest integral value of type @c T greater or equal to @c x.

    @par Decorators

    std_ for floating entries

    @see  floor, round, round2even, trunc, iceil

  **/
{
} }
#endif

#include <boost/simd/function/scalar/ceil.hpp>
#include <boost/simd/function/simd/ceil.hpp>

#endif


// Copyright 2002 Rensselaer Polytechnic Institute

// 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)

//  Authors: Lauren Foutz
//           Scott Hill

/*
  This file implements the functions

  template <class VertexListGraph, class DistanceMatrix,
    class P, class T, class R>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d,
    const bgl_named_params<P, T, R>& params)

  AND

  template <class VertexAndEdgeListGraph, class DistanceMatrix,
    class P, class T, class R>
  bool floyd_warshall_all_pairs_shortest_paths(
    const VertexAndEdgeListGraph& g, DistanceMatrix& d,
    const bgl_named_params<P, T, R>& params)
*/

#ifndef BOOST_GRAPH_FLOYD_WARSHALL_HPP
#define BOOST_GRAPH_FLOYD_WARSHALL_HPP

#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/relax.hpp>
#include <boost/concept/assert.hpp>

namespace boost
{
  namespace detail {
    template<typename T, typename BinaryPredicate>
    T min_with_compare(const T& x, const T& y, const BinaryPredicate& compare)
    {
      if (compare(x, y)) return x;
      else return y;
    }

    template<typename VertexListGraph, typename DistanceMatrix,
      typename BinaryPredicate, typename BinaryFunction,
      typename Infinity, typename Zero>
    bool floyd_warshall_dispatch(const VertexListGraph& g,
      DistanceMatrix& d, const BinaryPredicate &compare,
      const BinaryFunction &combine, const Infinity& inf,
      const Zero& zero)
    {
      typename graph_traits<VertexListGraph>::vertex_iterator
        i, lasti, j, lastj, k, lastk;

      for (boost::tie(k, lastk) = vertices(g); k != lastk; k++)
        for (boost::tie(i, lasti) = vertices(g); i != lasti; i++)
          if(d[*i][*k] != inf)
            for (boost::tie(j, lastj) = vertices(g); j != lastj; j++)
              if(d[*k][*j] != inf)
                d[*i][*j] =
                  detail::min_with_compare(d[*i][*j],
                                           combine(d[*i][*k], d[*k][*j]),
                                           compare);

      for (boost::tie(i, lasti) = vertices(g); i != lasti; i++)
        if (compare(d[*i][*i], zero))
          return false;
      return true;
    }
  }

  template <typename VertexListGraph, typename DistanceMatrix,
    typename BinaryPredicate, typename BinaryFunction,
    typename Infinity, typename Zero>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d,
    const BinaryPredicate& compare,
    const BinaryFunction& combine, const Infinity& inf,
    const Zero& zero)
  {
    BOOST_CONCEPT_ASSERT(( VertexListGraphConcept<VertexListGraph> ));

    return detail::floyd_warshall_dispatch(g, d, compare, combine,
    inf, zero);
  }

  template <typename VertexAndEdgeListGraph, typename DistanceMatrix,
    typename WeightMap, typename BinaryPredicate,
    typename BinaryFunction, typename Infinity, typename Zero>
  bool floyd_warshall_all_pairs_shortest_paths(
    const VertexAndEdgeListGraph& g,
    DistanceMatrix& d, const WeightMap& w,
    const BinaryPredicate& compare, const BinaryFunction& combine,
    const Infinity& inf, const Zero& zero)
  {
    BOOST_CONCEPT_ASSERT(( VertexListGraphConcept<VertexAndEdgeListGraph> ));
    BOOST_CONCEPT_ASSERT(( EdgeListGraphConcept<VertexAndEdgeListGraph> ));
    BOOST_CONCEPT_ASSERT(( IncidenceGraphConcept<VertexAndEdgeListGraph> ));

    typename graph_traits<VertexAndEdgeListGraph>::vertex_iterator
      firstv, lastv, firstv2, lastv2;
    typename graph_traits<VertexAndEdgeListGraph>::edge_iterator first, last;

    for(boost::tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
      for(boost::tie(firstv2, lastv2) = vertices(g); firstv2 != lastv2; firstv2++)
        d[*firstv][*firstv2] = inf;

    for(boost::tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
      d[*firstv][*firstv] = zero;

    for(boost::tie(first, last) = edges(g); first != last; first++)
    {
      if (d[source(*first, g)][target(*first, g)] != inf) {
        d[source(*first, g)][target(*first, g)] =
          detail::min_with_compare(
            get(w, *first),
            d[source(*first, g)][target(*first, g)],
            compare);
      } else
        d[source(*first, g)][target(*first, g)] = get(w, *first);
    }

    bool is_undirected = is_same<typename
      graph_traits<VertexAndEdgeListGraph>::directed_category,
      undirected_tag>::value;
    if (is_undirected)
    {
      for(boost::tie(first, last) = edges(g); first != last; first++)
      {
        if (d[target(*first, g)][source(*first, g)] != inf)
          d[target(*first, g)][source(*first, g)] =
            detail::min_with_compare(
              get(w, *first),
              d[target(*first, g)][source(*first, g)],
              compare);
        else
          d[target(*first, g)][source(*first, g)] = get(w, *first);
      }
    }

    return detail::floyd_warshall_dispatch(g, d, compare, combine,
      inf, zero);
  }

  namespace detail {
    template <class VertexListGraph, class DistanceMatrix,
      class WeightMap, class P, class T, class R>
    bool floyd_warshall_init_dispatch(const VertexListGraph& g,
      DistanceMatrix& d, WeightMap /*w*/,
      const bgl_named_params<P, T, R>& params)
    {
      typedef typename property_traits<WeightMap>::value_type WM;
      WM inf =
        choose_param(get_param(params, distance_inf_t()),
          std::numeric_limits<WM>::max BOOST_PREVENT_MACRO_SUBSTITUTION());

      return floyd_warshall_initialized_all_pairs_shortest_paths(g, d,
        choose_param(get_param(params, distance_compare_t()),
          std::less<WM>()),
        choose_param(get_param(params, distance_combine_t()),
          closed_plus<WM>(inf)),
        inf,
        choose_param(get_param(params, distance_zero_t()),
          WM()));
    }

    template <class VertexAndEdgeListGraph, class DistanceMatrix,
      class WeightMap, class P, class T, class R>
    bool floyd_warshall_noninit_dispatch(const VertexAndEdgeListGraph& g,
      DistanceMatrix& d, WeightMap w,
      const bgl_named_params<P, T, R>& params)
    {
      typedef typename property_traits<WeightMap>::value_type WM;

      WM inf =
        choose_param(get_param(params, distance_inf_t()),
          std::numeric_limits<WM>::max BOOST_PREVENT_MACRO_SUBSTITUTION());
      return floyd_warshall_all_pairs_shortest_paths(g, d, w,
        choose_param(get_param(params, distance_compare_t()),
          std::less<WM>()),
        choose_param(get_param(params, distance_combine_t()),
          closed_plus<WM>(inf)),
        inf,
        choose_param(get_param(params, distance_zero_t()),
          WM()));
    }

  }   // namespace detail

  template <class VertexListGraph, class DistanceMatrix, class P,
    class T, class R>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d,
    const bgl_named_params<P, T, R>& params)
  {
    return detail::floyd_warshall_init_dispatch(g, d,
      choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
      params);
  }

  template <class VertexListGraph, class DistanceMatrix>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d)
  {
    bgl_named_params<int,int> params(0);
    return detail::floyd_warshall_init_dispatch(g, d,
      get(edge_weight, g), params);
  }

  template <class VertexAndEdgeListGraph, class DistanceMatrix,
    class P, class T, class R>
  bool floyd_warshall_all_pairs_shortest_paths(
    const VertexAndEdgeListGraph& g, DistanceMatrix& d,
    const bgl_named_params<P, T, R>& params)
  {
    return detail::floyd_warshall_noninit_dispatch(g, d,
      choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
      params);
  }

  template <class VertexAndEdgeListGraph, class DistanceMatrix>
  bool floyd_warshall_all_pairs_shortest_paths(
    const VertexAndEdgeListGraph& g, DistanceMatrix& d)
  {
    bgl_named_params<int,int> params(0);
    return detail::floyd_warshall_noninit_dispatch(g, d,
      get(edge_weight, g), params);
  }

} // namespace boost

#endif


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

#define _USE_MATH_DEFINES
#include "mesh.h"
#include "transforms.h"

#include <Eigen/Geometry>
#include <cmath>

namespace
{
  const float PI = static_cast<float>(M_PI);
}

namespace scenepic
{
  Vector interpolate(const Vector& p0, const Vector& p1, float alpha)
  {
    return alpha * p0 + (1. - alpha) * p1;
  }

  void Mesh::add_cube(
    const Color& color,
    const Transform& transform,
    bool fill_triangles,
    bool add_wireframe)
  {
    this->check_instances();
    this->check_color(color);
    Vector p0(-0.5, -0.5, -0.5);
    Vector p1(+0.5, -0.5, -0.5);
    Vector p2(-0.5, +0.5, -0.5);
    Vector p3(+0.5, +0.5, -0.5);
    Vector p4(-0.5, -0.5, +0.5);
    Vector p5(+0.5, -0.5, +0.5);
    Vector p6(-0.5, +0.5, +0.5);
    Vector p7(+0.5, +0.5, +0.5);

    Mesh m = Mesh("").shared_color(m_shared_color).texture_id(m_texture_id);
    m.add_quad(
      color, p0, p2, p3, p1, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p1, p3, p7, p5, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p5, p7, p6, p4, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p4, p6, p2, p0, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p2, p6, p7, p3, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p4, p0, p1, p5, VectorNone(), fill_triangles, add_wireframe);

    if (!transform.isIdentity())
    {
      m.apply_transform(transform);
    }

    this->append_mesh(m);
  }

  void Mesh::add_thickline(
    const Color& color,
    const Vector& start_point,
    const Vector& end_point,
    float start_thickness,
    float end_thickness,
    const Transform& transform_init,
    bool fill_triangles,
    bool add_wireframe)
  {
    this->check_instances();
    this->check_color(color);

    // For convenience
    float length = (end_point - start_point).norm() * 0.5f;
    start_thickness *= 0.5f;
    end_thickness *= 0.5f;

    // Create points
    Vector p0(-length, -start_thickness, -start_thickness);
    Vector p1(+length, -end_thickness, -end_thickness);
    Vector p2(-length, +start_thickness, -start_thickness);
    Vector p3(+length, +end_thickness, -end_thickness);
    Vector p4(-length, -start_thickness, +start_thickness);
    Vector p5(+length, -end_thickness, +end_thickness);
    Vector p6(-length, +start_thickness, +start_thickness);
    Vector p7(+length, +end_thickness, +end_thickness);

    // Add quads to new mesh
    Mesh m = Mesh("").shared_color(m_shared_color).texture_id(m_texture_id);
    m.add_quad(
      color, p0, p2, p3, p1, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p1, p3, p7, p5, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p4, p5, p7, p6, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p4, p6, p2, p0, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p2, p6, p7, p3, VectorNone(), fill_triangles, add_wireframe);
    m.add_quad(
      color, p4, p0, p1, p5, VectorNone(), fill_triangles, add_wireframe);

    // Transform if not unit thickline
    Transform transform = transform_init;
    if (start_point != VectorNone())
    {
      assert(end_point != VectorNone());
      Vector center = 0.5 * (start_point + end_point);
      Vector axis = end_point - start_point;
      length = axis.norm();
      Transform rotation = Transforms::rotation_to_align_x_to_axis(axis);
      Transform translation = Transforms::translate(center);
      Transform tx = translation * rotation;
      if (transform.isIdentity())
      {
        transform = tx;
      }
      else
      {
        transform *= tx;
      }
    }

    if (!transform.isIdentity())
    {
      m.apply_transform(transform);
    }

    this->append_mesh(m);
  }

  void Mesh::add_cone(
    const Color& color,
    const Transform& transform,
    float truncation_height,
    std::uint32_t lat_count,
    std::uint32_t long_count,
    bool fill_triangles,
    bool add_wireframe)
  {
    this->check_instances();
    this->check_color(color);

    const float radius = 0.5f;
    Vector apex(-0.5f, 0., 0.);
    bool add_apex = std::abs(truncation_height - 1.) < 1e-6;
    const float base_center_x = 0.5f;
    Mesh m = Mesh("").shared_color(m_shared_color);
    for (std::uint32_t lat_index = 0; lat_index < lat_count; ++lat_index)
    {
      float alpha = static_cast<float>(lat_index) / lat_count;
      alpha = alpha * truncation_height;

      for (std::uint32_t long_index = 0; long_index < long_count; ++long_index)
      {
        float phi = long_index * 2.0f * PI / long_count;
        float cosPhi = std::cos(phi);
        float sinPhi = std::sin(phi);

        Vector base_point(base_center_x, cosPhi * radius, sinPhi * radius);
        Vector xyz = interpolate(apex, base_point, alpha);
        Vector normal = (base_point - apex).cross(xyz - Vector(xyz(0), 0, 0));
        normal = normal.cross(base_point - apex);
        normal.normalize();
        m.append_vertex(xyz, normal, color);
      }
    }

    if (add_apex)
    {
      m.append_vertex(apex, Vector(-1, 0, 0), color);
    }

    for (std::uint32_t lat_index = 0; lat_index < lat_count - 1; ++lat_index)
    {
      for (std::uint32_t long_index = 0; long_index < long_count; ++long_index)
      {
        auto base_index = lat_index * long_count;
        auto a = base_index + long_index;
        auto b = base_index + (long_index + 1) % long_count;
        auto c = base_index + long_index + long_count;
        auto d = base_index + (long_index + 1) % long_count + long_count;
        if (fill_triangles)
        {
          m.append_triangle(b, a, c);
          m.append_triangle(b, c, d);
        }

        if (add_wireframe)
        {
          m.append_line(a, b);
          m.append_line(b, d);
          m.append_line(d, c);
          m.append_line(c, a);
        }
      }
    }

    if (add_apex)
    {
      std::uint32_t lat_index = lat_count - 1;
      auto a = lat_count * long_count;
      for (std::uint32_t long_index = 0; long_index < long_count; ++long_index)
      {
        auto base_index = lat_index * long_count;
        auto b = base_index + long_index;
        auto c = base_index + (long_index + 1) % long_count;

        if (fill_triangles)
        {
          m.append_triangle(b, a, c);
        }

        if (add_wireframe)
        {
          m.append_line(a, b);
          m.append_line(a, c);
        }
      }
    }

    if (!transform.isIdentity())
    {
      m.apply_transform(transform);
    }

    this->append_mesh(m);
  }

  void Mesh::add_coordinate_axes(
    float length, float thickness, const Transform& transform)
  {
    this->check_instances();
    if (this->m_vertices.cols() == 6)
    {
      // shared color mesh
      std::cerr << "Converting shared color mesh to use vertex color "
                << "to accommodate coordinate axes.";
      VertexBuffer vertices = VertexBuffer::Zero(this->count_vertices(), 9);
      if (this->count_vertices())
      {
        vertices.leftCols(6) = this->m_vertices;
        vertices.col(6).fill(this->m_shared_color.r());
        vertices.col(7).fill(this->m_shared_color.g());
        vertices.col(8).fill(this->m_shared_color.b());
        this->m_shared_color = Color::None();
      }

      this->m_vertices = vertices;
    }
    else if (this->m_vertices.cols() == 8)
    {
      // image/texture mesh
      throw std::invalid_argument("Cannot add coordinate axes to a UV mesh");
    }

    Mesh m = Mesh("");
    m.add_thickline(
      {1, 0, 0}, {0, 0, 0}, {length, 0, 0}, thickness, 0.5f * thickness);
    m.add_thickline(
      {0, 1, 0}, {0, 0, 0}, {0, length, 0}, thickness, 0.5f * thickness);
    m.add_thickline(
      {0, 0, 1}, {0, 0, 0}, {0, 0, length}, thickness, 0.5f * thickness);
    m.add_sphere({1, 1, 1}, Transforms::scale(thickness * 1.1f));
    if (!transform.isIdentity())
    {
      m.apply_transform(transform);
    }

    this->append_mesh(m);
  }

  void Mesh::add_camera_frustum(
    const Color& color,
    float fov_y_degrees,
    float aspect_ratio,
    float depth,
    float thickness,
    const Transform& transform)
  {
    this->check_instances();
    this->check_color(color);
    Mesh m = Mesh("").shared_color(m_shared_color);
    const float fov_y_half_radians = fov_y_degrees / 2.0f * PI / 180.0f;
    const float height = depth * std::sin(fov_y_half_radians);
    const float width = height * aspect_ratio;

    m.add_thickline(
      color, {0, 0, 0}, {+width, +height, depth}, 0.4f * thickness, thickness);
    m.add_thickline(
      color, {0, 0, 0}, {+width, -height, depth}, 0.4f * thickness, thickness);
    m.add_thickline(
      color, {0, 0, 0}, {-width, -height, depth}, 0.4f * thickness, thickness);
    m.add_thickline(
      color,
      {0.0, 0.0, 0.0},
      {-width, +height, depth},
      0.4f * thickness,
      thickness);
    m.add_thickline(
      color,
      {+width, +height, depth},
      {-width, +height, depth},
      thickness,
      thickness);
    m.add_thickline(
      color,
      {-width, +height, depth},
      {-width, -height, depth},
      thickness,
      thickness);
    m.add_thickline(
      color,
      {-width, -height, depth},
      {+width, -height, depth},
      thickness,
      thickness);
    m.add_thickline(
      color,
      {+width, -height, depth},
      {+width, +height, depth},
      thickness,
      thickness);
    m.add_coordinate_axes(depth * 0.075f, thickness);

    if (!transform.isIdentity())
    {
      m.apply_transform(transform);
    }

    this->append_mesh(m);
  }

  void Mesh::add_camera_frustum(
    const Camera& camera, const Color& color, float thickness, float depth)
  {
    this->check_instances();
    this->check_color(color);
    Mesh m = Mesh("").shared_color(m_shared_color);
    Vector eye(0, 0, 0);
    Eigen::Vector3f top_left(-1, -1, 1);
    Eigen::Vector3f top_right(1, -1, 1);
    Eigen::Vector3f bottom_left(-1, 1, 1);
    Eigen::Vector3f bottom_right(1, 1, 1);

    Transform unprojection = camera.projection().inverse();
    top_left =
      (unprojection * top_left.homogeneous()).hnormalized().normalized() *
      depth;
    top_right =
      (unprojection * top_right.homogeneous()).hnormalized().normalized() *
      depth;
    bottom_left =
      (unprojection * bottom_left.homogeneous()).hnormalized().normalized() *
      depth;
    bottom_right =
      (unprojection * bottom_right.homogeneous()).hnormalized().normalized() *
      depth;

    m.add_thickline(color, eye, bottom_right, 0.4f * thickness, thickness);
    m.add_thickline(color, eye, top_right, 0.4f * thickness, thickness);
    m.add_thickline(color, eye, top_left, 0.4f * thickness, thickness);
    m.add_thickline(color, eye, bottom_left, 0.4f * thickness, thickness);
    m.add_thickline(color, bottom_right, bottom_left, thickness, thickness);
    m.add_thickline(color, bottom_left, top_left, thickness, thickness);
    m.add_thickline(color, top_left, top_right, thickness, thickness);
    m.add_thickline(color, top_right, bottom_right, thickness, thickness);
    m.add_coordinate_axes(-0.075f, thickness);

    if (!camera.camera_to_world().isIdentity())
    {
      m.apply_transform(camera.camera_to_world());
    }

    this->append_mesh(m);
  }

  void Mesh::add_camera_image(const Camera& camera, float depth)
  {
    this->check_instances();
    Mesh m = Mesh("").shared_color(m_shared_color).texture_id(m_texture_id);
    Vector eye(0, 0, 0);
    Eigen::Vector3f top_left(-1, -1, 1);
    Eigen::Vector3f top_right(1, -1, 1);
    Eigen::Vector3f bottom_left(-1, 1, 1);
    Eigen::Vector3f bottom_right(1, 1, 1);

    Transform unprojection = camera.projection().inverse();
    top_left =
      (unprojection * top_left.homogeneous()).hnormalized().normalized() *
      depth;
    top_right =
      (unprojection * top_right.homogeneous()).hnormalized().normalized() *
      depth;
    bottom_left =
      (unprojection * bottom_left.homogeneous()).hnormalized().normalized() *
      depth;
    bottom_right =
      (unprojection * bottom_right.homogeneous()).hnormalized().normalized() *
      depth;

    m.add_quad(Color::None(), top_left, top_right, bottom_right, bottom_left);
    if (!camera.camera_to_world().isIdentity())
    {
      m.apply_transform(camera.camera_to_world());
    }

    this->append_mesh(m);
  }

  void Mesh::add_disc(
    const Color& color,
    const Transform& transform,
    std::uint32_t segment_count,
    bool fill_triangles,
    bool add_wireframe)
  {
    this->check_instances();
    this->check_color(color);

    float radius = 0.5f;

    VectorBuffer vertices =
      VectorBuffer::Zero(static_cast<Eigen::Index>(segment_count) + 1, 3);
    auto thetas = Eigen::ArrayXf::LinSpaced(segment_count, 0, 2.0f * PI);
    auto ys = radius * thetas.cos();
    auto zs = radius * thetas.sin();
    vertices.block(1, 1, segment_count, 1) = ys;
    vertices.bottomRightCorner(segment_count, 1) = zs;

    TriangleBuffer triangles(segment_count, 3);
    triangles.col(0).fill(0);
    triangles.col(2) = arange(0, segment_count) + 1;
    triangles.col(1) = roll(triangles.col(2).array(), 1);

    ColorBuffer colors = ColorBufferNone();
    if (!color.is_none())
    {
      colors = ColorBuffer(segment_count + 1, 3);
      colors.col(0).fill(color(0));
      colors.col(1).fill(color(1));
      colors.col(2).fill(color(2));
    }

    VectorBuffer normals =
      VectorBuffer::Zero(static_cast<Eigen::Index>(segment_count) + 1, 3);
    normals.col(0).fill(1.0f);

    this->add_mesh_with_normals(
      vertices,
      normals,
      triangles,
      colors,
      UVBufferNone(),
      transform,
      false,
      fill_triangles,
      add_wireframe);
  }

  void Mesh::add_cylinder(
    const Color& color,
    const Transform& transform,
    std::uint32_t segment_count,
    bool fill_triangles,
    bool add_wireframe)
  {
    this->check_instances();
    this->check_color(color);

    // Unit diameter for consistency with other primitives
    float radius = 0.5f;

    auto N = segment_count;

    // Add discs at each end of the cylinder
    // Note, we do not re-use disc vertices for the barrel, as this gives bad
    // normals
    std::array<float, 2> x_vals = {-0.5f, +0.5f};
    for (auto& x : x_vals)
    {
      auto disc_transform = Transforms::rotation_about_y(x == 0.5f ? 0 : PI);
      disc_transform = Transforms::translate({x, 0, 0}) * disc_transform;
      if (!transform.isIdentity())
      {
        disc_transform = transform * disc_transform;
      }

      this->add_disc(color, disc_transform, N, fill_triangles, add_wireframe);
    }

    auto thetas = Eigen::ArrayXf::LinSpaced(N, 0, 2.0f * PI);
    auto ys = radius * thetas.cos();
    auto zs = radius * thetas.sin();

    VectorBuffer vertices(2 * N, 3);
    vertices.topLeftCorner(N, 1).fill(-0.5f);
    vertices.bottomLeftCorner(N, 1).fill(+0.5f);
    vertices.block(0, 1, N, 1) = ys;
    vertices.block(N, 1, N, 1) = ys;
    vertices.topRightCorner(N, 1) = zs;
    vertices.bottomRightCorner(N, 1) = zs;

    VectorBuffer normals = vertices;
    normals.col(0).fill(0.0f);

    TriangleBuffer triangles(2 * N, 3);
    auto range = arange(0, N);
    triangles.topLeftCorner(N, 1) = roll(range, 1);
    triangles.block(0, 1, N, 1) = range;
    triangles.topRightCorner(N, 1) = range + N;
    triangles.bottomLeftCorner(N, 1) = roll(range, 1);
    triangles.block(N, 1, N, 1) = range + N;
    triangles.bottomRightCorner(N, 1) = roll(range, 1) + N;

    ColorBuffer colors = ColorBufferNone();
    if (!color.is_none())
    {
      colors = ColorBuffer(2 * N, 3);
      colors.col(0).fill(color(0));
      colors.col(1).fill(color(1));
      colors.col(2).fill(color(2));
    }

    this->add_mesh_with_normals(
      vertices,
      normals,
      triangles,
      colors,
      UVBufferNone(),
      transform,
      false,
      fill_triangles,
      add_wireframe);
  }

  void Mesh::add_sphere(
    const Color& color,
    const Transform& transform,
    bool fill_triangles,
    bool add_wireframe)
  {
    this->add_icosphere(color, transform, 2, fill_triangles, add_wireframe);
  }

  void Mesh::add_icosphere(
    const Color& color,
    const Transform& transform,
    std::uint32_t steps,
    bool fill_triangles,
    bool add_wireframe)
  {
    this->check_instances();
    this->check_color(color);

    // Create a basic icosohedron primitive

    // Leads to unit diameter for consistency with other primitives
    const float radius = 0.5f;
    const float t = 0.5f * (1.0f + std::sqrt(5.0f));
    VectorBuffer vertex_positions(12, 3);
    vertex_positions << -1.0, +t, 0.0, +1.0, +t, 0.0, -1.0, -t, 0.0, +1.0, -t,
      0.0, 0.0, -1.0, +t, 0.0, +1.0, +t, 0.0, -1.0, -t, 0.0, +1.0, -t, +t, 0.0,
      -1.0, +t, 0.0, +1.0, -t, 0.0, -1.0, -t, 0.0, +1.0;
    vertex_positions.rowwise().normalize();
    vertex_positions *= radius;

    TriangleBuffer triangles(20, 3);
    triangles << 0, 11, 5, 0, 5, 1, 0, 1, 7, 0, 7, 10, 0, 10, 11, 1, 5, 9, 5,
      11, 4, 11, 10, 2, 10, 7, 6, 7, 1, 8, 3, 9, 4, 3, 4, 2, 3, 2, 6, 3, 6, 8,
      3, 8, 9, 4, 9, 5, 2, 4, 11, 6, 2, 10, 8, 6, 7, 9, 8, 1;

    typedef std::pair<std::uint32_t, std::uint32_t> Edge;
    // Apply subdivision
    for (std::uint32_t iter = 0; iter < steps; ++iter)
    {
      std::map<Edge, std::uint32_t> new_e_vs;
      TriangleBuffer new_triangles;

      // For each triangle we will make up to 3 new e-verts
      // (some will have less when shared with neighbouring triangles)
      for (auto triangle = 0; triangle < triangles.rows(); ++triangle)
      {
        auto a = triangles(triangle, 0);
        auto b = triangles(triangle, 1);
        auto c = triangles(triangle, 2);
        std::map<Edge, std::uint32_t> v_idx;

        // Loop over triangle edges
        std::array<Edge, 3> edges = {Edge(a, b), Edge(a, c), Edge(b, c)};
        for (const auto& edge : edges)
        {
          Edge back_edge(edge.second, edge.first);
          // Check this edge does not already have a new edge-vertex
          if (new_e_vs.count(edge))
          {
            v_idx[edge] = new_e_vs[edge];
          }
          else if (new_e_vs.count(back_edge))
          {
            v_idx[edge] = new_e_vs[back_edge];
          }
          else
          {
            Vector v = vertex_positions.row(edge.first) +
              vertex_positions.row(edge.second);
            v.normalize();
            v *= radius;
            v_idx[edge] = new_e_vs[edge] =
              static_cast<std::uint32_t>(vertex_positions.rows());
            append_row(vertex_positions, v);
          }
        }

        // Append new triangles
        append_row(
          new_triangles, Triangle(a, v_idx[Edge(a, b)], v_idx[Edge(a, c)]));
        append_row(
          new_triangles,
          Triangle(v_idx[Edge(a, b)], v_idx[Edge(b, c)], v_idx[Edge(a, c)]));
        append_row(
          new_triangles, Triangle(v_idx[Edge(a, c)], v_idx[Edge(b, c)], c));
        append_row(
          new_triangles, Triangle(v_idx[Edge(a, b)], b, v_idx[Edge(b, c)]));
      }

      // Replace existing triangles with new triangles
      triangles = new_triangles;
    }

    // Set up per-vertex colors if required
    ColorBuffer colors;
    if (!color.is_none())
    {
      colors = ColorBuffer(vertex_positions.rows(), 3);
      colors.col(0).fill(color.r());
      colors.col(1).fill(color.g());
      colors.col(2).fill(color.b());
    }

    // Set up per-vertex uvs if required
    UVBuffer uvs;
    if (!m_texture_id.empty())
    {
      // Compute azimuth and inclination values into the uv buffer
      uvs.resize(vertex_positions.rows(), Eigen::NoChange);
      for (auto v_idx = 0; v_idx < vertex_positions.rows(); ++v_idx)
      {
        uvs(v_idx, 0) = static_cast<float>(
          0.5f -
          0.5f *
            std::atan2(vertex_positions(v_idx, 2), vertex_positions(v_idx, 0)) /
            PI); // Azimuth
        uvs(v_idx, 1) = static_cast<float>(
          1.0f -
          std::acos(vertex_positions(v_idx, 1) * 2.0f) / PI); // Inclination
      }

      // Duplicate vertices across the longitude seam - not particularly
      // efficient and bit of a hack but works ok.  Results in non watertight
      // mesh. Loop over triangles
      TriangleBuffer new_triangles;
      for (auto triangle = 0; triangle < triangles.rows(); ++triangle)
      {
        auto a = triangles(triangle, 0);
        auto b = triangles(triangle, 1);
        auto c = triangles(triangle, 2);

        // Rotate triangle indices to ensure a is east most vertex, preserving
        // triangle winding order
        for (std::size_t attempt = 0; attempt < 2; ++attempt)
        {
          if (uvs(a, 0) < uvs(b, 0) || uvs(a, 0) < uvs(c, 0))
          {
            std::tie(a, b, c) = std::make_tuple(b, c, a);
          }
        }

        // Identify hemisphere for a, b, c
        // Hack - doesn't look great at poles for small number of subdiv steps
        bool a_isEast = uvs(a, 0) > 0.66f;
        bool b_isWest = uvs(b, 0) < 0.33f;
        bool c_isWest = uvs(c, 0) < 0.33f;

        // Duplicate vertices if necessary
        if (a_isEast && c_isWest) // Duplicate c vertex
        {
          Vertex new_vertex = vertex_positions.row(c);
          append_row(vertex_positions, new_vertex); // Concatenate a copy of c
          append_row(uvs, Eigen::Vector2f(1.0f + uvs(c, 0), uvs(c, 1)));
          c = static_cast<uint32_t>(
            vertex_positions.rows() - 1); // Will overwrite c in triangle
        }
        if (a_isEast && b_isWest) // Duplicate b vertex
        {
          Vertex new_vertex = vertex_positions.row(b);
          append_row(vertex_positions, new_vertex); // Concatenate a copy of b
          append_row(uvs, Eigen::Vector2f(1.0f + uvs(b, 0), uvs(b, 1)));
          b = static_cast<uint32_t>(
            vertex_positions.rows() - 1); // Will overwrite b in triangle
        }

        // Save the new triangle
        append_row(new_triangles, Triangle(a, b, c));
      }

      // Replace existing triangles with new triangles
      triangles = new_triangles;
    }

    // Populate mesh

    // vertices double as normals given they lie on unit sphere
    this->add_mesh_with_normals(
      std::move(vertex_positions),
      std::move(vertex_positions),
      std::move(triangles),
      std::move(colors),
      std::move(uvs),
      transform,
      false,
      fill_triangles,
      add_wireframe);
  }

  void Mesh::add_uv_sphere(
    const Color& color,
    const Transform& transform,
    std::uint32_t lat_count,
    std::uint32_t long_count,
    bool fill_triangles,
    bool add_wireframe)
  {
    this->check_instances();
    this->check_color(color);

    // Add vertices

    // Leads to unit diameter for consistency with other primitives
    const double radius = 0.5;
    Mesh m = Mesh("").shared_color(m_shared_color);
    for (std::uint32_t lat_index = 0; lat_index < lat_count + 1; ++lat_index)
    {
      double theta = lat_index * M_PI / lat_count;
      double cosTheta = std::cos(theta);
      double sinTheta = std::sin(theta);
      for (std::uint32_t long_index = 0; long_index < long_count; ++long_index)
      {
        double phi = long_index * 2.0 * M_PI / long_count;
        double cosPhi = std::cos(phi);
        double sinPhi = std::sin(phi);

        double dx = radius * cosPhi * sinTheta;
        double dy = radius * cosTheta;
        double dz = radius * sinPhi * sinTheta;
        Vector pos = Eigen::Vector3d(dx, dy, dz).cast<float>();
        m.append_vertex(pos, pos, color);
      }
    }

    // Add triangles
    for (std::uint32_t lat_index = 0; lat_index < lat_count; ++lat_index)
    {
      for (std::uint32_t long_index = 0; long_index < long_count; ++long_index)
      {
        auto base_index = lat_index * long_count;
        auto a = base_index + long_index;
        auto b = base_index + (long_index + 1) % long_count;
        auto c = base_index + long_index + long_count;
        auto d = base_index + (long_index + 1) % long_count + long_count;
        if (fill_triangles)
        {
          if (lat_index > 0)
          {
            m.append_triangle(a, b, c);
          }
          m.append_triangle(c, b, d);
        }
        if (add_wireframe)
        {
          m.append_line(a, b);
          m.append_line(b, d);
          m.append_line(d, c);
          m.append_line(c, a);
        }
      }
    }

    if (!transform.isIdentity())
    {
      m.apply_transform(transform);
    }

    this->append_mesh(m);
  }

} // namespace scenepic


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

/**
 * \file boost/numeric/ublasx/operation/diag.hpp
 *
 * \brief The \c diag operation.
 *
 * The \c diag operation takes inspiration from the \e diag MATLAB's function
 * and the \e DiagonalMatrix Mathematica's function.
 *
 * \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_UBLAS_DIAG_HPP
#define BOOST_NUMERIC_UBLAS_DIAG_HPP

#include <boost/numeric/ublasx/container/generalized_diagonal_matrix.hpp>
#include <boost/numeric/ublas/fwd.hpp>
#include <boost/numeric/ublasx/proxy/matrix_diagonal.hpp>
#include <boost/numeric/ublas/traits.hpp>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

/**
 * \brief A traits class for vector-to-diagonal-matrix transformation.
 * \tparam VectorT A model of VectorExpression.
 * \tparam Layout A matrix layout type.
 */
template <typename VectorT, typename LayoutT>
struct vector_matrix_diag_traits
{
    typedef typename vector_traits<VectorT>::value_type value_type;
    typedef typename vector_traits<VectorT>::difference_type difference_type;
    typedef typename vector_traits<VectorT>::size_type size_type;
    typedef LayoutT layout_type;
    typedef typename VectorT::array_type array_type; //FIXME: not in vector_traits
    typedef generalized_diagonal_matrix<value_type, layout_type, array_type> result_type;
};

/**
 * \brief A traits class for vector-to-diagonal-matrix transformation.
 * \tparam VectorExprT A model of VectorExpression.
 * \tparam Layout A matrix layout type.
 */
template <typename VectorExprT, typename LayoutT>
struct vector_matrix_diag_traits<vector_expression<VectorExprT>, LayoutT>
{
    typedef typename vector_traits<VectorExprT>::value_type value_type;
    typedef typename vector_traits<VectorExprT>::difference_type difference_type;
    typedef typename vector_traits<VectorExprT>::size_type size_type;
    typedef LayoutT layout_type;
    typedef typename VectorExprT::array_type array_type; //FIXME: not in vector_traits
    typedef generalized_diagonal_matrix<value_type, layout_type, array_type> result_type;
};

/**
 * \brief Create a square matrix of order \f$n+abs(k)\f$, with the elements of
 *  \a v on the \a k-th diagonal.
 * \tparam VectorT A model of VectorExpression.
 * \tparam LayoutT The layout type of the resulting matrix (e.g., row_major).
 * \param v A vector expression.
 * \param k The offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \param l The matrix layout.
 * \return A square diagonal matrix with the elements of \a v on the \a k-th
 *  diagonal.
 */
template <typename VectorT, typename LayoutT>
BOOST_UBLAS_INLINE
typename vector_matrix_diag_traits<VectorT,LayoutT>::result_type diag(vector_expression<VectorT>& v, typename vector_matrix_diag_traits<VectorT,LayoutT>::difference_type k=0, LayoutT /*l*/=LayoutT())
{
    typedef vector_matrix_diag_traits<VectorT,LayoutT> traits;
    typedef typename traits::size_type size_type;
    typedef typename traits::array_type array_type;
    typedef typename traits::result_type result_type;

    size_type d(k > 0 ? k : -k);

    return result_type(
            v().size() + d,
            k,
            array_type(v().data())
    );
}

/**
 * \brief Create a square matrix of order \f$n+abs(k)\f$, with the elements of
 *  \a v on the \a k-th diagonal.
 * \tparam VectorT A model of VectorExpression.
 * \tparam LayoutT The layout type of the resulting matrix (e.g., row_major).
 * \param v A vector expression.
 * \param k The offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \param l The matrix layout.
 * \return A square diagonal matrix with the elements of \a v on the \a k-th
 *  diagonal.
 *
 * Variant for const reference to vectors.
 */
template <typename VectorT, typename LayoutT>
BOOST_UBLAS_INLINE
typename vector_matrix_diag_traits<VectorT const,LayoutT>::result_type diag(vector_expression<VectorT> const& v, typename vector_matrix_diag_traits<VectorT,LayoutT>::difference_type k=0, LayoutT /*l*/=LayoutT())
{
    typedef vector_matrix_diag_traits<VectorT const,LayoutT> traits;
    typedef typename traits::size_type size_type;
    typedef typename traits::array_type array_type;
    typedef typename traits::result_type result_type;

    size_type d(k > 0 ? k : -k);

    return result_type(
            v().size() + d,
            k,
            array_type(v().data())
    );
}

/**
 * \brief create a square matrix of order \f$n+abs(k)\f$, with the elements of
 *  \a v on the \a k-th diagonal.
 * \tparam vectort a model of vectorexpression.
 * \param v a vector expression.
 * \param k the offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \return a square diagonal matrix with the elements of \a v on the \a k-th
 *  diagonal and with a row-major layout.
 */
template <typename VectorT>
BOOST_UBLAS_INLINE
typename vector_matrix_diag_traits<VectorT,row_major>::result_type diag(vector_expression<VectorT>& v, typename vector_matrix_diag_traits<VectorT,row_major>::difference_type k=0)
{
    typedef vector_matrix_diag_traits<VectorT,row_major> traits;
    typedef typename traits::size_type size_type;
    typedef typename traits::array_type array_type;
    typedef typename traits::result_type result_type;

    size_type d(k > 0 ? k : -k);

    return result_type(
            v().size() + d,
            k,
            array_type(v().data())
    );
}

/**
 * \brief create a square matrix of order \f$n+abs(k)\f$, with the elements of
 *  \a v on the \a k-th diagonal.
 * \tparam vectort a model of vectorexpression.
 * \param v a vector expression.
 * \param k the offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \return a square diagonal matrix with the elements of \a v on the \a k-th
 *  diagonal and with a row-major layout.
 *
 * Variant for const reference to vectors.
 */
template <typename VectorT>
BOOST_UBLAS_INLINE
typename vector_matrix_diag_traits<VectorT const,row_major>::result_type diag(vector_expression<VectorT> const& v, typename vector_matrix_diag_traits<VectorT,row_major>::difference_type k=0)
{
    typedef vector_matrix_diag_traits<VectorT const,row_major> traits;
    typedef typename traits::size_type size_type;
    typedef typename traits::array_type array_type;
    typedef typename traits::result_type result_type;

    size_type d(k > 0 ? k : -k);

    return result_type(
            v().size() + d,
            k,
            array_type(v().data())
    );
}

/**
 * \brief Create a \a size1 by \a size2 matrix with the elements of
 *  \a v on the \a k-th diagonal.
 * \tparam VectorT A model of VectorExpression.
 * \tparam LayoutT The layout type of the resulting matrix (e.g., row_major).
 * \param v A vector expression. If too long it will be truncated.
 * \param size1 The number of rows of the resulting matrix.
 * \param size2 The number of columns of the resulting matrix.
 * \param k The offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \param l The matrix layout.
 * \return A rectangular diagonal matrix with the elements of \a v on the
 *  \a k-th diagonal.
 */
template <typename VectorT, typename LayoutT>
BOOST_UBLAS_INLINE
typename vector_matrix_diag_traits<VectorT,LayoutT>::result_type diag(vector_expression<VectorT>& v, typename vector_matrix_diag_traits<VectorT,LayoutT>::size_type size1, typename vector_matrix_diag_traits<VectorT,LayoutT>::size_type size2, typename vector_matrix_diag_traits<VectorT,LayoutT>::difference_type k=0, LayoutT /*l*/=LayoutT())
{
    typedef vector_matrix_diag_traits<VectorT,row_major> traits;
    typedef typename traits::array_type array_type;
    typedef typename traits::result_type result_type;

    return result_type(
            size1,
            size2,
            k,
            array_type(v().data())
    );
}

/**
 * \brief Create a \a size1 by \a size2 matrix with the elements of
 *  \a v on the \a k-th diagonal.
 * \tparam VectorT A model of VectorExpression.
 * \tparam LayoutT The layout type of the resulting matrix (e.g., row_major).
 * \param v A vector expression. If too long it will be truncated.
 * \param size1 The number of rows of the resulting matrix.
 * \param size2 The number of columns of the resulting matrix.
 * \param k The offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \param l The matrix layout.
 * \return A rectangular diagonal matrix with the elements of \a v on the
 *  \a k-th diagonal.
 *
 * Variant for const reference to vectors.
 */
template <typename VectorT, typename LayoutT>
BOOST_UBLAS_INLINE
typename vector_matrix_diag_traits<VectorT const,LayoutT>::result_type diag(vector_expression<VectorT> const& v, typename vector_matrix_diag_traits<VectorT,LayoutT>::size_type size1, typename vector_matrix_diag_traits<VectorT,LayoutT>::size_type size2, typename vector_matrix_diag_traits<VectorT,LayoutT>::difference_type k=0, LayoutT /*l*/=LayoutT())
{
    typedef vector_matrix_diag_traits<VectorT const,row_major> traits;
    typedef typename traits::array_type array_type;
    typedef typename traits::result_type result_type;

    return result_type(
            size1,
            size2,
            k,
            array_type(v().data())
    );
}

/**
 * \brief Create a \a size1 by \a size2 matrix with the elements of
 *  \a v on the \a k-th diagonal.
 * \tparam VectorT A model of VectorExpression.
 * \param v A vector expression. If too long it will be truncated.
 * \param size1 The number of rows of the resulting matrix.
 * \param size2 The number of columns of the resulting matrix.
 * \param k The offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \return A rectangular diagonal matrix with the elements of \a v on the
 *  \a k-th diagonal and with a row-major layout.
 */
template <typename VectorT>
BOOST_UBLAS_INLINE
typename vector_matrix_diag_traits<VectorT,row_major>::result_type diag(vector_expression<VectorT>& v, typename vector_matrix_diag_traits<VectorT,row_major>::size_type size1, typename vector_matrix_diag_traits<VectorT,row_major>::size_type size2, typename vector_matrix_diag_traits<VectorT,row_major>::difference_type k=0)
{
    typedef vector_matrix_diag_traits<VectorT,row_major> traits;
    typedef typename traits::array_type array_type;
    typedef typename traits::result_type result_type;

    return result_type(
            size1,
            size2,
            k,
            array_type(v().data())
    );
}

/**
 * \brief Create a \a size1 by \a size2 matrix with the elements of
 *  \a v on the \a k-th diagonal.
 * \tparam VectorT A model of VectorExpression.
 * \param v A vector expression. If too long it will be truncated.
 * \param size1 The number of rows of the resulting matrix.
 * \param size2 The number of columns of the resulting matrix.
 * \param k The offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \return A rectangular diagonal matrix with the elements of \a v on the
 *  \a k-th diagonal and with a row-major layout.
 *
 * Variant for const reference to vectors.
 */
template <typename VectorT>
BOOST_UBLAS_INLINE
typename vector_matrix_diag_traits<VectorT const,row_major>::result_type diag(vector_expression<VectorT> const& v, typename vector_matrix_diag_traits<VectorT,row_major>::size_type size1, typename vector_matrix_diag_traits<VectorT,row_major>::size_type size2, typename vector_matrix_diag_traits<VectorT,row_major>::difference_type k=0)
{
    typedef vector_matrix_diag_traits<VectorT const,row_major> traits;
    typedef typename traits::array_type array_type;
    typedef typename traits::result_type result_type;

    return result_type(
            size1,
            size2,
            k,
            array_type(v().data())
    );
}

//[FIXME]: The two functions below are commented for problems on overloading:
// the ambiguity is caused by difference_type, size_type and LayoutT.
///**
// * \brief Create a square matrix of order \f$n+abs(k)\f$, with the elements of
// *  \a v on the \a k-th diagonal.
// * \tparam VectorT A model of VectorExpression.
// * \tparam LayoutT The layout type of the resulting matrix (e.g., row_major).
// * \param v A vector expression. If longer than \a size it will be truncated.
// * \param size The size of the resulting matrix.
// * \param k The offset from the main diagonal:
// *  - \a k = 0 represents the main diagonal,
// *  - \a k > 0 is the offset above the main diagonal,
// *  - \a k < 0 is the offset below the main diagonal.
// * \param l The matrix layout.
// *  .
// * \return A square diagonal matrix with the elements of \a v on the \a k-th
// *  diagonal.
// */
//template <typename VectorT, typename LayoutT>
//BOOST_UBLAS_INLINE
//typename vector_matrix_diag_traits<VectorT,LayoutT>::result_type diag(vector_expression<VectorT>& v, typename vector_matrix_diag_traits<VectorT,LayoutT>::size_type size, typename vector_matrix_diag_traits<VectorT,LayoutT>::difference_type k, LayoutT /*l*//*=LayoutT()*/)
//{
//  typedef vector_matrix_diag_traits<VectorT,LayoutT> traits;
//  typedef typename traits::size_type size_type;
//  typedef typename traits::array_type array_type;
//  typedef typename traits::result_type result_type;
//
//  return result_type(
//          size,
//          k,
//          array_type(v().data())
//  );
//}

///**
// * \brief Create a square matrix of order \f$n+abs(k)\f$, with the elements of
// *  \a v on the \a k-th diagonal.
// * \tparam VectorT A model of VectorExpression.
// * \param v A vector expression. If longer than \a size it will be truncated.
// * \param size The size of the resulting matrix.
// * \param k The offset from the main diagonal:
// *  - \a k = 0 represents the main diagonal,
// *  - \a k > 0 is the offset above the main diagonal,
// *  - \a k < 0 is the offset below the main diagonal.
// *  .
// * \return A square diagonal matrix with the elements of \a v on the \a k-th
// *  diagonal and with a row-major layout.
// */
//template <typename VectorT>
//BOOST_UBLAS_INLINE
//typename vector_matrix_diag_traits<VectorT,row_major>::result_type diag(vector_expression<VectorT>& v, typename vector_matrix_diag_traits<VectorT,row_major>::size_type size, typename vector_matrix_diag_traits<VectorT,row_major>::difference_type k)
//{
//  typedef vector_matrix_diag_traits<VectorT,row_major> traits;
//  typedef typename traits::size_type size_type;
//  typedef typename traits::array_type array_type;
//  typedef typename traits::result_type result_type;
//
//  return result_type(
//          size,
//          k,
//          array_type(v().data())
//  );
//}
//[/FIXME]

/**
 * \brief Create a view of the \a k-th diagonal of a matrix.
 * \tparam MatrixT A model of MatrixExpression.
 * \param me A matrix expression from which taking the \a k-th diagonal.
 * \param k The offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \return A view of the \a k-th diagonal of matrix \a me.
 */
template<typename MatrixT>
BOOST_UBLAS_INLINE
matrix_diagonal<MatrixT> diag(matrix_expression<MatrixT>& me, typename MatrixT::difference_type k=0)
{
    return matrix_diagonal<MatrixT>(me(), k);
}

/**
 * \brief Create an unmutable view of the \a k-th diagonal of a matrix.
 * \tparam MatrixT A model of MatrixExpression.
 * \param me A matrix expression from which taking the \a k-th diagonal.
 * \param k The offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \return An unmutable view of the \a k-th diagonal of matrix \a me.
 */
template<typename MatrixT>
BOOST_UBLAS_INLINE
matrix_diagonal<MatrixT const> const diag(matrix_expression<MatrixT> const& me, typename MatrixT::difference_type k=0)
{
    return matrix_diagonal<MatrixT const>(me(), k);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_DIAG_HPP


//          Copyright Oliver Kowalke 2014.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <cctype>
#include <cstdio>
#include <exception>
#include <functional>
#include <iostream>
#include <sstream>

#include <boost/coroutine2/all.hpp>

class parser_error : public std::runtime_error {
public:
    parser_error() :
        std::runtime_error("parsing failed") {
    }
};

/*
 * grammar:
 *   P ---> E '\0'
 *   E ---> T {('+'|'-') T}
 *   T ---> S {('*'|'/') S}
 *   S ---> digit | '(' E ')'
 */
class Parser{
   char next;
   std::istream& is;
   std::function<void(char)> cb;

   char pull(){
        return std::char_traits<char>::to_char_type(is.get());
   }

   void scan(){
       do{
           next=pull();
       }
       while(isspace(next));
   }

public:
   Parser(std::istream& is_,std::function<void(char)> cb_) :
      next(), is(is_), cb(cb_)
    {}

   void run() {
      scan();
      E();
   }

private:
   void E(){
      T();
      while (next=='+'||next=='-'){
         cb(next);
         scan();
         T();
      }
   }

   void T(){
      S();
      while (next=='*'||next=='/'){
         cb(next);
         scan();
         S();
      }
   }

   void S(){
      if (std::isdigit(next)){
         cb(next);
         scan();
      }
      else if(next=='('){
         cb(next);
         scan();
         E();
         if (next==')'){
             cb(next);
             scan();
         }else{
             throw parser_error();
         }
      }
      else{
         throw parser_error();
      }
   }
};

typedef boost::coroutines2::coroutine< char > coro_t;

int main() {
    try {
        std::istringstream is("1+1");
        // invert control flow
        coro_t::pull_type seq(
                [&is]( coro_t::push_type & yield) {
                Parser p( is,
                    [&yield](char ch){
                    yield(ch);
                    });
                p.run();
                });
        // user-code pulls parsed data from parser
        for(char c:seq){
            printf("Parsed: %c\n",c);
        }
        std::cout << "\nDone" << std::endl;
        return EXIT_SUCCESS;
    } catch ( std::exception const& ex) {
        std::cerr << "exception: " << ex.what() << std::endl;
    }
    return EXIT_FAILURE;
}


#include <iostream>
#include <cmath>
#include <vector>
#include <math.h>
#include <fstream>
#include "GeneralizedHeat.hpp"
#include "APDE.hpp"
#include "PDE_Q2.hpp"
#include "originalPDE.hpp"
#include "EllipticPDE_Q1.hpp"
#include "EllipticPDE2.hpp"
#include "Nonanalytic_1.hpp"
#include <string>
#include <boost/math/quadrature/gauss.hpp>
using namespace std;
using namespace boost::math::quadrature;
//const double M_PI = 2*acos(0);

void printTime(SpaceMesh a_smesh, TimeMesh a_tmesh);

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

SpaceMesh smesh;
smesh.GenerateDefaultSpaceMesh();
smesh.GloballyBisectSpaceMesh();
smesh.GloballyBisectSpaceMesh();
//smesh.GloballyBisectSpaceMesh();
//smesh.GloballyBisectSpaceMesh();

TimeMesh tmesh;
tmesh.GenerateUniformTimeMesh(pow(smesh.meshsize(), 1), 1.0);

PDE_Q2 anotherpde;
originalPDE firstpde;
EllipticPDE_Q1 Q1;
EllipticPDE2 elliptic2;
Nonanalytic NA_1;

GeneralHeat genheat;
smesh.PrintSpaceNodes();
genheat.SetSpaceTimeMesh(smesh, tmesh, NA_1);
//genheat.StationaryHeatEquation();
genheat.SolveWithBCs();
printTime(smesh, tmesh);
genheat.PrintSolution();

//genheat.GlobalSpaceError();

//for (int i=0; i<6; i++)
//{
//    genheat.SetSpaceTimeMesh(smesh, tmesh, elliptic2);
//    genheat.StationaryHeatEquation();
//    std::cout<< smesh.meshsize() + 1 <<"\n";
//    genheat.UnitTest1();
//    smesh.GloballyBisectSpaceMesh();
//    tmesh.GenerateUniformTimeMesh(pow(smesh.meshsize()+1, 2), 1.0);
//}

}

void printTime(SpaceMesh a_smesh, TimeMesh a_tmesh)
{
    ofstream myfile2;
    myfile2.open ("Y.csv");
    for(int j=0;j<a_tmesh.NumberOfTimeSteps()+1;j++ )
    {
    for(int i = 0; i<a_smesh.meshsize()+1; i++)
    {
        myfile2 << a_tmesh.ReadTimeStep(j) << ", ";
    }
    myfile2 << "\n";
    }
    myfile2.close();
}

Solver class generalised for all boundary conditions/driver.cpp