#ifndef TRIUMF_BNMR_SRF_NONLOCAL_HPP
#define TRIUMF_BNMR_SRF_NONLOCAL_HPP

// C++ standard library headers
#include <algorithm>
#include <cmath>
#include <limits>
#include <numeric>
#include <vector>

// Boost headers
#include <boost/math/interpolators/pchip.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>

// triumf++ headers
#include <triumf/bnmr/nuclei.hpp>
#include <triumf/nmr/dipole_dipole.hpp>
#include <triumf/nmr/nuclei.hpp>
#include <triumf/numpy.hpp>
#include <triumf/srim/pdf.hpp>
#include <triumf/superconductivity/bcs.hpp>
#include <triumf/superconductivity/pippard.hpp>

// ROOT headers
#include <ROOT/RCsvDS.hxx>
#include <ROOT/RDF/RInterface.hxx>
#include <ROOT/RDataFrame.hxx>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// β-detected nuclear magnetic resonance (β-NMR)
namespace bnmr {

// superconducting radio-frequency (SRF) materials
namespace srf {

// nonlocal electrodynamics
namespace nonlocal {

/// model slr rate
template <typename T = double>
T slr_rate_z(T z, T temperature, T critical_temperature, T gap_meV, T xi_0,
             T mean_free_path, T lambda_0, T exponent, T applied_field,
             T dipole_field, T correlation_rate, T slr_constant, T slr_exponent,
             T surface_thickness, T surface_rate, T electron_phonon_coupling) {
  // correct depth for the surface layer
  T _z_ = z - surface_thickness;
  if (_z_ < 0.0) {
    return surface_rate;
  } else {
    // convert to values in the weak coupling limit
    // see e.g., Eqs. (1) & (2) in:
    // http://dx.doi.org/10.1103/PhysRevB.87.104508
    T lambda_0_wc = xi_0 * electron_phonon_coupling;
    T xi_0_wc = lambda_0 / std::sqrt(electron_phonon_coupling);
    // calculate the local field from the screening profile
    T screened_field =
        temperature > critical_temperature
            ? applied_field
            : triumf::superconductivity::pippard::field_penetration<T>(
                  _z_, temperature, critical_temperature, gap_meV, xi_0_wc,
                  mean_free_path, lambda_0_wc, exponent, applied_field);
    // calculate the dipole-dipole SLR rate in the superconducting state
    T dd_rate = triumf::nmr::dipole_dipole::slr_rate<T>(
        screened_field, dipole_field, correlation_rate,
        triumf::bnmr::nuclei::lithium_8<T>::gyromagnetic_ratio(),
        triumf::nmr::nuclei::niobium_93<T>::gyromagnetic_ratio());
    // calculate the SLR rate in the normal state
    T ns_rate = slr_constant * std::pow(temperature, slr_exponent);
    // return the "surface" contribution at shallow depths
    return dd_rate + ns_rate;
  }
}

/// depth-resolved analyzer
template <typename T = double> class DepthResolvedAnalyzer {
public:
  /// constructor.
  DepthResolvedAnalyzer(const std::string &csv_filename) {
    // read the data into a ROOT DataFrame...
    auto df = ROOT::RDF::MakeCsvDataFrame(csv_filename);
    // ...and extract the values
    _energy = df.Take<T>("Energy (keV)").GetValue();
    _alpha = df.Take<T>("Alpha").GetValue();
    _alpha_error = df.Take<T>("Alpha Error").GetValue();
    _beta = df.Take<T>("Beta").GetValue();
    _beta_error = df.Take<T>("Beta Error").GetValue();
    _z_max = df.Take<T>("Max (nm)").GetValue();
    _z_max_error = df.Take<T>("Max Error (nm)").GetValue();

    // default initialized values
    temperature = 2.5;
    critical_temperature = 9.25;
    gap_meV =
        triumf::superconductivity::bcs::gap_meV<double>(critical_temperature);
    xi_0 = 39.0;
    mean_free_path = 1e4;
    lambda_0 = 40.0;
    exponent = 4.0;
    applied_field = 0.02;
    dipole_field = 1e-5;
    correlation_rate = 1.0 / 23.8e-6;
    slr_constant = 0.75;
    slr_exponent = 1.0;
    surface_thickness = 5.0;
    surface_rate = 10.0;
    electron_phonon_coupling = 1.0;
    //
    n_bins = 101;
  };

  /// Return the the minium energy available for interpolation.
  T energy_min() {
    return *std::min_element(_energy.begin(), _energy.end()) +
           std::sqrt(std::numeric_limits<T>::epsilon());
  };

  /// Return the the maximum energy availalbe for interpolation.
  T energy_max() {
    return *std::max_element(_energy.begin(), _energy.end()) -
           std::sqrt(std::numeric_limits<T>::epsilon());
  };

  /// Return an interpolated alpha value.
  T alpha(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>> alpha_interpolator(
        std::move(std::vector<T>(_energy)), std::move(std::vector<T>(_alpha)));
    return alpha_interpolator(energy_keV);
  };

  /// Return an interpolated beta value.
  T beta(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>> beta_interpolator(
        std::move(std::vector<T>(_energy)), std::move(std::vector<T>(_beta)));
    return beta_interpolator(energy_keV);
  };

  /// Return an interpolated z_max value.
  T z_max(T energy_keV) {
    static boost::math::interpolators::pchip<std::vector<T>> z_max_interpolator(
        std::move(std::vector<T>(_energy)), std::move(std::vector<T>(_z_max)));
    return z_max_interpolator(energy_keV);
  };

  /// Return the average implantation depth.
  T z_average(T energy_keV) {
    T a = alpha(energy_keV);
    T b = beta(energy_keV);
    T zm = z_max(energy_keV);
    return zm * a / (a + b);
  };

  /*
  /// depth-averaging using numeric integration
  T operator()(T energy_keV) {
    // T a = alpha(energy_keV);
    // T b = beta(energy_keV);
    // T zm = z_max(energy_keV);
    static boost::math::quadrature::tanh_sinh<T> integrator;
    auto integrand = [&](T z) {
      return slr_fcn(z, GLOBAL_R_0, GLOBAL_D_0) *
             triumf::srim::pdf::modified_beta<T>(
                 z, alpha(energy_keV), beta(energy_keV), z_max(energy_keV));
    };
    T Q = integrator.integrate(integrand, 0.0, z_max(energy_keV));
    return Q;
  };
  */

  // depth-averaging using "histogram" summation
  T operator()(T energy_keV) {
    // bin edges
    std::vector<T> z_edge =
        triumf::numpy::linspace<T>(0.0, z_max(energy_keV), n_bins);
    // bin widths - adjust ranges by one for "correct" size
    std::vector<T> dz(n_bins - 1);
    std::adjacent_difference(std::begin(z_edge) + 1, std::end(z_edge),
                             std::begin(dz));
    // bin centres
    std::vector<T> z(n_bins - 1);
    // probabilities
    std::vector<T> p_z(n_bins - 1);
    std::vector<T> weights(n_bins - 1);
    std::vector<T> slr_rates(n_bins - 1);

    for (std::size_t i = 0; i < z.size(); ++i) {
      z.at(i) = z_edge.at(i) + 0.5 * dz.at(i);
      p_z.at(i) = triumf::srim::pdf::modified_beta<T>(
          z.at(i), alpha(energy_keV), beta(energy_keV), z_max(energy_keV));
      // std::cout << "p(" << z.at(i) << " nm ) = " << p_z.at(i) << " nm^-1\n";
      weights.at(i) = dz.at(i) * p_z.at(i);
      slr_rates.at(i) = slr_rate_z<T>(
          z.at(i), temperature, critical_temperature, gap_meV, xi_0,
          mean_free_path, lambda_0, exponent, applied_field, dipole_field,
          correlation_rate, slr_constant, slr_exponent, surface_thickness,
          surface_rate, electron_phonon_coupling);
    }

    T sum_weights = std::reduce(std::begin(weights), std::end(weights), 0.0);
    // std::cout << "sum_weights = " << sum_weights << "\n";
    T sum_weights_slr = std::transform_reduce(
        std::begin(weights), std::end(weights), std::begin(slr_rates), 0.0);
    // std::cout << "sum_weights_slr = " << sum_weights_slr << "\n";
    T weighted_average = sum_weights_slr / sum_weights;
    return weighted_average;
  };

  /// model parameters
  T temperature;
  T critical_temperature;
  T gap_meV;
  T xi_0;
  T mean_free_path;
  T lambda_0;
  T exponent;
  T applied_field;
  T dipole_field;
  T correlation_rate;
  T slr_constant;
  T slr_exponent;
  T surface_thickness;
  T surface_rate;
  T electron_phonon_coupling;

private:
  /// vectors of data from csv file
  std::vector<T> _energy;
  std::vector<T> _alpha;
  std::vector<T> _alpha_error;
  std::vector<T> _beta;
  std::vector<T> _beta_error;
  std::vector<T> _z_max;
  std::vector<T> _z_max_error;
  /// number of bins + 1 used in the "histogram" summation
  std::size_t n_bins;
};

} // namespace nonlocal

} // namespace srf

} // namespace bnmr

} // namespace triumf

#endif // TRIUMF_BNMR_SRF_NONLOCAL_HPP


/*=============================================================================
Copyright (c) 2016 Paul W. Bible

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

#include <ggtk/SharedInformationInterface.hpp>
#include <ggtk/TermInformationContentMap.hpp>
#include <ggtk/GoGraph.hpp>
#include <ggtk/Accumulators.hpp>
#include <ggtk/SetUtilities.hpp>

#include <utility>
#include <algorithm>

#include <boost/unordered_map.hpp>
#include <boost/graph/breadth_first_search.hpp>

/*! \class CoutoGraSMSharedInformation
	\brief A class to calculate shared infromation accross disjoint common ancetors using the exact algorithm as written in the paper.

	This class calculates shared infromation accross disjoint common ancetors.

    F. M. Couto, M. J. Silva, and P. M. Coutinho, "Measuring semantic similarity
	between Gene Ontology terms," Data & Knowledge Engineering, vol. 61,
	pp. 137-152, Apr 2007.

	Couto proposing calculating this value a subsituite for the IC of the MICA in calculating
	 Resnik, Lin, and Jiang-Conrath

*/
class CoutoGraSMSharedInformation : public SharedInformationInterface{

public:

	//! A constructor
	/*!
		Creates the CoutoGraSMGreaterOrEqual class
	*/
	inline CoutoGraSMSharedInformation(GoGraph* goGraph, TermInformationContentMap &icMap){
		_goGraph = goGraph;
		_icMap = icMap;
		_pathMemory = boost::unordered_map<std::string, size_t>();
	}

	//! A method for determining the common disjunctive ancestors
	/*!
		This method returns the common disjunctive ancestors for two terms
	*/
	inline boost::unordered_set<std::string> getCommonDisjointAncestors(const std::string &termC1,const std::string &termC2){

		boost::unordered_set<std::string> ancestorsC1 = _goGraph->getAncestorTerms(termC1);
		ancestorsC1.insert(termC1);
		//std::cout << ancestorsC1.size() << std::endl;
		boost::unordered_set<std::string> ancestorsC2 = _goGraph->getAncestorTerms(termC2);
		ancestorsC2.insert(termC2);
		//std::cout << ancestorsC2.size() << std::endl;

		//Couto: CommonDisjAnc = {}
		boost::unordered_set<std::string> cda;

		if(termC1.compare(termC2) == 0){
			cda.insert(termC1);
			return cda;
		}

		//Couto: Anc = CommonAnc(c1,c2)
		boost::unordered_set<std::string> commonAncestors = SetUtilities::set_intersection(ancestorsC1,ancestorsC2);
		//std::cout << commonAncestors.size() << std::endl;

		std::vector<std::pair<double,std::string> > orderedCommonAncestors;

		//create a pair to associate a term with its information content
		boost::unordered_set<std::string>::iterator iter;
		for(iter = commonAncestors.begin(); iter != commonAncestors.end(); ++iter){
			std::string term = *iter;
			orderedCommonAncestors.push_back(std::pair<double,std::string>(_icMap[term],term));
		}

		//sort descending
		std::sort(orderedCommonAncestors.begin(),orderedCommonAncestors.end(),std::greater<std::pair<double,std::string> >());

		//start of main algorithm
		std::vector<std::pair<double,std::string> >::iterator pairIter;
		//Couto: for all a in sortDescByIC(Anc) do ...
		for(pairIter = orderedCommonAncestors.begin(); pairIter != orderedCommonAncestors.end(); ++pairIter){
			std::pair<double,std::string> myPair = *pairIter;
			//std::cout << myPair.first << " " << myPair.second << std::endl;

			std::string termA = myPair.second;

			//Couto: isDisj=true
			bool isDisj = true;

			//std::cout << "testing " << termA << std::endl;

			//Couto: for all cda in CommonDisjAnc do ...
			boost::unordered_set<std::string>::iterator cdaIter;
			for(cdaIter = cda.begin(); cdaIter != cda.end();++cdaIter){
				std::string termCda = *cdaIter;

				//std::cout << "VS " << termCda << std::endl;

				//continue if the terms are the same
				if(termCda.compare(termA) == 0){
					continue;
				}

				//Couto: isDisj = isDisj ^ ( DisjAnc(c1,(a,cda)) or DisjAnc(c2,(a,cda)) )
				isDisj = isDisj && (isDisjoint(termC1,termA,termCda) || isDisjoint(termC2,termA,termCda));

			}

			//Couto: if isDisj then...
			if(isDisj){
				//std::cout << myPair.second << " is cda " << std::endl;
				//Couto: addTo(CommonDisjAnc,a)
				cda.insert(myPair.second);
			}
		}
		return cda;
	}

