#include "stdafx.h"
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <fmt/format.h>
#include "contest_types.h"
#include "solver_registry.h"
#include "visual_editor.h"

namespace SimpleMatchingSolver {

constexpr int kNumTrialsPerComponent = 1000000;
constexpr int kMinComponentSize = 10;
constexpr int kMinDegree = 2;
constexpr bool kPruneLeavesOnly = true;

namespace bg = boost::geometry;
using BoostPoint = bg::model::d2::point_xy<double>;
using BoostPolygon = bg::model::polygon<BoostPoint>;
using BoostLinestring = bg::model::linestring<BoostPoint>;

template <typename T>
BoostPoint ToBoostPoint(const T& point) {
  const auto [x, y] = point;
  return BoostPoint(x, y);
}

template <typename T>
BoostPolygon ToBoostPolygon(const std::vector<T>& points) {
  BoostPolygon polygon;
  for (std::size_t i = 0; i <= points.size(); ++i) {
    polygon.outer().push_back(ToBoostPoint(points[i % points.size()]));
  }
  if (bg::area(polygon) < 0.0) {
    bg::reverse(polygon);
  }
  return polygon;
}

template <typename T, typename U>
auto SquaredDistance(const T& vertex0, const U& vertex1) {
  const auto [x0, y0] = vertex0;
  const auto [x1, y1] = vertex1;
  return (x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1);
}

class Solver : public SolverBase {
 public:
  SolverOutputs solve(const SolverArguments& args) override {
    counter_ = 0;
    if (args.visualize) {
      editor_ = std::make_shared<SVisualEditor>(args.problem, "SimpleMatchingSolver", "visualize");
    }

    hole_ = args.problem->hole_polygon;
    vertices_ = args.problem->vertices;
    edges_ = args.problem->edges;
    epsilon_ = args.problem->epsilon;
    const auto hole_polygon = ToBoostPolygon(hole_);

    N_ = vertices_.size();
    queued_.assign(N_, -1);
    assigned_.assign(N_, -1);
    adjacent_.assign(N_, {});
    std::vector<std::vector<double>> distances(
        N_, std::vector<double>(N_, std::numeric_limits<double>::infinity()));
    for (int i = 0; i < N_; ++i) {
      distances[i][i] = 0.0;
    }
    for (const auto& [a, b] : edges_) {
      const auto squared_distance = SquaredDistance(vertices_[a], vertices_[b]);
      const auto margin = epsilon_ * squared_distance / 1'000'000;
      const auto min = squared_distance - margin;
      const auto max = squared_distance + margin;
      adjacent_[a].emplace_back(b, min, max);
      adjacent_[b].emplace_back(a, min, max);
      distances[a][b] = distances[b][a] = std::sqrt(squared_distance) * (1.0 + 1.0e-6 * epsilon_);
    }
    for (int k = 0; k < N_; ++k) {
      for (int i = 0; i < N_; ++i) {
        for (int j = 0; j < N_; ++j) {
          distances[i][j] = std::min(distances[i][j], distances[i][k] + distances[k][j]);
        }
      }
    }
    vertex_max_distances_.resize(N_);
    for (int i = 0; i < N_; ++i) {
      vertex_max_distances_[i].resize(N_);
      for (int j = 0; j < N_; ++j) {
        vertex_max_distances_[i][j] = std::ceil(std::pow(distances[i][j], 2.0));
      }
    }

    M_ = hole_.size();
    hole_candidates_.clear();
    hole_distances_.resize(M_);
    hole_visibilities_.resize(M_);
    for (int i = 0; i < M_; ++i) {
      hole_candidates_.push_back(i);
      hole_distances_[i].resize(M_);
      hole_visibilities_[i].resize(M_);
      for (int j = 0; j < M_; ++j) {
        const BoostLinestring linestring{ToBoostPoint(hole_[i]), ToBoostPoint(hole_[j])};
        hole_visibilities_[i][j] = bg::covered_by(linestring, hole_polygon);
        hole_distances_[i][j] = SquaredDistance(hole_[i], hole_[j]);
      }
    }

    last_updated_ = 0;
    component_sizes_.assign(M_, M_);
    best_sizes_.assign(M_, 0);
    bool found = false;
    for (int i = 0; i < N_; ++i) {
      queued_[i] = N_;
      vertex_candidates_.push_back(i);
      assigned_counts_.assign(1, 0);
      if (Search()) {
        found = true;
        break;
      }
      vertex_candidates_.pop_back();
      queued_[i] = -1;
    }

    SolverOutputs outputs;
    if (found) {
      auto pose = vertices_;
      const auto assigned_vertices = kPruneLeavesOnly ? CleanupLeaves() : Cleanup();
      for (const int vertex : assigned_vertices) {
        pose[vertex] = hole_[assigned_[vertex]];
      }
      outputs.solution = args.problem->create_solution(pose);
    } else {
      outputs.solution = args.problem->create_solution();
    }
    return outputs;
  }

  std::set<int> Cleanup() const {
    std::set<int> assigned_vertices;
    for (int i = 0; i < N_; ++i) {
      if (assigned_[i] < 0) continue;
      assigned_vertices.insert(i);
    }
    while (true) {
      bool converged = true;
      for (int i = 0; i < N_; ++i) {
        if (!assigned_vertices.count(i)) continue;
        int degree = 0;
        for (const auto& [next, min, max] : adjacent_[i]) {
          degree += assigned_vertices.count(next);
        }
        if (degree < kMinDegree) {
          converged = false;
          assigned_vertices.erase(i);
        }
      }
      if (converged) break;
    }
    return assigned_vertices;
  }

  std::set<int> CleanupLeaves() const {
    std::set<int> assigned_vertices;
    for (int i = 0; i < N_; ++i) {
      if (assigned_[i] < 0) continue;
      assigned_vertices.insert(i);
    }
    for (int i = 0; i < N_; ++i) {
      if (assigned_[i] < 0) continue;
      int degree = 0;
      for (const auto& [next, min, max] : adjacent_[i]) {
        degree += assigned_[next] >= 0;
      }
      if (degree < kMinDegree) {
        assigned_vertices.erase(i);
      }
    }
    return assigned_vertices;
  }

  bool Search() {
    if (hole_candidates_.empty()) return true;
    if (++counter_ % 10000 == 0 && editor_) {
      auto pose = vertices_;
      std::vector<int> marked;
      for (int j = 0; j < N_; ++j) {
        if (assigned_[j] < 0) continue;
        pose[j] = hole_[assigned_[j]];
        marked.push_back(j);
      }
      editor_->set_pose(std::make_shared<SSolution>(pose));
      editor_->set_marked_indices(marked);
      editor_->set_persistent_custom_stat(fmt::format("assigned counts = {}", fmt::join(assigned_counts_, ", ")));
      editor_->show(1);
    }
    const int component_index = assigned_counts_.size() - 1;
    best_sizes_[component_index] = std::max(best_sizes_[component_index], assigned_counts_[component_index]);
    if (counter_ > last_updated_ + kNumTrialsPerComponent) {
      last_updated_ = counter_;
      component_sizes_[component_index] = best_sizes_[component_index];
      if (component_sizes_[component_index] < kMinComponentSize) return true;
    }
    for (int i = vertex_candidates_.size() - 1; i >= 0; --i) {
      const int vertex = vertex_candidates_[i];
      vertex_candidates_.erase(vertex_candidates_.begin() + i);
      for (int j = hole_candidates_.size() - 1; j >= 0; --j) {
        const int hole_vertex = hole_candidates_[j];
        bool feasible = true;
        for (const auto& [next, min, max] : adjacent_[vertex]) {
          if (assigned_[next] < 0) continue;
          const auto squared_distance = hole_distances_[assigned_[next]][hole_vertex];
          const auto visible = hole_visibilities_[assigned_[next]][hole_vertex];
          if (!visible || squared_distance < min || squared_distance > max) {
            feasible = false;
            break;
          }
        }
        for (int other = 0; feasible && other < N_; ++other) {
          if (assigned_[other] < 0) continue;
          if (hole_distances_[assigned_[other]][hole_vertex] > vertex_max_distances_[other][vertex]) {
            feasible = false;
          }
        }
        if (feasible) {
          hole_candidates_.erase(hole_candidates_.begin() + j);
          assigned_[vertex] = hole_vertex;
          ++assigned_counts_.back();
          for (const auto& [next, min, max] : adjacent_[vertex]) {
            if (queued_[next] >= 0) continue;
            queued_[next] = vertex;
            vertex_candidates_.push_back(next);
          }
          if (Search()) return true;
          for (const auto& [next, min, max] : adjacent_[vertex]) {
            if (queued_[next] != vertex) continue;
            queued_[next] = -1;
            vertex_candidates_.pop_back();
          }
          --assigned_counts_.back();
          assigned_[vertex] = -1;
          hole_candidates_.insert(hole_candidates_.begin() + j, hole_vertex);
        }
      }
      vertex_candidates_.insert(vertex_candidates_.begin() + i, vertex);
    }
    if (assigned_counts_.back() >= component_sizes_[component_index]) {
      if (assigned_counts_.size() == component_sizes_.size()) return true;
      for (int i = 0; i < N_; ++i) {
        if (queued_[i] >= 0) continue;
        queued_[i] = N_;
        vertex_candidates_.push_back(i);
        assigned_counts_.push_back(0);
        if (Search()) return true;
        assigned_counts_.pop_back();
        vertex_candidates_.pop_back();
        queued_[i] = -1;
      }
    }
    return false;
  }

 private:
  std::vector<Point> hole_;
  std::vector<Point> vertices_;
  std::vector<Edge> edges_;
  integer epsilon_;

  int N_;
  int M_;
  std::vector<int> queued_;
  std::vector<int> assigned_;
  std::vector<int> vertex_candidates_;
  std::vector<int> hole_candidates_;
  std::vector<std::vector<std::tuple<int, integer, integer>>> adjacent_;
  std::vector<std::vector<integer>> hole_distances_;
  std::vector<std::vector<bool>> hole_visibilities_;
  std::vector<std::vector<integer>> vertex_max_distances_;
  std::vector<int> assigned_counts_;
  std::vector<int> component_sizes_;
  std::vector<int> best_sizes_;
  std::size_t last_updated_;
  std::size_t counter_;
  SVisualEditorPtr editor_;
};

}

REGISTER_SOLVER("SimpleMatchingSolver", SimpleMatchingSolver::Solver);
// vim:ts=2 sw=2 sts=2 et ci


#ifndef EIGENGWAS_HPP_
#define EIGENGWAS_HPP_

#include "time.h"

#include <bits/stdc++.h>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <Eigen/QR>
#include <math.h>
#include <boost/math/distributions/students_t.hpp>
#include "genotype.h"
#include "Goptions.hpp"
#include "global.h"

using namespace std;

extern Goptions goptions;
extern genotype g;
typedef Matrix<double, Dynamic, Dynamic, RowMajor> MatrixXdr;

class EigenGWAS {
    public:
    EigenGWAS() {

    }

    void Scan(MatrixXdr IndEigenVec) {
    	clock_t eg_begin = clock();
	    cout << "---------------EigenGWAS scan-------------" << endl;
	    cout << "SNP number: " << g.Nsnp << endl;
	    cout << "Sample size: " << g.Nindv << endl;

    	cout << "Eigenvector number: " << IndEigenVec.cols() <<endl;

	    MatrixXdr evePS(IndEigenVec.rows(), IndEigenVec.cols());
	    cout << "Standardization eigenvec" << endl;
	    for (int i = 0; i < evePS.cols(); i++) {
		    double sum = 0, sd = 0, eSq = 0, m = 0;
		    for (int j = 0; j < evePS.rows(); j++) {
			    sum += IndEigenVec(j, i);
			    eSq += IndEigenVec(j, i) * IndEigenVec(j, i);
		    }
		    m = sum / IndEigenVec.rows();
		    sd = sqrt((eSq - m * m * IndEigenVec.rows()) / (IndEigenVec.rows() - 1));
		    for (int j = 0; j < IndEigenVec.rows(); j++) {
			    evePS(j, i) = (IndEigenVec(j, i) - m) / sd;
		    }
	    }

	//using mailman
	    MatrixXdr EgBeta(g.Nsnp, IndEigenVec.cols());
	    multiply_y_pre(evePS, evePS.cols(), EgBeta, true);

    	EgBeta = EgBeta / g.Nindv;
	    for (int i = 0; i < EgBeta.cols(); i++) {
		    cout << EgBeta(0, i) <<endl;
	    }

    	using boost::math::students_t;
	    students_t Tdist(g.Nindv - 1);

    	for (int i = 0; i < EgBeta.cols(); i++) {
			cout << "Scanning eigenvector " << i + 1 << endl;
			ofstream e_file;
			e_file.open((goptions.GetGenericOutFile() + string("eg."+std::to_string(i+1)+".txt")).c_str());
			e_file << "CHR\tSNP\tPOS\tBP\tA1\tA2\tBeta\tSE\tT-stat\tP" << endl;
			for (int j = 0; j <EgBeta.rows(); j++) {
			    double seB = sqrt((1 - pow(EgBeta(j, i), 2))/(g.Nindv - 1));
			    double t_stat = EgBeta(j, i) / seB;
				if (goptions.IsGenericDebug()) {
				    cout << "MK: " << j << " infor=" << g.get_bim_info(j) << " egB=" << EgBeta(j, i) << " seB=" << seB << " t=" << t_stat << endl;
				}
			    double pt2tail = cdf(complement(Tdist, fabs(t_stat))) * 2;
			    e_file << g.get_bim_info(j) << "\t" << std::setprecision(8) << EgBeta(j, i) << "\t" << seB << "\t" << t_stat << "\t" << pt2tail <<endl;
		    }
    		e_file.close();
	    }

    	clock_t eg_end = clock();
	    double eg_time = double(eg_end - eg_begin) / CLOCKS_PER_SEC;
	    cout << "EigenGWAS total time " << eg_time << endl;
    }
};

