#include "GraphSampler.h"
#include <random>
#include <cassert>
#include <iostream>
#include <cmath>
#include <boost/math/distributions/hypergeometric.hpp>
#include <boost/math/distributions/hypergeometric.hpp>
#include <boost/math/policies/policy.hpp>

using namespace std;

GraphSampler::GraphSampler(TriangleCounter* counter) : counter_(counter) {
	if (counter_){
		counter_->clear();
	}
}

GraphSampler::~GraphSampler(){}

FixedPSampler::FixedPSampler(double p, bool use_sample_and_hold, TriangleCounter* counter)
	: GraphSampler(counter), p_(p), use_sample_and_hold_(use_sample_and_hold){
}

FixedPSampler::~FixedPSampler(){}

void FixedPSampler::exec_operation(const EdgeUpdate& update){

	counter_->new_update(update);

	if (update.is_add){
		double u_rand = (double)rand() / ((double)RAND_MAX+1.0);

		if(use_sample_and_hold_){ // always count first
			counter_->add_triangles(update.node_u, update.node_v, 1.0); //Weight not used
		}

		if (u_rand < p_){
			// Undirected graph
			if(!use_sample_and_hold_){ // count only if sampled
				counter_->add_triangles(update.node_u, update.node_v, 1.0); //Weight not used
			}
			counter_->add_edge_sample(update.node_u, update.node_v);

		}
	} else { // Remove are always executed (if not present no effect)
		assert(!use_sample_and_hold_); // Not supported.
		bool succeed = counter_->remove_edge_sample(update.node_u, update.node_v);
		// Here !use_sample_and_hold_
		if (succeed){ // it was present
			counter_->remove_triangles(update.node_u, update.node_v, 1.0); //Weight not used
		}
	}
}

double FixedPSampler::get_triangle_est(){
	if (!use_sample_and_hold_){
		return (double)counter_->triangles()*pow(1.0/p_,3);
	} else {
		return (double)counter_->triangles()*pow(1.0/p_,2);
	}
}

double FixedPSampler::get_triangle_est_local(int node){
	assert(counter_->is_local());
	if (!use_sample_and_hold_){
		return (double)counter_->triangles_local(node)*pow(1.0/p_,3);
	} else {
		return (double)counter_->triangles_local(node)*pow(1.0/p_,2);
	}
}

//RESERVOIR SAMPLER

ReservoirSampler::ReservoirSampler(size_t reservoir_size, bool use_sample_and_hold, TriangleCounter* counter)
	: GraphSampler(counter), reservoir_size_(reservoir_size), use_sample_and_hold_(use_sample_and_hold){
		reservoir_.reserve(reservoir_size);
}

ReservoirSampler::~ReservoirSampler(){}

void ReservoirSampler::add_reservoir(const pair<int,int> edge){
	//cout<<"ADD RES"<<edge.first<<" "<<edge.second<<endl;
	assert(reservoir_map_.find(edge) == reservoir_map_.end());
	reservoir_.push_back(edge);
	reservoir_map_.insert(make_pair(edge, reservoir_.size()-1));
	counter_->add_edge_sample(edge.first, edge.second);
	if (!use_sample_and_hold_){
		counter_->add_triangles(edge.first, edge.second, 1.0); //Weight not used
	}
}

void ReservoirSampler::delete_reservoir(const pair<int,int> edge){
	if (reservoir_map_.find(edge) == reservoir_map_.end()){
		return;
	}
	int pos = reservoir_map_.at(edge);
	reservoir_map_.erase(edge);
	if (pos < reservoir_.size() -1){ // not the last item
		pair<int, int> last_edge = reservoir_.back();
		reservoir_[pos] = last_edge;
		reservoir_map_[last_edge] = pos;
	}
	reservoir_.pop_back();
	bool succ = counter_->remove_edge_sample(edge.first, edge.second);
	if (!use_sample_and_hold_){
		counter_->remove_triangles(edge.first, edge.second, 1.0); //Weight not used
	}

	assert(succ);
	assert(reservoir_.size()<=reservoir_size_);
	assert(reservoir_.size()==reservoir_map_.size());
}

void ReservoirSampler::exec_operation(const EdgeUpdate& update){
	assert(update.is_add); //only add supported
	assert(update.node_u != update.node_v);
	int max_n = (int)max(update.node_u, update.node_v);
	int min_n = (int)min(update.node_u, update.node_v);

	pair<int,int> edge = make_pair(min_n, max_n);

	counter_->new_update(update);

	//if(update.is_add){
	assert(reservoir_map_.find(edge) == reservoir_map_.end());

	// Prob of sampling at this step if using sample and hold
  double p = 0;
  unsigned long long int t = counter_->edges_present_original();

  if (t >= 2 && use_sample_and_hold_){
    p = ((double)reservoir_size_/t)*((double)(reservoir_size_-1)/(t-1));
    p = min(p, 1.0);
  }

	if (use_sample_and_hold_){ // Always count and rever remove.
		counter_->add_triangles(update.node_u, update.node_v, (double)1.0/p);
	}

	// Enough space
	if (reservoir_.size() < reservoir_size_){
		add_reservoir(edge);
	} else {
		double u_rand = (double)rand() / ((double)RAND_MAX+1.0);
		double thres = ((double)reservoir_size_)/counter_->edges_present_original();
		if (u_rand < thres){
			// Doing the exchange
			int rand_pos = rand()%reservoir_size_;
			const pair<int,int>& to_remove = reservoir_[rand_pos];
			delete_reservoir(to_remove);
			add_reservoir(edge);
		}
	}
	//} else { // is remove
	//	delete_reservoir(edge);
	//}
}

 double ReservoirSampler::get_triangle_est(){
	if (!use_sample_and_hold_){

    double p = 0;
    unsigned long long int t = counter_->edges_present_original();

    if (t >= 3){
      p = ((double)reservoir_size_/t)*((double)(reservoir_size_-1)/(t-1))
          *((double)(reservoir_size_-2)/(t-2));
      p = min(p, 1.0);
      return (double)counter_->triangles()/p;

    } else {
      return 0; // no triangles possible;
    }

	} else {
		return counter_->triangles_weight();
	}
}

 double ReservoirSampler::get_triangle_est_local(int node){
	assert(counter_->is_local());

	if (!use_sample_and_hold_){
    unsigned long long int t = counter_->edges_present_original();
    if (t >= 3){
      double p = ((double)reservoir_size_/t)*((double)(reservoir_size_-1)/(t-1))
          *((double)(reservoir_size_-2)/(t-2));
      p = min(p, 1.0);
      return (double)counter_->triangles_local(node)/p;

    } else {
      return 0; // no triangles possible;
    }
	} else {
		return counter_->triangles_weight_local(node);
	}
}

// ****************************************
// Reservoir add & remove

ReservoirAddRemSampler::ReservoirAddRemSampler(size_t reservoir_size, TriangleCounter* counter)
	: GraphSampler(counter), reservoir_size_(reservoir_size), d_i_(0), d_o_(0){
		reservoir_.reserve(reservoir_size);
}

ReservoirAddRemSampler::~ReservoirAddRemSampler(){}

void ReservoirAddRemSampler::add_reservoir(const pair<int,int> edge){
	//cout<<"ADD RES"<<edge.first<<" "<<edge.second<<endl;
  int before_size = reservoir_.size();

	assert(reservoir_map_.find(edge) == reservoir_map_.end());
	reservoir_.push_back(edge);
	reservoir_map_.insert(make_pair(edge, reservoir_.size()-1));
	bool succ = counter_->add_edge_sample(edge.first, edge.second);

  assert (succ);
  assert(reservoir_.size()<=reservoir_size_);
  assert(reservoir_.size()==reservoir_map_.size());
  assert (reservoir_map_.find(edge) != reservoir_map_.end());
  assert(reservoir_.size()== before_size +1);
}

void ReservoirAddRemSampler::delete_reservoir(const pair<int,int> edge){
  //cout<<"REM RES"<<edge.first<<" "<<edge.second<<endl;

  assert (reservoir_map_.find(edge) != reservoir_map_.end());
  int before_size = reservoir_.size();

	int pos = reservoir_map_.at(edge);
	reservoir_map_.erase(edge);
	if (pos < reservoir_.size() -1){ // not the last item
		pair<int, int> last_edge = reservoir_.back();
		reservoir_[pos] = last_edge;
		reservoir_map_[last_edge] = pos;
	}
	reservoir_.pop_back();
	bool succ = counter_->remove_edge_sample(edge.first, edge.second);
	counter_->remove_triangles(edge.first, edge.second, 1.0); //Weight not used

	assert(succ);
	assert(reservoir_.size()<=reservoir_size_);
	assert(reservoir_.size()==reservoir_map_.size());
  assert (reservoir_map_.find(edge) == reservoir_map_.end());
  assert(reservoir_.size()== before_size -1);
}

void ReservoirAddRemSampler::exec_operation(const EdgeUpdate& update){
	assert(update.node_u != update.node_v);
	int max_n = (int)max(update.node_u, update.node_v);
	int min_n = (int)min(update.node_u, update.node_v);

	pair<int,int> edge = make_pair(min_n, max_n);

	counter_->new_update(update);

	if(update.is_add){
		assert(reservoir_map_.find(edge) == reservoir_map_.end());

    if (d_o_ + d_i_ > 0) { // case d_o + d_i > 0
      double u_rand = (double)rand() / ((double)RAND_MAX+1.0);
			double thres = ((double)d_i_)/(d_i_+d_o_);
      if (u_rand < thres){ //with pro d_i / (d_i + d_o)
        d_i_ --;
        assert(reservoir_.size() < reservoir_size_);

        add_reservoir(edge);
        counter_->add_triangles(edge.first, edge.second, 1.0); //Weight not used
      } else {
        d_o_ --;
      }
    } else if (reservoir_.size() < reservoir_size_){ // enough space and d_i + d_o = 0

			add_reservoir(edge);
      counter_->add_triangles(edge.first, edge.second, 1.0); //Weight not used

		} else { // reservoid full and d_i + d_o = 0
      assert (counter_->edges_present_original()>reservoir_size_);

			double u_rand = (double)rand() / ((double)RAND_MAX+1.0);
			double thres = ((double)reservoir_size_)/(counter_->edges_present_original());
			if (u_rand < thres){
				// Doing the exchange
				int rand_pos = rand()%reservoir_size_;
				pair<int,int> to_remove = reservoir_[rand_pos];

        size_t before_size = reservoir_.size();

				delete_reservoir(to_remove);
				add_reservoir(edge);

        assert(reservoir_.size()<=reservoir_size_);
        assert(reservoir_.size()==reservoir_map_.size());
        assert (reservoir_map_.find(to_remove) == reservoir_map_.end());
        assert (reservoir_map_.find(edge) != reservoir_map_.end());
        assert(reservoir_.size()== before_size);

				counter_->add_triangles(edge.first, edge.second, 1.0); //Weight not used
			}
		}
	} else { // is remove
    assert(counter_->edges_present_original()>=0);

		if (reservoir_map_.find(edge) != reservoir_map_.end()){//Was present
      d_i_ ++; // deletion in sample

			delete_reservoir(edge);
		} else { // the edge deleted was not in the reservoir
      d_o_ ++; // deletion in sample

    }
	}
}