	//! A method for determining if for a term c, a pair (a1,a2) is disjoint in c
	/*!
		This method returns
	*/
	inline bool isDisjoint(const std::string &termC, const std::string &termA1, const std::string &termA2){

		//std::cout << "isDisjoint " << termC << " ("  << termA1 << " , " << termA2 << ") "; //<< std::endl;
		//if not from same ontology, return 0;
		if(_goGraph->getTermOntology(termA1) != _goGraph->getTermOntology(termA2) ||
		   _goGraph->getTermOntology(termC) != _goGraph->getTermOntology(termA1) ||
		   _goGraph->getTermOntology(termC) != _goGraph->getTermOntology(termA2))
		{
			return false;
		}

		if(_icMap[termA1] <= _icMap[termA2]){
			//std::cout << "case 1" << std::endl;
			size_t nPaths = getNumPaths(termA1,termA2);
			//std::cout << "nPaths " << termA1 << " to "  << termA2 << " " << nPaths << std::endl << std::endl;
			size_t nPaths1 = getNumPaths(termA1,termC);
			//std::cout << "nPaths " << termA1 << " to "  << termC << " " << nPaths1 << std::endl << std::endl;
			size_t nPaths2 = getNumPaths(termA2,termC);
			//std::cout << "nPaths " << termA2 << " to "  << termC << " " << nPaths2 << std::endl << std::endl;
			if(nPaths1 >= nPaths*nPaths2){
				//std::cout << "true" << std::endl;
				return true;
			}else{
				//std::cout << "false" << std::endl;
				return false;
			}
			//return nPaths1 > nPaths*nPaths2;
		}else{
			return false;
		}
	}

	//! A method for calculating the number of paths for one term to another.
	/*!
		This method returns the number of paths between two terms
	*/
	inline std::size_t getNumPaths(const std::string &termA, const std::string &termB){
		if(_icMap[termA] > _icMap[termB]){
			return 0;
		}

		return pathCount(termA, termB);
	}

	//! An method for returning the shared information of two terms
	/*!
		This method returns the mean information content disjoint common ancestors
	*/
	inline double sharedInformation(const std::string &termA, const std::string &termB){
		// return 0 for any terms not in the datbase
		if (!_icMap.hasTerm(termA) || !_icMap.hasTerm(termB)){
			return 0.0;
		}
		// return 0 for terms in different ontologies
		if (_goGraph->getTermOntology(termA) != _goGraph->getTermOntology(termB)){
			return 0.0;
		}

		Accumulators::MeanAccumulator meanIC;
		boost::unordered_set<std::string> cda = getCommonDisjointAncestors(termA,termB);
		//std::cout << "size " << cda.size() << std::endl;

		boost::unordered_set<std::string>::iterator iter = cda.begin();
		for(;iter != cda.end(); ++iter){
			//std::cout << *iter << std::endl;
			//std::cout << _icMap[*iter] << std::endl;
			meanIC(_icMap[*iter]);
		}

		return Accumulators::extractMean(meanIC);
	}

	//! An interface method for returning the shared information of a single terms,or information content
	/*!
		This method privdes a mechanism for returing a term's infromation content.
	*/
	inline double sharedInformation(const std::string &term){
		// return 0 for any terms not in the datbase
		if (!_icMap.hasTerm(term)){
			return 0.0;
		}
		return _icMap[term];
	}

	//! An interface method for returning the maximum information content for a term
	/*!
		This method provides the absolute max information content within a corpus for normalization purposes.
	*/
	inline double maxInformationContent(const std::string &term){

		double maxIC;

		//select the correct ontology normalization factor
		GO::Onto ontoType = _goGraph->getTermOntology(term);
		if(ontoType == GO::BP){
			maxIC = -std::log(_icMap.getMinBP());
		}else if(ontoType == GO::MF){
			maxIC = -std::log(_icMap.getMinMF());
		}else{
			maxIC = -std::log(_icMap.getMinCC());
		}

		return maxIC;
	}

	//! An interface method for determining if a term can be found
	/*!
		Determines if the term can be found in the current map.
	*/
	inline bool hasTerm(const std::string &term){
		return _icMap.hasTerm(term);
	}

	//! An interface method for determining if the two terms are of like ontologies.
	/*!
		Determine if two terms are of the same ontology.
	*/
	bool isSameOntology(const std::string &termA, const std::string &termB){
		return _goGraph->getTermOntology(termA) == _goGraph->getTermOntology(termB);
	}

private:

	//! Count paths from B to A
	/*!
		Count paths between B and A
	*/
	std::size_t pathCount(const std::string &termA, const std::string &termB){
		if (_icMap[termA] > _icMap[termB]){
			return 0;
		}

		boost::unordered_set<std::string> ancestors = _goGraph->getAncestorTerms(termB);
		boost::unordered_set<std::string> finished;
		boost::unordered_map<std::string, size_t> pathMap;
		ancestors.insert(termB);
		GoGraph::Graph* g = _goGraph->getGraph();
		GoGraph::GoVertex v = _goGraph->getTermRootVertex(termB);
		visitHelper(v, g, ancestors, finished, pathMap);

		return pathMap[termA];
	}

	//! Recursive helper method that performs the DFS topological sort for path counting
	/*!
		A path counting topological sort recursive method.
	*/
	void visitHelper(const GoGraph::GoVertex &v, GoGraph::Graph* g,
		boost::unordered_set<std::string> &ancestors,
		boost::unordered_set<std::string> &finished,
		boost::unordered_map<std::string, size_t> &pathMap)
	{
		size_t childCount = 0;
		std::string vTerm = (*g)[v].termId;
		//std::cout << "discover vertex " << vTerm << std::endl;

		//examine children and recurse
		GoGraph::InEdgeIterator it, end;
		for (boost::tie(it, end) = boost::in_edges(v, *g); it != end; ++it){
			GoGraph::GoVertex child = boost::source(*it, *g);
			std::string childTerm = (*g)[child].termId;
			if (!SetUtilities::set_contains(ancestors, childTerm)){
				continue;
			}
			//recurse if child is not finished
			if (!SetUtilities::set_contains(finished, childTerm)){
				visitHelper(child, g, ancestors, finished, pathMap);
			}
			++childCount;
		}

		//finish vertex
		finished.insert(vTerm);
		//std::cout << "finish vertex " << vTerm << ", childred " << childCount << std::endl;
		if (childCount == 0){
			pathMap[vTerm] = 1;
		}
		else{
			pathMap[vTerm] = 0;
			for (boost::tie(it, end) = boost::in_edges(v, *g); it != end; ++it){
				GoGraph::GoVertex child = boost::source(*it, *g);
				std::string childTerm = (*g)[child].termId;
				if (!SetUtilities::set_contains(ancestors, childTerm)){
					continue;
				}
				pathMap[vTerm] += pathMap[childTerm];
			}
		}
	}

	//! A private function to create a string key from a pair of terms
	/*!
		Creates a string key our of a pair to use in memorizing path counts
	*/
	std::string keyPair(const std::string &termA, const std::string &termB){
		if (termA.compare(termB) > 0){
			return termB + "_" + termA;
		}
		else{
			return termB + "_" + termA;
		}
	}

	//! A private function to test if the key as been seen already
	/*!
		A private function to test if the key as been seen already.
	*/
	bool hasSeenKey(const std::string &key){
		if (_pathMemory.find(key) != _pathMemory.end()){
			return true;
		}
		else{
			return false;
		}
	}

	GoGraph* _goGraph;
	TermInformationContentMap _icMap;
	boost::unordered_map<std::string, size_t> _pathMemory;
};
#endif


#include <map>
#include <set>
#include <utility>
#include <vector>

#include <Eigen/Core>

namespace from_igl {

  using namespace igl;

// Dijstra's algorithm for shortest paths, with multiple targets.
// Adapted from http://rosettacode.org/wiki/Dijkstra%27s_algorithm .
//
// Inputs:
//   source           index of source vertex
//   targets          target vector set
//   w                weight of edge corresponding to VV[i][j]
//   VV               #V list of lists of incident vertices (adjacency list), e.g.
//                    as returned by igl::adjacency_list
//
// Output:
//   min_distance     #V by 1 list of the minimum distances from source to all vertices
//   previous         #V by 1 list of the previous visited vertices (for each vertex) - used for backtracking
//
int dijkstra_compute_paths(const int &source,
                           const std::set<int> &targets,
                           const std::vector<std::vector<int> >& VV,
                           const std::vector<std::vector<double> > &w,
                           Eigen::VectorXd &min_distance,
                           Eigen::VectorXi &previous)
{
  int numV = VV.size();
  min_distance.setConstant(numV, 1, std::numeric_limits<double>::infinity());
  min_distance[source] = 0;
  previous.setConstant(numV, 1, -1);
  std::set<std::pair<double, int> > vertex_queue;
  vertex_queue.insert(std::make_pair(min_distance[source], source));

  while (!vertex_queue.empty())
  {
    double dist = vertex_queue.begin()->first;
    int u = vertex_queue.begin()->second;
    vertex_queue.erase(vertex_queue.begin());

    if (targets.find(u)!= targets.end())
      return u;

    // Visit each edge exiting u
    const std::vector<int> &neighbors = VV[u];
    for (int i = 0; i < neighbors.size(); ++i)
    {
      int v = neighbors[i];
      double distance_through_u = dist + w[u][i];

      if (distance_through_u < min_distance[v]) {
        vertex_queue.erase(std::make_pair(min_distance[v], v));

        min_distance[v] = distance_through_u;
        previous[v] = u;
        vertex_queue.insert(std::make_pair(min_distance[v], v));
      }

    }
  }
  //we should never get here
  return -1;
}
} //from_igl


/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
 *                                                                            *
 * Redistribution and use in source and binary forms, with or without         *
 * modification, are permitted provided that the following conditions         *
 * are met:                                                                   *
 * * Redistributions of source code must retain the above copyright           *
 *   notice, this list of conditions and the following disclaimer.            *
 * * Redistributions in binary form must reproduce the above copyright        *
 *   notice, this list of conditions and the following disclaimer in the      *
 *   documentation and/or other materials provided with the distribution.     *
 * * Neither the name of ANU nor the names of its contributors may be         *
 *   used to endorse or promote products derived from this software without   *
 *   specific prior written permission.                                       *
 *                                                                            *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE  *
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
 * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE        *
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT         *
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY  *
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF     *
 * SUCH DAMAGE.                                                               *
 ******************************************************************************/

#include <opengv/absolute_pose/methods.hpp>
#include <opengv/Indices.hpp>

#include <Eigen/NonLinearOptimization>
#include <Eigen/NumericalDiff>

#include <opengv/absolute_pose/modules/main.hpp>
#include <opengv/absolute_pose/modules/Epnp.hpp>
#include <opengv/OptimizationFunctor.hpp>
#include <opengv/math/cayley.hpp>
#include <opengv/math/quaternion.hpp>
#include <opengv/math/roots.hpp>

#include <iostream>

opengv::translation_t
opengv::absolute_pose::p2p(
    const AbsoluteAdapterBase & adapter,
    const std::vector<int> & indices )
{
  assert(indices.size()>1);
  return p2p( adapter, indices[0], indices[1] );
}

opengv::translation_t
opengv::absolute_pose::p2p(
    const AbsoluteAdapterBase & adapter,
    size_t index0,
    size_t index1)
{
  Eigen::Vector3d e1 = adapter.getBearingVector(index0);
  Eigen::Vector3d e3 = adapter.getBearingVector(index1);
  e3 = e1.cross(e3);
  e3 = e3/e3.norm();
  Eigen::Vector3d e2 = e3.cross(e1);

  rotation_t T;
  T.row(0) = e1.transpose();
  T.row(1) = e2.transpose();
  T.row(2) = e3.transpose();

  Eigen::Vector3d n1 = adapter.getPoint(index1) - adapter.getPoint(index0);
  n1 = n1/n1.norm();
  Eigen::Vector3d n3;
  if( (fabs(n1[0]) > fabs(n1[1])) && (fabs(n1[0]) > fabs(n1[2])) )
  {
    n3[1] = 1.0;
    n3[2] = 0.0;
    n3[0] = -n1[1]/n1[0];
  }
  else
  {
    if( (fabs(n1[1]) > fabs(n1[0])) && (fabs(n1[1]) > fabs(n1[2])) )
    {
      n3[2] = 1.0;
      n3[0] = 0.0;
      n3[1] = -n1[2]/n1[1];
    }
    else
    {
      n3[0] = 1.0;
      n3[1] = 0.0;
      n3[2] = -n1[0]/n1[2];
    }
  }
  n3 = n3 / n3.norm();
  Eigen::Vector3d n2 = n3.cross(n1);

  rotation_t N;
  N.row(0) = n1.transpose();
  N.row(1) = n2.transpose();
  N.row(2) = n3.transpose();

  Eigen::Matrix3d Q = T * adapter.getR().transpose() * N.transpose();
  Eigen::Vector3d temp1 = adapter.getPoint(index1) - adapter.getPoint(index0);
  double d_12 = temp1.norm();

  Eigen::Vector3d temp2 = adapter.getBearingVector(index1);
  double cos_beta = e1.dot(temp2);
  double b = 1/( 1 - pow( cos_beta, 2 ) ) - 1;

  if( cos_beta < 0 )
    b = -sqrt(b);
  else
    b = sqrt(b);

  double temp3 = d_12 * ( Q(1,0) * b - Q(0,0) );

  translation_t solution = -temp3 * Q.row(0).transpose();
  solution = adapter.getPoint(index0) + N.transpose()*solution;

  if(
    solution(0,0) != solution(0,0) ||
    solution(1,0) != solution(1,0) ||
    solution(2,0) != solution(2,0) )
    solution = Eigen::Vector3d::Zero();

  return solution;
}