#endif

/*
void EigenGWAS(MatrixXdr IndEigenVec) {

	clock_t eg_begin = clock();
	cout << "---------------EigenGWAS scan-------------" << endl;
	cout << "SNP number: " << g.Nsnp << endl;
	cout << "Sample size: " << g.Nindv << endl;

	cout << "Eigenvector number: " << IndEigenVec.cols() <<endl;

	MatrixXdr evePS(IndEigenVec.rows(), IndEigenVec.cols());
	cout << "Standardization eigenvec" << endl;
	for (int i = 0; i < evePS.cols(); i++) {
		double sum = 0, sd = 0, eSq = 0, m = 0;
		for (int j = 0; j < evePS.rows(); j++) {
			sum += IndEigenVec(j, i);
			eSq += IndEigenVec(j, i) * IndEigenVec(j, i);
		}
		m = sum / IndEigenVec.rows();
		sd = sqrt((eSq - m * m * IndEigenVec.rows()) / (IndEigenVec.rows() - 1));
		for (int j = 0; j < IndEigenVec.rows(); j++) {
			evePS(j, i) = (IndEigenVec(j, i) - m) / sd;
		}
	}

	//using mailman
	MatrixXdr EgBeta(g.Nsnp, IndEigenVec.cols());
	multiply_y_pre(evePS, evePS.cols(), EgBeta, true);

	EgBeta = EgBeta / g.Nindv;
	for (int i = 0; i < EgBeta.cols(); i++) {
		cout << EgBeta(0, i) <<endl;
	}

	using boost::math::students_t;
	students_t Tdist(g.Nindv - 1);

	for (int i = 0; i < EgBeta.cols(); i++) {
		cout << "Scanning eigenvector " << i + 1 << endl;
		ofstream e_file;
		e_file.open((goptions.GetGenericOutFile() + string("eg."+std::to_string(i+1)+".txt")).c_str());
		e_file << "CHR\tSNP\tPOS\tBP\tA1\tA2\tBeta\tSE\tT-stat\tP" << endl;
		for (int j = 0; j <EgBeta.rows(); j++) {
			double seB = sqrt((1 - pow(EgBeta(j, i), 2))/(g.Nindv - 1));
			double t_stat = EgBeta(j, i) / seB;
			cout << "MK: " << j << " infor=" << g.get_bim_info(j) << " egB=" << EgBeta(j, i) << " seB=" << seB << " t=" << t_stat << endl;
			double pt2tail = cdf(complement(Tdist, fabs(t_stat))) * 2;
			e_file << g.get_bim_info(j) << "\t" << std::setprecision(8) << EgBeta(j, i) << "\t" << seB << "\t" << t_stat << "\t" << pt2tail <<endl;
		}
		e_file.close();
	}

	clock_t eg_end = clock();
	double eg_time = double(eg_end - eg_begin) / CLOCKS_PER_SEC;
	cout << "EigenGWAS total time " << eg_time << endl;
}
*/


#ifndef EMPIRICALCPP_SRC_MESH_HPP_
#define EMPIRICALCPP_SRC_MESH_HPP_

#include <empiricalcpp/src/constants.hpp>
#include <empiricalcpp/src/quadrature.hpp>
#include <array>
#include <functional>
#include <vector>
#include <tuple>
#include <boost/multi_array.hpp>

namespace empirical {
    namespace mesh {

        class Mesh1D {
        public:
            typedef std::vector<Scalar> vector_type;
            typedef vector_type::size_type size_type;
            typedef std::function<vector_type(const size_type, const Scalar, const Scalar)> generator_function;

        protected:
            Scalar minVal, maxVal;
            vector_type points;
            generator_function generator;

            void recalculate(const size_type N, const Scalar min, const Scalar max) {
                minVal = min;
                maxVal = max;
                points = generator(N, min, max);
            }

        public:
            Mesh1D(generator_function generator, const size_type N, const Scalar min = -1, const Scalar max = 1)
                : minVal(min), maxVal(max), points(1), generator(generator) {
                recalculate(N, min, max);
            }
            Mesh1D(const Quadrature& quadrature);
            Mesh1D(std::function<Scalar(size_type i, size_type N, Scalar a, Scalar b)> pointFunction,
                const size_type N, const Scalar min = -1, const Scalar max = 1);

            size_type size() const { return points.size(); }
            Scalar min() const { return minVal; }
            Scalar max() const { return maxVal; }

            vector_type::iterator begin() { return points.begin(); }
            vector_type::iterator end() { return points.end(); }
            vector_type::const_iterator begin() const { return points.begin(); }
            vector_type::const_iterator end() const { return points.end(); }

            vector_type getPoints() { return points; }
            const vector_type getPoints() const { return points; }

            Scalar operator[](const size_type i) const { return points[i]; }
            Scalar& operator[](const size_type i) { return points[i]; }

            Scalar operator()(const size_type i) const { return points[i]; }
            Scalar& operator()(const size_type i) { return points[i]; }

            void resize(const size_type N, const Scalar min, const Scalar max) {
                if (points.size() == N && min == minVal && max == maxVal) {
                    return;
                }
                recalculate(N, min, max);
            }

            void resize(const size_type N) {
                resize(N, minVal, maxVal);
            }
        };

        class Mesh2D {
        public:
            typedef boost::multi_array<Scalar, 3> array_type;
            typedef array_type::size_type size_type;
            typedef std::array<size_type, 2> element_index_type;
            typedef std::array<Scalar, 2> point_type;
            typedef std::function<array_type*(const element_index_type, const point_type, const point_type)> generator_function;

        protected:
            point_type minVal, maxVal;
            std::unique_ptr<array_type> points;
            generator_function generator;

            void recalculate(const element_index_type N, const point_type min, const point_type max);

        public:
            Mesh2D(generator_function generator, const element_index_type N, const point_type min, const point_type max)
                : minVal(min), maxVal(max), generator(generator) {
                recalculate(N, min, max);
            }
            Mesh2D(generator_function generator, const element_index_type N) : generator(generator) {
                minVal[0] = minVal[1] = -1;
                maxVal[0] = maxVal[1] = 1;
                recalculate(N, minVal, maxVal);
            }
            Mesh2D(const Quadrature& xQuadrature, const Quadrature& yQuadrature);
            Mesh2D(std::function<Scalar(size_type i, size_type N, Scalar a, Scalar b)> xFunction,
                std::function<Scalar(size_type i, size_type N, Scalar a, Scalar b)> yFunction,
                const element_index_type N, const point_type min, const point_type max);

            size_type sizeX() const { return points->shape()[0]; }
            size_type sizeY() const { return points->shape()[1]; }
            point_type min() const { return minVal; }
            point_type max() const { return maxVal; }

            array_type::iterator begin();// { return points.begin(); }
            array_type::iterator end();// { return points.end(); }
            array_type::const_iterator begin() const;// { return points.begin(); }
            array_type::const_iterator end() const;// { return points.end(); }

            array_type getPoints() { return *points; }
            const array_type getPoints() const { return *points; }

            array_type::const_reference operator[](const size_type i) const { return (*points)[i]; }
            array_type::reference operator[](const size_type i) { return (*points)[i]; }

            point_type operator()(const size_type i, const size_type j) const { return{ { (*points)[i][j][0], (*points)[i][j][1] } }; }

            void resize(const element_index_type N, const point_type min, const point_type max) {
                if (points->shape()[0] == N[0] && points->shape()[1] == N[1] &&
                    min[0] == minVal[0] && min[1] == minVal[1] &&
                    max[0] == maxVal[0] && max[1] == maxVal[1]) {
                    return;
                }
                recalculate(N, min, max);
            }

            void resize(const element_index_type N) {
                resize(N, minVal, maxVal);
            }
        };

#ifndef EMPIRICAL_NO_OSTREAM_DEFINITIONS
        inline std::ostream& operator<<(std::ostream& os, const Mesh1D& mesh) {
            os << "Mesh1D {";
            os << std::fixed << std::setw(11) << std::setprecision(6);
            for (Scalar val : mesh) {
                os << val << ", ";
            }
            return os << "}";
        }
        std::ostream& operator<<(std::ostream& os, const Mesh2D& mesh);
#endif
    }

    typedef mesh::Mesh1D Mesh1D;
}

#endif /* EMPIRICALCPP_SRC_MESH_HPP_ */


/*
Copyright 2013 Henrik Mühe and Florian Funke

This file is part of CampersCoreBurner.

CampersCoreBurner is free software: you can redistribute it and/or modify
it under the terms of the GNU Affero General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

CampersCoreBurner is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Affero General Public License for more details.

You should have received a copy of the GNU Affero General Public License
along with CampersCoreBurner.  If not, see <http://www.gnu.org/licenses/>.
*/

/*
COMMENT SUMMARY FOR THIS FILE:

This file contains the implementation of the edit distance and hamming distance metrics.
*/

#pragma once

#include "core.h"
#include "utils.hpp"
#include <boost/utility/string_ref.hpp>
#include <cassert>
#include <cstdint>
#include <iostream>
#include <string>
#include <x86intrin.h>

namespace campers {

// [1] Helpful description: Section 3.3 in http://www.cs.uta.fi/~helmu/pubs/spire03a.pdf
// [2] Other helpful description: http://www.cs.uta.fi/~helmu/pubs/A2001-10.pdf, Figure 5
// [3] http://apps.topcoder.com/forums/;jsessionid=2E79B293F6EA2F509795A6C303F74CAA?module=Thread&threadID=639500&start=0&mc=8#1095148

/// Custom levenshtein distance. This is essentially the algorithm given in [1], Figure 5 which
/// was originally developed by Myeres et. al (G. Myers. A fast bit-vector algorithm for approximate
/// string matching based on dynamic progamming. Journal of the ACM, 46(3): 395-415, 1999.).
/// An example implementation is given at [2]. We found [3] helpful in understanding the original
/// paper better.
///
/// We improved the algorithm by replacing the inner loop with sse instructions effectively
/// removing it
///
/// [1] http://www.cs.uta.fi/~helmu/pubs/A2001-10.pdf
/// [2] http://apps.topcoder.com/forums/?module=Thread&threadID=639500&start=0&mc=8#1095148
/// [3] http://www.cs.uta.fi/~helmu/pubs/A2001-10.pdf (especially Figure 5)
static inline uint64_t similarity_levenshtein(boost::string_ref a,boost::string_ref b)
{
    unsigned qlen=a.length();
    unsigned blen=b.length();
    const char* qptr=a.data();
    const char* bptr=b.data();
    if (qlen>blen) {
        std::swap(qlen,blen);
        std::swap(qptr,bptr);
    }
    __m128i brep1,brep2;
    uint64_t bmask;
    if (blen<=16) {
        brep1=_mm_lddqu_si128(reinterpret_cast<const __m128i*>(bptr));
        bmask=(1<<blen)-1;
    } else {
        brep1=_mm_lddqu_si128(reinterpret_cast<const __m128i*>(bptr));
        brep2=_mm_lddqu_si128(reinterpret_cast<const __m128i*>(bptr)+1);
        bmask=(blen==32)?0xFFFFFFFF:((1<<blen)-1);
    }

    uint64_t dmj=blen;
    uint64_t vp = (uint64_t)-1;
    uint64_t vn = 0;
    uint64_t hp = 0;
    uint64_t hn = 0;
    for (unsigned j = 0; j < qlen; j++) {
        uint64_t pm;
        __m128i c=_mm_set1_epi8(qptr[j]);
        if (blen<=16) {
            pm=_mm_movemask_epi8(_mm_cmpeq_epi8(c,brep1))&bmask;
        } else {
            pm=(_mm_movemask_epi8(_mm_cmpeq_epi8(c,brep1))|(_mm_movemask_epi8(_mm_cmpeq_epi8(c,brep2))<<16))&bmask;
        }
        uint64_t d = ((((pm & vp) + vp)) ^ vp) | pm | vn;
        hp = (vn | ~(d | vp));
        uint64_t hpw = (hp << 1) | 1;
        hn = d & vp;

        int m=blen-1;
        if (hp&(1<<m)) ++dmj;
        if (hn&(1<<m)) --dmj;

        vp = (hn << 1) | ~(d | hpw);
        vn = d & hpw;
    }
    return dmj;
}

/// Hamming distance using sse. Both words must be padded and aligned to 16 bytes and loaded
/// into sse registers a and b. Then, we compute the xor of the two registers and use saturation
/// arithmetics to propagte differences from the differing bit to the while byte.
static inline unsigned similarity_hamming(__m128i a,__m128i b)
{
    __m128i mask=_mm_set1_epi8(254);
    union { __m128i a; uint64_t b[2]; } x;
    x.a=_mm_adds_epu8(_mm_xor_si128(a,b),mask);
    return (_mm_popcnt_u64(x.b[0])+_mm_popcnt_u64(x.b[1]))-(128-16);
}

}


#include <NTL/LLL.h>
#include <NTL/fileio.h>

#include <NTL/new.h>

NTL_START_IMPL