double ReservoirAddRemSampler::prob_sampling_triangle() const{
  if (counter_->edges_present_original() < 3 || reservoir_.size() < 3) {
    return 0;
  }
  unsigned long long int s = counter_->edges_present_original();

  //cout<<"SIZES: "<<reservoir_.size()<< " " <<s<<endl;
  assert ((unsigned long long int)reservoir_.size()<=s);

  double mt = reservoir_.size();

  double p = 0;

  if (reservoir_.size() == s) {
    p = 1;
  } else {
    p = (mt/s)*((mt-1)/(s-1))*((mt-2)/(s-2));
  }

  assert (p<=1);

  unsigned long long int n = min((unsigned long long int)reservoir_size_, s+d_i_+d_o_);

  boost::math::hypergeometric_distribution<double> hyper(s, n, d_i_+d_o_+s);
  double kt = 0;

  for (int i = 0; i<=2 ; i++){
    if (i >= max(0ull, (unsigned long long int)(n) +  - d_i_-d_o_) && i <= min((unsigned long long int)(n), s)){
      kt += boost::math::pdf<double>(hyper, i);
    }
  }

  return p*(1-kt);
}

double ReservoirAddRemSampler::get_triangle_est(){

  double prob = prob_sampling_triangle();
	return counter_->triangles() / prob;
}

double ReservoirAddRemSampler::get_triangle_est_local(int node){
	assert(counter_->is_local());

  double prob = prob_sampling_triangle();
	return counter_->triangles_local(node) / prob;
}

// ALI PINAR Paper

PinarSampler::PinarSampler(size_t edge_res_size, size_t wedge_res_size)
	: GraphSampler(NULL/* no need of TriangleCounter*/), t_(0), tot_wedges_(0), fraction_closed_(0.0), edge_res_size_(edge_res_size), wedge_res_size_(wedge_res_size){
		edge_reservoir_.resize(edge_res_size);
		wedge_reservoir_.resize(wedge_res_size);
		wedge_closed_.resize(wedge_res_size);
}

PinarSampler::~PinarSampler(){}

double PinarSampler::get_triangle_est(){
	return fraction_closed_*t_*t_/
		(static_cast<double>(edge_res_size_)*(edge_res_size_-1))*tot_wedges_;
}

void PinarSampler::exec_operation(const EdgeUpdate& update){
	assert(update.is_add); //only add supported
	assert(update.node_u != update.node_v);
	int max_n = (int)max(update.node_u, update.node_v);
	int min_n = (int)min(update.node_u, update.node_v);

	t_+=1;

	pair<int,int> edge = make_pair(min_n, max_n);

	long long int closed = 0;

	// Check if closing wedges
	for (int i = 0; i<wedge_reservoir_.size(); i++){
		const pair<int,pair<int,int>> &wedge = wedge_reservoir_[i];

		if ((wedge.second.first == max_n && wedge.second.second == min_n)
				||(wedge.second.first == min_n && wedge.second.second == max_n)
		){
				wedge_closed_[i] = true;

		}
		closed += (wedge_closed_[i] ? 1 : 0);
	}

	// Update edge reservoir
	bool updated = false;
	for (int i = 0; i<edge_reservoir_.size(); i++){
		double u_rand = (double)rand() / ((double)RAND_MAX+1.0);
		if (u_rand <= 1.0/t_){
			edge_reservoir_[i] = edge;
			updated = true;
		}
	}

	if(updated){
		// count total num of wedges
		UDynGraph sub_graph;
		for (const auto& e: edge_reservoir_){
			sub_graph.add_edge(e.first, e.second);
		}
		vector<int> nodes;
		sub_graph.nodes(&nodes);

		tot_wedges_ = 0;
		for (const auto& n:nodes){
			int deg = sub_graph.degree(n);
			tot_wedges_+= (deg*(deg-1))/2;
		}

		// get new wedges with this edge...
		vector<pair<int,pair<int,int>>> new_wedges;
		vector<int> neighbors_min;
		sub_graph.neighbors(min_n, &neighbors_min);
		for (const auto & neighbor: neighbors_min){
			// wedge min_n-neighbor, min_n-max_n
			if (neighbor != max_n){
				new_wedges.push_back(make_pair(min_n, make_pair(neighbor, max_n)));
			}
		}
		vector<int> neighbors_max;
		sub_graph.neighbors(max_n, &neighbors_max);
		for (const auto & neighbor: neighbors_max){
			// wedge max_n-neighbor, max_n-min_n
			if (neighbor != min_n){
				new_wedges.push_back(make_pair(max_n, make_pair(neighbor, min_n)));
			}
		}

		for (int i = 0; i<wedge_reservoir_.size(); i++){
			double u_rand = (double)rand() / ((double)RAND_MAX+1.0);
			if (u_rand <= 1.0*new_wedges.size()/tot_wedges_){
				wedge_reservoir_[i] = new_wedges[rand()%new_wedges.size()];
				closed -= (wedge_closed_[i] ? 1 : 0);
				wedge_closed_[i] = false; // This make absolutely no sense but it is done in ali pinar paper So I implemented it as stated. ****
				closed += (wedge_closed_[i] ? 1 : 0);
			}
		}

		fraction_closed_ = 1.0*closed/ wedge_reservoir_.size();
	}

}