opengv::transformations_t
opengv::absolute_pose::p3p_kneip(
    const AbsoluteAdapterBase & adapter,
    const std::vector<int> & indices )
{
  assert(indices.size()>2);
  return p3p_kneip( adapter, indices[0], indices[1], indices[2] );
}

opengv::transformations_t
opengv::absolute_pose::p3p_kneip(
    const AbsoluteAdapterBase & adapter,
    size_t index0,
    size_t index1,
    size_t index2)
{
  bearingVectors_t f;
  f.push_back(adapter.getBearingVector(index0));
  f.push_back(adapter.getBearingVector(index1));
  f.push_back(adapter.getBearingVector(index2));
  points_t p;
  p.push_back(adapter.getPoint(index0));
  p.push_back(adapter.getPoint(index1));
  p.push_back(adapter.getPoint(index2));
  transformations_t solutions;
  modules::p3p_kneip_main( f, p, solutions );
  return solutions;
}

opengv::transformations_t
opengv::absolute_pose::p3p_gao(
    const AbsoluteAdapterBase & adapter,
    const std::vector<int> & indices )
{
  assert(indices.size()>2);
  return p3p_gao( adapter, indices[0], indices[1], indices[2] );
}

opengv::transformations_t
opengv::absolute_pose::p3p_gao(
    const AbsoluteAdapterBase & adapter,
    size_t index0,
    size_t index1,
    size_t index2)
{
  bearingVectors_t f;
  f.push_back(adapter.getBearingVector(index0));
  f.push_back(adapter.getBearingVector(index1));
  f.push_back(adapter.getBearingVector(index2));
  points_t p;
  p.push_back(adapter.getPoint(index0));
  p.push_back(adapter.getPoint(index1));
  p.push_back(adapter.getPoint(index2));
  transformations_t solutions;
  modules::p3p_gao_main( f, p, solutions );
  return solutions;
}

opengv::transformations_t
opengv::absolute_pose::gp3p(
    const AbsoluteAdapterBase & adapter,
    const std::vector<int> & indices )
{
  assert(indices.size()>2);

  Eigen::Matrix3d f;
  Eigen::Matrix3d v;
  Eigen::Matrix3d p;

  for(size_t i = 0; i < 3; i++)
  {
    f.col(i) = adapter.getBearingVector(indices[i]);
    rotation_t R = adapter.getCamRotation(indices[i]);

    //unrotate the bearingVectors already so the camera rotation doesn't appear
    //in the problem
    f.col(i) = R * f.col(i);
    v.col(i) = adapter.getCamOffset(indices[i]);
    p.col(i) = adapter.getPoint(indices[i]);
  }

  transformations_t solutions;
  modules::gp3p_main(f,v,p,solutions);

  return solutions;
}

opengv::transformations_t
opengv::absolute_pose::gp3p(
    const AbsoluteAdapterBase & adapter,
    size_t index0,
    size_t index1,
    size_t index2)
{
  std::vector<int> indices;
  indices.push_back(index0);
  indices.push_back(index1);
  indices.push_back(index2);

  return gp3p(adapter,indices);
}

namespace opengv
{
namespace absolute_pose
{

transformation_t epnp(
    const AbsoluteAdapterBase & adapter,
    const Indices & indices )
{
  //starting from 4 points, we have a unique solution
  assert(indices.size() > 5);

  modules::Epnp PnP;
  PnP.set_maximum_number_of_correspondences(indices.size());
  PnP.reset_correspondences();

  for( size_t i = 0; i < indices.size(); i++ )
  {
    point_t p = adapter.getPoint(indices[i]);
    bearingVector_t f = adapter.getBearingVector(indices[i]);
    PnP.add_correspondence(p[0], p[1], p[2], f[0], f[1], f[2]);
  }

  double R_epnp[3][3], t_epnp[3];
  PnP.compute_pose(R_epnp, t_epnp);

  rotation_t rotation;
  translation_t translation;

  for(int r = 0; r < 3; r++)
  {
    for(int c = 0; c < 3; c++)
      rotation(r,c) = R_epnp[r][c];
  }

  translation[0] = t_epnp[0];
  translation[1] = t_epnp[1];
  translation[2] = t_epnp[2];

  //take inverse transformation
  rotation.transposeInPlace();
  translation = -rotation * translation;

  transformation_t transformation;
  transformation.col(3) = translation;
  transformation.block<3,3>(0,0) = rotation;
  return transformation;
}

}
}

opengv::transformation_t
opengv::absolute_pose::epnp( const AbsoluteAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return epnp(adapter,idx);
}

opengv::transformation_t
opengv::absolute_pose::epnp(
    const AbsoluteAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return epnp(adapter,idx);
}

namespace opengv
{
namespace absolute_pose
{

transformation_t gpnp(
    const AbsoluteAdapterBase & adapter,
    const Indices & indices )
{
  assert( indices.size() > 5 );

  //compute the centroid
  point_t c0 = Eigen::Vector3d::Zero();
  for( size_t i = 0; i < indices.size(); i++ )
    c0 = c0 + adapter.getPoint(indices[i]);
  c0 = c0 / indices.size();

  //compute the point-cloud
  Eigen::MatrixXd p(3,indices.size());
  for( size_t i = 0; i < indices.size(); i++ )
    p.col(i) = adapter.getPoint(indices[i]) - c0;

  //compute the moment
  Eigen::JacobiSVD< Eigen::MatrixXd > SVD(
      p,
      Eigen::ComputeThinU | Eigen::ComputeThinV );

  //define the control points
  points_t c;
  c.push_back(c0);
  //c.push_back(c0 + SVD.singularValues()[0] * SVD.matrixU().col(0));
  //c.push_back(c0 + SVD.singularValues()[1] * SVD.matrixU().col(1));
  //c.push_back(c0 + SVD.singularValues()[2] * SVD.matrixU().col(2));
  c.push_back(c0 + 15.0 * SVD.matrixU().col(0));
  c.push_back(c0 + 15.0 * SVD.matrixU().col(1));
  c.push_back(c0 + 15.0 * SVD.matrixU().col(2));

  //derive the barycentric frame
  Eigen::Vector3d e1 = c[1]-c0;
  double e1dote1 = e1.dot(e1);
  Eigen::Vector3d e2 = c[2]-c0;
  double e2dote2 = e2.dot(e2);
  Eigen::Vector3d e3 = c[3]-c0;
  double e3dote3 = e3.dot(e3);

  //derive the weighting factors
  Eigen::MatrixXd weights(4,indices.size());
  for( size_t i = 0; i < indices.size(); i++ )
  {
    Eigen::Vector3d temp = p.col(i);
    weights(1,i) = temp.dot(e1)/e1dote1;
    weights(2,i) = temp.dot(e2)/e2dote2;
    weights(3,i) = temp.dot(e3)/e3dote3;
    weights(0,i) = 1.0-(weights(1,i)+weights(2,i)+weights(3,i));
  }

  //setup matrix A and vector b
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(2*indices.size(),12);
  Eigen::MatrixXd b = Eigen::MatrixXd::Zero(2*indices.size(),1);
  for( size_t i = 0; i < indices.size(); i++ )
  {
    translation_t camOffset = adapter.getCamOffset(indices[i]);
    rotation_t camRotation = adapter.getCamRotation(indices[i]);
    //respect the rotation
    bearingVector_t f = camRotation * adapter.getBearingVector(indices[i]);

    A(2*i,0)  =  weights(0,i)*f[2];
    A(2*i,2)  = -weights(0,i)*f[0];
    A(2*i,3)  =  weights(1,i)*f[2];
    A(2*i,5)  = -weights(1,i)*f[0];
    A(2*i,6)  =  weights(2,i)*f[2];
    A(2*i,8)  = -weights(2,i)*f[0];
    A(2*i,9)  =  weights(3,i)*f[2];
    A(2*i,11) = -weights(3,i)*f[0];

    A(2*i+1,1)  =  weights(0,i)*f[2];
    A(2*i+1,2)  = -weights(0,i)*f[1];
    A(2*i+1,4)  =  weights(1,i)*f[2];
    A(2*i+1,5)  = -weights(1,i)*f[1];
    A(2*i+1,7)  =  weights(2,i)*f[2];
    A(2*i+1,8)  = -weights(2,i)*f[1];
    A(2*i+1,10) =  weights(3,i)*f[2];
    A(2*i+1,11) = -weights(3,i)*f[1];

    b(2*i,0)   = f[2]*camOffset[0]-f[0]*camOffset[2];
    b(2*i+1,0) = f[2]*camOffset[1]-f[1]*camOffset[2];
  }

  //computing the SVD
  Eigen::JacobiSVD< Eigen::MatrixXd > SVD2(
      A,
      Eigen::ComputeThinV | Eigen::ComputeThinU );

  //computing the pseudoinverse
  Eigen::MatrixXd invD = Eigen::MatrixXd::Zero(12,12);
  Eigen::MatrixXd D = SVD2.singularValues();
  for( size_t i = 0; i < 12; i++ )
  {
    if( D(i,0) > 1.e-6 )
      invD(i,i) = 1.0/D(i,0);
    else
      invD(i,i) = 0.0;
  }

  //Extract the nullsapce vectors;
  Eigen::MatrixXd V = SVD2.matrixV();

  //computing the nullspace intercept
  Eigen::MatrixXd pinvA = V * invD * SVD2.matrixU().transpose();

  //compute the intercept
  Eigen::Matrix<double,12,1> a = pinvA * b;

  //compute the solution
  transformation_t transformation;
  modules::gpnp_main( a, V, c, transformation );
  return transformation;
}

}
}

opengv::transformation_t
opengv::absolute_pose::gpnp( const AbsoluteAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return gpnp(adapter,idx);
}

opengv::transformation_t
opengv::absolute_pose::gpnp(
    const AbsoluteAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return gpnp(adapter,idx);
}