static void RowTransform(vec_ZZ& A, vec_ZZ& B, const ZZ& MU1)
// x = x - y*MU
{
   static ZZ T, MU;
   long k;

   long n = A.length();
   long i;

   MU = MU1;

   if (MU == 1) {
      for (i = 1; i <= n; i++)
         sub(A(i), A(i), B(i));

      return;
   }

   if (MU == -1) {
      for (i = 1; i <= n; i++)
         add(A(i), A(i), B(i));

      return;
   }

   if (MU == 0) return;

   if (NumTwos(MU) >= NTL_ZZ_NBITS)
      k = MakeOdd(MU);
   else
      k = 0;

   if (MU.WideSinglePrecision()) {
      long mu1;
      conv(mu1, MU);

      for (i = 1; i <= n; i++) {
         mul(T, B(i), mu1);
         if (k > 0) LeftShift(T, T, k);
         sub(A(i), A(i), T);
      }
   }
   else {
      for (i = 1; i <= n; i++) {
         mul(T, B(i), MU);
         if (k > 0) LeftShift(T, T, k);
         sub(A(i), A(i), T);
      }
   }
}

static void RowTransform2(vec_ZZ& A, vec_ZZ& B, const ZZ& MU1)
// x = x + y*MU
{
   static ZZ T, MU;
   long k;

   long n = A.length();
   long i;

   MU = MU1;

   if (MU == 1) {
      for (i = 1; i <= n; i++)
         add(A(i), A(i), B(i));

      return;
   }

   if (MU == -1) {
      for (i = 1; i <= n; i++)
         sub(A(i), A(i), B(i));

      return;
   }

   if (MU == 0) return;

   if (NumTwos(MU) >= NTL_ZZ_NBITS)
      k = MakeOdd(MU);
   else
      k = 0;

   if (MU.WideSinglePrecision()) {
      long mu1;
      conv(mu1, MU);

      for (i = 1; i <= n; i++) {
         mul(T, B(i), mu1);
         if (k > 0) LeftShift(T, T, k);
         add(A(i), A(i), T);
      }
   }
   else {
      for (i = 1; i <= n; i++) {
         mul(T, B(i), MU);
         if (k > 0) LeftShift(T, T, k);
         add(A(i), A(i), T);
      }
   }
}

class GivensCache_RR {
public:
   GivensCache_RR(long m, long n);
   ~GivensCache_RR();

   void flush();
   void selective_flush(long l);
   void swap(long l);
   void swap();
   void touch();
   void incr();

   long sz;

   mat_RR buf;

   long *bl;
   long *bv;
   long bp;
};

GivensCache_RR::GivensCache_RR(long m, long n)
{
   sz = min(m, n)/10;
   if (sz < 2)
      sz = 2;
   else if (sz > 20)
      sz = 20;

   typedef double *doubleptr;

   long i;

   buf.SetDims(sz, n);

   bl = NTL_NEW_OP long[sz];
   if (!bl) Error("out of memory");
   for (i = 0; i < sz; i++) bl[0] = 0;

   bv = NTL_NEW_OP long[sz];
   if (!bv) Error("out of memory");
   for (i = 0; i < sz; i++) bv[0] = 0;

   bp = 0;
}

GivensCache_RR::~GivensCache_RR()
{
   delete [] bl;
   delete [] bv;
}

void GivensCache_RR::flush()
{
   long i;
   for (i = 0; i < sz; i++) bl[i] = 0;
}

void GivensCache_RR::selective_flush(long l)
{
   long i;

   for (i = 0; i < sz; i++)
      if (bl[i] && bv[i] >= l)
         bl[i] = 0;
}

void GivensCache_RR::swap(long l)
{
   long k = bl[bp];
   long i;

   i = 0;
   while (i < sz && bl[i] != l)
      i++;

   if (i < sz) {
      bl[bp] = l;
      bl[i] = k;
   }
   else
      bl[bp] = l;

   selective_flush(l);
}

void GivensCache_RR::swap()
{
   swap(bl[bp] - 1);
}

void GivensCache_RR::touch()
{
   long k = bl[bp];
   bl[bp] = 0;
   selective_flush(k);
}

void GivensCache_RR::incr()
{
   long k = bl[bp];
   long k1 = k+1;
   long i;

   i = 0;
   while (i < sz && bl[i] != k1)
      i++;

   if (i < sz) {
      bp = i;
      return;
   }

   i = 0;
   while (i < sz && bl[i] != 0)
      i++;

   if (i < sz) {
      bp = i;
      return;
   }

   long max_val = 0;
   long max_index = 0;
   for (i = 0; i < sz; i++) {
      long t = labs(bl[i]-k1);
      if (t > max_val) {
         max_val = t;
         max_index = i;
      }
   }

   bp = max_index;
   bl[max_index] = 0;
}

static
void GivensComputeGS(mat_RR& B1, mat_RR& mu, mat_RR& aux, long k, long n,
                     GivensCache_RR& cache)
{
   long i, j;

   RR c, s, a, b, t;
   RR T1, T2;

   vec_RR& p = mu(k);

   vec_RR& pp = cache.buf[cache.bp];

   if (!cache.bl[cache.bp]) {
      for (j = 1; j <= n; j++)
         pp(j) = B1(k,j);

      long backoff;
      backoff = k/4;
      if (backoff < 2)
         backoff = 2;
      else if (backoff > cache.sz + 2)
         backoff = cache.sz + 2;

      long ub = k-(backoff-1);

      for (i = 1; i < ub; i++) {
         vec_RR& cptr = mu(i);
         vec_RR& sptr = aux(i);

         for (j = n; j > i; j--) {
            c = cptr(j);
            s = sptr(j);

            // a = c*pp(j-1) - s*pp(j);
            mul(T1, c, pp(j-1));
            mul(T2, s, pp(j));
            sub(a, T1, T2);

            // b = s*pp(j-1) + c*pp(j);
            mul(T1, s, pp(j-1));
            mul(T2, c, pp(j));
            add(b, T1, T2);

            pp(j-1) = a;
            pp(j) = b;
         }

         div(pp(i), pp(i), mu(i,i));
      }

      cache.bl[cache.bp] = k;
      cache.bv[cache.bp] = k-backoff;
   }

   for (j = 1; j <= n; j++)
      p(j) = pp(j);

   for (i = max(cache.bv[cache.bp]+1, 1); i < k; i++) {
      vec_RR& cptr = mu(i);
      vec_RR& sptr = aux(i);

      for (j = n; j > i; j--) {
         c = cptr(j);
         s = sptr(j);

         // a = c*p(j-1) - s*p(j);
         mul(T1, c, p(j-1));
         mul(T2, s, p(j));
         sub(a, T1, T2);

         // b = s*p(j-1) + c*p(j);
         mul(T1, s, p(j-1));
         mul(T2, c, p(j));
         add(b, T1, T2);

         p(j-1) = a;
         p(j) = b;
      }

      div(p(i), p(i), mu(i,i));
   }

   for (j = n; j > k; j--) {
      a = p(j-1);
      b = p(j);

      if (b == 0) {
         c = 1;
         s = 0;
      }
      else {
         abs(T1, b);
         abs(T2, a);

         if (T1 > T2) {
            // t = -a/b;
            div(T1, a, b);
            negate(t, T1);

            // s = 1/sqrt(1 + t*t);
            sqr(T1, t);
            add(T1, T1, 1);
            SqrRoot(T1, T1);
            inv(s, T1);

            // c = s*t;
            mul(c, s, t);
         }
         else {
            // t = -b/a;
            div(T1, b, a);
            negate(t, T1);

            // c = 1/sqrt(1 + t*t);
            sqr(T1, t);
            add(T1, T1, 1);
            SqrRoot(T1, T1);
            inv(c, T1);

            // s = c*t;
            mul(s, c, t);
         }
      }

      // p(j-1) = c*a - s*b;
      mul(T1, c, a);
      mul(T2, s, b);
      sub(p(j-1), T1, T2);

      p(j) = c;
      aux(k,j) = s;
   }

   if (k > n+1) Error("G_LLL_RR: internal error");
   if (k > n) p(k) = 0;

}

static RR red_fudge;
static long log_red = 0;

static void init_red_fudge()
{
   log_red = long(0.50*RR::precision());

   power2(red_fudge, -log_red);
}

static void inc_red_fudge()
{

   mul(red_fudge, red_fudge, 2);
   log_red--;

   cerr << "G_LLL_RR: warning--relaxing reduction (" << log_red << ")\n";

   if (log_red < 4)
      Error("G_LLL_RR: can not continue...sorry");
}

static long verbose = 0;

static unsigned long NumSwaps = 0;
static double StartTime = 0;
static double LastTime = 0;

static void G_LLLStatus(long max_k, double t, long m, const mat_ZZ& B)
{
   cerr << "---- G_LLL_RR status ----\n";
   cerr << "elapsed time: ";
   PrintTime(cerr, t-StartTime);
   cerr << ", stage: " << max_k;
   cerr << ", rank: " << m;
   cerr << ", swaps: " << NumSwaps << "\n";

   ZZ t1;
   long i;
   double prodlen = 0;

   for (i = 1; i <= m; i++) {
      InnerProduct(t1, B(i), B(i));
      if (!IsZero(t1))
         prodlen += log(t1);
   }

   cerr << "log of prod of lengths: " << prodlen/(2.0*log(2.0)) << "\n";

   if (LLLDumpFile) {
      cerr << "dumping to " << LLLDumpFile << "...";

      ofstream f;
      OpenWrite(f, LLLDumpFile);

      f << "[";
      for (i = 1; i <= m; i++) {
         f << B(i) << "\n";
      }
      f << "]\n";

      f.close();

      cerr << "\n";
   }

   LastTime = t;

}

static
long ll_G_LLL_RR(mat_ZZ& B, mat_ZZ* U, const RR& delta, long deep,
           LLLCheckFct check, mat_RR& B1, mat_RR& mu,
           mat_RR& aux, long m, long init_k, long &quit,
           GivensCache_RR& cache)
{
   long n = B.NumCols();

   long i, j, k, Fc1;
   ZZ MU;
   RR mu1, t1, t2, cc;
   ZZ T1;

   quit = 0;
   k = init_k;

   long counter;

   long trigger_index;
   long small_trigger;
   long cnt;

   RR half;
   conv(half,  0.5);
   RR half_plus_fudge;
   add(half_plus_fudge, half, red_fudge);

   long max_k = 0;
   double tt;

   cache.flush();

   while (k <= m) {

      if (k > max_k) {
         max_k = k;
      }

      if (verbose) {
         tt = GetTime();

         if (tt > LastTime + LLLStatusInterval)
            G_LLLStatus(max_k, tt, m, B);
      }

      GivensComputeGS(B1, mu, aux, k, n, cache);

      counter = 0;
      trigger_index = k;
      small_trigger = 0;
      cnt = 0;

      do {
         // size reduction

         counter++;
         if (counter > 10000) {
            cerr << "G_LLL_XD: warning--possible infinite loop\n";
            counter = 0;
         }

         Fc1 = 0;

         for (j = k-1; j >= 1; j--) {
            abs(t1, mu(k,j));
            if (t1 > half_plus_fudge) {

               if (!Fc1) {
                  if (j > trigger_index ||
                      (j == trigger_index && small_trigger)) {

                     cnt++;

                     if (cnt > 10) {
                        inc_red_fudge();
                        add(half_plus_fudge, half, red_fudge);
                        cnt = 0;
                     }
                  }

                  trigger_index = j;
                  small_trigger = (t1 < 4);
               }

               Fc1 = 1;

               mu1 = mu(k,j);
               if (sign(mu1) >= 0) {
                  sub(mu1, mu1, half);
                  ceil(mu1, mu1);
               }
               else {
                  add(mu1, mu1, half);
                  floor(mu1, mu1);
               }

               if (mu1 == 1) {
                  for (i = 1; i <= j-1; i++)
                     sub(mu(k,i), mu(k,i), mu(j,i));
               }
               else if (mu1 == -1) {
                  for (i = 1; i <= j-1; i++)
                     add(mu(k,i), mu(k,i), mu(j,i));
               }
               else {
                  for (i = 1; i <= j-1; i++) {
                     mul(t2, mu1, mu(j,i));
                     sub(mu(k,i), mu(k,i), t2);
                  }
               }

               conv(MU, mu1);

               sub(mu(k,j), mu(k,j), mu1);

               RowTransform(B(k), B(j), MU);
               if (U) RowTransform((*U)(k), (*U)(j), MU);
            }
         }

         if (Fc1) {
            for (i = 1; i <= n; i++)
               conv(B1(k, i), B(k, i));
            cache.touch();
            GivensComputeGS(B1, mu, aux, k, n, cache);
         }
      } while (Fc1);

      if (check && (*check)(B(k)))
         quit = 1;

      if (IsZero(B(k))) {
         for (i = k; i < m; i++) {
            // swap i, i+1
            swap(B(i), B(i+1));
            swap(B1(i), B1(i+1));
            if (U) swap((*U)(i), (*U)(i+1));
         }

         cache.flush();

         m--;
         if (quit) break;
         continue;
      }

      if (quit) break;

      if (deep > 0) {
         // deep insertions

         Error("sorry...deep insertions not implemented");

      } // end deep insertions

      // test G_LLL reduction condition

      if (k <= 1) {
         cache.incr();
         k++;
      }
      else {
         sqr(t1, mu(k,k-1));
         sub(t1, delta, t1);
         sqr(t2, mu(k-1,k-1));
         mul(t1, t1, t2);
         sqr(t2, mu(k, k));
         if (t1 > t2) {
            // swap rows k, k-1
            swap(B(k), B(k-1));
            swap(B1(k), B1(k-1));
            if (U) swap((*U)(k), (*U)(k-1));

            cache.swap();

            k--;
            NumSwaps++;
         }
         else {
            cache.incr();
            k++;
         }
      }
   }

   if (verbose) {
      G_LLLStatus(m+1, GetTime(), m, B);
   }

   return m;
}