/**
 * Copyright 2004-present Facebook. All Rights Reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include <gflags/gflags.h>
#include <glog/logging.h>
#include <opencv2/opencv.hpp>

#include <boost/format.hpp>

#include "source/util/Camera.h"
#include "source/util/ImageUtil.h"

using namespace fb360_dep;
using namespace fb360_dep::image_util;

const std::string kUsageMessage = R"(
   - Aligns colors using separate (calibrated) color rigs.

   - Example:
     ./AlignColors \
     --output=/path/to/output \
     --color=/path/to/video/color \
     --calibrated_rig=/path/to/rigs/rig_calibrated.json \
     --rig_blue=/path/to/rigs/rig_blue.json \
     --rig_green=/path/to/rigs/rig_green.json \
     --rig_red=/path/to/rigs/rig_red.json
 )";

DEFINE_string(calibrated_rig, "", "path to calibrated green rig .json filename (required)");
DEFINE_string(cameras, "", "cameras to align (comma-separated)");
DEFINE_string(color, "", "path to input color images (required)");
DEFINE_string(first, "", "first frame to process (lexical)");
DEFINE_string(last, "", "last frame to process (lexical)");
DEFINE_string(output, "", "path to output directory (must be different than color path)");
DEFINE_string(rig_blue, "", "path to camera blue rig .json filename (required)");
DEFINE_string(rig_green, "", "path to camera green rig .json filename (required)");
DEFINE_string(rig_red, "", "path to camera red rig .json filename (required)");

using Image = cv::Mat_<cv::Vec3w>;
using WarpMap = cv::Mat_<cv::Point2f>;

cv::Vec3w getPixelColor(const Image& srcImage, const cv::Point2f& srcPixel) {
  return cv_util::getPixelBilinear(srcImage, srcPixel.x, srcPixel.y);
}

WarpMap createWarpMap(const Camera& srcCamera, const Camera& dstCamera) {
  const int width = int(srcCamera.resolution.x());
  const int height = int(srcCamera.resolution.y());
  cv::Mat_<cv::Point2f> warpMap(height, width);
  for (double x = 0.5; x < width; ++x) {
    for (double y = 0.5; y < height; ++y) {
      const Camera::Vector2 dstPixel(x, y);
      const double dstR = (dstPixel - dstCamera.principal).norm() / dstCamera.getScalarFocal();

      const double theta = dstCamera.undistort(dstR);
      const double srcR = srcCamera.distort(theta);
      const Camera::Vector2 srcPixel = (dstPixel - dstCamera.principal) /
              (dstPixel - dstCamera.principal).norm() * srcCamera.getScalarFocal() * srcR +
          dstCamera.principal;
      warpMap(y, x) = cv::Point2f(srcPixel.x(), srcPixel.y());
    }
  }
  return warpMap;
}

void createCalibratedRBRigs(
    Camera::Rig& calibratedRedRig,
    Camera::Rig& calibratedBlueRig,
    const Camera::Rig& referenceRedCamera,
    const Camera::Rig& referenceGreenCamera,
    const Camera::Rig& referenceBlueCamera,
    const Camera::Rig& calibratedGreenRig) {
  const double referenceGreenFocal = referenceGreenCamera[0].getScalarFocal();
  const double referenceRedFocal = referenceRedCamera[0].getScalarFocal();
  const double referenceBlueFocal = referenceBlueCamera[0].getScalarFocal();
  for (auto& calibratedGreenCamera : calibratedGreenRig) {
    Camera calibratedRedCamera = calibratedGreenCamera;
    Camera calibratedBlueCamera = calibratedGreenCamera;
    const double calibratedGreenFocal = calibratedGreenCamera.getScalarFocal();

    const float focalRatio = calibratedGreenFocal / referenceGreenFocal;
    calibratedRedCamera.setScalarFocal(referenceRedFocal * focalRatio);
    calibratedRedCamera.setDistortion(referenceRedCamera[0].getDistortion());
    calibratedBlueCamera.setScalarFocal(referenceBlueFocal * focalRatio);
    calibratedBlueCamera.setDistortion(referenceBlueCamera[0].getDistortion());

    calibratedRedRig.emplace_back(calibratedRedCamera);
    calibratedBlueRig.emplace_back(calibratedBlueCamera);
  }
}

Image warpImage(
    const Image& currentImage,
    const WarpMap& redWarpMap,
    const WarpMap& blueWarpMap,
    const int width,
    const int height) {
  Image alignedImage(height, width);
  for (double x = 0.5; x < width; ++x) {
    for (double y = 0.5; y < height; ++y) {
      cv::Vec3w alignedColor(0, 0, 0);
      alignedColor[1] = currentImage(y, x)[1];
      cv::Vec3w interpolatedRed = getPixelColor(currentImage, redWarpMap(y, x));
      alignedColor[2] = interpolatedRed[2];
      cv::Vec3w interpolatedBlue = getPixelColor(currentImage, blueWarpMap(y, x));
      alignedColor[0] = interpolatedBlue[0];
      alignedImage(y, x) = alignedColor;
    }
  }
  return alignedImage;
}

int main(int argc, char* argv[]) {
  system_util::initDep(argc, argv, kUsageMessage);

  CHECK_NE(FLAGS_color, "");
  CHECK_NE(FLAGS_calibrated_rig, "");
  CHECK_NE(FLAGS_rig_red, "");
  CHECK_NE(FLAGS_rig_green, "");
  CHECK_NE(FLAGS_rig_blue, "");

  CHECK_NE(FLAGS_color, FLAGS_output);

  // Load calibrated rig
  const Camera::Rig calibratedGreenRig =
      filterDestinations(Camera::loadRig(FLAGS_calibrated_rig), FLAGS_cameras);

  // Load reference camera models for each color
  const Camera::Rig redRig = Camera::loadRig(FLAGS_rig_red);
  const Camera::Rig greenRig = Camera::loadRig(FLAGS_rig_green);
  const Camera::Rig blueRig = Camera::loadRig(FLAGS_rig_blue);

  // Create calibrated rigs for red and blue by rescaling the focal parameter
  Camera::Rig calibratedRedRig;
  Camera::Rig calibratedBlueRig;
  createCalibratedRBRigs(
      calibratedRedRig, calibratedBlueRig, redRig, greenRig, blueRig, calibratedGreenRig);

  std::vector<WarpMap> redWarpMaps;
  std::vector<WarpMap> blueWarpMaps;
  for (unsigned long i = 0; i < calibratedGreenRig.size(); ++i) {
    redWarpMaps.emplace_back(createWarpMap(calibratedRedRig[i], calibratedGreenRig[i]));
    blueWarpMaps.emplace_back(createWarpMap(calibratedBlueRig[i], calibratedGreenRig[i]));
  }

  std::pair<int, int> frameRange =
      getFrameRange(FLAGS_color, calibratedGreenRig, FLAGS_first, FLAGS_last);

  for (int iFrame = frameRange.first; iFrame <= frameRange.second; ++iFrame) {
    const std::string frameName = intToStringZeroPad(iFrame);
    LOG(INFO) << boost::format("Loading frame %1%") % frameName;

    // Load input color images
    const std::vector<Image> images =
        loadImages<cv::Vec3w>(FLAGS_color, calibratedGreenRig, frameName);
    CHECK_GT(images.size(), 0) << "no images loaded!";
    CHECK_EQ(images.size(), calibratedGreenRig.size());
    CHECK_EQ(greenRig.size(), 1);
    CHECK_EQ(redRig.size(), 1);
    CHECK_EQ(blueRig.size(), 1);

    // Align to green
    for (ssize_t imageIndex = 0; imageIndex < ssize(images); ++imageIndex) {
        LOG(INFO) << boost::format("Aligning camera : %1%") % calibratedGreenRig[imageIndex].id;
      const int width = int(calibratedGreenRig[imageIndex].resolution.x());
      const int height = int(calibratedGreenRig[imageIndex].resolution.y());
      const Image& alignedImage = warpImage(
          images[imageIndex], redWarpMaps[imageIndex], blueWarpMaps[imageIndex], width, height);

      const filesystem::path camDir =
          filesystem::path(FLAGS_output) / calibratedGreenRig[imageIndex].id;
      filesystem::create_directories(camDir);
      const std::string outputFile = (boost::format("%1%/%2%.png") % camDir.string() % frameName).str();
      cv_util::imwriteExceptionOnFail(outputFile, cv_util::convertTo<uint16_t>(alignedImage));
    }
  }

  return EXIT_SUCCESS;
}


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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2015, 2016, 2018, 2019.
// Modifications copyright (c) 2015-2019, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// 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_GEOMETRY_STRATEGIES_GEOGRAPHIC_ENVELOPE_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_ENVELOPE_HPP

#include <boost/geometry/srs/spheroid.hpp>

#include <boost/geometry/strategies/geographic/envelope_segment.hpp>
#include <boost/geometry/strategies/geographic/expand_segment.hpp>
#include <boost/geometry/strategies/geographic/parameters.hpp>
#include <boost/geometry/strategies/spherical/envelope.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace envelope
{

template
<
    typename FormulaPolicy = strategy::andoyer,
    typename Spheroid = geometry::srs::spheroid<double>,
    typename CalculationType = void
>
class geographic
    : public spherical<CalculationType>
{
public:
    typedef Spheroid model_type;

    inline geographic()
        : m_spheroid()
    {}

    explicit inline geographic(Spheroid const& spheroid)
        : m_spheroid(spheroid)
    {}

    typedef geographic_segment
        <
            FormulaPolicy, Spheroid, CalculationType
        > element_envelope_strategy_type;
    inline element_envelope_strategy_type get_element_envelope_strategy() const
    {
        return element_envelope_strategy_type(m_spheroid);
    }

    typedef expand::geographic_segment
        <
            FormulaPolicy, Spheroid, CalculationType
        > element_expand_strategy_type;
    inline element_expand_strategy_type get_element_expand_strategy() const
    {
        return element_expand_strategy_type(m_spheroid);
    }

    template <typename Point1, typename Point2, typename Box>
    inline void apply(Point1 const& point1, Point2 const& point2, Box& box) const
    {
        geographic_segment
            <
                FormulaPolicy,
                Spheroid,
                CalculationType
            >(m_spheroid).apply(point1, point2, box);
    }

private:
    Spheroid m_spheroid;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <typename Tag, typename CalculationType>
struct default_strategy<Tag, geographic_tag, CalculationType>
{
    typedef strategy::envelope::geographic
        <
            strategy::andoyer,
            geometry::srs::spheroid<double>,
            CalculationType
        > type;
};

}

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::envelope

}} //namepsace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_ENVELOPE_HPP


#pragma once

#include "IAnt.hpp"
#include "WeightGraph.hpp"
#include "BestPosition.hpp"
#include <boost/optional.hpp>
#include <cmath>
#include <iostream>
#include <memory>
#include "Edge.hpp"

class BestPosition;

class Ant: public IAnt
{
public:
    Ant(){}
    Ant(Node mN):
        mPosition(mN){}
    double moveAnt(std::vector<std::reference_wrapper<Edge> > neighbours) override;
    Node position() const;
    Node getLastPosition() const override ;
    void positionToString() const override;

private:
    bool wasAntThere(const Edge& E) const;
    void addEdgeToMemory (const Edge &edge);
    void changePosition(const Node &mN);
    std::vector<std::reference_wrapper<Edge> > getUnvisitedNeighbours(std::vector<std::reference_wrapper<Edge>> n);

    Node mPosition;
    Node mlastPosition;
    double mProbability=0;
    double mNodeProbability=0;
    std::vector<double> nodeProbability;
    std::vector<Edge> whereAntWas;
    std::shared_ptr<BestPosition> bestPosition;

};


//
//  bpp.cpp
//  PhyloAcc
//
//  Created by hzr on 3/8/16.
//  Copyright © 2016 hzr. All rights reserved.
//

#include "bpp.hpp"
#include <armadillo>
#include <sys/types.h>
#include <dirent.h>
#include<queue>

#include <cmath>
#include <cassert>

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_cdf.h>

#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <ctype.h>

#include "newick.h"
#include "utils.h"
#include "bpp_c.hpp"

using namespace std;
using namespace arma;

// load the phylogenetic tree
void BPP::InitPhyloTree(PhyloTree & tree) //, double indel_pi), double indel, double indel2
{

//    cx_mat bvec;
//    cx_vec aval;
//
//    if(num_base <= 4)  // no indel
//    {
//
//    }else{
//        tree.subs_rate *= (1-indel);
//        mat B = ones<mat>(4,1) * indel;
//        mat C = ones<mat>(1,5) * indel2;
//        //tree.subs_rate.diag() -= indel;
//        tree.subs_rate.insert_cols(4, B);
//        tree.subs_rate.insert_rows(4, C);
//        //tree.subs_rate(4,4) = -0.04;
//        colvec c = sum(tree.subs_rate,1);
//        tree.subs_rate.diag() -=c;
//
////        tree.pi.insert_rows(4,1);
////        tree.pi.head(4) *= 1 - indel_pi;
////        tree.pi[4] = indel_pi;
//
//    }
//
//    eig_gen(aval, bvec, tree.subs_rate);
//    eigenval = conv_to<mat>::from(aval);
//    eigenvec = conv_to<mat>::from(bvec).t();
//    eigeninv = inv(eigenvec);
//    submat = tree.subs_rate;
//
//    //cout <<eigenval;
//    //cout <<"eigenvec: " << eigenvec;
//    //cout <<tree.subs_rate;
//
//    mat a = null(tree.subs_rate.t());
//
//
//    pi = a/accu(a); //tree.pi;
//    //cout <<"pi: " <<  pi.t();
//
//    log_pi = log(pi);

    submat = tree.subs_rate;
    children    = new int[N][2];
    parent      = new int[N];
    distances   = new double[N];

    for(int s=0; s<N; s++)
    {
        distances[s] = tree.distances[s];

    }

    for(int i=0; i<N; i++)
    {
        children[i][0] = -1;
        children[i][1] = -1;
        parent[i] = -1;
    }

    for(int i=0; i<N; i++)
    {
        int p = -1;
        for(int j=0; j<N; j++)
        {
            if (tree.dag[i][j])
            {
                p++;
                children[i][p] = j;
                parent[j] = i;
            }
        }
    }

//    distances[children[N-1][1]] += distances[children[N-1][0]];
//    distances[children[N-1][0]] = 0;
//    moveroot = children[N-1][0];

    //distances[83] += distances[42];  // modify distance for root!
    //distances[42] =0 ;

}

// try to match the phylogenetic profile and tree
void BPP::MatchProfAndTree(PhyloProf & _prof, PhyloTree & _tree)
{

    // try to match the species
    bool success_match = true;
    int S = _tree.S;
    int S2 = _prof.S;
    vector<int> reorder(S);  //each species in the tree where is in prof
    for(int s1=0; s1<S; s1++)
    {
        bool has_same_species = false;
        string sname1 = _tree.species_names[s1];
        for(int s2=0; s2<S2; s2++)
        {
            string sname2 = _prof.species_names[s2];
            //            cout << sname1 << " ? " << sname2 << endl;
            if (sname1 == sname2)
            {
                has_same_species = true;
                reorder[s1] = s2;
                break;
            }
        }
        if (!has_same_species)
        {
            cout << "No matrix species " << _prof.species_names[s1] << " found in tree." << endl;
            success_match = false;
            break;
        }
    }

    if (!success_match) // if cannot match literally
    {
        cout << endl << "The species in phylogenetic profile and tree cannot be matched literally:" << endl;
        cout << "The program will use the default mapping in data:" << endl;
        for(int s=0; s<S; s++)
            cout << "(" << _prof.species_names[s] << "\t=  " << _tree.species_names[s] << ")" << endl;
        cout << endl;
    }
    else                // if successully matched
    {
        cout << "The species in profile and tree match perfectly. Reorder the species in profile matrix by the tree." << endl << endl;
        vector<string> old_X = _prof.X;
        for(int s=0; s<S; s++)
        {
            int reorder_s = reorder[s];
            _prof.X[s] = old_X[reorder_s];
        }
        _prof.species_names = _tree.species_names;
    }

}

void BPP::InitMCMC(int _num_burn, int _num_mcmc, int _num_thin)
{
    // init parameters
    num_burn = _num_burn;

    num_mcmc = _num_mcmc;
    num_thin = _num_thin;

    last_time = time(NULL);

    // init MCMC sampling storage
    Max_Z = vector<vector < vector <int > >> (3,vector < vector <int >>  (C,vector <int > (N,0)));
    cur_Z = vector<vector < vector <int > >> (3,vector < vector <int >>  (C,vector <int > (N,0)));

    log_liks_null = vector <double>(C,0);
    log_liks_Z = vector<vector <double>>(3,vector <double>(C,0));
    log_liks_sgl = vector <double>(C,0);
    log_liks_resZ = vector <double>(C,0);

    log_liks_curZ = vector <double>(C,0);
    log_liks_propZ = vector <double>(C,0);
    MH_ratio_gain = vector <double>(C,0);
    MH_ratio_loss = vector <double>(C,0);

    cur_crate = vector <double>(C,ratio0);
    cur_nrate = vector <double>(C,ratio1);

    cur_lrate = vector <double>(C,ind_lrate);
    cur_lrate2 = vector <double>(C,ind_lrate2);
    cur_grate = vector <double>(C, ind_grate);

}

void BPP::sample_proposal(int iter, double & lrate_prop, double & grate_prop, ofstream & output)
{
    //indel_prop =gsl_ran_gamma(RNG,20 ,indel/20);
    //indel2_prop =gsl_ran_gamma(RNG,10 ,indel2/10);
    lrate_prop =gsl_ran_beta(RNG, ind_lrate * vlr, (1 - ind_lrate) *vlr); // let vlr == vgr!!
    grate_prop =gsl_ran_beta(RNG, ind_grate * vgr, (1 - ind_grate - ind_lrate) *vgr); //gsl_ran_gamma(RNG, vgr, ind_grate/vgr);
    grate_prop = grate_prop * (1 - lrate_prop);

//    submat.submat(0,0,3,3) *= (1 - indel_prop)/(1 - indel);
//    submat.col(4) = ones<mat>(5,1) * indel_prop;
//    submat.row(4) = ones<mat>(1,5) * indel2_prop;
//    colvec c = sum(submat,1);
//    submat.diag() -=c;
//
//    cx_mat bvec;
//    cx_vec aval;
//    eig_gen(aval, bvec, submat);
//    eigenvalprop = conv_to<mat>::from(aval);
//    eigenvecprop = conv_to<mat>::from(bvec).t();
//    eigeninvprop = inv(eigenvecprop);
//
//    mat a = null(submat.t());
//    piprop = a/accu(a);
//    //log_piprop = log(a/accu(a));
//
//    //cout <<"proposed subs matrix: " << submat << endl;
//    //cout <<"piprop: " <<  piprop.t();

    output << iter << "\t"<< nprior_a<< "\t"<< nprior_b <<"\t"<< cprior_a << "\t"<< cprior_b << "\t"<< indel << "\t"<< indel2 << "\t"<<ind_grate<< "\t"<< ind_lrate <<endl;
}

double BPP::log_lik(vector< vector<vec> > & lambda, double _indel, double _indel2, int start1, int end1, vector<unsigned int> & v, double p)
{
    // compute loglik
    double result =0;
    mat x(2,2);

    int rr = *subtree.rbegin();
    // 1. sending the lambda msg from leaves bottom up through the network
    for(vector<int>::iterator it = subtree.begin(); it!=subtree.end(); it++) //int s=S; s<N; s++)
    {
        int s = *it;
        if(s<S) continue;
        int* p = children[s];
        for(int it = start1; it < end1; it++)  lambda[v[it]][s].fill(0);

        for(int cc=0;cc<2;cc++)
        {
            int chi = p[cc];
            assert(chi != -1);
            if(distances[chi]>0 )
            {
                double tt = (1 - exp(-(_indel + _indel2) * distances[chi]))/(_indel + _indel2);
                x.at(1,0) = _indel * tt;
                x.at(0,0) = 1 - x.at(1,0);

                x.at(0,1) = _indel2 * tt;
                x.at(1,1) = 1 - x.at(0,1);

                //cout << x;
                x = log(x);

            }
            else{
                x.fill(-INFINITY); //83, root
                x.diag().fill(0);
            }

            #pragma omp parallel for schedule (guided)
            for(int it = start1; it < end1; it++)  lambda[v[it]][s] +=  BPP::log_multi(x,lambda[v[it]][chi]);
        }

    }

    // 2. processing the distribution of root species

    for(int it = start1; it < end1; it++)
    {

//        if(lambda[v[it]][N-1][1] < -1e3)
//        {
//            cout <<v[it]<<": "<< lambda[v[it]][N-1].t();
//            cout <<children[N-1][0]<<": " << lambda[v[it]][children[N-1][0]].t();
//            cout <<children[N-1][1]<<": " << lambda[v[it]][children[N-1][1]].t();
//
//        }
        lambda[v[it]][rr][0] += log(1-p); //N-1
        lambda[v[it]][rr][1] += log(p) ;
        result += BPP::log_exp_sum(lambda[v[it]][rr]);
    }

    return(result);
}

void BPP::sample_hyperparam(int iter, vector<int> & ids, ofstream & output) // recompute log_TM, double indel_prop, double indel2_prop,
{
// 0702 no sample rates
    //indepent MH to sample hyperparam of rates
    double p=1,r = 1; //hyperparam for shape
    double q=0.1,s = 0.1; // hyperparam for scale

    double vna = 100, vnb = 100, vca = 100, vcb = 100;
    double nprior_a_prop =gsl_ran_gamma(RNG, vna, nprior_a/vna);
    double cprior_a_prop =gsl_ran_gamma(RNG, vca, cprior_a/vca);

    double nprior_b_prop =gsl_ran_gamma(RNG, vnb, nprior_b/vnb);
    double cprior_b_prop =gsl_ran_gamma(RNG, vcb, cprior_b/vcb);

    //MH proposal
    double sum_r = 0;
    double log_prod_r = 0;
    //double var_r = 0;
    //for(vector<double>::iterator it = cur_nrate.begin(); it< cur_nrate.end(); it++)
    for(std::size_t i = 0; i < ids.size(); i++ )  // 0702
    {
        int c = ids[i];
        sum_r += cur_nrate[c];
        //var_r += pow(*it, 2);
        log_prod_r += log(cur_nrate[c]);
    }

    double M_ratio = (nprior_a_prop - 1) * (log(p) + log_prod_r) - (q + sum_r)/nprior_b_prop - (ids.size() + r)*lgamma(nprior_a_prop) - log(nprior_b_prop) * nprior_a_prop * (s + ids.size());
    M_ratio -= (nprior_a - 1) * (log(p) + log_prod_r) - (q + sum_r)/nprior_b - (ids.size() + r)*lgamma(nprior_a) - log(nprior_b) * nprior_a * (s + ids.size());

    double H_ratio = log(gsl_ran_gamma_pdf(nprior_a,vna,nprior_a_prop/vna)) - log(gsl_ran_gamma_pdf(nprior_a_prop,vna,nprior_a/vna)) + log(gsl_ran_gamma_pdf(nprior_b,vnb,nprior_b_prop/vnb)) - log(gsl_ran_gamma_pdf(nprior_b_prop,vnb,nprior_b/vnb));

    cout << "nrate_MH_ratio: " << M_ratio <<", " << H_ratio << ", " << nprior_a << ", " << nprior_a_prop << ", " << nprior_b << ", " << nprior_b_prop << endl;

    if(log(gsl_rng_uniform(RNG)) < M_ratio + H_ratio)
    {
        nprior_a = nprior_a_prop;
        nprior_b = nprior_b_prop;
    }

    sum_r = 0;
    log_prod_r = 0;
    //for(vector<double>::iterator it = cur_crate.begin(); it< cur_crate.end(); it++)
    for(std::size_t i = 0; i < ids.size(); i++ )  // 0702
    {
        int c = ids[i];
        sum_r += cur_crate[c];
        //var_r += pow(*it, 2);
        log_prod_r += log(cur_crate[c]);
    }

    M_ratio = (cprior_a_prop - 1) * (log(p) + log_prod_r) - (q + sum_r)/cprior_b_prop - (ids.size() + r)*lgamma(cprior_a_prop) - log(cprior_b_prop) * cprior_a_prop * (s + ids.size()) - ((cprior_a - 1) * (log(p) + log_prod_r) - (q + sum_r)/cprior_b - (ids.size() + r)*lgamma(cprior_a) - log(cprior_b) * cprior_a * (s + ids.size()));

    H_ratio = log(gsl_ran_gamma_pdf(cprior_a,vca,cprior_a_prop/vca)) - log(gsl_ran_gamma_pdf(cprior_a_prop,vca,cprior_a/vca)) + log(gsl_ran_gamma_pdf(cprior_b,vcb,cprior_b_prop/vcb)) - log(gsl_ran_gamma_pdf(cprior_b_prop,vcb,cprior_b/vcb));

    cout << "crate_MH_ratio: " << M_ratio + H_ratio << ", " << cprior_a << ", " << cprior_a_prop <<", " << cprior_b << ", " << cprior_b_prop << endl;

    if(log(gsl_rng_uniform(RNG)) < M_ratio + H_ratio)
    {
        cprior_a = cprior_a_prop;
        cprior_b = cprior_b_prop;
    }

  // sample hyperparameters of lrate and grate, exponential prior for prior_l_a and prior_l_b, prior_g_a and prior_g_b
    double u = gsl_rng_uniform(RNG)*(1.3 - 0.7) + 0.7; // generate uniform from (1/prop_n, prop_n);
    double prior_l_a_prop = prior_l_a *  u;

    u = gsl_rng_uniform(RNG)*(1.3 - 0.7) + 0.7; // generate uniform from (1/prop_n, prop_n);
    double prior_l_b_prop = prior_l_b *  u;
    double log_p = 0, log_pc = 0;
    for(std::size_t i = 0; i < ids.size(); i++ )  // 0702
    {
        int c = ids[i];
        log_p += log(cur_lrate[c]);
        log_pc += log(1 - cur_lrate[c]);
    }

    M_ratio = (prior_l_a_prop - prior_l_a) * (log_p - 1) + (prior_l_b_prop - prior_l_b) * (log_pc - 1);
    M_ratio += ids.size() * (gsl_sf_lnbeta(prior_l_a, prior_l_b) - gsl_sf_lnbeta(prior_l_a_prop, prior_l_b_prop));

    H_ratio = log(prior_l_a) - log(prior_l_a_prop) + log(prior_l_b) - log(prior_l_b_prop);

    cout << "lrate_MH_ratio: " << M_ratio + H_ratio << ", " << prior_l_a << ", " << prior_l_a_prop <<", " << prior_l_b << ", " << prior_l_b_prop  << endl;

    if(log(gsl_rng_uniform(RNG)) < M_ratio + H_ratio)
    {
        prior_l_a = prior_l_a_prop;
        prior_l_b = prior_l_b_prop;
    }

    // sample grate
    u = gsl_rng_uniform(RNG)*(1.3 - 0.7) + 0.7; // generate uniform from (1/prop_n, prop_n);
    double prior_g_a_prop = prior_g_a *  u;

    u = gsl_rng_uniform(RNG)*(1.3 - 0.7) + 0.7; // generate uniform from (1/prop_n, prop_n);
    double prior_g_b_prop = prior_g_b *  u;
    log_p = 0; log_pc = 0;
    for(std::size_t i = 0; i < ids.size(); i++ )  // 0702
    {
        int c = ids[i];
        log_p += log(cur_grate[c]);
        log_pc += log(1 - cur_grate[c]);
    }

    M_ratio = (prior_g_a_prop - prior_g_a) * (log_p - 1) + (prior_g_b_prop - prior_g_b) * (log_pc - 1);
    M_ratio += ids.size() * (gsl_sf_lnbeta(prior_g_a, prior_g_b) - gsl_sf_lnbeta(prior_g_a_prop, prior_g_b_prop));

    H_ratio = log(prior_g_a) - log(prior_g_a_prop) + log(prior_g_b) - log(prior_g_b_prop);

    cout << "grate_MH_ratio: " << M_ratio + H_ratio << ", " << prior_g_a << ", " << prior_g_a_prop <<", " << prior_g_b << ", " << prior_g_b_prop  << endl;

    if(log(gsl_rng_uniform(RNG)) < M_ratio + H_ratio)
    {
        prior_g_a = prior_g_a_prop;
        prior_g_b = prior_g_b_prop;
    }

     output << iter << "\t"<< nprior_a<< "\t"<< nprior_b <<"\t"<< cprior_a << "\t"<< cprior_b << "\t"<< prior_l_a << "\t"<< prior_l_b << "\t"<< prior_g_a << "\t"<< prior_g_b <<endl;

}

void BPP::getUppertree(int root, vector<int>& child, set<int> & visited_init)  // include root!
{
    for(vector<int>::iterator it = child.begin(); it!=child.end(); it++)
    {
        int p = *it;
       while(p!=root)
       {
           visited_init.insert(p);
           p = parent[p];
       }

    }

    visited_init.insert(root);

}

void BPP::getSubtree(int root, vector<int> & visited_init)  // traverse from root to children, include root
{

    int j = root;

    //cout << nodes_names[j]<<"\t";

    if(children[j][0]!=-1)
    {

        for(int chi=0;chi<2;chi++)
        {
            getSubtree(children[j][chi], visited_init);

        }

    }

    visited_init.push_back(j);

}

void BPP::getSubtree(int root, set<int>& child, vector<int> & visited_init)  // traverse from root, stop at children, 74 & 64; do include 1-S!
{

    int j = root;

    //cout << nodes_names[j]<<"\t";
    if(child.find(j) != child.end())
    {

        visited_init.push_back(j);

        return;
    }

    if(children[j][0]!=-1)
    {

        for(int chi=0;chi<2;chi++)
        {
            getSubtree(children[j][chi], child, visited_init);

        }

      //  visited_init.push_back(j);

    }

     visited_init.push_back(j);

}

void BPP::Output_init(PhyloProf & prof, string output_path, vector<int> & ids){

        string outpath_elem = output_path+ "_elem_lik.txt";
        ofstream out_lik(outpath_elem.c_str());
        out_lik.precision(8);
        out_lik << "No.\tID\tloglik_Null\tloglik_Acc\tloglik_Full\tlogBF1\tlogBF2\tloglik_Max_M0\tloglik_Max_M1\tloglik_Max_M2"<<endl;
        //for(int cc=0; cc<C;cc++)
        for(vector<int>::iterator it = ids.begin(); it !=ids.end(); it++)
        {
            int cc = *it;
            out_lik <<cc << "\t" << prof.element_names[cc] << "\t" << log_liks_null[cc] <<"\t"  <<log_liks_resZ[cc] <<"\t"  <<log_liks_sgl[cc]<< "\t";
            out_lik << log_liks_resZ[cc] -  log_liks_null[cc] << "\t" << log_liks_resZ[cc] -  log_liks_sgl[cc];
            //for(int r=0;r<3;r++)
            out_lik <<"\t" <<log_liks_Z[0][cc] << "\t" <<log_liks_Z[2][cc]<<"\t" <<log_liks_Z[1][cc];
            out_lik << endl;
        }

        out_lik.close();

    ofstream out_z;
    for(int r =0;r<3;r++)
    {
            if(r == 2)
            {
                outpath_elem = output_path+"_M" +to_string(1) + "_elem_Z.txt";
            }else if(r == 1)
            {
                outpath_elem = output_path+"_M" +to_string(2) + "_elem_Z.txt";
            }else{
                outpath_elem = output_path+"_M" +to_string(0) + "_elem_Z.txt";
            }

            out_z.open(outpath_elem.c_str());
            out_z<<"No.";
            for(int s =0 ;s<N;s++){  // header: species name
                out_z<< "\t" << nodes_names[s] ;
            }
            out_z <<endl;

            for(vector<int>::iterator it = ids.begin(); it !=ids.end(); it++)
            {
                int c = *it;
                out_z<<c;
                for(int s=0; s<N;s++)
                    out_z <<"\t" << Max_Z[r][c][s];
                out_z <<endl;
            }
        out_z.close();
    }

}

void BPP::Output_init0(PhyloProf & prof, ofstream& out_lik, vector<int> & ids){

    //for(int cc=0; cc<C;cc++)
    for(vector<int>::iterator it = ids.begin(); it !=ids.end(); it++)
    {
        int cc = *it;
        out_lik <<cc << "\t" << prof.element_names[cc] << "\t"  <<log_liks_sgl[cc]<< "\t"<<log_liks_Z[1][cc];
        out_lik << endl;
    }

    //out_lik.close();

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_SSE_SSE2_TOFLOAT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_SSE_SSE2_TOFLOAT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/arithmetic/functions/tofloat.hpp>
#include <boost/simd/include/functions/simd/make.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/simd/include/functions/simd/if_else_zero.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/is_ltz.hpp>
#include <boost/simd/include/functions/simd/bitwise_notand.hpp>
#include <boost/simd/include/constants/twoto31.hpp>
#include <boost/simd/include/constants/signmask.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( tofloat_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<int32_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef typename dispatch::meta::as_floating<A0>::type result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return _mm_cvtepi32_ps(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( tofloat_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<uint64_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef typename dispatch::meta::as_floating<A0>::type  result_type;
    typedef typename meta::scalar_of<result_type>::type     stype;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return boost::simd::make<result_type> ( static_cast<stype>(a0[0])
                                            , static_cast<stype>(a0[1])
                                            );
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( tofloat_
                                    , boost::simd::tag::sse2_ , (A0)
                                    , ((simd_<uint32_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef typename dispatch::meta::as_floating<A0>::type result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::as_integer<A0, signed>::type  si_type;
      si_type a00 = bitwise_cast<si_type>(a0);
      si_type a01 = bitwise_notand(Signmask<si_type>(), a0);
      result_type inc = if_else_zero(is_ltz(a00), Twoto31<result_type>());
      return tofloat(a01)+inc;
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( tofloat_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<int64_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef typename dispatch::meta::as_floating<A0>::type        result_type;
    typedef typename meta::scalar_of<result_type>::type           sftype;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return boost::simd::make<result_type> ( static_cast<sftype>(a0[0])
                                            , static_cast<sftype>(a0[1])
                                            );
    }
  };
} } }

#endif
#endif


/**
 * Command line tool for the superpixel algorithm proposed in [1].
 *
 * [1] O. Veksler, Y. Boykov, P. Mehrani.
 *     Superpixels and supervoxels in an energy optimization framework.
 *     European Conference on Computer Vision, pages 211–224, 2010.
 *
 * **Unfortunately, the license of this superpixel algorithm prohibits us from
 * redistributing the code, thereore we only provide the command line interface.**
 *
 * Installation intructions:
 *
 * - Got to http://www.csd.uwo.ca/faculty/olga/ and download the code.
 * - Extract the archive into lib_cis to obtain the following folder structure:
 *
 *  lib_cis
 *  |- vlib
 *    |- include
 *    |- utils
 *    |- CMakeLists.txt (provided by this library)
 *  |- README.txt
 *  |- maxflow.cpp
 *  |- ...
 *  |- CMakeLists.txt (provided by this library)
 *
 * - Comment out the `main` function in `superpixels.cpp`.
 * - Either change the declaration of loadEdges in superpixels.cpp to
 *
 *  void loadEdges(vector<Value> &weights,int num_pixels,int width,int height,
 *			   Value lambda, char *name)
 *
 * or use -fpermissive (default).
 * - Depending on the operating system, someminor changes within the
 * downloaded code will be necessary. In particular, in energy.h change
 * occurences of
 *
 *  add_tweights(y, 0, C);
 *  add_edge(x, y, B+C, 0);
 *
 * to
 *
 *  this->add_tweights(y, 0, C);
 *  this->add_edge(x, y, B+C, 0);
 *
 * The code was used for evaluation purposes in [2]:
 *
 * [2] D. Stutz, A. Hermans, B. Leibe.
 *     Superpixel Segmentation using Depth Information.
 *     Bachelor thesis, RWTH Aachen University, Aachen, Germany, 2014.
 *
 * [2] is available online at
 *
 *      http://davidstutz.de/bachelor-thesis-superpixel-segmentation-using-depth-information/
 *
 * **How to use the command line tool?**
 *
 *  $ ./bin/cis_cli --help
 *  Allowed options:
 *   --help                  produce help message
 *   --input arg             folder containing the images to process
 *   --region-size arg (=10) maxmimum allowed region size (that is region size x
 *                           region size patches)
 *   --type arg (=1)         0 for compact superpixels, 1 for constant intensity
 *                           superpixels
 *   --iterations arg (=2)   number of iterations
 *   --lambda arg (=50)      lambda only influences constant intensity
 *                           superpixels; larger lambda results in smoother
 *                           boundaries
 *   --process               show additional information while processing
 *   --time arg              time the algorithm and save results to the given
 *                           directory
 *   --csv                   save segmentation as CSV file
 *   --contour               save contour image of segmentation
 *   --mean                  save mean colored image of segmentation
 *   --time                  save timings in BSD evaluation format in the given
 *                           directory
 *  --output arg (=output)  specify the output directory (default is ./output)
 *
 * The code (only concerning this command line tool, not the downloaded code
 * from Olga Veksler!) is published under the BSD 3-Clause:
 *
 * Copyright (c) 2014, David Stutz
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
#include "SeedsRevised.h"
#include "Tools.h"
#include "superpixels.h"
#include <opencv2/opencv.hpp>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <boost/timer.hpp>

#if defined(WIN32) || defined(_WIN32)
    #define DIRECTORY_SEPARATOR "\\"
#else
    #define DIRECTORY_SEPARATOR "/"
#endif

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

    boost::program_options::options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("input", boost::program_options::value<std::string>(), "folder containing the images to process")
        ("region-size", boost::program_options::value<int>()->default_value(10), "maxmimum allowed region size (that is region size x region size patches)")
        ("type", boost::program_options::value<int>()->default_value(1), "0 for compact superpixels, 1 for constant intensity superpixels")
        ("iterations", boost::program_options::value<int>()->default_value(2), "number of iterations")
        ("lambda", boost::program_options::value<int>()->default_value(50), "lambda only influences constant intensity superpixels; larger lambda results in smoother boundaries")
        ("process", "show additional information while processing")
        ("time", boost::program_options::value<std::string>(), "time the algorithm and save results to the given directory")
        ("csv", "save segmentation as CSV file")
        ("contour", "save contour image of segmentation")
        ("mean", "save mean colored image of segmentation")
        ("output", boost::program_options::value<std::string>()->default_value("output"), "specify the output directory (default is ./output)");

    boost::program_options::positional_options_description positionals;
    positionals.add("input", 1);

    boost::program_options::variables_map parameters;
    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(desc).positional(positionals).run(), parameters);
    boost::program_options::notify(parameters);

    if (parameters.find("help") != parameters.end()) {
        std::cout << desc << std::endl;
        return 1;
    }

    boost::filesystem::path imageDir(parameters["input"].as<std::string>());
    if (!boost::filesystem::is_directory(imageDir)) {
        std::cout << "Image directory not found ..." << std::endl;
        return 1;
    }

    boost::filesystem::path outputDir(parameters["output"].as<std::string>());
    if (!boost::filesystem::is_directory(outputDir)) {
        boost::filesystem::create_directory(outputDir);
    }

    bool process = false;
    if (parameters.find("process") != parameters.end()) {
        process = true;
    }

    std::vector<boost::filesystem::path> pathVector;
    std::vector<boost::filesystem::path> images;

    std::copy(boost::filesystem::directory_iterator(imageDir), boost::filesystem::directory_iterator(), std::back_inserter(pathVector));

    std::sort(pathVector.begin(), pathVector.end());

    int count = 0;
    for (std::vector<boost::filesystem::path>::const_iterator iterator (pathVector.begin()); iterator != pathVector.end(); ++iterator) {
        if (boost::filesystem::is_regular_file(*iterator)) {

            std::string extension = iterator->extension().string();
            std::transform(extension.begin(), extension.end(), extension.begin(), ::tolower);

            if (extension == ".png" || extension == ".jpg") {
                images.push_back(*iterator);

                if (process == true) {
                    std::cout << "Found " << iterator->string() << " ..." << std::endl;
                }

                ++count;
            }
        }
    }

    std::cout << count << " images found ..." << std::endl;

    double totalTime = 0;

    int iterations = parameters["iterations"].as<int>();
    int lambda = parameters["lambda"].as<int>();
    int type = parameters["type"].as<int>();

    if (type != 0 && type != 1) {
        std::cout << "Type needs to be 0 for compact superpixels or 1 for constant intensity superpixels ...";
        return 1;
    }

    int regionSize = parameters["region-size"].as<int>();

    boost::timer timer;

    cv::Mat time(images.size(), 2, cv::DataType<double>::type);
    for(std::vector<boost::filesystem::path>::iterator iterator = images.begin(); iterator != images.end(); ++iterator) {

        cv::Mat mat = cv::imread(iterator->string());

        // Convert to PGM.
        boost::filesystem::path extension = iterator->filename().extension();
        int position = iterator->filename().string().find(extension.string());

        cv::Mat matGrayScale;
        cv::cvtColor(mat, matGrayScale, SEEDS_REVISED_OPENCV_BGR2GRAY);

        std::string storeGray = outputDir.string() + DIRECTORY_SEPARATOR + iterator->filename().string().substr(0, position) + ".pgm";
        cv::imwrite(storeGray, matGrayScale);

        // Generate edge maps (that is, gradient maps in x and y direction)
        cv::Mat gradX;
        cv::Mat gradY;
        cv::Mat absGradX;
        cv::Mat absGradY;

        cv::GaussianBlur(matGrayScale, matGrayScale, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT);

        cv::Sobel(matGrayScale, gradX, CV_16S, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT);
        cv::convertScaleAbs(gradX, absGradX);

        cv::Sobel(matGrayScale, gradY, CV_16S, 0, 1, 3, 1, 0, cv::BORDER_DEFAULT);
        cv::convertScaleAbs(gradY, absGradY);

        std::string storeX = outputDir.string() + DIRECTORY_SEPARATOR + iterator->filename().string().substr(0, position) + "_x.pgm";
        std::string storeY = outputDir.string() + DIRECTORY_SEPARATOR + iterator->filename().string().substr(0, position) + "_y.pgm";

        cv::imwrite(storeX, absGradX);
        cv::imwrite(storeY, absGradY);

        image<unsigned char> *I = loadPGM(storeGray.c_str());
        assert(I != 0);

        int width  = I->width();
        int height = I->height();
        int num_pixels = width*height;

        timer.restart();
        int index = std::distance(images.begin(), iterator);

        float variance = computeImageVariance(I, width, height);

        // Initialize and place seeds
        std::vector<int> Seeds(num_pixels);
        int numSeeds = 0;
        PlaceSeeds(I, width, height, num_pixels, Seeds, &numSeeds, regionSize);
        MoveSeedsFromEdges(I, width, height, num_pixels, Seeds, numSeeds, regionSize);

        std::vector<int> horizWeights(num_pixels,lambda);
        std::vector<int> vertWeights(num_pixels,lambda);
        std::vector<int> diag1Weights(num_pixels,lambda);
        std::vector<int> diag2Weights(num_pixels,lambda);

        loadEdges(horizWeights, num_pixels, width, height, lambda, storeX.c_str());
        loadEdges(vertWeights, num_pixels, width, height, lambda, storeY.c_str());
        // loadEdges(diag1Weights, num_pixels, width, height, lambda, edges[2].c_str());
        // loadEdges(diag2Weights, num_pixels, width, height, lambda, edges[3].c_str());
        computeWeights(horizWeights, num_pixels, width,height, lambda, variance, -1, 0, I, type);
        computeWeights(vertWeights, num_pixels, width,height, lambda, variance, 0, -1, I, type);
        computeWeights(diag1Weights, num_pixels, width,height, lambda, variance, -1, -1, I, type);
        computeWeights(diag2Weights, num_pixels, width,height, lambda, variance, 1, -1, I, type);

        vector<int> labeling(num_pixels);

        initializeLabeling(labeling, width, height, Seeds, numSeeds, regionSize);

        int oldEnergy, newEnergy;

        std::vector<int> changeMask(num_pixels, 1);
        std::vector<int> changeMaskNew(num_pixels, 0);

        std::vector<int> order(numSeeds);
        for (int i = 0; i < numSeeds; i++) {
            order[i] = i;
        }

        int j = 0;
        //purturbSeeds(order,numSeeds);

        while (true) {
            newEnergy = computeEnergy(labeling, width, height, num_pixels, horizWeights, vertWeights, diag1Weights, diag2Weights, Seeds, I, type);

            if (j == 0) {
                oldEnergy = newEnergy + 1;
            }

            if (newEnergy == oldEnergy || j >= iterations) {
                break;
            }

            oldEnergy = newEnergy;

            for (int i = 0; i < numSeeds; i++) {
                expandOnLabel(order[i], width, height, num_pixels, Seeds, numSeeds, labeling, horizWeights,
                             vertWeights, lambda, diag1Weights, diag2Weights, regionSize, changeMask,
                             changeMaskNew, I, type, variance);
            }

            for (int i = 0; i < num_pixels; i++) {
                changeMask[i] = changeMaskNew[i];
                changeMaskNew[i] = 0;
            }

            //purturbSeeds(order,numSeeds);
            j++;
        }

        time.at<double>(index, 1) = timer.elapsed();
        time.at<double>(index, 0) = index + 1;
        totalTime += time.at<double>(index, 1);

        delete I;

        int** labels = new int*[height];
        for (int i = 0; i < height; ++i) {
            labels[i] = new int[width];

            for (int j = 0; j < width; ++j) {
                labels[i][j] = labeling[j + i*width];
            }
        }

        if (parameters.find("csv") != parameters.end()) {

            boost::filesystem::path csvFile(outputDir.string() + DIRECTORY_SEPARATOR + iterator->filename().string().substr(0, position) + ".csv");
            Export::CSV(labels, height, width, csvFile);

            if (process == true) {
                std::cout << "Labels for image " << iterator->string() << " saved in " << csvFile.string() << " ..." << std::endl;
            }
        }

        if (parameters.find("contour") != parameters.end()) {

            std::string store = outputDir.string() + DIRECTORY_SEPARATOR + iterator->filename().string().substr(0, position) + "_contours.png";

            int bgr[] = {0, 0, 204};
            cv::Mat contourImage = Draw::contourImage(labels, mat, bgr);
            cv::imwrite(store, contourImage);

            if (process == true) {
                std::cout << "Image " << iterator->string() << " with contours saved to " << store << " ..." << std::endl;
            }
        }

        if (parameters.find("mean") != parameters.end()) {

            std::string store = outputDir.string() + DIRECTORY_SEPARATOR + iterator->filename().string().substr(0, position) + "_mean.png";

            cv::Mat meanImage = Draw::meanImage(labels, mat);
            cv::imwrite(store, meanImage);

            if (process == true) {
                std::cout << "Image " << iterator->string() << " with mean colors saved to " << store << " ..." << std::endl;
            }
        }

        for (int i = 0; i < height; ++i) {
            delete[] labels[i];
        }

        delete[] labels;
    }

    if (parameters.find("time") != parameters.end()) {

        boost::filesystem::path timeDir(parameters["time"].as<std::string>());
        if (!boost::filesystem::is_directory(timeDir)) {
            boost::filesystem::create_directories(timeDir);
        }

        boost::filesystem::path timeImgFile(timeDir.string() + DIRECTORY_SEPARATOR + "eval_time_img.txt");
        boost::filesystem::path timeFile(timeDir.string() + DIRECTORY_SEPARATOR + "eval_time.txt");

        Export::BSDEvaluationFile<double>(time, 4, timeImgFile);

        cv::Mat avgTime(1, 1, cv::DataType<double>::type);
        avgTime.at<double>(0, 0) = totalTime/((double) images.size());
        Export::BSDEvaluationFile<double>(avgTime, 6, timeFile);
    }

    std::cout << "On average, " << totalTime/images.size() << " seconds needed ..." << std::endl;

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/core/utility/config.hpp>
#include <nt2/core/utility/details/rands.hpp>
#include <nt2/core/utility/lagged_fibonacci2281stream.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/normal_distribution.hpp>

namespace nt2
{
  NT2_IMPLEMENT_RAND( lagged_fibonacci2281stream_ )

  NT2_CORE_RANDOM_DECL nt2::randstream_* lagged_fibonacci2281stream()
  {
    static nt2::lagged_fibonacci2281stream_ instance;
    return &instance;
  }
}


#include <gtest/gtest.h>

#include <stan/mcmc/hmc/hamiltonians/unit_e_point.hpp>
#include <stan/mcmc/hmc/hamiltonians/diag_e_point.hpp>
#include <stan/mcmc/hmc/hamiltonians/dense_e_point.hpp>
#include <stan/mcmc/hmc/nuts_classic/unit_e_nuts_classic.hpp>
#include <stan/mcmc/hmc/nuts_classic/diag_e_nuts_classic.hpp>
#include <stan/mcmc/hmc/nuts_classic/dense_e_nuts_classic.hpp>
#include <test/unit/mcmc/hmc/mock_hmc.hpp>
#include <boost/random/additive_combine.hpp>

typedef boost::ecuyer1988 rng_t;

TEST(McmcDerivedNutsClassic, compute_criterion_unit_e) {

  rng_t base_rng(0);

  int model_size = 1;

  stan::mcmc::ps_point start(model_size);
  stan::mcmc::unit_e_point finish(model_size);
  Eigen::VectorXd rho(model_size);

  stan::mcmc::mock_model model(model_size);
  stan::mcmc::unit_e_nuts_classic<stan::mcmc::mock_model, rng_t>
    sampler(model, base_rng);

  start.q(0) = 1;
  start.p(0) = 1;

  finish.q(0) = 2;
  finish.p(0) = 1;

  rho = start.p + finish.p;

  EXPECT_EQ(true, sampler.compute_criterion(start, finish, rho));

  start.q(0) = 1;
  start.p(0) = 1;

  finish.q(0) = 2;
  finish.p(0) = -1;

  rho = start.p + finish.p;

  EXPECT_FALSE(sampler.compute_criterion(start, finish, rho));

}

TEST(McmcDerivedNutsClassic, compute_criterion_diag_e) {

  rng_t base_rng(0);

  int model_size = 1;

  stan::mcmc::ps_point start(model_size);
  stan::mcmc::diag_e_point finish(model_size);
  Eigen::VectorXd rho(model_size);

  stan::mcmc::mock_model model(model_size);
  stan::mcmc::diag_e_nuts_classic<stan::mcmc::mock_model, rng_t>
    sampler(model, base_rng);

  start.q(0) = 1;
  start.p(0) = 1;

  finish.q(0) = 2;
  finish.p(0) = 1;

  rho = start.p + finish.p;

  EXPECT_EQ(true, sampler.compute_criterion(start, finish, rho));

  start.q(0) = 1;
  start.p(0) = 1;

  finish.q(0) = 2;
  finish.p(0) = -1;

  rho = start.p + finish.p;

  EXPECT_FALSE(sampler.compute_criterion(start, finish, rho));
}

TEST(McmcDerivedNutsClassic, compute_criterion_dense_e) {

  rng_t base_rng(0);

  int model_size = 1;

  stan::mcmc::ps_point start(model_size);
  stan::mcmc::dense_e_point finish(model_size);
  Eigen::VectorXd rho(model_size);

  stan::mcmc::mock_model model(model_size);
  stan::mcmc::dense_e_nuts_classic<stan::mcmc::mock_model, rng_t>
    sampler(model, base_rng);

  start.q(0) = 1;
  start.p(0) = 1;

  finish.q(0) = 2;
  finish.p(0) = 1;

  rho = start.p + finish.p;

  EXPECT_EQ(true, sampler.compute_criterion(start, finish, rho));

  start.q(0) = 1;
  start.p(0) = 1;

  finish.q(0) = 2;
  finish.p(0) = -1;

  rho = start.p + finish.p;

  EXPECT_FALSE(sampler.compute_criterion(start, finish, rho));

}


#include <boost/any.hpp>
#include <vector>
#include <string>
#include <typeinfo>
#include <algorithm>
#include <iostream>

// This typedefs and methods will be in our header,
// that wraps around native SQL interface
typedef boost::any cell_t;
typedef std::vector<cell_t> db_row_t;

// This is just an example, no actual work with database.
db_row_t get_row(const char* /*query*/) {
    // In real application 'query' parameter shall have a 'const
    // char*' or 'const std::string&' type? See recipe "Type
    // 'reference to string'" for an answer.
    db_row_t row;
    row.push_back(10);
    row.push_back(10.1f);
    row.push_back(std::string("hello again"));
    return row;
}