namespace opengv
{
namespace absolute_pose
{

void fill3x10( const Eigen::Vector3d & x, Eigen::Matrix<double,3,10> & Phi )
{
  double x1 = x[0];
  double x2 = x[1];
  double x3 = x[2];

  Phi << x1,  x1, -x1, -x1,     0.0,  2.0*x3, -2.0*x2, 2.0*x2, 2.0*x3,    0.0,
         x2, -x2,  x2, -x2, -2.0*x3,     0.0,  2.0*x1, 2.0*x1,    0.0, 2.0*x3,
         x3, -x3, -x3,  x3,  2.0*x2, -2.0*x1,     0.0,    0.0, 2.0*x1, 2.0*x2;
}

void f(
    const Eigen::Matrix<double,10,10> & M,
    const Eigen::Matrix<double,1,10> & C,
    double gamma,
    Eigen::Vector3d & f )
{
  f[0] = (2*M(0,4)+2*C(0,4));
  f[1] = (2*M(0,5)+2*C(0,5));
  f[2] = (2*M(0,6)+2*C(0,6));
}

void Jac(
    const Eigen::Matrix<double,10,10> & M,
    const Eigen::Matrix<double,1,10> & C,
    double gamma,
    Eigen::Matrix3d & Jac )
{
  Jac(0,0) = (2*M(4,4)+4*M(0,1)-4*M(0,0)+4*C(0,1)-4*C(0,0));
  Jac(0,1) = (2*M(5,4)+2*M(0,7)+2*C(0,7));
  Jac(0,2) = (2*M(6,4)+2*M(0,8)+2*C(0,8));
  Jac(1,0) = (2*M(4,5)+2*M(0,7)+2*C(0,7));
  Jac(1,1) = (2*M(5,5)+4*M(0,2)-4*M(0,0)+4*C(0,2)-4*C(0,0));
  Jac(1,2) = (2*M(6,5)+2*M(0,9)+2*C(0,9));
  Jac(2,0) = (2*M(4,6)+2*M(0,8)+2*C(0,8));
  Jac(2,1) = (2*M(5,6)+2*M(0,9)+2*C(0,9));
  Jac(2,2) = (2*M(6,6)+4*M(0,3)-4*M(0,0)+4*C(0,3)-4*C(0,0));
}

transformations_t upnp(
    const AbsoluteAdapterBase & adapter,
    const Indices & indices )
{
  assert( indices.size() > 2 );

  Eigen::Matrix<double,3,3> F = Eigen::Matrix3d::Zero();
  for( int i = 0; i < (int) indices.size(); i++ )
  {
    Eigen::Matrix<double,3,1> f = adapter.getCamRotation(indices[i]) * adapter.getBearingVector(indices[i]);
    F += f * f.transpose();
  }

  Eigen::Matrix<double,3,3> H_inv = (indices.size() * Eigen::Matrix<double,3,3>::Identity()) - F;
  Eigen::Matrix<double,3,3> H = H_inv.inverse();

  Eigen::Matrix<double,3,10> I = Eigen::Matrix<double,3,10>::Zero();
  Eigen::Matrix<double,3,1> J = Eigen::Matrix<double,3,1>::Zero();
  Eigen::Matrix<double,3,10> Phi;

  for( int i = 0; i < (int) indices.size(); i++ )
  {
    Eigen::Matrix<double,3,1> f = adapter.getCamRotation(indices[i]) * adapter.getBearingVector(indices[i]);
    Eigen::Matrix<double,3,3> Vk = H * ( f * f.transpose() - Eigen::Matrix<double,3,3>::Identity() );
    Eigen::Matrix<double,3,1> p = adapter.getPoint(indices[i]);
    Eigen::Matrix<double,3,1> v = adapter.getCamOffset(indices[i]);

    fill3x10(p,Phi);
    I += Vk * Phi;
    J += Vk * v;
  }

  Eigen::Matrix<double,10,10> M = Eigen::Matrix<double,10,10>::Zero();
  Eigen::Matrix<double,1,10>  C = Eigen::Matrix<double,1,10>::Zero();
  double gamma = 0.0;

  for(int i = 0; i < (int) indices.size(); i++ )
  {
    Eigen::Matrix<double,3,1> f = adapter.getCamRotation(indices[i]) * adapter.getBearingVector(indices[i]);
    Eigen::Matrix<double,3,1> v = adapter.getCamOffset(indices[i]);
    Eigen::Matrix<double,3,1> p = adapter.getPoint(indices[i]);

    fill3x10(p,Phi);
    Eigen::Matrix<double,3,3> temp = f*f.transpose() - Eigen::Matrix<double,3,3>::Identity();
    Eigen::Matrix<double,3,10> Ai =  temp * (Phi + I);
    Eigen::Matrix<double,3, 1> bi = -temp * (  v + J);

    M     += (Ai.transpose() * Ai);
    C     += (bi.transpose() * Ai);
    gamma += (bi.transpose() * bi);
  }

  //now do the main computation
  std::vector<std::pair<double,Eigen::Vector4d>,Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> > > quaternions1;
  if( indices.size() > 4 )
    modules::upnp_main_sym( M, C, gamma, quaternions1 );
  else
    modules::upnp_main( M, C, gamma, quaternions1 );

  //prepare the output vector
  transformations_t transformations;

  //Round 1: chirality check
  std::vector<std::pair<double,Eigen::Vector4d>,Eigen::aligned_allocator< std::pair<double,Eigen::Vector4d> > > quaternions2;
  for( int i = 0; i < quaternions1.size(); i++ )
  {
    rotation_t Rinv = math::quaternion2rot(quaternions1[i].second);

    Eigen::Matrix<double,10,1> s;
    modules::upnp_fill_s( quaternions1[i].second, s );
    translation_t tinv = I*s - J;

    if( transformations.size() == 0 )
    {
      transformation_t newTransformation;
      newTransformation.block<3,3>(0,0) = Rinv.transpose();
      newTransformation.block<3,1>(0,3) = -newTransformation.block<3,3>(0,0) * tinv;
      transformations.push_back(newTransformation);
    }

    int count_negative = 0;

    for( int j = 0; j < (int) indices.size(); j++ )
    {
      Eigen::Matrix<double,3,1> f = adapter.getCamRotation(indices[j]) * adapter.getBearingVector(indices[j]);
      Eigen::Matrix<double,3,1> p = adapter.getPoint(indices[j]);
      Eigen::Matrix<double,3,1> v = adapter.getCamOffset(indices[j]);

      Eigen::Vector3d p_est = Rinv*p + tinv - v;

      if( p_est.transpose()*f < 0.0 )
        count_negative++;
    }

    if( count_negative < floor(0.2 * indices.size() + 0.5) )
      quaternions2.push_back(quaternions1[i]);
  }

  if( quaternions2.size() == 0 )
    return transformations;
  else
    transformations.clear();

  //Round 2: Second order optimality (plus polishing)
  Eigen::Matrix<double,3,10> I_cay;
  Eigen::Matrix<double,10,10> M_cay;
  Eigen::Matrix<double,1,10>  C_cay;
  double gamma_cay;

  for( size_t q = 0; q < quaternions2.size(); q++ )
  {
    I_cay = Eigen::Matrix<double,3,10>::Zero();
    rotation_t Rinv = math::quaternion2rot(quaternions2[q].second);

    for( int i = 0; i < (int) indices.size(); i++ )
    {
      Eigen::Matrix<double,3,1> f = adapter.getCamRotation(indices[i]) * adapter.getBearingVector(indices[i]);
      Eigen::Matrix<double,3,3> Vk = H * ( f * f.transpose() - Eigen::Matrix<double,3,3>::Identity() );
      Eigen::Matrix<double,3,1> p = Rinv * adapter.getPoint(indices[i]);

      fill3x10(p,Phi);
      I_cay += Vk * Phi;
    }

    M_cay = Eigen::Matrix<double,10,10>::Zero();
    C_cay = Eigen::Matrix<double,1,10>::Zero();
    gamma_cay = 0.0;

    for(int i = 0; i < (int) indices.size(); i++ )
    {
      Eigen::Matrix<double,3,1> f = adapter.getCamRotation(indices[i]) * adapter.getBearingVector(indices[i]);
      Eigen::Matrix<double,3,1> v = adapter.getCamOffset(indices[i]);
      Eigen::Matrix<double,3,1> p = Rinv * adapter.getPoint(indices[i]);

      fill3x10(p,Phi);
      Eigen::Matrix<double,3,3> temp = f*f.transpose() - Eigen::Matrix<double,3,3>::Identity();
      Eigen::Matrix<double,3,10> Ai =  temp * (Phi + I_cay);
      Eigen::Matrix<double,3,1> bi = -temp * (  v + J);

      M_cay     += (Ai.transpose() * Ai);
      C_cay     += (bi.transpose() * Ai);
      gamma_cay += (bi.transpose() * bi);
    }

    //now analyze the eigenvalues of the "Hessian"
    Eigen::Vector3d val;
    Eigen::Matrix3d Jacobian;
    f( M_cay, C_cay, gamma_cay, val );
    Jac( M_cay, C_cay, gamma_cay, Jacobian );
    std::vector<double> characteristicPolynomial;
    characteristicPolynomial.push_back(-1.0);
    characteristicPolynomial.push_back(Jacobian(2,2)+Jacobian(1,1)+Jacobian(0,0));
    characteristicPolynomial.push_back(-Jacobian(2,2)*Jacobian(1,1)-Jacobian(2,2)*Jacobian(0,0)-Jacobian(1,1)*Jacobian(0,0)+pow(Jacobian(1,2),2)+pow(Jacobian(0,2),2)+pow(Jacobian(0,1),2));
    characteristicPolynomial.push_back(Jacobian(2,2)*Jacobian(1,1)*Jacobian(0,0)+2*Jacobian(1,2)*Jacobian(0,2)*Jacobian(0,1)-Jacobian(2,2)*pow(Jacobian(0,1),2)-pow(Jacobian(1,2),2)*Jacobian(0,0)-Jacobian(1,1)*pow(Jacobian(0,2),2));
    std::vector<double> roots = opengv::math::o3_roots( characteristicPolynomial );

    bool allPositive = true;
    for( size_t i = 0; i < roots.size(); i++ )
    {
      if( roots[i] < 0.0 )
      {
        allPositive = false;
        break;
      }
    }

    if( true )//allPositive)//use all results for the moment
    {
      //perform the polishing step
      Eigen::Vector3d cay = - Jacobian.inverse() * val;
      rotation_t Rinv2 = math::cayley2rot(cay) * Rinv;
      quaternion_t q = math::rot2quaternion(Rinv2);

      Eigen::Matrix<double,10,1> s;
      modules::upnp_fill_s(q,s);
      translation_t tinv = I*s - J;

      transformation_t newTransformation;
      newTransformation.block<3,3>(0,0) = Rinv2.transpose();
      newTransformation.block<3,1>(0,3) = -newTransformation.block<3,3>(0,0) * tinv;
      transformations.push_back(newTransformation);
    }
  }

  //if there are no results, simply add the one with lowest score
  if( transformations.size() == 0 )
  {
    Eigen::Vector4d q = quaternions2[0].second;
    Eigen::Matrix<double,10,1> s;
    modules::upnp_fill_s(q,s);
    translation_t tinv = I*s - J;
    rotation_t Rinv = math::quaternion2rot(q);

    transformation_t newTransformation;
    newTransformation.block<3,3>(0,0) = Rinv.transpose();
    newTransformation.block<3,1>(0,3) = -newTransformation.block<3,3>(0,0) * tinv;
    transformations.push_back(newTransformation);
  }

  return transformations;
}

}
}

opengv::transformations_t
opengv::absolute_pose::upnp( const AbsoluteAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return upnp(adapter,idx);
}

opengv::transformations_t
opengv::absolute_pose::upnp(
    const AbsoluteAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return upnp(adapter,idx);
}

namespace opengv
{
namespace absolute_pose
{

struct OptimizeNonlinearFunctor1 : OptimizationFunctor<double>
{
  const AbsoluteAdapterBase & _adapter;
  const Indices & _indices;

  OptimizeNonlinearFunctor1(
      const AbsoluteAdapterBase & adapter,
      const Indices & indices ) :
      OptimizationFunctor<double>(6,indices.size()),
      _adapter(adapter),
      _indices(indices) {}

  int operator()(const VectorXd &x, VectorXd &fvec) const
  {
    assert( x.size() == 6 );
    assert( (unsigned int) fvec.size() == _indices.size());

    //compute the current position
    translation_t translation = x.block<3,1>(0,0);
    cayley_t cayley = x.block<3,1>(3,0);
    rotation_t rotation = math::cayley2rot(cayley);

    //compute inverse transformation
    transformation_t inverseSolution;
    inverseSolution.block<3,3>(0,0) = rotation.transpose();
    inverseSolution.col(3) = -inverseSolution.block<3,3>(0,0)*translation;

    Eigen::Matrix<double,4,1> p_hom;
    p_hom[3] = 1.0;

    for(size_t i = 0; i < _indices.size(); i++)
    {
      //get point in homogeneous form
      p_hom.block<3,1>(0,0) = _adapter.getPoint(_indices[i]);

      //compute the reprojection (this is working for both central and
      //non-central case)
      point_t bodyReprojection = inverseSolution * p_hom;
      point_t reprojection = _adapter.getCamRotation(_indices[i]).transpose() *
          (bodyReprojection - _adapter.getCamOffset(_indices[i]));
      reprojection = reprojection / reprojection.norm();

      //compute the score
      double factor = 1.0;
      fvec[i] = factor *
          (1.0 -
          (reprojection.transpose() * _adapter.getBearingVector(_indices[i])));
    }

    return 0;
  }
};

transformation_t optimize_nonlinear(
    const AbsoluteAdapterBase & adapter,
    const Indices & indices )
{
  const int n=6;
  VectorXd x(n);

  x.block<3,1>(0,0) = adapter.gett();
  x.block<3,1>(3,0) = math::rot2cayley(adapter.getR());

  OptimizeNonlinearFunctor1 functor( adapter, indices );
  NumericalDiff<OptimizeNonlinearFunctor1> numDiff(functor);
  LevenbergMarquardt< NumericalDiff<OptimizeNonlinearFunctor1> > lm(numDiff);

  lm.resetParameters();
  lm.parameters.ftol = 1.E1*NumTraits<double>::epsilon();
  lm.parameters.xtol = 1.E1*NumTraits<double>::epsilon();
  lm.parameters.maxfev = 1000;
  lm.minimize(x);

  transformation_t transformation;
  transformation.col(3) = x.block<3,1>(0,0);
  transformation.block<3,3>(0,0) = math::cayley2rot(x.block<3,1>(3,0));
  return transformation;
}

}
}

opengv::transformation_t
opengv::absolute_pose::optimize_nonlinear( const AbsoluteAdapterBase & adapter )
{
  Indices idx(adapter.getNumberCorrespondences());
  return optimize_nonlinear(adapter,idx);
}

opengv::transformation_t
opengv::absolute_pose::optimize_nonlinear(
    const AbsoluteAdapterBase & adapter,
    const std::vector<int> & indices )
{
  Indices idx(indices);
  return optimize_nonlinear(adapter,idx);
}


#include "precompiled.h"
#include "explicit_fields.h"
#include "auto_connecting_body.h"

#ifndef AUTOMATIC_PRECOMPILATION
#include <string>
#include <boost/optional.hpp>
#include <pretty_printer.h>
#endif

#include "logging.h"
#include "grid_fields.h"
#include "black_scholes.h"
#include "global_config.h"
#include "Math.h"
#include "finite_difference_weights_config.h"

namespace fipster { namespace explicit_fields {

	using namespace std;

	// #################################################
	// ##############   PHASE 1:			############
	// ##############   CONFIGURATION       ############
	// #################################################

	node_factory::node_factory(const ptree& pt,shared_const_objs_t objs)
		: pt(pt),objs(objs)
	{
		string stype(pt.get<string>("type"));
		if(stype == "CallPayoff") type = CallPayoff;
		else if(stype == "PutPayoff") type = PutPayoff;
		else if(stype == "BinaryCallPayoff") type = BinaryCallPayoff;
		else if(stype == "Put") type = Put;
		else if(stype == "Call") type = Call;
		else BOOST_THROW_EXCEPTION(runtime_error(stype+" is no valid Explicit Field Type"));
	}

	// #################################################
	// ##############   PHASE 3:			############
	// ##############   Node Bodies         ############
	// #################################################

	/** this class calculates the payoff of a Call/Put on the first
			assert
	 */
	template<type_t type>
	struct VanillaPayoff_t{
		double strike;