static
long G_LLL_RR(mat_ZZ& B, mat_ZZ* U, const RR& delta, long deep,
           LLLCheckFct check)
{
   long m = B.NumRows();
   long n = B.NumCols();

   long i, j;
   long new_m, dep, quit;
   RR s;
   ZZ MU;
   RR mu1;

   RR t1;
   ZZ T1;

   init_red_fudge();

   if (U) ident(*U, m);

   mat_RR B1;  // approximates B
   B1.SetDims(m, n);

   mat_RR mu;
   mu.SetDims(m, n+1);

   mat_RR aux;
   aux.SetDims(m, n);

   for (i = 1; i <=m; i++)
      for (j = 1; j <= n; j++)
         conv(B1(i, j), B(i, j));

   GivensCache_RR cache(m, n);

   new_m = ll_G_LLL_RR(B, U, delta, deep, check, B1, mu, aux, m, 1, quit, cache);

   dep = m - new_m;
   m = new_m;

   if (dep > 0) {
      // for consistency, we move all of the zero rows to the front

      for (i = 0; i < m; i++) {
         swap(B(m+dep-i), B(m-i));
         if (U) swap((*U)(m+dep-i), (*U)(m-i));
      }
   }

   return m;
}

long G_LLL_RR(mat_ZZ& B, double delta, long deep,
            LLLCheckFct check, long verb)
{
   verbose = verb;
   NumSwaps = 0;
   if (verbose) {
      StartTime = GetTime();
      LastTime = StartTime;
   }

   if (delta < 0.50 || delta >= 1) Error("G_LLL_RR: bad delta");
   if (deep < 0) Error("G_LLL_RR: bad deep");
   RR Delta;
   conv(Delta, delta);
   return G_LLL_RR(B, 0, Delta, deep, check);
}

long G_LLL_RR(mat_ZZ& B, mat_ZZ& U, double delta, long deep,
           LLLCheckFct check, long verb)
{
   verbose = verb;
   NumSwaps = 0;
   if (verbose) {
      StartTime = GetTime();
      LastTime = StartTime;
   }

   if (delta < 0.50 || delta >= 1) Error("G_LLL_RR: bad delta");
   if (deep < 0) Error("G_LLL_RR: bad deep");
   RR Delta;
   conv(Delta, delta);
   return G_LLL_RR(B, &U, Delta, deep, check);
}

static vec_RR G_BKZConstant;

static
void ComputeG_BKZConstant(long beta, long p)
{
   RR c_PI;
   ComputePi(c_PI);

   RR LogPI = log(c_PI);

   G_BKZConstant.SetLength(beta-1);

   vec_RR Log;
   Log.SetLength(beta);

   long i, j, k;
   RR x, y;

   for (j = 1; j <= beta; j++)
      Log(j) = log(to_RR(j));

   for (i = 1; i <= beta-1; i++) {
      // First, we compute x = gamma(i/2)^{2/i}

      k = i/2;

      if ((i & 1) == 0) { // i even
         x = 0;
         for (j = 1; j <= k; j++)
            x += Log(j);

         x = exp(x/k);

      }
      else { // i odd
         x = 0;
         for (j = k + 2; j <= 2*k + 2; j++)
            x += Log(j);

         x += 0.5*LogPI - 2*(k+1)*Log(2);

         x = exp(2*x/i);
      }

      // Second, we compute y = 2^{2*p/i}

      y = exp(-(2*p/to_RR(i))*Log(2));

      G_BKZConstant(i) = x*y/c_PI;
   }

}

static vec_RR G_BKZThresh;

static
void ComputeG_BKZThresh(RR *c, long beta)
{
   G_BKZThresh.SetLength(beta-1);

   long i;
   RR x;
   RR t1;

   x = 0;

   for (i = 1; i <= beta-1; i++) {
      log(t1, c[i-1]);
      add(x, x, t1);
      div(t1, x, i);
      exp(t1, t1);
      mul(G_BKZThresh(i), t1, G_BKZConstant(i));
   }
}

static
void G_BKZStatus(double tt, double enum_time, unsigned long NumIterations,
               unsigned long NumTrivial, unsigned long NumNonTrivial,
               unsigned long NumNoOps, long m,
               const mat_ZZ& B)
{
   cerr << "---- G_BKZ_RR status ----\n";
   cerr << "elapsed time: ";
   PrintTime(cerr, tt-StartTime);
   cerr << ", enum time: ";
   PrintTime(cerr, enum_time);
   cerr << ", iter: " << NumIterations << "\n";
   cerr << "triv: " << NumTrivial;
   cerr << ", nontriv: " << NumNonTrivial;
   cerr << ", no ops: " << NumNoOps;
   cerr << ", rank: " << m;
   cerr << ", swaps: " << NumSwaps << "\n";

   ZZ t1;
   long i;
   double prodlen = 0;

   for (i = 1; i <= m; i++) {
      InnerProduct(t1, B(i), B(i));
      if (!IsZero(t1))
         prodlen += log(t1);
   }

   cerr << "log of prod of lengths: " << prodlen/(2.0*log(2.0)) << "\n";

   if (LLLDumpFile) {
      cerr << "dumping to " << LLLDumpFile << "...";

      ofstream f;
      OpenWrite(f, LLLDumpFile);

      f << "[";
      for (i = 1; i <= m; i++) {
         f << B(i) << "\n";
      }
      f << "]\n";

      f.close();

      cerr << "\n";
   }

   LastTime = tt;

}

static
long G_BKZ_RR(mat_ZZ& BB, mat_ZZ* UU, const RR& delta,
         long beta, long prune, LLLCheckFct check)
{
   long m = BB.NumRows();
   long n = BB.NumCols();
   long m_orig = m;

   long i, j;
   ZZ MU;

   RR t1, t2;
   ZZ T1;

   init_red_fudge();

   mat_ZZ B;
   B = BB;

   B.SetDims(m+1, n);

   mat_RR B1;
   B1.SetDims(m+1, n);

   mat_RR mu;
   mu.SetDims(m+1, n+1);

   mat_RR aux;
   aux.SetDims(m+1, n);

   vec_RR c;
   c.SetLength(m+1);

   RR cbar;

   vec_RR ctilda;
   ctilda.SetLength(m+1);

   vec_RR vvec;
   vvec.SetLength(m+1);

   vec_RR yvec;
   yvec.SetLength(m+1);

   vec_RR uvec;
   uvec.SetLength(m+1);

   vec_RR utildavec;
   utildavec.SetLength(m+1);

   vec_long Deltavec;
   Deltavec.SetLength(m+1);

   vec_long deltavec;
   deltavec.SetLength(m+1);

   mat_ZZ Ulocal;
   mat_ZZ *U;

   if (UU) {
      Ulocal.SetDims(m+1, m);
      for (i = 1; i <= m; i++)
         conv(Ulocal(i, i), 1);
      U = &Ulocal;
   }
   else
      U = 0;

   long quit;
   long new_m;
   long z, jj, kk;
   long s, t;
   long h;

   for (i = 1; i <=m; i++)
      for (j = 1; j <= n; j++)
         conv(B1(i, j), B(i, j));

   // cerr << "\n";
   // cerr << "first G_LLL\n";

   GivensCache_RR cache(m, n);

   m = ll_G_LLL_RR(B, U, delta, 0, check, B1, mu, aux, m, 1, quit, cache);

   double tt;

   double enum_time = 0;
   unsigned long NumIterations = 0;
   unsigned long NumTrivial = 0;
   unsigned long NumNonTrivial = 0;
   unsigned long NumNoOps = 0;

   long verb = verbose;

   verbose = 0;

   if (m < m_orig) {
      for (i = m_orig+1; i >= m+2; i--) {
         // swap i, i-1

         swap(B(i), B(i-1));
         if (U) swap((*U)(i), (*U)(i-1));
      }
   }

   long clean = 1;

   if (!quit && m > 1) {
      // cerr << "continuing\n";

      if (beta > m) beta = m;

      if (prune > 0)
         ComputeG_BKZConstant(beta, prune);

      z = 0;
      jj = 0;

      while (z < m-1) {
         jj++;
         kk = min(jj+beta-1, m);

         if (jj == m) {
            jj = 1;
            kk = beta;
            clean = 1;
         }

         if (verb) {
            tt = GetTime();
            if (tt > LastTime + LLLStatusInterval)
               G_BKZStatus(tt, enum_time, NumIterations, NumTrivial,
                         NumNonTrivial, NumNoOps, m, B);
         }

         // ENUM

         double tt1;

         if (verb) {
            tt1 = GetTime();
         }

         for (i = jj; i <= kk; i++)
            sqr(c(i), mu(i,i));

         if (prune > 0)
            ComputeG_BKZThresh(&c(jj), kk-jj+1);

         cbar = c(jj);
         conv(utildavec(jj), 1);
         conv(uvec(jj), 1);

         conv(yvec(jj), 0);
         conv(vvec(jj), 0);
         Deltavec(jj) = 0;

         s = t = jj;
         deltavec(jj) = 1;

         for (i = jj+1; i <= kk+1; i++) {
            conv(ctilda(i), 0);
            conv(uvec(i), 0);
            conv(utildavec(i), 0);
            conv(yvec(i), 0);
            Deltavec(i) = 0;
            conv(vvec(i), 0);
            deltavec(i) = 1;
         }

         long enum_cnt = 0;

         while (t <= kk) {
            if (verb) {
               enum_cnt++;
               if (enum_cnt > 100000) {
                  enum_cnt = 0;
                  tt = GetTime();
                  if (tt > LastTime + LLLStatusInterval) {
                     enum_time += tt - tt1;
                     tt1 = tt;
                     G_BKZStatus(tt, enum_time, NumIterations, NumTrivial,
                               NumNonTrivial, NumNoOps, m, B);
                  }
               }
            }

            add(t1, yvec(t), utildavec(t));
            sqr(t1, t1);
            mul(t1, t1, c(t));
            add(ctilda(t), ctilda(t+1), t1);

            if (prune > 0 && t > jj)
               sub(t1, cbar, G_BKZThresh(t-jj));
            else
               t1 = cbar;

            if (ctilda(t) <t1) {
               if (t > jj) {
                  t--;
                  clear(t1);
                  for (i = t+1; i <= s; i++) {
                     mul(t2, utildavec(i), mu(i,t));
                     add(t1, t1, t2);
                  }

                  yvec(t) = t1;
                  negate(t1, t1);
                  if (sign(t1) >= 0) {
                     sub(t1, t1, 0.5);
                     ceil(t1, t1);
                  }
                  else {
                     add(t1, t1, 0.5);
                     floor(t1, t1);
                  }

                  utildavec(t) = t1;
                  vvec(t) = t1;
                  Deltavec(t) = 0;

                  negate(t1, t1);

                  if (t1 < yvec(t))
                     deltavec(t) = -1;
                  else
                     deltavec(t) = 1;
               }
               else {
                  cbar = ctilda(jj);
                  for (i = jj; i <= kk; i++) {
                     uvec(i) = utildavec(i);
                  }
               }
            }
            else {
               t++;
               s = max(s, t);
               if (t < s) Deltavec(t) = -Deltavec(t);
               if (Deltavec(t)*deltavec(t) >= 0) Deltavec(t) += deltavec(t);
               add(utildavec(t), vvec(t), Deltavec(t));
            }
         }

         if (verb) {
            tt1 = GetTime() - tt1;
            enum_time += tt1;
         }

         NumIterations++;

         h = min(kk+1, m);

         mul(t1, red_fudge, -8);
         add(t1, t1, delta);
         mul(t1, t1, c(jj));

         if (t1 > cbar) {

            clean = 0;

            // we treat the case that the new vector is b_s (jj < s <= kk)
            // as a special case that appears to occur most of the time.

            s = 0;
            for (i = jj+1; i <= kk; i++) {
               if (uvec(i) != 0) {
                  if (s == 0)
                     s = i;
                  else
                     s = -1;
               }
            }

            if (s == 0) Error("G_BKZ_RR: internal error");

            if (s > 0) {
               // special case
               // cerr << "special case\n";

               NumTrivial++;

               for (i = s; i > jj; i--) {
                  // swap i, i-1
                  swap(B(i-1), B(i));
                  swap(B1(i-1), B1(i));
                  if (U) swap((*U)(i-1), (*U)(i));
               }

               new_m = ll_G_LLL_RR(B, U, delta, 0, check,
                                B1, mu, aux, h, jj, quit, cache);
               if (new_m != h) Error("G_BKZ_RR: internal error");
               if (quit) break;
            }
            else {
               // the general case

               NumNonTrivial++;

               for (i = 1; i <= n; i++) conv(B(m+1, i), 0);

               if (U) {
                  for (i = 1; i <= m_orig; i++)
                     conv((*U)(m+1, i), 0);
               }

               for (i = jj; i <= kk; i++) {
                  if (uvec(i) == 0) continue;
                  conv(MU, uvec(i));
                  RowTransform2(B(m+1), B(i), MU);
                  if (U) RowTransform2((*U)(m+1), (*U)(i), MU);
               }

               for (i = m+1; i >= jj+1; i--) {
                  // swap i, i-1
                  swap(B(i-1), B(i));
                  swap(B1(i-1), B1(i));
                  if (U) swap((*U)(i-1), (*U)(i));
               }

               for (i = 1; i <= n; i++)
                  conv(B1(jj, i), B(jj, i));

               if (IsZero(B(jj))) Error("G_BKZ_RR: internal error");

               // remove linear dependencies

               // cerr << "general case\n";
               new_m = ll_G_LLL_RR(B, U, delta, 0, 0, B1, mu, aux,
                                  kk+1, jj, quit, cache);

               if (new_m != kk) Error("G_BKZ_RR: internal error");

               // remove zero vector

               for (i = kk+2; i <= m+1; i++) {
                  // swap i, i-1
                  swap(B(i-1), B(i));
                  swap(B1(i-1), B1(i));
                  if (U) swap((*U)(i-1), (*U)(i));
               }

               quit = 0;
               if (check) {
                  for (i = 1; i <= kk; i++)
                     if ((*check)(B(i))) {
                        quit = 1;
                        break;
                     }
               }

               if (quit) break;

               if (h > kk) {
                  // extend reduced basis

                  new_m = ll_G_LLL_RR(B, U, delta, 0, check,
                                   B1, mu, aux, h, h, quit, cache);

                  if (new_m != h) Error("G_BKZ_RR: internal error");
                  if (quit) break;
               }
            }

            z = 0;
         }
         else {
            // G_LLL_RR
            // cerr << "progress\n";

            NumNoOps++;

            if (!clean) {
               new_m = ll_G_LLL_RR(B, U, delta, 0, check, B1, mu, aux,
                                   h, h, quit, cache);
               if (new_m != h) Error("G_BKZ_RR: internal error");
               if (quit) break;
            }

            z++;
         }
      }
   }

   if (verb) {
      G_BKZStatus(GetTime(), enum_time, NumIterations, NumTrivial, NumNonTrivial,
                NumNoOps, m, B);
   }

   // clean up

   if (m_orig > m) {
      // for consistency, we move zero vectors to the front

      for (i = m+1; i <= m_orig; i++) {
         swap(B(i), B(i+1));
         if (U) swap((*U)(i), (*U)(i+1));
      }

      for (i = 0; i < m; i++) {
         swap(B(m_orig-i), B(m-i));
         if (U) swap((*U)(m_orig-i), (*U)(m-i));
      }
   }

   B.SetDims(m_orig, n);
   BB = B;

   if (U) {
      U->SetDims(m_orig, m_orig);
      *UU = *U;
   }

   return m;
}