// This is how a user will use your classes
struct db_sum: public std::unary_function<boost::any, void> {
private:
    double& sum_;

public:
    explicit db_sum(double& sum)
        : sum_(sum)
    {}

    void operator()(const cell_t& value) {
        const std::type_info& ti = value.type();
        if (ti == typeid(int)) {
            sum_ += boost::any_cast<int>(value);
        } else if (ti == typeid(float)) {
            sum_ += boost::any_cast<float>(value);
        }
    }
};

int main() {
    db_row_t row = get_row("Query: Give me some row, please.");
    double res = 0.0;
    std::for_each(row.begin(), row.end(), db_sum(res));
    std::cout << "Sum of arithmetic types in database row is: " << res << std::endl;
}


#include "Icontroller.hpp"
#include <iostream>
#include <Eigen/Eigen>
#include "include/Sensor.h"
#include "include/SensorArray.h"
#include "math.h"
#include "lqr.cpp"
#include <algorithm>
#include <chrono>
#include <vector>
#include <memory>
#include <string>
//#include "m5op.h"
#include <stdio.h>      /* printf, scanf, puts, NULL */
#include <stdlib.h>     /* srand, rand */
#include <time.h>

int main()
	{
	std::vector<int> controlData;
	unsigned long int auxCount=0;

	simulator_msgs::SensorArray arraymsg;
	std::vector<double> out;
	std::vector<double> xref;
	std::vector<double> error;
	std::vector<double> x;
	int k=0;

//		Foo* foo1 = new Foo ();
//		std::cout << "test if main works" << std::endl;;
		teste* control = new teste();

		control->config();

//		simulator_msgs::Sensor msgstates;
//		arraymsg.header;
//		msgstates = arraymsg.values.at(0);
		auto all = std::chrono::high_resolution_clock::now();
		while(k<100){
		auto start = std::chrono::high_resolution_clock::now();

		out=control->execute(arraymsg);

		auto elapsed = std::chrono::high_resolution_clock::now() - start;
		long long microseconds = std::chrono::duration_cast<std::chrono::microseconds>(elapsed).count();
		controlData.push_back(microseconds);
		k++;
		}
		auto elapsed = std::chrono::high_resolution_clock::now() - all;
		long long microseconds = std::chrono::duration_cast<std::chrono::microseconds>(elapsed).count();
		controlData.push_back(microseconds);
			//FILE *fCON = fopen("control.txt", "w");
			//	fprintf(fCON, "count;dtCon\n");
			//	fclose(fCON);
			//	for (std::vector<int>::iterator it = controlData.begin() ; it != controlData.end(); ++it){
			//		auxCount++;
			//		FILE *fCON = fopen("control.txt", "a");
			//		fprintf(fCON, "%d;%lu\n",auxCount,*it);
			//		fclose(fCON);
			//	}
		return 0;
	}


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2021, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "LocalLinearLeastSquaresExtrapolator.h"