		/** constructor that reads in configuration */
		VanillaPayoff_t
		(const ptree& pt,const field_arg_t<>&,shared_const_objs_t)
			:strike(pt.get<double>("strike")){}

		/** core function, computing the value for a given state */
		double getValue(const state_t& state){
			/*if(state[0]>120){
				auto s2=state;
				s2[0]-=120;
				return getValue(s2);
			}
			if(state[0]>75) return 0;*/
			double a=30;
			if(type==BinaryCall) return state[0] >= strike ? 1 : 0;

			return max( (type==Call ? +1 : -1 )*(state[0]-strike-0*a),0.0);
// -
// 			max( (type==Call ? +1 : -1 )*(state[0]-strike-a),0.0)-
// 				max( (type==Call ? +1 : -1 )*(state[0]-strike-2*a),0.0)+
// 				max( (type==Call ? +1 : -1 )*(state[0]-strike-3*a),0.0);
		};
	};

	/** this class calculates the Black Scholes Values for a Call/Put on
	the geometric average of grid.D assets (using equal correlation, dividend and volatility)
	*/
	template<type_t type>
	struct BS_t{
		double strike,interest,volatility,correlation;
		black_scholes bs;

		/** constructor that reads in configuration */
		BS_t(const ptree& pt,const field_arg_t<>& arg,shared_const_objs_t objs){

			auto same_as = pt.get_optional<string>("sameAs");

			if(!same_as)
				bs=black_scholes(0,
					pt.get<double>("strike"),
					global_config::get().years_per_btick*(pt.get<btime>("expiration")-arg.time),
					pt.get<double>("interest"),
					pt.get<double>("volatility"),
					pt.get("dividend",0.0),
					pt.get("correlation",0.0),
					arg.grid->D);
			else{
				FIPSTER_THROW_EXCEPTION(runtime_error("not implemented"));
				/*
				//get the expectation value to take the interval from (if available)
				auto nf=dynamic_cast<const expectation_values::node_factory*>
					(objs->spacetime_fields.at(*same_as).get());
				if(!nf) FIPSTER_THROW_EXCEPTION(runtime_error("sameAs: "+*same_as+" is no expcetation_value"));
				auto fdw=objs->fdweights_configs.at(nf->config->fd_weights_s);
				fdw->
				*/
			}
		}

		/** core function, computing the value for a given state */
		double getValue(const state_t& state);
	};

	template<>
	double BS_t<Call>::getValue(const state_t& state)
	{
		return bs.call(geometric_average(state,bs.n));
	};

	template<>
	double BS_t<Put>::getValue(const state_t& state)
	{
		return bs.put(geometric_average(state,bs.n));
	};

	//#######################  Node body ##############################
	template<int sg, class result_t, class state_it,class core_t>//specify the sub-grid to work on
	struct explicit_field_body :
		auto_connecting_body<explicit_field_body<sg,result_t,state_it,core_t>,//body_t
							result_t>//result
	{
		core_t core;
		const btime time;
		grid_ptr grid;
		string meta_info;
		double factor;

		using result_sptr_t = typename explicit_field_body::result_sptr_t;
		using input_t = typename explicit_field_body::input_t;

		explicit_field_body(field_arg_t<> field_args,const ptree& pt,shared_const_objs_t objs)
			:core(core_t(pt,field_args,objs))
			,time(field_args.time)
			,grid(field_args.grid)
			,meta_info(pt.get<string>("type")
								 + " EXPL FL ("+toS(time)+") "+pt.get<string>("<xmlattr>.id"))
			,factor(pt.get("factor",1.0))
		{};

		//computation function #####################################################
		result_sptr_t operator()(const input_t&){
			// logging
			if(1)thread_logger()<<"EXPL FL ("<<time<<") | START"<<endl;
			auto result = make_shared<result_t>
				(meta_info);

			//allocate new space for the result on the specified Grid
			auto end = result->resize(*grid);

			for(state_it it(*grid); it!=end; ++it)
				result->at(it.index()) = factor*core.getValue(it.state());

	//tbb::this_tbb_thread::sleep(tbb::tick_count::interval_t(0.2));

			if(1)thread_logger()<<"EXPL FL| DONE"<<endl;
			return result;
		};

	};

	// #################################################
	// ##############   PHASE 2:			############
	// ##############   Node Factory        ############
	// #################################################

	boost::optional<node_factory::arg_t>
	node_factory::delegation(const arg_t& arg){
		if((type==CallPayoff || type==PutPayoff || type==BinaryCallPayoff) && arg.time != 0 ){
			arg_t a=arg; a.time = 0;
			return a;
		}
		return boost::none;
	}

	template<int sg,class result2_t,class iterator2_t,class sender2_t>
	sender2_t node_factory::selecting_setup( const node_factory::arg_t& arg)
	{
		switch(type){
		case CallPayoff:
			return create_node
				(explicit_field_body<sg,result2_t,iterator2_t,VanillaPayoff_t<Call> >(arg,pt,objs));
		case PutPayoff:
			return create_node
				(explicit_field_body<sg,result2_t,iterator2_t,VanillaPayoff_t<Put> >(arg,pt,objs));
		case BinaryCallPayoff:
			return create_node
				(explicit_field_body<sg,result2_t,iterator2_t,VanillaPayoff_t<BinaryCall> >(arg,pt,objs));
		case Call:
			return create_node
				(explicit_field_body<sg,result2_t,iterator2_t,BS_t<Call> >          (arg,pt,objs));
		case Put:
			return create_node
				(explicit_field_body<sg,result2_t,iterator2_t,BS_t<Put> >           (arg,pt,objs));
		default:
			BOOST_THROW_EXCEPTION(runtime_error(toS(type)+" is no valid Explicit Field Type"));
		}
	}

	// spacetime_field: create_node
	node_factory::sender_ptr node_factory::inner_setup(const arg_t& arg){
		auto a = delegation(arg); if(a) return get_node(*a);
		return selecting_setup<J_SGS,
								discretized_space_field<J_SGS>,
								grid_iterator<J_SGS,true>,
								sender_ptr
							>(arg);
	}

	node_factory::bv_sender_ptr
		node_factory::boundary_setup( const arg_t& arg ){
		auto a = delegation(arg); if(a) return bv_get_node(*a);
		return selecting_setup<J_SGS,
								boundary_field_t<J_SGS>,
								boundary_iterator<J_SGS-1,false,true>,
								bv_sender_ptr
							>(arg);
	}

}}


#include <boost/optional.hpp>
#include <numeric>
#include <vector>

// 重み付きUnionFind
template <typename T = int>
class WeightedUnionFind {
	std::vector<size_t> parents;
	std::vector<size_t> rank;
	std::vector<T> diff_weight;
	T identity;

	public:
	WeightedUnionFind(size_t size, T id = static_cast<T>(0))
		: parents(size), rank(size, 0), diff_weight(size, id), identity(id) {
		std::iota(this->parents.begin(), this->parents.end(), 0);
	}

	// 併合
	bool merge(size_t a, size_t b, T w) {
		size_t ar = this->root(a);
		size_t br = this->root(b);
		if(ar == br) {
			return false;
		}
		T dw = w + this->weight(a) - this->weight(b);
		if(this->rank[ar] < this->rank[br]) {
			std::swap(ar, br);
			dw = -dw;
		}
		if(this->rank[ar] == this->rank[br]) {
			this->rank[ar]++;
		}
		this->diff_weight[br] = dw;
		this->parents[br] = ar;
		return true;
	}
	bool unite(size_t a, size_t b, T w) { return this->merge(a, b, w); }

	// 同集合か判定
	bool is_same(size_t a, size_t b) { return this->root(a) == this->root(b); }
	bool is_union(size_t a, size_t b) { return this->is_same(a, b); }

	// 二要素感の距離(同集合に属していなければboost::none)
	optional<T> diff(size_t a, size_t b) {
		if(!this->is_same(a, b)) {
			return nullopt;
		}
		return optional<T>(this->weight(b) - this->weight(a));
	}

	private:
	size_t root(int n) {
		if(this->parents[n] == n) {
			return n;
		}

		size_t r = this->root(this->parents[n]);
		this->diff_weight[n] += this->diff_weight[this->parents[n]];
		this->parents[n] = r;
		return r;
	}

	T weight(size_t n) {
		root(n);
		return this->diff_weight[n];
	}
};


/* ----------------------------------------------------------------------- *//**
 *
 * @file svd.cpp
 *
 * @brief Functions for Singular Value Decomposition
 *
 * @date Jul 10, 2013
 *//* ----------------------------------------------------------------------- */

#include <dbconnector/dbconnector.hpp>
#include <math.h>
#include <iostream>
#include <algorithm>
#include <functional>
#include <numeric>
#include "svd.hpp"
#include <Eigen/SVD>