long G_BKZ_RR(mat_ZZ& BB, mat_ZZ& UU, double delta,
         long beta, long prune, LLLCheckFct check, long verb)
{
   verbose = verb;
   NumSwaps = 0;
   if (verbose) {
      StartTime = GetTime();
      LastTime = StartTime;
   }

   if (delta < 0.50 || delta >= 1) Error("G_BKZ_RR: bad delta");
   if (beta < 2) Error("G_BKZ_RR: bad block size");

   RR Delta;
   conv(Delta, delta);

   return G_BKZ_RR(BB, &UU, Delta, beta, prune, check);
}

long G_BKZ_RR(mat_ZZ& BB, double delta,
         long beta, long prune, LLLCheckFct check, long verb)
{
   verbose = verb;
   NumSwaps = 0;
   if (verbose) {
      StartTime = GetTime();
      LastTime = StartTime;
   }

   if (delta < 0.50 || delta >= 1) Error("G_BKZ_RR: bad delta");
   if (beta < 2) Error("G_BKZ_RR: bad block size");

   RR Delta;
   conv(Delta, delta);

   return G_BKZ_RR(BB, 0, Delta, beta, prune, check);
}

NTL_END_IMPL


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <iostream>

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

#include <boost/compute/function.hpp>
#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/types/fundamental.hpp>

namespace compute = boost::compute;

// this example shows how to compute the determinant of many 4x4 matrices
// using a determinant function and the transform() algorithm. in OpenCL the
// float16 type can be used to store a 4x4 matrix and the components are laid
// out in the following order:
//
// M = [ s0 s4 s8 sc ]
//     [ s1 s5 s9 sd ]
//     [ s2 s6 sa se ]
//     [ s3 s7 sb sf ]
//
// the input matrices are created using eigen's random matrix and then
// used again at the end to verify the results of the determinant function.
int main()
{
    // get default device and setup context
    compute::device gpu = compute::system::default_device();
    compute::context context(gpu);
    compute::command_queue queue(context, gpu);
    std::cout << "device: " << gpu.name() << std::endl;

    size_t n = 1000;

    // create random 4x4 matrices on the host
    std::vector<Eigen::Matrix4f, mi_stl_allocator<Eigen::Matrix4f>> matrices(n);
    for (size_t i = 0; i < n; i++)
    {
        matrices[i] = Eigen::Matrix4f::Random();
    }

    // copy matrices to the device
    using compute::float16_;
    compute::vector<float16_> input(n, context);
    compute::copy(
        matrices.begin(), matrices.end(), input.begin(), queue);

    // function returning the determinant of a 4x4 matrix.
    BOOST_COMPUTE_FUNCTION(float, determinant4x4, (const float16_ m),
                           {
                               return m.s0 * m.s5 * m.sa * m.sf + m.s0 * m.s6 * m.sb * m.sd + m.s0 * m.s7 * m.s9 * m.se +
                                      m.s1 * m.s4 * m.sb * m.se + m.s1 * m.s6 * m.s8 * m.sf + m.s1 * m.s7 * m.sa * m.sc +
                                      m.s2 * m.s4 * m.s9 * m.sf + m.s2 * m.s5 * m.sb * m.sc + m.s2 * m.s7 * m.s8 * m.sd +
                                      m.s3 * m.s4 * m.sa * m.sd + m.s3 * m.s5 * m.s8 * m.se + m.s3 * m.s6 * m.s9 * m.sc -
                                      m.s0 * m.s5 * m.sb * m.se - m.s0 * m.s6 * m.s9 * m.sf - m.s0 * m.s7 * m.sa * m.sd -
                                      m.s1 * m.s4 * m.sa * m.sf - m.s1 * m.s6 * m.sb * m.sc - m.s1 * m.s7 * m.s8 * m.se -
                                      m.s2 * m.s4 * m.sb * m.sd - m.s2 * m.s5 * m.s8 * m.sf - m.s2 * m.s7 * m.s9 * m.sc -
                                      m.s3 * m.s4 * m.s9 * m.se - m.s3 * m.s5 * m.sa * m.sc - m.s3 * m.s6 * m.s8 * m.sd;
                           });

    // calculate determinants on the gpu
    compute::vector<float> determinants(n, context);
    compute::transform(
        input.begin(), input.end(), determinants.begin(), determinant4x4, queue);

    // check determinants
    std::vector<float, mi_stl_allocator<float>> host_determinants(n);
    compute::copy(
        determinants.begin(), determinants.end(), host_determinants.begin(), queue);

    for (size_t i = 0; i < n; i++)
    {
        float det = matrices[i].determinant();

        if (std::abs(det - host_determinants[i]) > 1e-6)
        {
            std::cerr << "error: wrong determinant at " << i << " ("
                      << host_determinants[i] << " != " << det << ")"
                      << std::endl;
            return -1;
        }
    }

    return 0;
}


/**
 *
 * Copyright (c) 2010 Matthias Walter (xammy@xammy.homelinux.net)
 *
 * Authors: Matthias Walter
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 */

#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/bipartite.hpp>

using namespace boost;

/// Example to test for bipartiteness and print the certificates.

template < typename Graph > void print_bipartite(const Graph& g)
{
    typedef graph_traits< Graph > traits;
    typename traits::vertex_iterator vertex_iter, vertex_end;

    /// Most simple interface just tests for bipartiteness.

    bool bipartite = is_bipartite(g);

    if (bipartite)
    {
        typedef std::vector< default_color_type > partition_t;
        typedef
            typename property_map< Graph, vertex_index_t >::type index_map_t;
        typedef iterator_property_map< partition_t::iterator, index_map_t >
            partition_map_t;

        partition_t partition(num_vertices(g));
        partition_map_t partition_map(partition.begin(), get(vertex_index, g));

        /// A second interface yields a bipartition in a color map, if the graph
        /// is bipartite.

        is_bipartite(g, get(vertex_index, g), partition_map);

        for (boost::tie(vertex_iter, vertex_end) = vertices(g);
             vertex_iter != vertex_end; ++vertex_iter)
        {
            std::cout
                << "Vertex " << *vertex_iter << " has color "
                << (get(partition_map, *vertex_iter)
                               == color_traits< default_color_type >::white()
                           ? "white"
                           : "black")
                << std::endl;
        }
    }
    else
    {
        typedef std::vector< typename traits::vertex_descriptor >
            vertex_vector_t;
        vertex_vector_t odd_cycle;

        /// A third interface yields an odd-cycle if the graph is not bipartite.

        find_odd_cycle(g, get(vertex_index, g), std::back_inserter(odd_cycle));

        std::cout << "Odd cycle consists of the vertices:";
        for (size_t i = 0; i < odd_cycle.size(); ++i)
        {
            std::cout << " " << odd_cycle[i];
        }
        std::cout << std::endl;
    }
}

int main(int argc, char** argv)
{
    typedef adjacency_list< vecS, vecS, undirectedS > vector_graph_t;
    typedef std::pair< int, int > E;

    /**
     * Create the graph drawn below.
     *
     *       0 - 1 - 2
     *       |       |
     *   3 - 4 - 5 - 6
     *  /      \   /
     *  |        7
     *  |        |
     *  8 - 9 - 10
     **/

    E bipartite_edges[]
        = { E(0, 1), E(0, 4), E(1, 2), E(2, 6), E(3, 4), E(3, 8), E(4, 5),
              E(4, 7), E(5, 6), E(6, 7), E(7, 10), E(8, 9), E(9, 10) };
    vector_graph_t bipartite_vector_graph(&bipartite_edges[0],
        &bipartite_edges[0] + sizeof(bipartite_edges) / sizeof(E), 11);

    /**
     * Create the graph drawn below.
     *
     *       2 - 1 - 0
     *       |       |
     *   3 - 6 - 5 - 4
     *  /      \   /
     *  |        7
     *  |       /
     *  8 ---- 9
     *
     **/

    E non_bipartite_edges[] = { E(0, 1), E(0, 4), E(1, 2), E(2, 6), E(3, 6),
        E(3, 8), E(4, 5), E(4, 7), E(5, 6), E(6, 7), E(7, 9), E(8, 9) };
    vector_graph_t non_bipartite_vector_graph(&non_bipartite_edges[0],
        &non_bipartite_edges[0] + sizeof(non_bipartite_edges) / sizeof(E), 10);

    /// Call test routine for a bipartite and a non-bipartite graph.

    print_bipartite(bipartite_vector_graph);

    print_bipartite(non_bipartite_vector_graph);

    return 0;
}


#ifndef HYPSYS1D_DG_RATE_OF_CHANGE_HPP
#define HYPSYS1D_DG_RATE_OF_CHANGE_HPP

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

#include <ancse/grid.hpp>
#include <ancse/model.hpp>
#include <ancse/rate_of_change.hpp>
#include <ancse/simulation_time.hpp>

#include <ancse/polynomial_basis.hpp>
#include <ancse/dg_handler.hpp>

/// Compute the rate of change due to DG method.
/** The semidiscrete approximation of a PDE using DG is
 *      du_i/dt = - (<F_{i+0.5},\phi^{-}> - <F_{i-0.5},\phi^{+}>)
 *                + \int_K <F, d/dx {\phi}>
 *  This computes the right hand side of the ODE.
 *
 * @tparam NumericalFlux see e.g. `CentralFlux`.
 */
template <class NumericalFlux>
class DGRateOfChange : public RateOfChange {
  public:

    DGRateOfChange (const Grid &grid,
                    const std::shared_ptr<Model> &model,
                    const NumericalFlux &numerical_flux,
                    const PolynomialBasis &poly_basis,
                    const DGHandler &dg_handler)
        : grid (grid),
          model (model),
          numerical_flux (numerical_flux),
          poly_basis (poly_basis),
          dg_handler (dg_handler)
    {
        std::tie(quad_points, quad_weights) = dg_handler.get_quadrature();
    }

    virtual void operator() (Eigen::MatrixXd &dudt,
                             const Eigen::MatrixXd &u0) const override {
        dudt.setZero();
        eval_numerical_flux(dudt, u0);
        eval_volume_integral(dudt, u0);
    }

    void eval_numerical_flux (Eigen::MatrixXd &dudt,
                              const Eigen::MatrixXd &u0) const;

    void eval_volume_integral (Eigen::MatrixXd &dudt,
                               const Eigen::MatrixXd &u0) const;

  private:
    Grid grid;
    std::shared_ptr<Model> model;
    NumericalFlux numerical_flux;
    PolynomialBasis poly_basis;
    DGHandler dg_handler;

    mutable Eigen::VectorXd quad_points, quad_weights;
};

std::shared_ptr<RateOfChange>
make_dg_rate_of_change(const nlohmann::json &config,
                       const Grid &grid,
                       const std::shared_ptr<Model> &model,
                       const PolynomialBasis &poly_basis,
                       const DGHandler &dg_handler,
                       const std::shared_ptr<SimulationTime> &simulation_time);