#include <Eigen/SVD>

#include "BaseLib/Logging.h"
#include "ExtrapolatableElementCollection.h"
#include "MathLib/LinAlg/Eigen/EigenMapTools.h"
#include "MathLib/LinAlg/LinAlg.h"
#include "MathLib/LinAlg/MatrixVectorTraits.h"
#include "NumLib/Assembler/SerialExecutor.h"
#include "NumLib/Function/Interpolation.h"

namespace NumLib
{
LocalLinearLeastSquaresExtrapolator::LocalLinearLeastSquaresExtrapolator(
    NumLib::LocalToGlobalIndexMap const& dof_table)
    : _dof_table_single_component(dof_table)
{
    /* Note in case the following assertion fails:
     * If you copied the extrapolation code, for your processes from
     * somewhere, note that the code from the groundwater flow process might
     * not suit your needs: It is a special case and is therefore most
     * likely too simplistic. You better adapt the extrapolation code from
     * some more advanced process, like the TES process.
     */
    if (dof_table.getNumberOfGlobalComponents() != 1)
    {
        OGS_FATAL(
            "The d.o.f. table passed must be for one variable that has only "
            "one component!");
    }
}

void LocalLinearLeastSquaresExtrapolator::extrapolate(
    const unsigned num_components,
    ExtrapolatableElementCollection const& extrapolatables,
    const double t,
    std::vector<GlobalVector*> const& x,
    std::vector<NumLib::LocalToGlobalIndexMap const*> const& dof_table)
{
    auto const num_nodal_dof_result =
        _dof_table_single_component.dofSizeWithoutGhosts() * num_components;

    std::vector<GlobalIndexType> ghost_indices;
    {  // Create num_components times version of ghost_indices arranged by
       // location. For example for 3 components and ghost_indices {5,6,10} we
       // compute {15, 16, 17,  18, 19, 20,  30, 31, 32}.
        auto const& single_component_ghost_indices =
            _dof_table_single_component.getGhostIndices();
        auto const single_component_ghost_indices_size =
            single_component_ghost_indices.size();
        ghost_indices.reserve(single_component_ghost_indices_size *
                              num_components);
        for (unsigned i = 0; i < single_component_ghost_indices_size; ++i)
        {
            for (unsigned c = 0; c < num_components; ++c)
            {
                ghost_indices.push_back(
                    single_component_ghost_indices[i] * num_components + c);
            }
        }
    }

    if (!_nodal_values ||
#ifdef USE_PETSC
        _nodal_values->getLocalSize() + _nodal_values->getGhostSize()
#else
        _nodal_values->size()
#endif
            != static_cast<GlobalIndexType>(num_nodal_dof_result))
    {
        _nodal_values = MathLib::MatrixVectorTraits<GlobalVector>::newInstance(
            {num_nodal_dof_result, num_nodal_dof_result, &ghost_indices,
             nullptr});
    }
    _nodal_values->setZero();

    // counts the writes to each nodal value, i.e., the summands in order to
    // compute the average afterwards
    auto counts =
        MathLib::MatrixVectorTraits<GlobalVector>::newInstance(*_nodal_values);
    counts->setZero();

    auto const size = extrapolatables.size();
    for (std::size_t i = 0; i < size; ++i)
    {
        extrapolateElement(i, num_components, extrapolatables, t, x, dof_table,
                           *counts);
    }
    MathLib::LinAlg::finalizeAssembly(*_nodal_values);

    MathLib::LinAlg::componentwiseDivide(*_nodal_values, *_nodal_values,
                                         *counts);
}

void LocalLinearLeastSquaresExtrapolator::calculateResiduals(
    const unsigned num_components,
    ExtrapolatableElementCollection const& extrapolatables,
    const double t,
    std::vector<GlobalVector*> const& x,
    std::vector<NumLib::LocalToGlobalIndexMap const*> const& dof_table)
{
    auto const num_element_dof_result = static_cast<GlobalIndexType>(
        _dof_table_single_component.size() * num_components);

    if (!_residuals || _residuals->size() != num_element_dof_result)
    {
#ifndef USE_PETSC
        _residuals.reset(new GlobalVector{num_element_dof_result});
#else
        _residuals.reset(new GlobalVector{num_element_dof_result, false});
#endif
    }

    if (static_cast<std::size_t>(num_element_dof_result) !=
        extrapolatables.size() * num_components)
    {
        OGS_FATAL("mismatch in number of D.o.F.");
    }

    auto const size = extrapolatables.size();
    for (std::size_t i = 0; i < size; ++i)
    {
        calculateResidualElement(i, num_components, extrapolatables, t, x,
                                 dof_table);
    }
    MathLib::LinAlg::finalizeAssembly(*_residuals);
}

void LocalLinearLeastSquaresExtrapolator::extrapolateElement(
    std::size_t const element_index,
    const unsigned num_components,
    ExtrapolatableElementCollection const& extrapolatables,
    const double t,
    std::vector<GlobalVector*> const& x,
    std::vector<NumLib::LocalToGlobalIndexMap const*> const& dof_table,
    GlobalVector& counts)
{
    auto const& integration_point_values =
        extrapolatables.getIntegrationPointValues(
            element_index, t, x, dof_table, _integration_point_values_cache);

    // Empty vector means to ignore the values and not to change the counts.
    if (integration_point_values.empty())
    {
        return;
    }

    auto const& N_0 = extrapolatables.getShapeMatrix(element_index, 0);
    auto const num_nodes = static_cast<unsigned>(N_0.cols());
    auto const num_values =
        static_cast<unsigned>(integration_point_values.size());

    if (num_values % num_components != 0)
    {
        OGS_FATAL(
            "The number of computed integration point values is not divisible "
            "by the number of num_components. Maybe the computed property is "
            "not a {:d}-component vector for each integration point.",
            num_components);
    }

    // number of integration points in the element
    const auto num_int_pts = num_values / num_components;

    if (num_int_pts < num_nodes)
    {
        OGS_FATAL(
            "Least squares is not possible if there are more nodes than "
            "integration points.");
    }

    auto const pair_it_inserted = _qr_decomposition_cache.emplace(
        std::make_pair(num_nodes, num_int_pts), CachedData{});

    auto& cached_data = pair_it_inserted.first->second;
    if (pair_it_inserted.second)
    {
        DBUG("Computing new singular value decomposition");

        // interpolation_matrix * nodal_values = integration_point_values
        // We are going to pseudo-invert this relation now using singular value
        // decomposition.
        auto& interpolation_matrix = cached_data.A;
        interpolation_matrix.resize(num_int_pts, num_nodes);

        interpolation_matrix.row(0) = N_0;
        for (unsigned int_pt = 1; int_pt < num_int_pts; ++int_pt)
        {
            auto const& shp_mat =
                extrapolatables.getShapeMatrix(element_index, int_pt);
            assert(shp_mat.cols() == num_nodes);

            // copy shape matrix to extrapolation matrix row-wise
            interpolation_matrix.row(int_pt) = shp_mat;
        }

        // JacobiSVD is extremely reliable, but fast only for small matrices.
        // But we usually have small matrices and we don't compute very often.
        // Cf.
        // http://eigen.tuxfamily.org/dox/group__TopicLinearAlgebraDecompositions.html
        //
        // Decomposes interpolation_matrix = U S V^T.
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(
            interpolation_matrix, Eigen::ComputeThinU | Eigen::ComputeThinV);

        auto const& S = svd.singularValues();
        auto const& U = svd.matrixU();
        auto const& V = svd.matrixV();

        // Compute and save the pseudo inverse V * S^{-1} * U^T.
        auto const rank = svd.rank();
        assert(rank == num_nodes);

        // cf. http://eigen.tuxfamily.org/dox/JacobiSVD_8h_source.html
        cached_data.A_pinv.noalias() = V.leftCols(rank) *
                                       S.head(rank).asDiagonal().inverse() *
                                       U.leftCols(rank).transpose();
    }
    else if (cached_data.A.row(0) != N_0)
    {
        OGS_FATAL("The cached and the passed shapematrices differ.");
    }

    auto const& global_indices =
        _dof_table_single_component(element_index, 0).rows;

    if (num_components == 1)
    {
        auto const integration_point_values_vec =
            MathLib::toVector(integration_point_values);

        // Apply the pre-computed pseudo-inverse.
        Eigen::VectorXd const nodal_values =
            cached_data.A_pinv * integration_point_values_vec;

        // TODO does that give rise to PETSc problems? E.g., writing to ghost
        // nodes? Furthermore: Is ghost nodes communication necessary for PETSc?
        _nodal_values->add(global_indices, nodal_values);
        counts.add(global_indices,
                   std::vector<double>(global_indices.size(), 1.0));
    }
    else
    {
        auto const integration_point_values_mat = MathLib::toMatrix(
            integration_point_values, num_components, num_int_pts);

        // Apply the pre-computed pseudo-inverse.
        Eigen::MatrixXd const nodal_values =
            cached_data.A_pinv * integration_point_values_mat.transpose();

        std::vector<GlobalIndexType> indices;
        indices.reserve(num_components * global_indices.size());

        // _nodal_values is ordered location-wise
        for (unsigned comp = 0; comp < num_components; ++comp)
        {
            transform(cbegin(global_indices), cend(global_indices),
                      back_inserter(indices),
                      [&](auto const i) { return num_components * i + comp; });
        }

        // Nodal_values are passed as a raw pointer, because PETScVector and
        // EigenVector implementations differ slightly.
        _nodal_values->add(indices, nodal_values.data());
        counts.add(indices, std::vector<double>(indices.size(), 1.0));
    }
}

void LocalLinearLeastSquaresExtrapolator::calculateResidualElement(
    std::size_t const element_index,
    const unsigned num_components,
    ExtrapolatableElementCollection const& extrapolatables,
    const double t,
    std::vector<GlobalVector*> const& x,
    std::vector<NumLib::LocalToGlobalIndexMap const*> const& dof_table)
{
    auto const& int_pt_vals = extrapolatables.getIntegrationPointValues(
        element_index, t, x, dof_table, _integration_point_values_cache);

    auto const num_values = static_cast<unsigned>(int_pt_vals.size());
    if (num_values % num_components != 0)
    {
        OGS_FATAL(
            "The number of computed integration point values is not divisible "
            "by the number of num_components. Maybe the computed property is "
            "not a {:d}-component vector for each integration point.",
            num_components);
    }

    // number of integration points in the element
    const auto num_int_pts = num_values / num_components;

    const auto& global_indices =
        _dof_table_single_component(element_index, 0).rows;
    const auto num_nodes = static_cast<unsigned>(global_indices.size());

    auto const& interpolation_matrix =
        _qr_decomposition_cache.find({num_nodes, num_int_pts})->second.A;

    Eigen::VectorXd nodal_vals_element(num_nodes);
    auto const int_pt_vals_mat =
        MathLib::toMatrix(int_pt_vals, num_components, num_int_pts);

    MathLib::LinAlg::setLocalAccessibleVector(
        *_nodal_values);  // For access in the for-loop.
    for (unsigned comp = 0; comp < num_components; ++comp)
    {
        // filter nodal values of the current element
        for (unsigned i = 0; i < num_nodes; ++i)
        {
            // TODO PETSc negative indices?
            auto const idx = num_components * global_indices[i] + comp;
            nodal_vals_element[i] = _nodal_values->get(idx);
        }

        double const residual = (interpolation_matrix * nodal_vals_element -
                                 int_pt_vals_mat.row(comp).transpose())
                                    .squaredNorm();

        auto const eidx =
            static_cast<GlobalIndexType>(num_components * element_index + comp);
        // The residual is set to the root mean square value.
        auto const root_mean_square = std::sqrt(residual / num_int_pts);
        _residuals->set(eidx, root_mean_square);
    }
}

}  // namespace NumLib