namespace madlib {

using namespace dbal::eigen_integration;// Use Eigen

namespace modules {
namespace linalg {

using madlib::dbconnector::postgres::madlib_construct_array;

// To get a rank-k approximation of the original matrix if we perform k + s Lanczos
// bidiagonalization steps followed by the SVD of a small matrix B(k+s) then the
// algorithm constructs the best rank-k subspace in an extended subspace
// Span[U(k+s)]. Hence we obtain a better rank-k approximation than the one
// obtained after k steps steps of the standard Lanczos bidiagonalization algorithm.
// There is a memory limit to the number of extended steps and we restrict that to
// fixed number of steps for now.
// Magic number computed using the 1GB memory limit.
// MAX_LANCZOS_STEPS^2 < 10^9 bytes / (8 bytes * 3 matrices)
const size_t MAX_LANCZOS_STEPS = 5000;

// For floating point equality comparisons,
// it is safer to define a small range of values
// that are "zero", rather than use the exact value of 0.
const double ZERO_THRESHOLD = 1e-8;

/* Project the vector v into the vector u (in-place) */
static void __project(MappedColumnVector& u, MutableNativeColumnVector& v){
    double uu = u.dot(u);
    double uv = u.dot(v);

    double coef;
    if (uu <= ZERO_THRESHOLD) { // if u is the zero vector, we have a division by zero problem
        coef = 0;
    } else
        coef = uv / uu;
    v = coef * u;
}

/**
 * @brief This function returns a random normalized unit vector of specified size
 * @param args[0]   The dimension
 * @return          The unit-norm vector
 **/
AnyType svd_unit_vector::run(AnyType & args)
{
    int32_t dim = args[0].getAs<int32_t>();

    if(dim < 1){
        throw std::invalid_argument(
            "invalid argument - Positive integer expected for dimension");
    }

    MutableNativeColumnVector vectorEigen;
    Allocator& allocator = defaultAllocator();
    vectorEigen.rebind(allocator.allocateArray<double>(dim));
    vectorEigen.setRandom();
    vectorEigen = vectorEigen.normalized();

    // AnyType tuple;
    // tuple << vectorEigen;
    // return tuple;

    return vectorEigen;
}

/**
 * @brief This function is the transition function of the aggregator computing the Lanczos vectors
 * @param args[0]   State variable (i.e. A * q_j OR A_trans * p_(j-1))
 * @param args[1]   Matrix row id
 * @param args[2]   Matrix row array
 * @param args[3]   Previous P/Q vector
 * @param args[4]   Row/Column dimension
 **/
AnyType svd_lanczos_sfunc::run(AnyType & args){
    int32_t row_id = args[1].getAs<int32_t>();
    MappedColumnVector row_array = args[2].getAs<MappedColumnVector >();
    MappedColumnVector vec = args[3].getAs<MappedColumnVector >();
    int32_t dim = args[4].getAs<int32_t>();

    if(dim < 1){
        throw std::invalid_argument(
            "invalid argument - Positive integer expected for dimension");
    }

    if(row_id <= 0 || row_id > dim){
        throw std::invalid_argument(
            "invalid argument: row_id is out of range [1, dim]");
    }

    if(row_array.size() != vec.size()){
        throw std::invalid_argument(
            "dimensions mismatch: row_array.size() != vec.size(). "
            "Data contains different sized arrays");
    }

    // FIXME: construct_array functions circumvent the abstraction layer. These
    // should be replaced with appropriate Allocator:: calls.
    MutableArrayHandle<double> state(NULL);
    if(args[0].isNull()){
        state = MutableArrayHandle<double>(
            madlib_construct_array(
                NULL, dim, FLOAT8OID, sizeof(double), true, 'd'));
        for (int i = 0; i < dim; i++)
            state[i] = 0;
    }else{
        state = args[0].getAs<MutableArrayHandle<double> >();
    }

    state[row_id - 1] = row_array.dot(vec);

    return state;
}

/**
 * @brief This function is the merge function of the aggregator computing the Lanczos vectors
 * @param args[0]   State variable 1
 * @param args[1]   State variable 2
 **/
AnyType svd_lanczos_prefunc::run(AnyType & args){
    MutableArrayHandle<double> state1 = args[0].getAs<MutableArrayHandle<double> >();
    ArrayHandle<double> state2 = args[1].getAs<ArrayHandle<double> >();

    if(state1.size() != state2.size()){
        throw std::runtime_error("dimension mismatch: state1.size() != state2.size()");
    }

    for(size_t i = 0; i < state1.size(); i++)
        state1[i] += state2[i];

    return state1;
}

/**
 * @breif This function completes the computation of Lanczoc P vector
 * @param args[0]   Partial P vector from the aggregator
 * @param args[1]   Previous P vector
 * @param args[2]   Previous beta
 **/
AnyType svd_lanczos_pvec::run(AnyType & args){
    MutableNativeColumnVector partial_pvec = args[0].getAs<MutableNativeColumnVector>();

    // When args[1] is NULL, it's special case for computing p_1
    if (!args[1].isNull()){
        MappedColumnVector prev_pvec = args[1].getAs< MappedColumnVector >();
        double beta = args[2].getAs<double>();

        if(partial_pvec.size() != prev_pvec.size()){
            throw std::invalid_argument(
                "dimension mismatch: partial_pvec.size() != prev_pvec.size()");
        }
        partial_pvec = partial_pvec - beta * prev_pvec;
    }

    double norm = partial_pvec.norm();
    partial_pvec.normalize();

    AnyType tuple;
    tuple << norm << partial_pvec;
    return tuple;
}

/**
 * @breif This function completes the computation of Lanczoc Q vector
 * @param args[0]   Partial Q vector from the aggregator
 * @param args[1]   Previous Q vector
 * @param args[2]   Current alpha
 **/
AnyType svd_lanczos_qvec::run(AnyType & args){
    MutableNativeColumnVector partial_qvec = args[0].getAs<MutableNativeColumnVector>();

    MappedColumnVector prev_qvec = args[1].getAs< MappedColumnVector >();
    double alpha = args[2].getAs<double>();

    if(partial_qvec.size() != prev_qvec.size()){
        throw std::invalid_argument(
            "dimension mismatch: partial_qvec.size() != prev_qvec.size()");
    }

    partial_qvec = partial_qvec - alpha * prev_qvec;

    // Different with svd_lanczos_pvec, the Q vector will be furhter orthogonalized
    // and then be normalized in a separate function
    return partial_qvec;
}

/**
 * @brief This function is the transition function of the aggregaror doing the Gram-Schmidt orthogonalization
 * @param args[0]   State variable: sum of projected vectors | vector v
 * @param args[1]   Unorthogonalized vector (v)
 * @param args[2]   Orthogonalized vector (u)
 **/
AnyType svd_gram_schmidt_orthogonalize_sfunc::run(AnyType & args){
	MutableNativeColumnVector v = args[1].getAs<MutableNativeColumnVector >();
	MappedColumnVector u = args[2].getAs<MappedColumnVector >();

    if(u.size() != v.size()){
        throw std::invalid_argument(
            "dimensions mismatch: u.size() != v.size()");
    }

    // FIXME: construct_array functions circumvent the abstraction layer. These
    // should be replaced with appropriate Allocator:: calls.
    MutableArrayHandle<double> state(NULL);
    if(args[0].isNull()){
        state = MutableArrayHandle<double>(
            madlib_construct_array(NULL,
                                   static_cast<int>(u.size()) * 2,
                                   FLOAT8OID, sizeof(double), true, 'd'));

        // Save v into the state variable
        memcpy(state.ptr() + u.size(), v.data(), v.size() * sizeof(double));
    }else{
        state = args[0].getAs<MutableArrayHandle<double> >();
    }

    // In-place projection
    __project(u, v);

    for(int i = 0; i < u.size(); i++){
        state[i] += v[i];
    }

    return state;
}

/**
 * @brief This function is the merge function of the aggregator doing the Gram-Schmidt orthogonalization
 * @param args[0]   State variable 1
 * @param args[1]   State variable 2
 **/
AnyType svd_gram_schmidt_orthogonalize_prefunc::run(AnyType & args){
    MutableArrayHandle<double> state1 = args[0].getAs<MutableArrayHandle<double> >();
    ArrayHandle<double> state2 = args[1].getAs<ArrayHandle<double> >();

    if(state1.size() != state2.size()){
        throw std::runtime_error("dimension mismatch: state1.size() != state2.size()");
    }

    // Note that the second half of the state variable stores the vector v
    for(size_t i = 0; i < state1.size() / 2; i++)
        state1[i] += state2[i];

    return state1;
}

/**
 * @brief This function is the final function of the aggregator doing the Gram-Schmidt orthogonalization
 * @param args[0]   State variable
 **/
AnyType svd_gram_schmidt_orthogonalize_ffunc::run(AnyType & args){
    ArrayHandle<double> state = args[0].getAs<ArrayHandle<double> >();

    MutableNativeColumnVector u;
    Allocator& allocator = defaultAllocator();
    u.rebind(allocator.allocateArray<double>(state.size() / 2));

    for(int i = 0; i < u.size(); i++){
        u[i] = state[u.size() + i] - state[i];
    }

    double norm = u.norm();
    u.normalize();

    AnyType tuple;
    tuple << norm << u;

    return tuple;
}

// -- SVD Decomposition for Bidiagonal Matrix --------------------------------
/**
 * @brief This function is the transition function of the aggregator computing the SVD
 * of a sparse bidiagonal matrix
 * @param args[0]   State variable (in-memory dense matrix)
 * @param args[1]   Dimension of the matrix (i.e. k)
 * @param args[2]   Row ID (i.e. row_id)
 * @param args[3]   Column ID (i.e. col_id)
 * @param args[4]   Value
 **/
AnyType svd_decompose_bidiagonal_sfunc::run(AnyType & args){
    if(args[1].isNull() || args[2].isNull()
        || args[3].isNull() || args[4].isNull())
        return args[0];

    int32_t k = args[1].getAs<int32_t>();
    int32_t row_id = args[2].getAs<int32_t>();
    int32_t col_id = args[3].getAs<int32_t>();
    double value = args[4].getAs<double>();

    if(k < 0){
        throw std::invalid_argument(
            "SVD error: k should be a positive integer");
    }
    if(k > (int)MAX_LANCZOS_STEPS){
        throw std::invalid_argument(
            "SVD error: k is too large, try with a value in the range of [1, 6000]");
    }
    if(row_id <= 0 || row_id > k){
        throw std::invalid_argument(
            "SVD error: row_id should be in the range of [1, k]");
    }
    if(col_id <= 0 || col_id > k){
        throw std::invalid_argument(
            "invalid parameter: col_id should be in the range of [1, k]");
    }

    // FIXME: construct_array functions circumvent the abstraction layer. These
    // should be replaced with appropriate Allocator:: calls.
    MutableArrayHandle<double> state(NULL);
    if(args[0].isNull()){
        state = MutableArrayHandle<double>(
            madlib_construct_array(NULL, k * k, FLOAT8OID, sizeof(double), true, 'd'));
    } else {
        state = args[0].getAs<MutableArrayHandle<double> >();
    }

    state[(row_id - 1) * k + col_id - 1] = value;
    return state;
}

/**
 * @brief This function is the merge function of the aggregator computing the SVD
 * of a sparse bidiagonal matrix
 * @param args[0]   State variable 1
 * @param args[1]   State varaible 2
 **/
AnyType svd_decompose_bidiagonal_prefunc::run(AnyType & args){
    MutableArrayHandle<double> state1 = args[0].getAs<MutableArrayHandle<double> >();
    ArrayHandle<double> state2 = args[1].getAs<ArrayHandle<double> >();

    if(state1.size() != state2.size()){
        throw std::runtime_error("dimension mismatch: state1.size() != state2.size()");
    }

    for(size_t i = 0; i < state1.size(); i++){
        state1[i] += state2[i];
    }

    return state1;
}

/**
 * @brief Take the final matrix and run it by Eigen JacobiSVD to get the left and right
    decompositions along with the eigen values
 **/
AnyType svd_decompose_bidiagonal_ffunc::run(AnyType & args){
    MappedColumnVector state = args[0].getAs<MappedColumnVector>();
    size_t k = static_cast<size_t>(sqrt(static_cast<double>(state.size())));

    // Note that Eigen Matrix deserializes the vector in the column order
    // Thus transpose() is needed after resize()
    Matrix b = state;
    b.resize(k, k);
    b.transposeInPlace();
    Eigen::JacobiSVD<Matrix> svd(b, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // Note that AnyType serializes the Matrix Object in the column order
    // Thus transpose() is needed before output
    Matrix u = svd.matrixU().transpose();
    Matrix v = svd.matrixV().transpose();
    Matrix s = svd.singularValues();

    AnyType tuple;
    tuple << u << v << s;
    return tuple;
}

AnyType svd_decompose_bidiag::run(AnyType & args){

    // <row_id, col_id, value> triple indicate the values of a bidiagonal matrix
    ArrayHandle<int32_t> row_id = args[0].getAs<ArrayHandle<int32_t> >();
    ArrayHandle<int32_t> col_id = args[1].getAs<ArrayHandle<int32_t> >();
    MappedColumnVector value = args[2].getAs<MappedColumnVector>();

    // since row_id, col_id start indexing from 1, the max element indicates the
    // dimension of of the bidiagonal matrix
    int32_t row_dim = *std::max_element(row_id.ptr(), row_id.ptr() + row_id.size());
    int32_t col_dim = *std::max_element(col_id.ptr(), col_id.ptr() + col_id.size());

    Matrix b = Matrix::Zero(row_dim, col_dim);
    for(size_t i = 0; i < row_id.size(); i++){
        // we use -1 since row_id and col_id start from 1
        b(row_id[i] - 1, col_id[i] - 1) = value[i];
    }

    Eigen::JacobiSVD<Matrix> svd(b, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // Note that AnyType serializes the Matrix Object in the column order
    // Thus transpose() is needed before output
    Matrix u = svd.matrixU().transpose();
    Matrix v = svd.matrixV().transpose();
    Matrix s = svd.singularValues();

    AnyType tuple;
    tuple << u << v << s;
    return tuple;
}

/**
 * @brief This function is the transition function of the aggregator computing the Lanczos vectors
 * @param args[0]   State variable (i.e. A * q_j OR A_trans * p_(j-1))
 * @param args[1]   Matrix block row id
 * @param args[2]   Matrix block col id
 * @param args[3]   Matrix block
 * @param args[4]   Previous P/Q vector
 * @param args[5]   Row/Column dimension
 **/
AnyType svd_block_lanczos_sfunc::run(AnyType & args){
    int32_t row_id = args[1].getAs<int32_t>();
    int32_t col_id = args[2].getAs<int32_t>();
    MappedMatrix block = args[3].getAs<MappedMatrix>();
    MappedColumnVector vec = args[4].getAs<MappedColumnVector >();
    int32_t dim = args[5].getAs<int32_t>();

    if(row_id <= 0){
        throw std::invalid_argument(
            "SVD error: row_id should be in the range of [1, dim]");
    }
    if(col_id <= 0){
        throw std::invalid_argument(
            "invalid parameter: col_id should be in the range of [1, dim]");
    }

    // FIXME: construct_array functions circumvent the abstraction layer. These
    // should be replaced with appropriate Allocator:: calls.
    MutableArrayHandle<double> state(NULL);
    if(args[0].isNull()){
        state = MutableArrayHandle<double>(
            madlib_construct_array(
                NULL, dim, FLOAT8OID, sizeof(double), true, 'd'));
    }else{
        state = args[0].getAs<MutableArrayHandle<double> >();
    }

    // Note that block is constructed in the column-major order
    size_t row_size = block.cols();
    size_t col_size = block.rows();

    Matrix v = block.transpose() * vec.segment((col_id - 1) * col_size, col_size);
    for(int32_t i = 0; i < v.rows(); i++)
        state[(row_id - 1) * row_size + i] += v.col(0)[i];

    return state;
}

/**
 * @brief This function is the transition function of the aggregator computing the Lanczos vectors
 * @param args[0]   State variable (i.e. A * q_j OR A_trans * p_(j-1))
 * @param args[1]   Row ID
 * @param args[2]   Column ID
 * @param args[3]   Value
 * @param args[4]   Previous P/Q vector
 * @param args[5]   Row/Column dimension
 **/
AnyType svd_sparse_lanczos_sfunc::run(AnyType & args){
    int32_t row_id = args[1].getAs<int32_t>();
    int32_t col_id = args[2].getAs<int32_t>();
    double value = args[3].getAs<double>();

    MappedColumnVector vec = args[4].getAs<MappedColumnVector >();
    int32_t dim = args[5].getAs<int32_t>();

    // FIXME: construct_array functions circumvent the abstraction layer. These
    // should be replaced with appropriate Allocator:: calls.
    MutableArrayHandle<double> state(NULL);
    if(args[0].isNull()){
        state = MutableArrayHandle<double>(
            madlib_construct_array(
                NULL, dim, FLOAT8OID, sizeof(double), true, 'd'));
    }else{
        state = args[0].getAs<MutableArrayHandle<double> >();
    }

    state[row_id - 1] += value * vec[col_id - 1];
    return state;
}

/*
 *  @brief In-memory multiplication of a vector with a matrix
 *  @param vec  a 1 x r vector
 *  @param mat  a r x n matrix
 *  @param k    a positive number < n
 *  @note first cut mat to r x k, then return vec * mat
 */
AnyType svd_vec_mult_matrix::run(AnyType & args){
    MappedColumnVector vec = args[0].getAs<MappedColumnVector>();
    MappedMatrix mat = args[1].getAs<MappedMatrix>();
    int32_t k = args[2].getAs<int32_t>();

    // Any integer is ok
    if(k <= 0 || k > mat.rows()){
        k = static_cast<int32_t>(mat.rows());
    }

    // Note mat is constructed in the column-first order
    // which means that mat is actually transposed
    if(vec.size() != mat.cols()){
        throw std::invalid_argument(
            "dimensions mismatch: vec.size() != matrix.rows()");
    };

    // trans(vec) * trans(mat) = mat * vec
    Matrix r = mat.topRows(k) * vec;
    ColumnVector v = r.col(0);
    return v;
}

typedef struct __sr_ctx{
    ColumnVector vec;
    Matrix mat;
    int32_t max_call;
    int32_t cur_call;
    int32_t row_id;
    int32_t k;
} sr_ctx;

/**
 * @param arg[0]    Column vector
 * @param arg[1]    Matrix (l x l)
 * @param args[2]   Column ID
 # @param arg[3]    k (Sub-matrix: l * k)
 **/
void * svd_vec_trans_mult_matrix::SRF_init(AnyType &args){
    sr_ctx * ctx = new sr_ctx;
    ctx->vec = args[0].getAs<MappedColumnVector>();
    ctx->mat = args[1].getAs<MappedMatrix>().transpose();
    ctx->row_id = args[2].getAs<int32_t>();
    ctx->k = args[3].getAs<int32_t>();

    if(ctx->row_id <= 0 || ctx->row_id > ctx->mat.rows()){
        elog(ERROR,
            "invalid parameter - row_id should be in the range of [1, mat.rows()]");
    }

    if(ctx->k > ctx->mat.cols()){
        elog(ERROR,
            "invalid parameter - k should be in the range of [0, mat.cols()]");
    }

    ctx->max_call = static_cast<int32_t>(ctx->vec.size());
    ctx->cur_call = 0;

    return ctx;
}

AnyType svd_vec_trans_mult_matrix::SRF_next(void *user_fctx, bool *is_last_call){
    sr_ctx * ctx = (sr_ctx *) user_fctx;
    if (ctx->max_call == 0) {
        *is_last_call = true;
        return Null();
    }

    ColumnVector res = ctx->vec[ctx->cur_call] *
                            ctx->mat.row(ctx->row_id - 1).segment(0, ctx->k);
    AnyType tuple;
    tuple << ctx->cur_call << res;

    ctx->cur_call++;
    ctx->max_call--;
    *is_last_call = false;

    return tuple;
}

} //namespace linalg
} // namespace modules
} //namespace madlib


/*
 * Dijkstra.cpp
 *
 * 	\brief     My dijkstra implementation for the tenth exercise
 *  \details   This class computes the longest shortest path for given startingPoint
 *  \author    Julia Baumbach
 *  \date      15.07.2017
 */

#include "DijkstraSolver.h"

#include <climits>
#include <iostream>
#include <boost/heap/fibonacci_heap.hpp>
#include <exception>

/**
 * \struct compare_function
 * \brief defines how 2 dijkstra pairs should be compared at fibonacci-heap
 * \param edge1 first edge to compare
 * \param edge2 second edge to compare
 * \return true if the weight of edge2 is smaller than edge1's weight
 */
struct compare_function{
	bool operator()(const DijkstraPair edge1, const DijkstraPair edge2) const{
		return edge1.second > edge2.second;
	}
};

/**
 * \fn the constructor
 * \brief initialize a new dijkstra instance while sorting the given edges
 * \param weights Vector of the weights for the given graph
 * \param edges Vector of the edges for the given graph
 * \return the new dijkstra instance
 */
DijkstraSolver::DijkstraSolver(SortedEdges sortedEdges, unsigned int numberOfVertices):
	sortedEdges(sortedEdges), numberOfVertices(numberOfVertices) {
}

/**
 * \fn void dijkstra::computeShortestPath
 * \brief computes all shortest paths from given start vertex for the initialized dijkstra instance and save the weights and predecessorMaps in the given maps
 * \param unsigned int numberOfVertices Number of Vertices for the graph
 * \param WeightMap& weightsToVertices map to store the weight to the i-th node at the i-th position
 * \param vector<int>& predecessorMap map to store the i-th's predecessor at the i-th position
 */
void DijkstraSolver::computeShortestPath(unsigned int startNode, WeightMap& weightsToVertices, std::vector<int>& predecessorMap){
	if(startNode > numberOfVertices){
		std::cerr << "Index of StartVertex must be less or equal to number of vertices" << std::endl;
		throw std::exception();
	}
	VisitedMap alreadyVisited(numberOfVertices+1, false);

	//Start in point startNode
	weightsToVertices[startNode] = 0;
	predecessorMap[startNode] = startNode;
	int currentVertex = startNode;
	int currentDist = 0;

	boost::heap::fibonacci_heap<DijkstraPair, boost::heap::compare<compare_function>> heap;
	//Put the start vertex in the verticesToVisit list
	heap.push(std::make_pair(startNode, 0));

	while(!heap.empty()){
		if (!alreadyVisited.at(heap.top().first)){
			currentVertex = heap.top().first;
			currentDist = heap.top().second;

			heap.pop();

			std::vector<DijkstraPair> currentEdges = sortedEdges.at(currentVertex);

			for (const DijkstraPair pair : currentEdges){
				//Find out the current neighbor vertex and the weight of the current edge
				int neighborVertex = pair.first;
				int currentWeight = pair.second;
				//Search for the right position in verticesToVisit-list. If Vertex is already visited, don't add this edge
				if (!alreadyVisited.at(neighborVertex)){
					heap.push(std::make_pair(neighborVertex, currentDist + currentWeight));
				}

				int currentDistance = currentDist + currentWeight;

				//Update the predecessor and weightsToVerices maps
				if (currentDistance < weightsToVertices[neighborVertex]){
					weightsToVertices[neighborVertex] = currentDistance;
					predecessorMap[neighborVertex] = currentVertex;
				}
			}
			//Current vertex is visited now
			alreadyVisited.at(currentVertex) = true;
		} else {
			heap.pop();
		}
	}
}

/*
 * \fn int updateEdgeWeight(int edgeStart, int edgeEnd)
 * \brief sets EdgeWeight of edge (edgestart, edgeEnd) to 0
 * \param int edgeStart start vertex of the edge
 * \param int edgeEnd end vertex of the edge
 * \return returns the old weight of the edge
 */
int DijkstraSolver::setEdgeWeightToZero(int edgeStart, int edgeEnd){
	std::vector<DijkstraPair>& edges = sortedEdges[edgeStart];

	for(DijkstraPair& pair : edges){
		if(pair.first == edgeEnd){
			int oldWeight = pair.second;
			pair.second = 0;
			return oldWeight;
		}
	}

	return 0;
}


/**********************************************************************
%
% /// ASAR/MARA Research Group
%
% Technology Arts Sciences TH K�ln
% Technical University of Berlin
% Deutsche Telekom Laboratories
% University of Rostock
% WDR Westdeutscher Rundfunk
% IOSONO GmbH Erfurt
%
% SOFiA sound field analysis
%
% M/F Modal radial filters R13-0306
%     Soft amplification limiting
%     On-axis powerloss compensation with
%     N0plc to N0 interpolation.
%
% Copyright 2011-2017 Benjamin Bernsch�tz, rockzentrale 'AT' me.com
%                        and Nils Peters, nils 'AT' icsi.berkeley.edu
%
% This file is part of the SOFiA toolbox under MIT-License
%
%
% [dn, beam] = SOFIA_MF(N, kr, ac, [a_max], [plc], [fadeover])
% ------------------------------------------------------------------------
% dn          Vector of modal 0-N frequency domain filters
% beam        Expected free field On-Axis kr-response
% ------------------------------------------------------------------------
% N           Maximum Order
% kr          Vector or Matrix of kr values
%             First Row   (M=1) N: kr values Microphone Radius
%             Second Row  (M=2) N: kr values Sphere/Microphone2 Radius
%             [kr_mic;kr_sphere] for Rigid/Dual Sphere Configurations
%             ! If only one kr-vector is given using a Rigid/Dual Sphere
%             Configuration: kr_sphere = kr_mic
% ac          Array Configuration:
%             0  Open Sphere with pressure Transducers (NO plc!)
%             1  Open Sphere with cardioid Transducers
%             2  Rigid Sphere with pressure Transducers
%             3  Rigid Sphere with cardioid Transducers (Thx to Nils Peters!)
%             4  Dual Open Sphere with pressure Transducers (Thx to Nils Peters!)
% a_max       Maximum modal amplification limit in [dB]
% plc         OnAxis powerloss-compensation:
%             0  Off
%             1  Full kr-spectrum plc
%             2  Low kr only -> set fadeover
% fadeover    Number of kr values to fade over +/- around min-distance
%             gap of powerloss compensated filter and normal N0 filters.
%             0 = auto fadeover
%
@ end of header
%
% CONTACT AND LICENSE INFORMATION:
%
% /// ASAR/MARA Research Group
%
%     [1] Technology Arts Sciences TH K�ln
%     [2] Technical University of Berlin
%     [3] Deutsche Telekom Laboratories
%     [4] University of Rostock
%     [5] WDR Westdeutscher Rundfunk
%     [6] IOSONO GmbH Erfurt
%
% SOFiA sound field analysis toolbox
%
% Copyright 2011-2017 Benjamin Bernsch�tz et al.(�)
%
% Contact ------------------------------------
% Technology Arts Sciences TH K�ln
% Institute of Communications Systems
% Betzdorfer Street 2
% D-50679 Germany (Europe)
%
% phone       +49 221 8275 -2496
% cell phone  +49 171 4176069
% mail        rockzentrale 'at' me.com
% --------------------------------------------
%
% This file is part of the SOFiA sound field analysis toolbox
%
% Licence Type: MIT License
%
% Permission is hereby granted, free of charge, to any person obtaining a
% copy of this software and associated documentation files (the "Software"),
% to deal in the Software without restriction, including without limitation
% the rights to use, copy, modify, merge, publish, distribute, sublicense,
% and/or sell copies of the Software, and to permit persons to whom the
% Software is furnished to do so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included
% in all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
% OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
% MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
% IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
% DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
% OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
% USE OR OTHER DEALINGS IN THE SOFTWARE.
%
%
% (�) Christoph P�rschmann [1]     christoph.poerschmann 'at' th-koeln.de
%     Sascha Spors         [2,3,4] sascha.spors 'at' uni-rostock.de
%     Stefan Weinzierl     [2]     stefan.weinzierl 'at' tu-berlin.de
%     Nils Peters                  nils 'at' icsi.berkeley.edu
%
**********************************************************************/

#include <mex.h>
#include <complex>
#include <math.h>
#include <sofia_radial.h>
#include <boost/math/special_functions/spherical_harmonic.hpp>

#ifndef M_PI
	#define M_PI 3.141592653589793238462643383279502884197169399375105820974944592307816406286208998628034825342117068
#endif

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
   using namespace std;
   using namespace boost::math;