#endif // HYPSYS1D_DG_RATE_OF_CHANGE_HPP


#include <iostream>
#include <stdio.h>

#include <boost/numeric/mtl/utility/complexity.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/less.hpp>

using namespace std;
using namespace mtl::complexity_classes;
namespace mpl = boost::mpl;

struct wahr {
  void operator() () {
    cout << "vrai\n"; }
};

struct falsch {
  void operator() () {
    cout << "faute\n"; }
};

void schreib(infinite) {
  cout << "unendlich\n"; }

void schreib(polynomial) {
    cout << "polynomial\n"; }

void schreib(quadratic) {
    cout << "quadratisch\n"; }

void schreib(n_polylog_n) {
    cout << "n log^k n\n"; }

void schreib(n_log_n) {
    cout << "n log n\n"; }

void schreib(linear) {
    cout << "linear\n"; }

void schreib(linear_cached) {
    cout << "linear cached\n"; }

void schreib(polylog_n) {
    cout << "polynomial log\n"; }

void schreib(log_n) {
    cout << "log\n"; }

void schreib(constant) {
    cout << "constant\n"; }

void schreib(cached) {
    cout << "cached\n"; }

template <typename X, typename Y> void write_less(X, Y) {
  typedef mpl::less<X, Y> less_res;
  typename mpl::if_<less_res, wahr, falsch>::type()();
}

template <typename X, typename Y> void write_plus(X, Y) {
  schreib(typename mtl::complexity_classes::plus<X, Y>::type());
}

template <typename X, typename Y> void write_mal(X, Y) {
  schreib(typename mtl::complexity_classes::times<X, Y>::type());
}

int main (int, char**) {

  write_less(quadratic(), infinite());
  write_less(quadratic(), linear());

  write_plus(quadratic(), infinite());
  write_plus(quadratic(), linear());
  write_plus(linear(), quadratic());
  write_plus(n_log_n(), linear());

  write_mal(quadratic(), infinite());
  write_mal(infinite(), quadratic());
  write_mal(quadratic(), quadratic());
  write_mal(linear(), quadratic());
  write_mal(linear(), log_n());
  write_mal(linear(), polylog_n());
  write_mal(n_log_n(), log_n());
  write_mal(n_log_n(), polylog_n());
  write_mal(cached(), log_n());
  write_mal(log_n(), log_n());
  write_mal(cached(), constant());

  return 0;
}


// [[Rcpp::plugins(openmp)]]
#ifdef _OPENMP
#include <omp.h>
#endif
#include <Rcpp.h>
#include <RcppEigen.h>
#include <Eigen/Core>
#include <random>
#include "distributions.h"
#include "concurrentqueue.h"

// [[Rcpp::depends(RcppEigen)]]
using namespace Rcpp;
using namespace RcppEigen;
using namespace Eigen;
inline void initialize_file( std::ofstream& outFile,int M,int N){
  bool queueFull;
  queueFull=0;

  outFile<< "iteration,"<<"mu,";
  for(unsigned int i = 0; i < M; ++i){
    outFile << "beta[" << (i+1) << "],";

  }
  outFile<<"sigmaE,"<<"sigmaG,";
  for(unsigned int i = 0; i < M; ++i){
    outFile << "comp[" << (i+1) << "],";
  }
  unsigned int i;
  for(i = 0; i < (N-1);i++){
    outFile << "epsilon[" << (i+1) << "],";
  }

  outFile << "epsilon[" << (i+1) << "]";
  outFile<<"\n";
}

//' BayesR sampler
//'
//' @param outputFile The file in which the samples aftare burnin will be stored
//' @param seed random seed
//' @param max_iterations total of number of samples taken
//' @param burn_in integer leq than max_iterations, number of samples used for burn in, after which, al samples will  be stored in the outputFile
//' @param thinning thinning regime, not implemented
//' @param X matrix of snp markers, or covariates of interest
//' @param Y vector of response variates, must have the same number of rows as X
//' @param sigma0 variance of the zero-centered normal prior over the intercept
//' @param v0E degrees of  freedom of the prior inverse scaled chi-squared distribution over residues variance
//' @param s02E  scale parameter of the prior inverse scaled chi-squared distribution over residues variance
//' @param v0G degrees of freedom of the prior inverse scaled chi-squared distribution over genetic effects variance
//' @param s02G  scale parameter of the prior inverse scaled chi-squared distribution over residues variance
//' @param cva Vector of mixture components variances.
//'
//' @return (max_iteration - burn_in) modulo thinning samples saved in outputFile.
//' @export
//'
//' @examples
// [[Rcpp::export]]
void BayesRSamplerV2(std::string outputFile, int seed, int max_iterations, int burn_in, int thinning, Eigen::MatrixXd X, Eigen::VectorXd Y,double sigma0, double v0E, double s02E, double v0G, double s02G,Eigen::VectorXd cva) {
  int flag;
  moodycamel::ConcurrentQueue<Eigen::VectorXd> q;
  flag=0;
  int N(Y.size());
  int M(X.cols());
  VectorXd components(M);
  std::ofstream outFile;

  outFile.open(outputFile);
  initialize_file(outFile,M,N);
  VectorXd sampleq(2*M+4+N);
  IOFormat CommaInitFmt(StreamPrecision, DontAlignCols, ", ", ", ", "", "", "", "");
  int K(cva.size()+1);
  ////////////validate inputs

  if(max_iterations < burn_in || max_iterations<1 || burn_in<1) //validations related to mcmc burnin and iterations
  {
    Rcpp::Rcerr<<"error: burn_in has to be a positive integer and smaller than the maximum number of iterations ";
    return;
  }
  if(sigma0 < 0 || v0E < 0 || s02E < 0 || v0G < 0||  s02G < 0 )//validations related to hyperparameters
  {
    Rcpp::Rcerr<<"error: hyper parameters have to be positive";
    //return;
  }
  if((cva.array()==0).any() )//validations related to hyperparameters
  {
    Rcpp::Rcerr<<"error: the zero component is already included in the model by default";
    //return;
  }
  if((cva.array()<0).any() )//validations related to hyperparameters
  {
    Rcpp::Rcerr<<"error: the variance of the components should be positive";
    //return;
  }
  /////end of declarations//////

//  Eigen::initParallel();
 // Eigen::setNbThreads(10);

#ifdef _OPENMP
 omp_set_num_threads(2);
#endif
#pragma omp parallel num_threads(2) shared(flag,q,M,N)
{
#pragma omp sections
{
  //begin producer
  {

    //mean and residual variables
    double mu; // mean or intercept
    double sigmaG; //genetic variance
    double sigmaE; // residuals variance

    //component variables
    VectorXd priorPi(K); // prior probabilities for each component
    VectorXd pi(K); // mixture probabilities
    VectorXd cVa(K); //component-specific variance
    VectorXd logL(K); // log likelihood of component
    VectorXd muk(K); // mean of k-th component marker effect size
    VectorXd denom(K-1); // temporal variable for computing the inflation of the effect variance for a given non-zero componnet
    double num;//storing dot product
    int m0; // total num ber of markes in model
    VectorXd v(K); //variable storing the component assignment
    VectorXd cVaI(K);// inverse of the component variances

    //linear model variables
    MatrixXd beta(M,1); // effect sizes
    VectorXd y_tilde(N); // variable containing the adjusted residuals to exclude the effects of a given marker
    VectorXd epsilon(N); // variable containing the residuals
    VectorXd xsquared(M); //variable containing the squared norm of the X columns

    //sampler variables
    VectorXd sample(2*M+4+N); // varible containg a sambple of all variables in the model, M marker effects, M component assigned to markers, sigmaE, sigmaG, mu, iteration number and Explained variance
    std::vector<int> markerI;
    for (int i=0; i<M; ++i) {
      markerI.push_back(i);
    }

    int marker;
    double acum;

    priorPi[0]=0.5;

    priorPi.segment(1,(K-1))=priorPi[0]*cVa.segment(1,(K-1)).segment(1,(K-1)).array()/cVa.segment(1,(K-1)).segment(1,(K-1)).sum();
    y_tilde.setZero();
    cVa[0] = 0;
    cVa.segment(1,(K-1))=cva;

    cVaI[0] = 0;
    cVaI.segment(1,(K-1))=cVa.segment(1,(K-1)).cwiseInverse();

    beta.setZero();

    mu=0;

    sigmaG=beta_rng(1,1);

    pi=priorPi;

    components.setZero();
    std::chrono::high_resolution_clock::time_point t1 = std::chrono::high_resolution_clock::now();
    epsilon= Y.array() - mu - (X*beta).array();
    sigmaE=epsilon.squaredNorm()/N*0.5;
    xsquared=X.colwise().squaredNorm();
    for(int iteration=0; iteration < max_iterations; iteration++){

      if(iteration>0)
        if( iteration % (int)std::ceil(max_iterations/10) ==0)
          Rcpp::Rcout << "iteration: "<<iteration <<"\n";

      epsilon= epsilon.array()+mu;//  we substract previous value
      mu = norm_rng(epsilon.sum()/(double)N, sigmaE/(double)N); //update mu
      epsilon= epsilon.array()-mu;// we substract again now epsilon =Y-mu-X*beta

      std::random_shuffle(markerI.begin(), markerI.end());

      m0=0;
      v.setZero();
      for(int j=0; j < M; j++){

        marker= markerI[j];

        y_tilde= epsilon.array()+(X.col(marker)*beta(marker,0)).array();//now y_tilde= Y-mu-X*beta+ X.col(marker)*beta(marker)_old

        muk[0]=0.0;//muk for the zeroth component=0

       // std::cout<< muk;
        //we compute the denominator in the variance expression to save computations
        denom=xsquared(marker)+(sigmaE/sigmaG)*cVaI.segment(1,(K-1)).array();
        //we compute the dot product to save computations
        num=(X.col(marker).cwiseProduct(y_tilde)).sum();
        //muk for the other components is computed according to equaitons
        muk.segment(1,(K-1))= num/denom.array();

        logL= pi.array().log();//first component probabilities remain unchanged

        //update the log likelihood for each component
        logL.segment(1,(K-1))=logL.segment(1,(K-1)).array() - 0.5*((((sigmaG/sigmaE)*(xsquared(marker)))*cVa.segment(1,(K-1)).array() + 1).array().log()) + 0.5*( muk.segment(1,(K-1)).array()*num)/sigmaE;

        double p(beta_rng(1,1));//I use beta(1,1) because I cant be bothered in using the std::random or create my own uniform distribution, I will change it later

        if(((logL.segment(1,(K-1)).array()-logL[0]).abs().array() >700 ).any() ){
         acum=0;
        }else{
          acum=1.0/((logL.array()-logL[0]).exp().sum());
        }

        for(int k=0;k<K;k++){
          if(p<=acum){
            //if zeroth component
            if(k==0){
              beta(marker,0)=0;
            }else{
              beta(marker,0)=norm_rng(muk[k],sigmaE/denom[k-1]);
            }
            v[k]+=1.0;
            components[marker]=k;
            break;
          }else{
            //if too big or too small
            if(((logL.segment(1,(K-1)).array()-logL[k+1]).abs().array() >700 ).any() ){
              acum+=0;
            }
            else{
              acum+=1.0/((logL.array()-logL[k+1]).exp().sum());
            }
          }
        }
       epsilon=y_tilde-X.col(marker)*beta(marker,0);//now epsilon contains Y-mu - X*beta+ X.col(marker)*beta(marker)_old- X.col(marker)*beta(marker)_new

      }

      m0=M-v[0];
      sigmaG=inv_scaled_chisq_rng(v0G+m0,(beta.squaredNorm()*m0+v0G*s02G)/(v0G+m0));

      sigmaE=inv_scaled_chisq_rng(v0E+N,((epsilon).squaredNorm()+v0E*s02E)/(v0E+N));

      pi=dirichilet_rng(v.array() + 1.0);

      if(iteration >= burn_in)
      {
        if(iteration % thinning == 0){
          sample<< iteration,mu,beta,sigmaE,sigmaG,components,epsilon;
          q.enqueue(sample);
#ifdef _OPENMP

#else
          if(q.try_dequeue(sampleq))
            outFile<< sampleq.transpose().format(CommaInitFmt) << "\n";
#endif
           //here we have the consumer

        }

      }

    }

    std::chrono::high_resolution_clock::time_point t2 = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::seconds>( t2 - t1 ).count();
    Rcpp::Rcout << "duration: "<<duration << "s\n";
    flag=1;
  }//end producer
#ifdef _OPENMP
#pragma omp section
      {

        while(!flag ){
          if(q.try_dequeue(sampleq))
            outFile<< sampleq.transpose().format(CommaInitFmt) << "\n";
        }
      }//end consumer
#endif
}
}

}