#pragma once

#include <polyfem/ProblemWithSolution.hpp>

#include <Eigen/Dense>

#include <vector>
#include <string>

namespace polyfem
{
	class State;

	class KernelProblem : public ProblemWithSolution
	{
	public:
		KernelProblem(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt, const double t) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt, const double t) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt, const double t) const override
		{
			assert(false);
			return AutodiffHessianPt(1);
		}

		void rhs(const AssemblerUtils &assembler, const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		void set_parameters(const json &params) override;
		bool is_scalar() const override;

		State *state;

	private:
		std::string formulation_ = "Laplacian";
		int n_kernels_ = 5;
		double kernel_distance_ = 0.05;
		Eigen::VectorXd kernel_weights_;
	};
} // namespace polyfem


#ifndef PYTHONIC_INCLUDE_NUMPY_SIGN_HPP
#define PYTHONIC_INCLUDE_NUMPY_SIGN_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/sign.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
  namespace wrapper
  {
    template <class T>
    auto sign(T const &v) -> decltype(boost::simd::sign(v))
    {
      return boost::simd::sign(v);
    }

    template <class T>
    std::complex<T> sign(std::complex<T> const &v)
    {
      if (v.real())
        return {boost::simd::sign(v.real()), 0};
      else
        return {boost::simd::sign(v.imag()), 0};
    }
  }