   int N, plc, ac, noplcflag, limiteronflag, normalizebeam, zeroflag;
   long long unsigned int ctr, ctrb, ctrc, numberOfkrValues, locatemindistance, fadeover;
   long long signed int ctrd;
   double *kr, *krm, *krs;
   double *ReturnReal,*ReturnImag,*ReturnRealB,*ReturnImagB;
   double a_max, a_maxdB, amplicalc, mindistance, mix, filtergap;
   complex<double> bnval;
   complex<double> *xi, *beamresponse;
   complex<double> **OutputArray;

   #ifndef DEBUG
    mexPrintf("SOFiA M/F - Modal radial filters R13-0306\n");
   #endif

   if(nrhs<3) {mexErrMsgIdAndTxt("SOFiA:MF:notEnoughInputs",
                 "Minimum 3 Inputs required: N, kr, ac, [a_max], [plc], [fadeover]");}

    //Get first 3 ARGS
    N           =           (int)mxGetScalar(prhs[0]);
    kr          =                    mxGetPr(prhs[1]);
    ac          =           (int)mxGetScalar(prhs[2]);

    if(nrhs<4)
    {
        a_max=1;
        a_maxdB=0;
        limiteronflag=0;
    }
    else
    {
        a_maxdB =(double)mxGetScalar(prhs[3]);
        limiteronflag=1;
        a_max=pow(10,(a_maxdB/(double)20)); //dB-Value to double skalar
    }

    if(nrhs<5) {plc=0;}
    else
    {
       plc = (int)mxGetScalar(prhs[4]);
    }

    if(nrhs<6) {fadeover=0;}
    else
    {
       fadeover = (long long int)mxGetScalar(prhs[5]);
    }

    numberOfkrValues=mxGetN(prhs[1]);

    if(mxGetM(prhs[1])>2)
    {
        {mexErrMsgIdAndTxt("SOFiA:MF:InputArgError",
                 "Vector size not valid: NxM with N=1 or 2 and M=[...] expected.");}
    }

    if(N<0)
    {
        {mexErrMsgIdAndTxt("SOFiA:MF:InputArgError",
                 "N: Order not valid.");}
    }
    if(a_maxdB<-20.0f)
    {
        {mexErrMsgIdAndTxt("SOFiA:MF:InputArgError",
                 "a_max: Amplification limit too low.");}
    }

    if(ac!=0 && ac!=1 && ac!=2 && ac!=3 && ac!=4)
    {
        {mexErrMsgIdAndTxt("SOFiA:MF:InputArgError",
                 "ac: Array configuration not valid. ac=[0,1,2,3,4]");}
    }

    if(plc!=0 && plc!=1 && plc!=2)
    {
        {mexErrMsgIdAndTxt("SOFiA:MF:InputArgError",
                 "plc: Choice not valid. plc=[0,1,2]");}
    }