/*** R
M=200 #non zero marker effects
N=2000 #observations
MT=2000 #number of markers
B=matrix(rnorm(MT,sd=sqrt(0.5)),ncol=1) #marker effects, M marquers explain approx 50% of the variance
B[sample(1:MT,MT-M),1]=0 #we set MT-M marker effects to zero
#B=-abs(B)
X <- matrix(rnorm(MT*N), N, MT); var(X[,1])
G <- X%*%B; var(G)
Y=X%*%B+rnorm(N,sd=sqrt(0.4)); var(Y)
Y=Y
X=scale(X)
P=0.5 #prior probability of a marker being excluded from the model
sigma0=0.01# prior  variance of a zero mean gaussian prior over the mean mu NOT IMPLEMENTED
v0E=0.01 # degrees of freedom over the inv scaled chi square prior over residuals variance
s02E=0.01 #scale of the inv scaled chi square prior over residuals variance
v0G=0.01 #degrees of freedom of the inv bla bla prior over snp effects
s02G=0.01 # scale for the same
BayesRSamplerV2("./test2.csv",2, 5000, 2000,10,X, Y,sigma0,v0E,s02E,v0G,s02G,P)
library(readr)
tmp <- read_csv("./test2.csv")
#names(tmp)
#plot(tmp$sigmaG); mean(tmp$sigmaG)
plot(B,colMeans(tmp[,grep("beta",names(tmp))]))
lines(B,B)
abline(h=0)
var(G)
mean(tmp$EV)
1-var(G)
mean(tmp$sigmaE)
plot(tmp$mu)
plot(tmp$sigmaE)
plot(tmp$sigmaG)
hist(as.matrix(tmp[,grep("comp",names(tmp))])) #histogram of components, component 0= variance 0, component 1= variance 0.0001 and so long so forth
*/


#ifndef SDE_HPP_
#define SDE_HPP_

#include <boost/numeric/odeint.hpp>

#include "aux.hpp"

/** EM implementation taking std::vector.
 * Works with non-diagonal noise.
 */
class EulerMaruyamaStepper {
public:
  EulerMaruyamaStepper(Rng & rng) : rng(rng) { /* empty */ }
  typedef std::vector<double> state_type;
  typedef std::vector<double> deriv_type;
  typedef double value_type;
  typedef double time_type;
  typedef double order_type;
  typedef boost::numeric::odeint::stepper_tag stepper_category;
  static order_type order() {
    return 0.5;
  }
  template<class System>
  void do_step(System system, state_type & x, time_type t, time_type dt) const {
    deriv_type F(x.size()), G(x.size());
    system(x, F, t);
    // first query dimension of the Wiener process
    int wdim = system.get().wiener_dim();
    // sample a step of the Wiener process
    std::vector<double> Zi(wdim);
    std::generate(Zi.begin(), Zi.end(), [&](){return rng.normal(0, 1);});
    // now query for the dimension of the SDEs to allow for OD
    int cdim = system.get().continuous_dim();
    // sample a step of the Wiener process
    std::vector<double> Zo(cdim);
    std::generate(Zo.begin(), Zo.end(), [&](){return rng.normal(0, 1);});
    // use the volatility_vec_prod method from VectorField
    system.get().volatility_vec_prod(x, G, t, Zi, Zo);
    for ( size_t i = 0; i < x.size(); ++i ) {
      x[i] += dt * F[i] + sqrt(dt) * G[i]; // G = sigma * Z
    }
  }
private:
  /** ref to an RNG. Used to create normal deviates
   * @todo: construct actual RNG instead?
   */
  Rng & rng;
};

/** EM implementation taking std::vector.
 * Only works with diagonal noise.
 */
class EulerMaruyamaStepperDiag {
public:
  EulerMaruyamaStepperDiag(Rng & rng) : rng(rng) { /* empty */ }
  typedef std::vector<double> state_type;
  typedef std::vector<double> deriv_type;
  typedef double value_type;
  typedef double time_type;
  typedef double order_type;
  typedef boost::numeric::odeint::stepper_tag stepper_category;
  static order_type order() {
    return 0.5;
  }
  template<class System>
  void do_step(System system, state_type & x, time_type t, time_type dt) const {
    deriv_type F(x.size()), G(x.size());
    system(x, F, t);
    system.get().diffusion(x, G, t);
    for ( size_t i = 0; i < x.size(); ++i ) {
      double Z = rng.normal(0, 1);
      x[i] += dt * F[i] + sqrt(dt) * G[i] * Z;
    }
  }
private:
  /** ref to an RNG. Used to create normal deviates
   * @todo: construct actual RNG instead?
   */
  Rng & rng;
};

/** Runge-Kutta discretization of the Milstein scheme.
 * Only works for diagonal noise.
 *
 * @todo: check restrictions: we need that sigma_ii is
 * only dependent of x_i.
 */
 class RungeKuttaMilsteinStepper {
 public:
   RungeKuttaMilsteinStepper(Rng & rng) : rng(rng) { /* empty */ }
   typedef std::vector<double> state_type;
   typedef std::vector<double> deriv_type;
   typedef double value_type;
   typedef double time_type;
   typedef double order_type;
   typedef boost::numeric::odeint::stepper_tag stepper_category;
   static order_type order() {
     return 1.0;
   }
   /** The Millstein method takes steps as follows:
    * \f[
    *  Y_{n+1} = Y_n + f_n h + g_n \Delta W_n + \frac12 g_n g_n' [\Delta W_n^2 - h]
    * \f]
    * with \f$ f_n = f(Y_n) \f$, \f$ g_n = g(Y_n) \f$ and
    * \f$ g_n' = \frac{\partial g}{\partial x}(Y_n) \f$, and
    * \f$ \Delta W_n \sim \sqrt{h} \mathcal{N}(0,1) \f$.
    * The Kunge-Kutta approximation removes the need for a derivative,
    * and uses an intermediate step \f$ \bar{Y}_n = Y_n + f_n h + g_n \sqrt{h} \f$.
    * The next value is then calculated as
    * \f[
    *  Y_{n+1} = Y_n + f_n h + g_n \Delta W_n +
    *    \frac12 g_n \frac{1}{\sqrt{h}}(\bar{g}_n-g_n)[\Delta W_n^2 - h]
    * \f]
    * where \f$ \bar{g}_n = g(\bar{Y}_n) \f$.
    */
   template<class System>
   void do_step(System system, state_type & x, time_type t, time_type dt) const {
     deriv_type F(x.size()), G(x.size()), Gtilde(x.size());
     state_type xtilde(x.size());
     time_type sqdt = sqrt(dt);
     system(x, F, t);
     system.get().diffusion(x, G, t);
     // define intermediate step
     for ( size_t i = 0; i < x.size(); ++i ) {
       xtilde[i] = x[i] + dt*F[i] + sqdt*G[i];
     }
     // define Gtilde
     system.get().diffusion(xtilde, Gtilde, t);
     // update x
     for ( size_t i = 0; i < x.size(); ++i ) {
       double dW = rng.normal(0, 1) * sqdt;
       x[i] += dt*F[i] + G[i]*dW + 0.5*(Gtilde[i] - G[i]) * (dW*dW - dt) / sqdt;
     }
   }
 private:
   Rng & rng; // ref to an RNG
 };

/** @todo Implementation of an adaptive Stochastic Runge Kutta (SRK) method
 * using Rejection Sampling with Memory (RSwM)
 */

#endif


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

 medInria

 Copyright (c) INRIA 2013. All rights reserved.
 See LICENSE.txt for details.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

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

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

#include <vtkSphericalHarmonicSource.h>

#include <vtkPolyDataNormals.h>
#include <vtkCellArray.h>
#include <vtkFloatArray.h>
#include <vtkInformation.h>
#include <vtkInformationVector.h>
#include <vtkMath.h>
#include <vtkObjectFactory.h>
#include <vtkPointData.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkMatrix4x4.h>
#include <vtkStreamingDemandDrivenPipeline.h>

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Compute spherical associated Legendre function
#if (defined __APPLE__ || defined WIN32 || defined CLANG)
#include <boost/math/special_functions/legendre.hpp>
#else
#include <tr1/cmath>
#endif //WIN32

#if (defined __APPLE__ || defined WIN32 || defined CLANG)
double sphLegendre(int _l, int _m, double theta) {
  double factor = sqrt(((double)(2*_l+1) / (4.0*vtkMath::DoublePi()))*(boost::math::factorial<double>((unsigned int)(_l - _m))
                                                        / boost::math::factorial<double>((unsigned int)(_l + _m))))*boost::math::legendre_p (_l, _m, cos(theta));
    return factor;
}
#else
double sphLegendre(int _l, int _m, double theta) {
    double factor =std::tr1::sph_legendre(_l,_m,theta);
    return factor;
}
#endif //WIN32
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

static itk::Vector<double, 3> Cartesian2Spherical(const itk::Vector<double, 3> vITK );

static itk::VariableSizeMatrix<double>
ComputeSHMatrixMaxThesis(const int rank,vtkPolyData *shell,const bool FlipX,const bool FlipY,
                         const bool FlipZ,itk::VariableSizeMatrix<double>& PhiThetaDirections);

static itk::VariableSizeMatrix<double>
ComputeSHMatrixTournier(const int rank,vtkPolyData *shell,const bool FlipX,const bool FlipY,
                        const bool FlipZ,itk::VariableSizeMatrix<double>& PhiThetaDirections);

static itk::VariableSizeMatrix<double>
ComputeSHMatrixRshBasis(const int rank,vtkPolyData *shell,const bool FlipX,const bool FlipY,
                        const bool FlipZ,itk::VariableSizeMatrix<double>& PhiThetaDirections);

static itk::VariableSizeMatrix<double>
ComputeSHMatrix (const int rank,vtkPolyData *shell,const bool FlipX,const bool FlipY,
                 const bool FlipZ, itk::VariableSizeMatrix<double>& PhiThetaDirections);

static void
TranslateAndDeformShell(vtkPolyData* shell,vtkPoints* outPts,double center[3],
                        bool deform,vtkMatrix4x4* transform=0);

vtkCxxRevisionMacro(vtkSphericalHarmonicSource,"$Revision: 0 $");
vtkStandardNewMacro(vtkSphericalHarmonicSource);

vtkSphericalHarmonicSource::vtkSphericalHarmonicSource(int tess)
{
  this->Radius = 0.5;
  this->Center[0] = 0.0;
  this->Center[1] = 0.0;
  this->Center[2] = 0.0;
  this->RotationMatrix = 0;

  this->SetNumberOfInputPorts(0);

  this->DeformOn();
  this->NormalizeOff();
  this->FlipXOff();
  this->FlipYOff();
  // By Default we flip the z-axis, the internal x,y,z have z flipped with respect to visu
  this->FlipZOn();
  this->MaxThesisFuncOff();

  this->TesselationType = Icosahedron;
  this->TesselationBasis = SHMatrix;
  this->Tesselation = tess;
  this->sphereT = vtkTessellatedSphereSource::New();
 this-> SphericalHarmonics = 0;
 this-> Order = 4;
  this->SetNumberOfSphericalHarmonics (15);
}

vtkSphericalHarmonicSource::~vtkSphericalHarmonicSource()
{
  if (this->sphereT)
    this->sphereT->Delete();

  if(this->RotationMatrix)
    this->RotationMatrix->Delete();

  SphericalHarmonics = 0;
}

void vtkSphericalHarmonicSource::SetNumberOfSphericalHarmonics(const int number)
{
  NumberOfSphericalHarmonics = number;
  if (SphericalHarmonics)
    delete[] SphericalHarmonics;
  SphericalHarmonics = new double[NumberOfSphericalHarmonics];
  SphericalHarmonics[0] = 1.0;
  for(int i=1;i<NumberOfSphericalHarmonics;++i)
    SphericalHarmonics[i] = 0.0;

  this->UpdateSphericalHarmonicSource();
  this->Modified();
}

int* vtkSphericalHarmonicSource::GetTesselationRange()
{
  int* range = new int[2];
  GetTesselationRange(range);
  return range;
}

void vtkSphericalHarmonicSource::GetTesselationRange(int *range)
{
  range[0] = 2;
  range[1] = 5;
}

void vtkSphericalHarmonicSource::SetSphericalHarmonicComponent(int i,double v)
{
  if (SphericalHarmonics[i]!=v)
  {
    vtkDebugMacro(<< this->GetClassName() << " (" << this
                  <<"): setting Spherical Harmonics coefficient "<< i <<" to "<< v);
    this->Modified();
    SphericalHarmonics[i] = v;
  }
}

int
vtkSphericalHarmonicSource::
RequestData(vtkInformation *vtkNotUsed(request),vtkInformationVector **vtkNotUsed(inputVector),
            vtkInformationVector *outputVector)
{
  // Get the info and output objects.
  vtkInformation* outInfo = outputVector->GetInformationObject(0);
  vtkPolyData*    output  = vtkPolyData::SafeDownCast(outInfo->Get(vtkDataObject::DATA_OBJECT()));
  int N = this->sphereT->GetOutput()->GetNumberOfPoints();
  // SH Source stuff.
  vtkFloatArray* sValues = vtkFloatArray::New();
  sValues->SetNumberOfTuples(N);
  sValues->SetNumberOfComponents(1);

  /* Project data on the sphere:
    In matrix form:
    S := spherical function (discrete data on the sphere)
    C := Spherical harmonic coefficients
    B := Basis function
    S = CB
    */

  itk::VariableSizeMatrix<double> C(1,NumberOfSphericalHarmonics);

  for (int i=0;i<NumberOfSphericalHarmonics;++i){
    C(0,i) = SphericalHarmonics[i];
  }

  itk::VariableSizeMatrix<double> S/*(1,BasisFunction.GetVnlMatrix().size())*/;
  S = C*BasisFunction;

  if (Normalize) {

    double min = S.GetVnlMatrix().min_value();
    double max = S.GetVnlMatrix().max_value();

//    std::cout << "Normalizing by min/max" << min << "/" << max << "\n";

    if (max!=min) {
      for (unsigned i=0; i<S.Rows(); ++i)
        for (unsigned j=0; j<S.Cols(); ++j)
          S(i,j) -= min;
      S /= (max-min);
    } else {
      for (unsigned i=0; i<S.Rows(); ++i)
        for (unsigned j=0; j<S.Cols(); ++j)
          S(i,j) = 1.0;
    }
  }
  S *= this->Radius;

  for(int i=0; i<N; ++i) {
    if  (i%10000==0) {
      this->UpdateProgress ((vtkFloatingPointType)i/N);
      if (this->GetAbortExecute())
        break;
    }
    sValues->SetTuple1(i,S(0,i));
  }

  this->sphereT->GetOutput()->GetPointData()->SetScalars(sValues);
  sValues->Delete();

  // Don't know how to copy everything but the points.
  output->DeepCopy(this->sphereT->GetOutput());
  output->GetPoints()->Reset();

  TranslateAndDeformShell(this->sphereT->GetOutput(),output->GetPoints(),this->Center,Deform,RotationMatrix);

  return 1;
}