#define NUMPY_NARY_FUNC_NAME sign
#define NUMPY_NARY_FUNC_SYM wrapper::sign
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


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

    @Copyright 2016 Numscale SAS

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_MULTIPLIES_S_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_MULTIPLIES_S_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/function/group.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/shift_right.hpp>
#include <boost/simd/function/split_multiplies.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/detail/dispatch/meta/as_unsigned.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#include <boost/simd/detail/dispatch/meta/upgrade.hpp>

/* No native groups for 64-bit SSE;
 * we use bit tricks instead of calling saturate. */

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;

  BOOST_DISPATCH_OVERLOAD ( multiplies_
                          , (typename A0)
                          , bs::sse2_
                          , bs::saturated_tag
                          , bs::pack_<bd::int32_<A0>, bs::sse_>
                          , bs::pack_<bd::int32_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const saturated_tag &
                                    , const A0 & a0
                                    , const A0 & a1 ) const BOOST_NOEXCEPT
    {
      using stype  = bd::scalar_of_t<A0>;
      using untype = bd::as_unsigned_t<A0>;
      using utype  = bd::upgrade_t<A0>;

      utype res0, res1;
      std::tie(res0, res1) = split_multiplies(a0, a1);

      untype res2 = shift_right(bitwise_cast<untype>(a0 ^ a1), sizeof(stype)*CHAR_BIT-1)
                  + static_cast<typename bd::scalar_of<untype>::type>(Valmax<stype>());

      A0 hi = group( shift_right(res0, sizeof(stype)*CHAR_BIT)
                   , shift_right(res1, sizeof(stype)*CHAR_BIT)
                   );
      A0 lo = group(res0, res1);

      return if_else( hi != shift_right(lo, sizeof(stype)*CHAR_BIT-1)
                    , bitwise_cast<A0>(res2)
                    , lo
                    );
    }
  };

  BOOST_DISPATCH_OVERLOAD ( multiplies_
                          , (typename A0)
                          , bs::sse2_
                          , bs::saturated_tag
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const saturated_tag &
                                    ,  const A0 & a0
                                    , const A0 & a1
                                    ) const BOOST_NOEXCEPT
    {
      using stype  = bd::scalar_of_t<A0>;
      using utype  = bd::upgrade_t<A0>;

      utype res0, res1;
      std::tie(res0, res1) = split_multiplies(a0, a1);

      return group(res0, res1)
           | genmask( group( shift_right(res0, sizeof(stype)*CHAR_BIT)
                           , shift_right(res1, sizeof(stype)*CHAR_BIT)
                           )
                    );
    }
  };

} } }

#endif