    try
     {
        //Allocate Output Array Size [N]*[size(kr)]
        OutputArray = new complex<double>*[N+1];

        for(ctr = 0; ctr <= N; ctr++)
           {OutputArray[ctr] = new complex<double>[numberOfkrValues];}

        //Initializate Output Array
        for(ctr = 0; ctr <= N; ctr++)
           {
            for(ctrb = 0; ctrb < numberOfkrValues; ctrb++)
               {
                    OutputArray[ctr][ctrb] = complex<double>(0,0);
               }
           }

     }
     catch(...)
     { mexErrMsgIdAndTxt("SOFiA:MF:OutArrayAllocation",
                 "Not able to allocate memory for the output Matrix. Maybe to large?");
     }

    //Make Dynamic Arrays
    krm    = new double [mxGetN(prhs[1])];
    krs    = new double [mxGetN(prhs[1])];

    for(ctr=0; ctr<(mxGetN(prhs[1])); ctr++)// ctr=ctr+mxGetM(prhs[1]))
    {
        krm[ctr]    = (double)kr[ctr*mxGetM(prhs[1])];
        krs[ctr]    = (double)kr[ctr*mxGetM(prhs[1])+mxGetM(prhs[1])-1];
    }

     //Check for Zero-Elements in kr-Vector
     for(ctr = 0; ctr < numberOfkrValues; ctr++)
     {
        zeroflag=0;
        if (krm[ctr]<=0)
        {
            //mexPrintf("\nWarning: kr (mic) contains zero element or negative value.\n");
            zeroflag=1;

            for(ctrb = ctr; ctrb < numberOfkrValues; ctrb++) //Try to repair with next valid element
            {
                if(krm[ctrb]>0)
                {
                    krm[ctr]=krm[ctrb];
                    //mexPrintf("         Replaced by next valid kr element [%f].\n", krm[ctrb]);
                    zeroflag=0;
                    break;
                }
            }
            if(zeroflag==1)
              {
                 mexErrMsgIdAndTxt("SOFiA:MF:DataNotValid",
                "SOFiA MF Error: kr vector is not valid.");
              }

        }

        zeroflag=0;
        if (krs[ctr]<=0)
        {
            //mexPrintf("\nWarning: kr (sphere) contains zero element or negative value.\n");
            zeroflag=1;

            for(ctrb = ctr; ctrb < numberOfkrValues; ctrb++) //Try to repair with next valid element
            {
                if(krs[ctrb]>0)
                {
                    krs[ctr]=krs[ctrb];
                    //mexPrintf("         Replaced by next valid kr element [%f].\n", krs[ctrb]);
                    zeroflag=0;
                    break;
                }
            }
            if(zeroflag==1)
              {
                 mexErrMsgIdAndTxt("SOFiA:MF:DataNotValid",
                "SOFiA MF Error: kr vector is not valid.");
              }

        }
     }

    //DO bn-Filter Calculation
     for(ctr = 0; ctr <= N; ctr++)
           {

            for(ctrb = 0; ctrb < numberOfkrValues; ctrb++)
               {
                    bnval=bn(ctr, krm[ctrb], krs[ctrb], ac);
                    if(limiteronflag==1)
                    {
                        amplicalc=(2*a_max/M_PI)*abs(bnval)*atan(M_PI/(2*a_max*abs(bnval)));
                    }
                    else {amplicalc=1;}
                    OutputArray[ctr][ctrb]=complex<double>(amplicalc,0)/bnval;
               }
           }

     if(numberOfkrValues<32 && plc!=0)
      {mexPrintf("\nWARNING: Not enough kr values for PLC fading. PLC disabled.\n");
          plc=0;}

     //POWERLOSS COMPENSATION FILTER
     noplcflag=0;
     if(plc!=0)
     {
         xi= new complex<double>[numberOfkrValues]; //Array for xi-Term

         for(ctr=0; ctr<numberOfkrValues; ctr++)
         {
            xi[ctr]=0;
             for(ctrb=0; ctrb<=N; ctrb++)
             {
                 xi[ctr]+=complex<double>(2*ctrb+1,0)*(complex<double>(1,0)-OutputArray[ctrb][ctr]*bn(ctrb,krm[ctr],krs[ctr],ac));
             }
            xi[ctr]*=complex<double>(1,0)/bn(0,krm[ctr],krs[ctr],ac);
            xi[ctr]+=OutputArray[0][ctr];
         }
     }//plc!=0 flag

     if(plc==1) // -------------------- low kr only
     {

         //Find minimum distance
         mindistance=(double)abs(OutputArray[0][0]-xi[0]);
         locatemindistance=0;

         for(ctr=0; ctr<numberOfkrValues; ctr++)
         {
             if((double)abs(OutputArray[0][ctr]-xi[ctr]) < mindistance)
             {
                 mindistance=(double)abs(OutputArray[0][ctr]-xi[ctr]);
                 locatemindistance=ctr;
             }

         }

         filtergap=20*log10(1/abs(OutputArray[0][locatemindistance]/xi[locatemindistance]));
         mexPrintf("\nFilter fade gap: %4.2f dB",filtergap);

         if (filtergap>(double)20.0f || filtergap<(double)-20.0f)
         {mexPrintf("\nWARNING: Filtergap is too large. Nonsense filter expected.\nNo powerloss compensation filter applied.");
          noplcflag=1;}

         //Overwrite order 0 filter in output matrix
         if (noplcflag==0)
         {
             if (filtergap>(double)5.0f)
                {mexPrintf("\nWARNING: Filtergap is very large.");}

             if(fadeover==0) //Auto fadeover size
             {
                 fadeover=(long long int)(numberOfkrValues/100);
                 if (a_maxdB>0)
                 {
                     fadeover=fadeover/(long long int)ceil(a_max/4);
                 }

                if((fadeover>locatemindistance) || ((locatemindistance+fadeover)>numberOfkrValues))
                     {
                     if ((long long int)(locatemindistance-fadeover)<(long long int)(numberOfkrValues-(locatemindistance+fadeover)))
                      {
                         fadeover=locatemindistance;
                      }
                       else
                       {
                         fadeover=numberOfkrValues-locatemindistance;
                      }
                      }
                  mexPrintf("\nAuto filter size: %d Taps.", fadeover);
             }

             if((fadeover>locatemindistance) || ((locatemindistance+fadeover)>numberOfkrValues))
             {
               if ((long long int)(locatemindistance-fadeover)<(long long int)(numberOfkrValues-(locatemindistance+fadeover)))
                {
                    fadeover=locatemindistance;
                }
                 else
                 {
                    fadeover=numberOfkrValues-locatemindistance;
                }
                mexPrintf("\nWARNING: Filter fade size too high. Reduced to %d Taps.", fadeover);
             }

             mix=0;
             for(ctr=0; ctr<=locatemindistance-fadeover; ctr++)
             {
                OutputArray[0][ctr]=xi[ctr];
             }

             for(ctr=locatemindistance-fadeover+1; ctr<=locatemindistance+fadeover-1; ctr++)
                 {
                    mix+=1/(double)(2*fadeover);
                    OutputArray[0][ctr]=OutputArray[0][ctr]*complex<double>(mix,0)+xi[ctr]*complex<double>(1.0f-mix,0);
                }

             }//NoPLCFlagEnd
     }//PLC=1 low kr only

     if(plc==2) // full spectrum
     {
             for(ctr=0; ctr<numberOfkrValues; ctr++)
             {
                OutputArray[0][ctr]=xi[ctr];
             }
     }

     //Ouput kr-Response
     beamresponse= new complex<double>[numberOfkrValues]; //Array for kr-response-Term

     normalizebeam=((N+1)*(N+1));

     for(ctr=0; ctr<numberOfkrValues; ctr++)
     {
         beamresponse[ctr]=0;                            //Initialize
         for(ctrb=0; ctrb<=N; ctrb++)                   //ctrb=n;
         {
             for(ctrc=0; ctrc<=(2*ctrb); ctrc++)        //ctrc=m;
             {
                 beamresponse[ctr]=beamresponse[ctr]+bn(ctrb,krm[ctr],krs[ctr],ac)*OutputArray[ctrb][ctr];
             }
         }
         beamresponse[ctr]/=complex<double>((double)normalizebeam,0);
     }

     //Declare return ARG Matrix
     plhs[0] = mxCreateDoubleMatrix(N+1,numberOfkrValues,mxCOMPLEX);
     plhs[1] = mxCreateDoubleMatrix(1,numberOfkrValues,mxCOMPLEX);

     ReturnReal = mxGetPr(plhs[0]);
     ReturnImag = mxGetPi(plhs[0]);

     //Return ARG Matrix Fill
     ctrc=0;
     for(ctr=0;ctr<numberOfkrValues;ctr++)
        {
        for(ctrb=0;ctrb<=N;ctrb++)
             {
                ReturnReal[ctrc]=real(OutputArray[ctrb][ctr]);
                ReturnImag[ctrc]=imag(OutputArray[ctrb][ctr]);
                ctrc++;
              }
        }

    //Return Beamresponse
     ReturnRealB = mxGetPr(plhs[1]);
     ReturnImagB = mxGetPi(plhs[1]);

     for(ctr=0;ctr<numberOfkrValues;ctr++)
         {
                ReturnRealB[ctr]=real(beamresponse[ctr]);
                ReturnImagB[ctr]=imag(beamresponse[ctr]);
         }

      if(plc!=0)
      {delete [] xi;}
      delete [] beamresponse;
      for(ctr = 0; ctr < (N+1); ctr++) delete OutputArray[ctr];

}


#include <random>
#include <unordered_map>
#include <cstdio>

#include <boost/functional/hash.hpp>

#include "agms.hh"

using namespace std;

//---------------------------
// hash_family methods
//---------------------------

using namespace agms;

static mt19937_64 engine;

hash_family::hash_family(depth_type _D) : D(_D) {
    if (_D == 0) throw std::domain_error("0 depth in hash family");

    uniform_int_distribution<int64_t> U;

    for (size_t i = 0; i < 6; i++) {
        F[i] = new int64_t[_D];
        for (size_t d = 0; d < _D; d++)
            F[i][d] = U(engine);
    }

#if 0  // printout the random projection seeds
    for(depth_type d=0; d<D; d++) {
        printf("%4d:",d);
        for(size_t i=0;i<6; i++) {
            printf(" %12ld", F[i][d]);
        }
        printf("\n");
    }
#endif
}

hash_family::~hash_family() {
    for (size_t i = 0; i < 6; i++)
        delete[] F[i];
}

// A cache type for hash families, which is cleared at destruction
struct agms_hf_cache
        : unordered_map<depth_type, hash_family *> {
    ~agms_hf_cache() {
        for (auto x : *this)
            delete x.second;
    }
};

// The hash family cache variable
static agms_hf_cache cache;

hash_family *hash_family::get_cached(depth_type D) {
    if (cache.find(D) != cache.end())
        return cache[D];
    else {
        hash_family *ret = new hash_family(D);
        cache[D] = ret;
        return ret;
    }
}

//
// Return a 31-bit random hash
//
inline int64_t hash31(int64_t a, int64_t b, int64_t x) {
    // use 64-bit arithmetic
    int64_t result = (a * x) + b;
    return ((result >> 31) ^ result) & 2147483647ll;
}

size_t hash_family::hash(depth_type d, size_t x) const {
    assert(d < D);
    return hash31(F[0][d], F[1][d], x);
}

/// Return a 4-wise independent bit
bool hash_family::fourwise(depth_type d, size_t x) const {
    return
            hash31(hash31(hash31(x, F[2][d], F[3][d]), x, F[4][d]), x, F[5][d])
            & (1 << 15);
}

void projection::update_index(size_t key, Index &idx) const {
    assert(idx.size() == depth());
    size_t stride = 0;
    for (size_t d = 0; d < depth(); d++) {
        idx[d] = stride + hf->hash(d, key) % L;
        stride += width();
    }
}

void projection::update_mask(size_t key, Mask &mask) const {
    assert(mask.size() == depth());
    for (size_t d = 0; d < depth(); d++) {
        mask[d] = hf->fourwise(d, key);
    }
}

size_t std::hash<projection>::operator()(const projection &p) const {
    using boost::hash_value;
    using boost::hash_combine;

    size_t seed = 0;
    hash_combine(seed, hash_value(p.hashf()));
    hash_combine(seed, hash_value(p.width()));
    return seed;
}

template<typename T>
void print_vec(const string &name, const T &a) {
    size_t n = a.size();
    cout << name << "[" << n << "]={";
    for (size_t i = 0; i < n; i++)
        cout << (i ? "," : "") << a[i];
    cout << "}" << endl;
}

inc_sketch_updater::inc_sketch_updater(sketch &_sk)
        : sk(_sk),
          delta(_sk.proj.depth()),
          mask(_sk.proj.depth()) {}

void inc_sketch_updater::update(size_t key, double freq) {
    sk.proj.update_index(key, delta.index);
    sk.proj.update_mask(key, mask);

    delta.xold = sk[delta.index];
    for (size_t d = 0; d < mask.size(); d++) {
        if (mask[d])
            delta.xnew[d] = delta.xold[d] + freq;
        else
            delta.xnew[d] = delta.xold[d] - freq;
    }

    sk[delta.index] = delta.xnew;
}

isketch::isketch(const projection &_proj)
        : sketch(_proj),
          delta(proj.depth()),
          mask(proj.depth()) {}

void isketch::update(size_t key, double freq) {
    proj.update_index(key, delta.index);
    proj.update_mask(key, mask);

    delta.xold = (*this)[delta.index];
    for (size_t d = 0; d < mask.size(); d++) {
        if (mask[d])
            delta.xnew[d] = delta.xold[d] + freq;
        else
            delta.xnew[d] = delta.xold[d] - freq;
    }

    (*this)[delta.index] = delta.xnew;
}