void vtkSphericalHarmonicSource::PrintSelf(ostream& os,vtkIndent indent)
{
  this->Superclass::PrintSelf(os,indent);

  os << indent << "Tessellation Order: " << Tesselation << std::endl;
  os << indent << "Tessellation Type: " << TesselationType << std::endl;
  os << indent << "Tessellation Basis: " << TesselationBasis << std::endl;

  os << indent << "SH Basis Order: " << Order << std::endl;
  os << indent << "Length of SH coefficient vector: "
     << NumberOfSphericalHarmonics << std::endl;
  os << indent << "SH Basis: " << NumberOfSphericalHarmonics << "x"
     << this->sphereT->GetOutput()->GetNumberOfPoints() << std::endl;
  os << indent << "SH Coefficients:" << std::endl << '[';
  for (int i=0;i<NumberOfSphericalHarmonics;++i)
    os << indent << SphericalHarmonics[i] << " ";
  os << indent << ']' << std::endl;
}

int vtkSphericalHarmonicSource::RequestInformation(vtkInformation*,vtkInformationVector**,
                                                   vtkInformationVector *outputVector)
{
  // Get the info object
  vtkInformation* outInfo = outputVector->GetInformationObject(0);

  outInfo->Set(vtkStreamingDemandDrivenPipeline::MAXIMUM_NUMBER_OF_PIECES(),-1);
  outInfo->Set(vtkStreamingDemandDrivenPipeline::WHOLE_BOUNDING_BOX(),
               this->Center[0]-this->Radius,this->Center[0]+this->Radius,
               this->Center[1]-this->Radius,this->Center[1]+this->Radius,
               this->Center[2]-this->Radius,this->Center[2]+this->Radius);

  return 1;
}

itk::Vector<double, 3> Cartesian2Spherical(const itk::Vector<double, 3> vITK)
{
  const double r =vITK.GetNorm();
  if (r==0) {
    std::cerr << "Cannot have a 0 radius in spherical coordinates!\n" << std::endl;
    exit(1);
  }
  // r, phi [0, PI), theta [0, PI) computation
  const double spherical [3] = { vITK.GetNorm(), atan2(vITK[1],vITK[0]), acos(vITK[2]/r)};
  const itk::Vector<double, 3> s (spherical);

  return s;
}

/*
  Compute SH matrix for discrete samplings on the sphere
  rank := number of spherical harmonics (rank of HOT in Reseach report 5681)
  n_s  := number of spherical values on the sphere (Research report 5681)
*/
itk::VariableSizeMatrix<double>
ComputeSHMatrix(const int order,vtkPolyData* shell,const bool FlipX,const bool FlipY,
                const bool FlipZ, itk::VariableSizeMatrix<double>& PhiThetaDirections)
{
  const int n_s   = shell->GetNumberOfPoints();
  const int rank =  (order+1)*(order+2)/2;
  itk::VariableSizeMatrix<double> B(rank,n_s);
  vtkPoints* vertices = shell->GetPoints();

  for (int i=0;i<n_s;++i) {
    // Get spherical component of the point direction and transform them in spherical coordinates.
    double p[3];
    vertices->GetPoint(i,p);

    itk::Vector<double, 3> dITK;
    dITK[0] = (FlipX) ? -p[0] : p[0];
    dITK[1] = (FlipY) ? -p[1] : p[1];
    dITK[2] = (FlipZ) ? -p[2] : p[2];

    const itk::Vector<double, 3> v = Cartesian2Spherical(dITK);

    const double phi   = v[1];
    const double theta = v[2];
    double temp=0;
    PhiThetaDirections(i,0) = phi;
    PhiThetaDirections(i,1) = theta;

    for (int l=0,j=0;l<=order;l+=2) {

      //  Handle the case m=0
      B(j,i) = sphLegendre(l,0,theta);
      j = j+1;

      for(int m=1,s=-1;m<=l;++m,++j,s=-s) {
        temp = sphLegendre(l, m,theta)*std::sqrt(2.0);

        //-m Real like t3 at hardi.cpp but math simplified and with tr1
        B(j,i) = s*temp*(cos(m*phi));

        //+m Imag like t3 at hardi.cpp but math simplified and with tr1
        B(++j,i) = temp*(sin(s*m*phi));
      }
    }
  }

  return B;
}

itk::VariableSizeMatrix<double>
ComputeSHMatrixMaxThesis(const int order,vtkPolyData *shell,const bool FlipX,const bool FlipY,
                         const bool FlipZ,itk::VariableSizeMatrix<double>& PhiThetaDirections)
{
  const int n_s   = shell->GetNumberOfPoints();
  const int rank =  (order+1)*(order+2)/2;
  itk::VariableSizeMatrix<double> B(rank,n_s);
  vtkPoints* vertices = shell->GetPoints();
  for (int i=0;i<n_s;++i) {

    // Get spherical component of the point direction and transform them in spherical coordinates.
    double p[3];
    vertices->GetPoint(i,p);

    itk::Vector<double, 3> dITK;
    dITK[0] = (FlipX) ? -p[0] : p[0];
    dITK[1] = (FlipY) ? -p[1] : p[1];
    dITK[2] = (FlipZ) ? -p[2] : p[2];

    const itk::Vector<double, 3> v = Cartesian2Spherical(dITK);
    const double phi   = v[1];// v.y;
    const double theta = v[2];//v.z;

    PhiThetaDirections(i,0) = phi;
    PhiThetaDirections(i,1) = theta;

    //  It is even nicer to compute the SH once (for m>0 and for m<0).
    //  The central term is given by the suite u_n
    //  TO
    double temp=0;
    for (int l=0,j=0;l<=order;l+=2,j+=2*l-1) {
      B(j,i) = sphLegendre(l,0,theta);

      for(int m=1,j1=j-1,j2=j+1;m<=l;++m,--j1,++j2) {
        temp = std::sqrt(2.0)*sphLegendre(l,m,theta);
        B(j1,i) = temp*cos(m*phi);
        B(j2,i) = temp*sin(m*phi);
      }
    }
  }

  return B;
}

itk::VariableSizeMatrix<double>
ComputeSHMatrixTournier(const int order,vtkPolyData *shell,const bool FlipX,const bool FlipY,
                        const bool FlipZ,itk::VariableSizeMatrix<double>& PhiThetaDirections)
{
  const int n_s   = shell->GetNumberOfPoints();
  const int rank =  (order+1)*(order+2)/2;
  /*
      We declare the Bmatrix of size n_s x n_b.
    */
  itk::VariableSizeMatrix<double> B(rank,n_s);

  vtkPoints* vertices = shell->GetPoints();
  std::complex<float>  cplx_1;

  for(int i = 0; i < n_s; i++) {
    // Get spherical component of the point direction and transform them in spherical coordinates.
    double p[3];
    vertices->GetPoint(i,p);

    itk::Vector<double, 3> dITK;
    dITK[0] = (FlipX) ? -p[0] : p[0];
    dITK[1] = (FlipY) ? -p[1] : p[1];
    dITK[2] = (FlipZ) ? -p[2] : p[2];

    const itk::Vector<double, 3> v = Cartesian2Spherical(dITK);
    const double phi   = v[1]; //v.y;
    const double theta = v[2]; //v.z;

    PhiThetaDirections(i,0) = phi;
    PhiThetaDirections(i,1) = theta;
    int j = 0;
    //counter for the j dimension of B
    //get spherical component of the direction vector
    double temp=0;

    for(int l = 0; l <= order; l+=2)
      for(int m = -l,s=1; m <= l; m++,s=-s) {
        temp = sphLegendre(l,std::abs(m),theta);
        if(m >= 0) { /* positive "m" SH */
          B(j,i) =  temp*cos(m*phi);;
        }
        else { /* negative "m" SH  */
          B(j,i) = s*temp*sin(m*phi);
        }
        j++;
      }
  }

  return B;
}

itk::VariableSizeMatrix<double>
ComputeSHMatrixRshBasis(const int order,vtkPolyData* shell,const bool FlipX,const bool FlipY,
                        const bool FlipZ,itk::VariableSizeMatrix<double>& PhiThetaDirections)
{
  const int n_s   = shell->GetNumberOfPoints();
  const int rank =  (order+1)*(order+2)/2;

  itk::VariableSizeMatrix<double> B(rank,n_s);
  vtkPoints* vertices = shell->GetPoints();

  for (int i=0;i<n_s;++i) {

    // Get spherical component of the point direction and transform them in spherical coordinates.
    double p[3];
    vertices->GetPoint(i,p);

    itk::Vector<double, 3> dITK;
    dITK[0] = (FlipX) ? -p[0] : p[0];
    dITK[1] = (FlipY) ? -p[1] : p[1];
    dITK[2] = (FlipZ) ? -p[2] : p[2];

    const itk::Vector<double, 3> v = Cartesian2Spherical(dITK);
    const double phi   = v[1]; //v.y;
    const double theta = v[2]; // v.z;
    PhiThetaDirections(i,0) = phi;
    PhiThetaDirections(i,1) = theta;
    double temp=0;

    for (int l=0,j=0;l<=order;l+=2) {
      B(j,i)=sphLegendre(l,0,theta);
      j=j+1;
      for(int m=1,s=-1;m<=l;++m,++j,s=-s) {
        temp = sphLegendre(l, m,theta)*std::sqrt(2.0);
        //-m Real like RshBasis.pdf Luke Bloy eq 1.2 but math simplified and with tr1
        B(j,i)   = temp*(cos(m*phi));
        //+m Imag
        B(++j,i) = temp*(sin(m*phi));
      }
    }
  }
  return B;
}

void vtkSphericalHarmonicSource::SetSphericalHarmonics(double* _arg)
{
  vtkDebugMacro(<< this->GetClassName() << " (" << this
                << "): setting Spherical Harmonics to " << _arg);
  this->SphericalHarmonics = _arg;
  this->Modified();
}

void vtkSphericalHarmonicSource::UpdateSphericalHarmonicSource()
{
  this->sphereT->SetPolyhedraType(TesselationType);
  this->sphereT->SetResolution(Tesselation);
  this->sphereT->Update();

  itk::VariableSizeMatrix<double>  PhiThetaDirection(this->sphereT->GetOutput()->GetNumberOfPoints(),2);

  switch (TesselationBasis) {
  case SHMatrix:
  {
    BasisFunction = ComputeSHMatrix(Order,this->sphereT->GetOutput(),FlipX,FlipY,FlipZ,PhiThetaDirection);
    break;
  }
  case SHMatrixMaxThesis:
  {
    BasisFunction = ComputeSHMatrixMaxThesis(Order,this->sphereT->GetOutput(),FlipX,FlipY,FlipZ,PhiThetaDirection);
    break;
  }
  case SHMatrixTournier:
  {
    BasisFunction = ComputeSHMatrixTournier(Order,this->sphereT->GetOutput(),FlipX,FlipY,FlipZ,PhiThetaDirection);
    break;
  }
  case SHMatrixRshBasis:
  {
    BasisFunction = ComputeSHMatrixRshBasis(Order,this->sphereT->GetOutput(),FlipX,FlipY,FlipZ,PhiThetaDirection);
    break;
  }
  }
  PhiThetaShellDirections = PhiThetaDirection;

}

void TranslateAndDeformShell(vtkPolyData *shell,vtkPoints* outPts,double center[3],
                             bool deform,vtkMatrix4x4* transform)
{
  vtkPoints* inPts = shell->GetPoints();
  const int  n     = inPts->GetNumberOfPoints();

//  double range[2];
//  shell->GetPointData()->GetScalars()->GetRange(range);
//  const double rangeDiff = (range[0]!=range[1]) ? range[1]-range[0] : 1.;

  vtkDataArray* sValues  = shell->GetPointData()->GetScalars();

  for (int i=0;i<n;++i) {
    double point[4];
    inPts->GetPoint(i,point);

    if (deform) {
      const double val = sValues->GetTuple1(i);
      point[0] = (val)*point[0];// ((val-range[0])/rangeDiff)*point[0];
      point[1] = (val)*point[1];//((val-range[0])/rangeDiff)*point[1];
      point[2] =(val)*point[2];// ((val-range[0])/rangeDiff)*point[2];
    }
    point[3] = 1.0;
    const double* pointOut = (transform!=0) ? transform->MultiplyDoublePoint(point) : &point[0];
    outPts->InsertNextPoint(pointOut[0]+center[0],pointOut[1]+center[1],pointOut[2]+center[2]);
  }
}