// morpho/cdl/bgl_expansions/algorithms/ullmann.hpp header file//
// Copyright (c) 2003-2008 Vladimir J. Sykora
// Copyright (c) 2007-2008 Vladimir J. Sykora and NCU Studies Ltd
// Modifications by Greg Landrum, January 2009
//
//*****************************************************************************
// Permission is hereby granted, free of charge, to any person or organization
// obtaining a copy of the software and accompanying documentation covered by
// this license (the "Software") to use, reproduce, display, distribute,
// execute, and transmit the Software, and to prepare derivative works of the
// Software, and to permit third-parties to whom the Software is furnished to
// do so, all subject to the following:
//
// The copyright notices in the Software and this entire statement, including
// the above license grant, this restriction and the following disclaimer,
// must be included in all copies of the Software, in whole or in part, and
// all derivative works of the Software, unless such copies or derivative
// works are solely in the form of machine-executable object code generated by
// a source language processor.
//
// 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, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

//*****************************************************************************

//-----------------------------------------------------------------------------

// Algorithm due to JR Ullmann. "An Algorithm for Subgraph
// Isomorphism". Journal of the Association for
// Computing Machinery, Vol 23, No.1, January 1976, pp 31-42.

#ifndef MORPHO_CDL_BGL_EXP_ULLMANN_HPP
#define MORPHO_CDL_BGL_EXP_ULLMANN_HPP

// -------- boost
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/graph/graph_traits.hpp>
// -------- std
#include <utility>  // for std::pair<>
#include <algorithm>

namespace boost {
namespace detail {

template <class Graph, class UblasMatrix, class BackInsertionSequence,
          class EdgeLabeling>
bool forward_checking(const Graph& g1, const Graph& g2, UblasMatrix& M,
                      size_t count, BackInsertionSequence& F,
                      size_t num_vert_g1, size_t num_vert_g2,
                      EdgeLabeling& edge_labeling) {
  typedef std::pair<typename graph_traits<Graph>::edge_descriptor, bool>
      edge_presence;
  typename BackInsertionSequence::iterator fi, fi_end = F.end();
  for (size_t k = count + 1; k < num_vert_g1; ++k) {
    for (size_t l = 0; l < num_vert_g2; ++l) {
      if (!M(k, l)) continue;
      // check mapping:
      fi = F.begin();
      while (fi != fi_end) {
        bool flag1(0), flag1_1(0), flag2(0), flag2_1(0);
        edge_presence ep1 = edge(k, fi->first, g1);
        if (ep1.second) {
          flag1_1 = true;
          edge_presence ep2 = edge(l, fi->second, g2);
          if (ep2.second) flag1 = edge_labeling(ep1.first, ep2.first);
        }
        if (flag1_1 && flag1) {
          M(k, l) = 1;
          ++fi;
          continue;
        }
        edge_presence ep2 = edge(l, fi->second, g2);
        if (ep2.second) {
          flag2_1 = true;
          ep1 = edge(k, fi->first, g1);
          if (ep1.second) {
            flag2 = edge_labeling(ep1.first, ep2.first);
          } else {
            // ring closed in main structure, not closed in query. This should
            // pass
            flag2 = true;
          }
        }
        if (flag2_1 && flag2) {  // if one edge exists, there must be a mapping
          M(k, l) = 1;
        } else if (!flag1_1 && !flag2_1) {  // or both edges are not present
          M(k, l) = 1;
        } else
          M(k, l) = 0;  // if not, there's no mapping
        ++fi;
      }
    }
  }
  // TODO: change the data structure of the M matrix to sparse matrix. This
  // wouldn't be necessary
  size_t cero_row(0);
  for (size_t k = 0; k < num_vert_g1; ++k) {
    for (size_t l = 0; l < num_vert_g2; ++l) {
      if (M(k, l))
        break;
      else
        ++cero_row;
    }
    if (cero_row == num_vert_g2) return false;  // if there is a cero row
    cero_row = 0;
  }
  return true;
}

template <class Graph, class EdgeLabeling, class UblasMatrix,
          class BackInsertionSequence>
bool backtrack(const Graph& g1, const Graph& g2, size_t count,
               const UblasMatrix& M, BackInsertionSequence& F,
               const size_t num_vert_g1, const size_t num_vert_g2,
               EdgeLabeling& edge_labeling) {
  if (count == num_vert_g1) return true;
  for (size_t i = 0; i < num_vert_g2; ++i) {
    if (M(count, i)) {
      F.push_back(std::make_pair(count, i));
      UblasMatrix M_prime(M);
      for (size_t m = count + 1; m < num_vert_g1; ++m) {
        M_prime(m, i) = 0;
      }

      if (forward_checking(g1, g2, M_prime, count, F, num_vert_g1, num_vert_g2,
                           edge_labeling)) {
        if (backtrack(g1, g2, count + 1, M_prime, F, num_vert_g1, num_vert_g2,
                      edge_labeling)) {
          return true;
        }
      }
      F.erase(std::remove(F.begin(), F.end(), std::make_pair(count, i)),
              F.end());
    }
  }
  return false;
}

template <class Graph, class EdgeLabeling, class UblasMatrix,
          class DoubleBackInsertionSequence>
void backtrack_all(const Graph& g1, const Graph& g2, size_t count,
                   const UblasMatrix& M, DoubleBackInsertionSequence& FF,
                   const size_t num_vert_g1, const size_t num_vert_g2,
                   EdgeLabeling& edge_labeling) {
  if (count == num_vert_g1) return;
  DoubleBackInsertionSequence holdFF;
  holdFF.insert(holdFF.begin(), FF.begin(), FF.end());
  FF.clear();
  for (size_t i = 0; i < num_vert_g2; ++i) {
    if (M(count, i)) {
      DoubleBackInsertionSequence tFF;

      UblasMatrix M_prime(M);
      for (size_t m = count + 1; m < num_vert_g1; ++m) {
        M_prime(m, i) = 0;
      }

      if (holdFF.size()) {
        for (typename DoubleBackInsertionSequence::const_iterator iter =
                 holdFF.begin();
             iter != holdFF.end(); ++iter) {
          typename DoubleBackInsertionSequence::value_type F = *iter;
          F.push_back(std::make_pair(count, i));
          if (forward_checking(g1, g2, M_prime, count, F, num_vert_g1,
                               num_vert_g2, edge_labeling)) {
            tFF.push_back(F);
          }
        }
      } else {
        typename DoubleBackInsertionSequence::value_type F;
        F.push_back(std::make_pair(count, i));
        if (forward_checking(g1, g2, M_prime, count, F, num_vert_g1,
                             num_vert_g2, edge_labeling)) {
          tFF.push_back(F);
        }
      }
      backtrack_all(g1, g2, count + 1, M_prime, tFF, num_vert_g1, num_vert_g2,
                    edge_labeling);
      if (tFF.size()) {
        for (typename DoubleBackInsertionSequence::const_iterator iter =
                 tFF.begin();
             iter != tFF.end(); ++iter) {
          FF.push_back(*iter);
        }
      }
    }
  }
}

template <class Graph, class VertexLabeling  // binary predicate
          ,
          class UblasMatrix>
void prepareM(const Graph& g1, const Graph& g2, VertexLabeling& vertex_labeling,
              UblasMatrix& M) {
  size_t rows(num_vertices(g1));
  size_t cols(num_vertices(g2));
  M.resize(rows, cols);
  // initialize the matrix:
  for (size_t i = 0; i < rows; ++i) {
    for (size_t j = 0; j < cols; ++j) {
      if (out_degree(j, g2) >= out_degree(i, g1) && vertex_labeling(i, j)) {
        M(i, j) = 1;
      } else
        M(i, j) = 0;
    }
  }
}
}  // namespace detail

// test if g1 is a subgraph of g2. mapped vertices are returned in F
// Mapping : first: g1 vertices, second : g2 vertices
// O( num_vertices(g1)! num_vertices(g1) ^ 3 )
// This function doesnt Work with filtered graphs!
// The size of F doesn't indicate match!
template <
    class Graph, class VertexLabeling  // binary predicate
    ,
    class EdgeLabeling  // binary predicate
    ,
    class
    BackInsertionSequence  // contains
                           // std::pair<vertex_descriptor,vertex_descriptor>
    >
bool ullmann(const Graph& g1, const Graph& g2, VertexLabeling& vertex_labeling,
             EdgeLabeling& edge_labeling, BackInsertionSequence& F) {
  typedef ::boost::numeric::ublas::matrix<int> matrix_t;
  size_t rows(num_vertices(g1));
  size_t cols(num_vertices(g2));
  matrix_t M;
  detail::prepareM(g1, g2, vertex_labeling, M);
  size_t count(0);
  return detail::backtrack(g1, g2, count, M, F, rows, cols, edge_labeling);
}

// test if g1 is a subgraph of g2.
// F returns all mappings of g1 in g2. mapping in separate containers
template <class Graph, class VertexLabeling  // binary predicate
          ,
          class EdgeLabeling  // binary predicate
          ,
          class DoubleBackInsertionSequence  // contains a back insertion
                                             // sequence
          >
bool ullmann_all(const Graph& g1, const Graph& g2,
                 VertexLabeling& vertex_labeling, EdgeLabeling& edge_labeling,
                 DoubleBackInsertionSequence& F) {
  typedef ::boost::numeric::ublas::matrix<int> matrix_t;
  size_t rows(num_vertices(g1));
  size_t cols(num_vertices(g2));
  matrix_t M;
  detail::prepareM(g1, g2, vertex_labeling, M);
  size_t count(0);
  detail::backtrack_all(g1, g2, count, M, F, rows, cols, edge_labeling);
  return !F.empty();
}

}  // namespace boost

#endif  // MORPHO_CDL_BGL_EXP_ULLMANN_HPP


/**
 * GeoDa TM, Copyright (C) 2011-2015 by Luc Anselin - all rights reserved
 *
 * This file is part of GeoDa.
 *
 * GeoDa is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * GeoDa is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <boost/thread.hpp>
#include <boost/bind.hpp>

#include <time.h>
#include <math.h>
#include <wx/filename.h>
#include <wx/stopwatch.h>
#include "../DataViewer/TableInterface.h"
#include "../ShapeOperations/RateSmoothing.h"
#include "../ShapeOperations/Randik.h"
#include "../ShapeOperations/WeightsManState.h"
#include "../ShapeOperations/WeightUtils.h"
#include "../VarCalc/WeightsManInterface.h"
#include "../logger.h"
#include "../Project.h"
#include "LisaCoordinatorObserver.h"
#include "LisaCoordinator.h"

LisaWorkerThread::LisaWorkerThread(const GalElement* W_,
                                   const std::vector<bool>& undefs_,
                                   int obs_start_s, int obs_end_s,
								   uint64_t	seed_start_s,
								   LisaCoordinator* lisa_coord_s,
								   wxMutex* worker_list_mutex_s,
								   wxCondition* worker_list_empty_cond_s,
								   std::list<wxThread*> *worker_list_s,
								   int thread_id_s)
: wxThread(),
W(W_),
undefs(undefs_),
obs_start(obs_start_s), obs_end(obs_end_s), seed_start(seed_start_s),
lisa_coord(lisa_coord_s),
worker_list_mutex(worker_list_mutex_s),
worker_list_empty_cond(worker_list_empty_cond_s),
worker_list(worker_list_s),
thread_id(thread_id_s)
{
}

LisaWorkerThread::~LisaWorkerThread()
{
}

wxThread::ExitCode LisaWorkerThread::Entry()
{
	LOG_MSG(wxString::Format("LisaWorkerThread %d started", thread_id));

	// call work for assigned range of observations
	lisa_coord->CalcPseudoP_range(W, undefs, obs_start, obs_end, seed_start);

	wxMutexLocker lock(*worker_list_mutex);

	// remove ourself from the list
	worker_list->remove(this);

    // if empty, signal on empty condition since only main thread
	// should be waiting on this condition
	if (worker_list->empty()) {
		worker_list_empty_cond->Signal();
	}

	return NULL;
}

/**
 Since the user has the ability to synchronise either variable over time,
 we must be able to reapply weights and recalculate lisa values as needed.

 1. We will have original data as complete space-time data for both variables

 2. From there we will work from info in var_info for both variables.  Must
    determine number of time_steps for canvas.

 3. Adjust data1(2)_vecs sizes and initialize from data.

 3.5. Resize localMoran, sigLocalMoran, sigCat, and cluster arrays

 4. If rates, then calculate rates for working_data1

 5. Standardize working_data1 (and 2 if bivariate)

 6. Compute LISA for all time-stesp and save in localMoran sp/time array

 7. Calc Pseudo P for all time periods.  Results saved in sigLocalMoran,
    sigCat and cluster arrays

 8. Notify clients that values have been updated.

 */

LisaCoordinator::
LisaCoordinator(boost::uuids::uuid weights_id,
                Project* project,
                const std::vector<GdaVarTools::VarInfo>& var_info_s,
                const std::vector<int>& col_ids,
                LisaType lisa_type_s,
                bool calc_significances_s,
                bool row_standardize_s)
: w_man_state(project->GetWManState()),
w_man_int(project->GetWManInt()),
w_id(weights_id),
num_obs(project->GetNumRecords()),
permutations(999),
lisa_type(lisa_type_s),
calc_significances(calc_significances_s),
isBivariate(lisa_type_s == bivariate),
var_info(var_info_s),
data(var_info_s.size()),
undef_data(var_info_s.size()),
last_seed_used(0), reuse_last_seed(false),
row_standardize(row_standardize_s)
{
    reuse_last_seed = GdaConst::use_gda_user_seed;
    if ( GdaConst::use_gda_user_seed) {
        last_seed_used = GdaConst::gda_user_seed;
    }

	TableInterface* table_int = project->GetTableInt();
	for (int i=0; i<var_info.size(); i++) {
		table_int->GetColData(col_ids[i], data[i]);
        table_int->GetColUndefined(col_ids[i], undef_data[i]);
        var_info[i].is_moran = true;
	}

    undef_tms.resize(var_info_s[0].time_max - var_info_s[0].time_min + 1);

	weight_name = w_man_int->GetLongDispName(w_id);

    weights = w_man_int->GetGal(w_id);

	SetSignificanceFilter(1);

	InitFromVarInfo();
	w_man_state->registerObserver(this);
}

LisaCoordinator::
LisaCoordinator(wxString weights_path,
                int n,
                std::vector<double> vals_1,
                std::vector<double> vals_2,
                int lisa_type_s,
                int permutations_s,
                bool calc_significances_s,
                bool row_standardize_s)
{
    num_obs = n;
    num_time_vals = 1;
    permutations = permutations_s;
    calc_significances = calc_significances_s;
    row_standardize = row_standardize_s;
    last_seed_used = 0;
    reuse_last_seed = false;
    isBivariate = false;

    // std::vector<GdaVarTools::VarInfo> var_info;
    int num_vars = 1;
    isBivariate = false;

    if (lisa_type_s == 0) {
        lisa_type = univariate;

    } else if (lisa_type_s == 1) {
        lisa_type = bivariate;
        isBivariate = true;
        num_vars = 2;

    } else if (lisa_type_s == 2) {
        lisa_type = eb_rate_standardized;
        num_vars = 2;

    } else if (lisa_type_s == 3) {
        lisa_type = differential;
        num_vars = 2;
    }

    undef_tms.resize(num_time_vals);
    data.resize(num_vars);
    undef_data.resize(num_vars);
    var_info.resize(num_vars);

    // don't handle time variable for now
    for (int i=0; i<var_info.size(); i++) {
        data[i].resize(boost::extents[num_time_vals][num_obs]);
        undef_data[i].resize(boost::extents[num_time_vals][num_obs]);
        var_info[i].is_moran = true;
        var_info[i].is_time_variant = false;
        var_info[i].fixed_scale = true;
        var_info[i].sync_with_global_time  = false;
        var_info[i].time_max = 0;
        var_info[i].time_min = 0;
    }

    for (int i=0; i<num_obs; i++) {
        data[0][0][i] = vals_1[i];
        undef_data[0][0][i] = false;
    }
    if (num_vars == 2) {
        for (int i=0; i<num_obs; i++) {
            data[1][0][i] = vals_1[i];
            undef_data[1][0][i] = false;
        }
    }

    // create weights
    w_man_state = NULL;
    w_man_int = NULL;

    wxString ext = GenUtils::GetFileExt(weights_path).Lower();
    GalElement* tempGal = 0;
    if (ext == "gal") {
        tempGal = WeightUtils::ReadGal(weights_path, NULL);
    } else {
        tempGal = WeightUtils::ReadGwtAsGal(weights_path, NULL);
    }

    weights = new GalWeight();
    weights->num_obs = num_obs;
    weights->wflnm = weights_path;
    weights->id_field = "ogc_fid";
    weights->gal = tempGal;

    SetSignificanceFilter(1);
    InitFromVarInfo();
}

LisaCoordinator::~LisaCoordinator()
{
    if (w_man_state) {
        w_man_state->removeObserver(this);
    }
	DeallocateVectors();
}

void LisaCoordinator::DeallocateVectors()
{
	for (int i=0; i<lags_vecs.size(); i++) {
		if (lags_vecs[i]) delete [] lags_vecs[i];
	}
	lags_vecs.clear();

	for (int i=0; i<local_moran_vecs.size(); i++) {
		if (local_moran_vecs[i]) delete [] local_moran_vecs[i];
	}
	local_moran_vecs.clear();
	for (int i=0; i<sig_local_moran_vecs.size(); i++) {
		if (sig_local_moran_vecs[i]) delete [] sig_local_moran_vecs[i];
	}
	sig_local_moran_vecs.clear();
	for (int i=0; i<sig_cat_vecs.size(); i++) {
		if (sig_cat_vecs[i]) delete [] sig_cat_vecs[i];
	}
	sig_cat_vecs.clear();
	for (int i=0; i<cluster_vecs.size(); i++) {
		if (cluster_vecs[i]) delete [] cluster_vecs[i];
	}
	cluster_vecs.clear();
	for (int i=0; i<data1_vecs.size(); i++) {
		if (data1_vecs[i]) delete [] data1_vecs[i];
	}
	data1_vecs.clear();
	for (int i=0; i<data2_vecs.size(); i++) {
		if (data2_vecs[i]) delete [] data2_vecs[i];
	}
	data2_vecs.clear();

    // clear W_vecs
    for (size_t i=0; i<has_undefined.size(); i++) {
        if (has_undefined[i]) {
            // clean the copied weights
            delete Gal_vecs[i];
        }
    }
    Gal_vecs.clear();
}

/** allocate based on var_info and num_time_vals **/
void LisaCoordinator::AllocateVectors()
{
	int tms = num_time_vals;

	lags_vecs.resize(tms);
	local_moran_vecs.resize(tms);
	sig_local_moran_vecs.resize(tms);
	sig_cat_vecs.resize(tms);
	cluster_vecs.resize(tms);
	data1_vecs.resize(tms);
	map_valid.resize(tms);
	map_error_message.resize(tms);
	has_isolates.resize(tms);
	has_undefined.resize(tms);

	for (int i=0; i<tms; i++) {
		lags_vecs[i] = new double[num_obs];
		local_moran_vecs[i] = new double[num_obs];
		if (calc_significances) {
			sig_local_moran_vecs[i] = new double[num_obs];
			sig_cat_vecs[i] = new int[num_obs];
		}
		cluster_vecs[i] = new int[num_obs];
		data1_vecs[i] = new double[num_obs];
		map_valid[i] = true;
		map_error_message[i] = wxEmptyString;
	}

	if (lisa_type == bivariate) {
		data2_vecs.resize((var_info[1].time_max - var_info[1].time_min) + 1);
		for (int i=0; i<data2_vecs.size(); i++) {
			data2_vecs[i] = new double[num_obs];
		}
	}
}

/** We assume only that var_info is initialized correctly.
 ref_var_index, is_any_time_variant, is_any_sync_with_global_time and
 num_time_vals are first updated based on var_info */
void LisaCoordinator::InitFromVarInfo()
{
	DeallocateVectors();

	num_time_vals = 1;
    is_any_time_variant = false;
    is_any_sync_with_global_time = false;
    ref_var_index = -1;

    if (lisa_type != differential) {
        for (int i=0; i<var_info.size(); i++) {
            if (var_info[i].is_time_variant && var_info[i].sync_with_global_time) {
                num_time_vals = (var_info[i].time_max - var_info[i].time_min) + 1;
                is_any_sync_with_global_time = true;
                ref_var_index = i;
                break;
            }
        }
        for (int i=0; i<var_info.size(); i++) {
            if (var_info[i].is_time_variant) {
                is_any_time_variant = true;
                break;
            }
        }
    }

	AllocateVectors();

    if (lisa_type == differential) {
        int t=0;
        for (int i=0; i<num_obs; i++) {
            int t0 = var_info[0].time;
            int t1 = var_info[1].time;
            data1_vecs[0][i] = data[0][t0][i] - data[0][t1][i];
        }

    } else if (lisa_type == univariate || lisa_type == bivariate) {
		for (int t=var_info[0].time_min; t<=var_info[0].time_max; t++) {
			int d1_t = t - var_info[0].time_min;
            for (int i=0; i<num_obs; i++) {
                data1_vecs[d1_t][i] = data[0][t][i];
            }
		}
		if (lisa_type == bivariate) {
			for (int t=var_info[1].time_min; t<=var_info[1].time_max; t++) {
				int d2_t = t - var_info[1].time_min;
				for (int i=0; i<num_obs; i++) {
					data2_vecs[d2_t][i] = data[1][t][i];
				}
			}
		}
	} else { // lisa_type == eb_rate_standardized
		std::vector<bool> undef_res(num_obs, false);
		double* smoothed_results = new double[num_obs];
		double* E = new double[num_obs]; // E corresponds to var_info[0]
		double* P = new double[num_obs]; // P corresponds to var_info[1]
		// we will only fill data1 for eb_rate_standardized and
		// further lisa calcs will treat as univariate
		for (int t=0; t<num_time_vals; t++) {
			int v0_t = var_info[0].time_min;
			if (var_info[0].is_time_variant &&
				var_info[0].sync_with_global_time) {
				v0_t += t;
			}
			for (int i=0; i<num_obs; i++) E[i] = data[0][v0_t][i];
			int v1_t = var_info[1].time_min;
			if (var_info[1].is_time_variant &&
				var_info[1].sync_with_global_time) {
				v1_t += t;
			}
			for (int i=0; i<num_obs; i++) P[i] = data[1][v1_t][i];
			bool success = GdaAlgs::RateStandardizeEB(num_obs, P, E,
														smoothed_results,
														undef_res);
			if (!success) {
				map_valid[t] = false;
				map_error_message[t] << "Emprical Bayes Rate ";
				map_error_message[t] << "Standardization failed.";
			} else {
				for (int i=0; i<num_obs; i++) {
					data1_vecs[t][i] = smoothed_results[i];
				}
			}
		}
		if (smoothed_results) delete [] smoothed_results;
		if (E) delete [] E;
		if (P) delete [] P;
	}

	StandardizeData();

    CalcLisa();

    if (calc_significances) {
        CalcPseudoP();
    }

}

void LisaCoordinator::GetRawData(int time, double* data1, double* data2)
{
    if (lisa_type == differential) {
        int t=0;
        for (int i=0; i<num_obs; i++) {
            int t0 = var_info[0].time;
            int t1 = var_info[1].time;
            data1[i] = data[0][t0][i] - data[0][t1][i];
        }

    } else if (lisa_type == univariate || lisa_type == bivariate) {
        for (int i=0; i<num_obs; i++) {
            data1[i] = data[0][time][i];
        }
        if (lisa_type == bivariate) {
            for (int i=0; i<num_obs; i++) {
                data2[i] = data[1][time][i];
            }
        }
    } else { // lisa_type == eb_rate_standardized
        std::vector<bool> undef_res(num_obs, false);
        double* smoothed_results = new double[num_obs];
        double* E = new double[num_obs]; // E corresponds to var_info[0]
        double* P = new double[num_obs]; // P corresponds to var_info[1]
        // we will only fill data1 for eb_rate_standardized and
        // further lisa calcs will treat as univariate
        for (int i=0; i<num_obs; i++) {
            E[i] = data[0][time][i];
        }
        for (int i=0; i<num_obs; i++) {
            P[i] = data[1][time][i];
        }
        bool success = GdaAlgs::RateStandardizeEB(num_obs, P, E,
                                                  smoothed_results,
                                                  undef_res);
        if (success) {
            for (int i=0; i<num_obs; i++) {
                data1[i] = smoothed_results[i];
            }
        }
        if (smoothed_results) delete [] smoothed_results;
        if (E) delete [] E;
        if (P) delete [] P;
    }
}

/** Update Secondary Attributes based on Primary Attributes.
 Update num_time_vals and ref_var_index based on Secondary Attributes. */
void LisaCoordinator::VarInfoAttributeChange()
{
	GdaVarTools::UpdateVarInfoSecondaryAttribs(var_info);

	is_any_time_variant = false;
	is_any_sync_with_global_time = false;
	for (int i=0; i<var_info.size(); i++) {
		if (var_info[i].is_time_variant) is_any_time_variant = true;
		if (var_info[i].sync_with_global_time) {
			is_any_sync_with_global_time = true;
		}
	}
	ref_var_index = -1;
	num_time_vals = 1;
	for (int i=0; i<var_info.size() && ref_var_index == -1; i++) {
		if (var_info[i].is_ref_variable) ref_var_index = i;
	}
	if (ref_var_index != -1) {
		num_time_vals = (var_info[ref_var_index].time_max -
						 var_info[ref_var_index].time_min) + 1;
	}
	//GdaVarTools::PrintVarInfoVector(var_info);
}

void LisaCoordinator::StandardizeData()
{
	for (int t=0; t<data1_vecs.size(); t++) {
        undef_tms[t].resize(num_obs);

        for (int i=0; i<num_obs; i++) {
            undef_tms[t][i] = undef_tms[t][i] || undef_data[0][t][i];
        }
        if (isBivariate) {
            for (int i=0; i<num_obs; i++) {
                if ( undef_data[1].size() > t ) {
                    undef_tms[t][i] = undef_tms[t][i] || undef_data[1][t][i];
                }
            }
        }
    }

	for (int t=0; t<data1_vecs.size(); t++) {
		GenUtils::StandardizeData(num_obs, data1_vecs[t], undef_tms[t]);
        if (isBivariate) {
            if (data2_vecs.size() > t)
                GenUtils::StandardizeData(num_obs, data2_vecs[t], undef_tms[t]);
        }
	}
}

/** assumes StandardizeData already called on data1 and data2 */
void LisaCoordinator::CalcLisa()
{
	for (int t=0; t<num_time_vals; t++) {
		data1 = data1_vecs[t];
		if (isBivariate) {
			data2 = data2_vecs[0];
			if (var_info[1].is_time_variant && var_info[1].sync_with_global_time)
                data2 = data2_vecs[t];
		}
		lags = lags_vecs[t];
		localMoran = local_moran_vecs[t];
		cluster = cluster_vecs[t];

		has_isolates[t] = false;

        // get undefs of objects/values at this time step
        std::vector<bool> undefs;
        bool has_undef = false;
        for (int i=0; i<undef_data[0][t].size(); i++){
            bool is_undef = undef_data[0][t][i];
            if (isBivariate) {
                if (undef_data[1].size() > t)
                    is_undef = is_undef || undef_data[1][t][i];
            }
            if (is_undef && !has_undef) {
                has_undef = true;
            }
            undefs.push_back(is_undef);
        }
        has_undefined[t] = has_undef;

        // local weights copy
        GalWeight* gw = NULL;
        if ( has_undef ) {
            gw = new GalWeight(*weights);
            gw->Update(undefs);
        } else {
            gw = weights;
        }
        GalElement* W = gw->gal;
        Gal_vecs.push_back(gw);
        Gal_vecs_orig.push_back(weights);

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

            if (undefs[i] == true) {
                lags[i] = 0;
                localMoran[i] = 0;
                cluster[i] = 6; // undefined value
                continue;
            }

			double Wdata = 0;
			if (isBivariate) {
				Wdata = W[i].SpatialLag(data2);
			} else {
				Wdata = W[i].SpatialLag(data1);
			}
			lags[i] = Wdata;
			localMoran[i] = data1[i] * Wdata;

			// assign the cluster
			if (W[i].Size() > 0) {
				if (data1[i] > 0 && Wdata < 0) cluster[i] = 4;
				else if (data1[i] < 0 && Wdata > 0) cluster[i] = 3;
				else if (data1[i] < 0 && Wdata < 0) cluster[i] = 2;
				else cluster[i] = 1; //data1[i] > 0 && Wdata > 0
			} else {
				has_isolates[t] = true;
				cluster[i] = 5; // neighborless
			}
		}
	}
}

void LisaCoordinator::CalcPseudoP()
{
	if (!calc_significances) return;
	wxStopWatch sw;
	int nCPUs = wxThread::GetCPUCount();

	// To ensure thread safety, only work on one time slice of data
	// at a time.  For each time period t:
	// 1. copy data for time period t into data1 and data2 arrays
	// 2. Perform multi-threaded computation
	// 3. copy results into results array

	for (int t=0; t<num_time_vals; t++) {

        std::vector<bool>& undefs = undef_tms[t];
		data1 = data1_vecs[t];
		if (isBivariate) {
			data2 = data2_vecs[0];
			if (var_info[1].is_time_variant &&
				var_info[1].sync_with_global_time)
                data2 = data2_vecs[t];
		}
		lags = lags_vecs[t];
		localMoran = local_moran_vecs[t];
		sigLocalMoran = sig_local_moran_vecs[t];
		sigCat = sig_cat_vecs[t];
		cluster = cluster_vecs[t];

		if (nCPUs <= 1 || num_obs <= nCPUs * 10) {
            if (!reuse_last_seed) {
                last_seed_used = time(0);
            }
			CalcPseudoP_range(Gal_vecs[t]->gal, undefs,
                              0, num_obs-1, last_seed_used);
		} else {
			CalcPseudoP_threaded(Gal_vecs[t]->gal, undefs);
			//CalcPseudoP_range(Gal_vecs[t]->gal, undefs,
                        //      0, num_obs-1, last_seed_used);
		}
	}

	{
		wxString m;
		m << "LISA on " << num_obs << " obs with " << permutations;
		m << " perms over " << num_time_vals << " time periods took ";
		m << sw.Time() << " ms. Last seed used: " << last_seed_used;
	}
	LOG_MSG("Exiting LisaCoordinator::CalcPseudoP");
}

void LisaCoordinator::CalcPseudoP_threaded(const GalElement* W,
                                           const std::vector<bool>& undefs)
{
	int nCPUs = wxThread::GetCPUCount();

	// mutext protects access to the worker_list
    wxMutex worker_list_mutex;
	// signals that worker_list is empty
	wxCondition worker_list_empty_cond(worker_list_mutex);
    // mutex should be initially locked
	worker_list_mutex.Lock();

    // List of all the threads currently alive.  As soon as the thread
	// terminates, it removes itself from the list.
	std::list<wxThread*> worker_list;

	// divide up work according to number of observations
	// and number of CPUs
	int work_chunk = num_obs / nCPUs;

    if (work_chunk == 0) {
        work_chunk = 1;
    }

	int obs_start = 0;
	int obs_end = obs_start + work_chunk;

	bool is_thread_error = false;
	int quotient = num_obs / nCPUs;
	int remainder = num_obs % nCPUs;
	int tot_threads = (quotient > 0) ? nCPUs : remainder;

    boost::thread_group threadPool;

	if (!reuse_last_seed)
        last_seed_used = time(0);
	for (int i=0; i<tot_threads && !is_thread_error; i++) {
		int a=0;
		int b=0;
		if (i < remainder) {
			a = i*(quotient+1);
			b = a+quotient;
		} else {
			a = remainder*(quotient+1) + (i-remainder)*quotient;
			b = a+quotient-1;
		}
		uint64_t seed_start = last_seed_used+a;
		uint64_t seed_end = seed_start + ((uint64_t) (b-a));
		int thread_id = i+1;
		wxString msg;
		msg << "thread " << thread_id << ": " << a << "->" << b;
		msg << ", seed: " << seed_start << "->" << seed_end;

        /*
		LisaWorkerThread* thread =
			new LisaWorkerThread(W, undefs, a, b, seed_start, this,
								 &worker_list_mutex,
								 &worker_list_empty_cond,
								 &worker_list, thread_id);
		if ( thread->Create() != wxTHREAD_NO_ERROR ) {
			delete thread;
			is_thread_error = true;
		} else {
			worker_list.push_front(thread);
		}
         */
        boost::thread* worker = new boost::thread(boost::bind(&LisaCoordinator::CalcPseudoP_range,this, W, undefs, a, b, seed_start));
        threadPool.add_thread(worker);
	}
    threadPool.join_all();
    /*
	if (is_thread_error) {
		// fall back to single thread calculation mode
		CalcPseudoP_range(W, undefs, 0, num_obs-1, last_seed_used);
	} else {
		std::list<wxThread*>::iterator it;
		for (it = worker_list.begin(); it != worker_list.end(); it++) {
			(*it)->Run();
		}

		while (!worker_list.empty()) {
			// wait until thread_list might be empty
			worker_list_empty_cond.Wait();
			// We have been woken up. If this was not a false
			// alarm (sprious signal), the loop will exit.
		}
	}
     */
}

void LisaCoordinator::CalcPseudoP_range(const GalElement* W,
                                        const std::vector<bool>& undefs,
                                        int obs_start, int obs_end,
										uint64_t seed_start)
{
	GeoDaSet workPermutation(num_obs);
	//Randik rng;
	int max_rand = num_obs-1;
	for (int cnt=obs_start; cnt<=obs_end; cnt++) {

        if (undefs[cnt])
            continue;

		const int numNeighbors = W[cnt].Size();

		uint64_t countLarger = 0;
		for (int perm=0; perm<permutations; perm++) {
			int rand=0;
			while (rand < numNeighbors) {
				// computing 'perfect' permutation of given size
                double rng_val = Gda::ThomasWangHashDouble(seed_start++) * max_rand;
                // round is needed to fix issue
                //https://github.com/GeoDaCenter/geoda/issues/488
				int newRandom = (int) (rng_val < 0.0 ? ceil(rng_val - 0.5) : floor(rng_val + 0.5));
				if (newRandom != cnt &&
                    !workPermutation.Belongs(newRandom) &&
                    undefs[newRandom] == false)
				{
					workPermutation.Push(newRandom);
					rand++;
				}
			}
			double permutedLag=0;
			// use permutation to compute the lag
			// compute the lag for binary weights
			if (isBivariate) {
				for (int cp=0; cp<numNeighbors; cp++) {
					permutedLag += data2[workPermutation.Pop()];
				}
			} else {
				for (int cp=0; cp<numNeighbors; cp++) {
					permutedLag += data1[workPermutation.Pop()];
				}
			}

			//NOTE: we shouldn't have to row-standardize or
			// multiply by data1[cnt]
            if (numNeighbors && row_standardize) {
                permutedLag /= numNeighbors;
            }
			const double localMoranPermuted = permutedLag * data1[cnt];
            if (localMoranPermuted >= localMoran[cnt]) {
                countLarger++;
            }
		}
		// pick the smallest
		if (permutations-countLarger <= countLarger) {
			countLarger = permutations-countLarger;
		}

		sigLocalMoran[cnt] = (countLarger+1.0)/(permutations+1);
		// 'significance' of local Moran
		if (sigLocalMoran[cnt] <= 0.0001) sigCat[cnt] = 4;
		else if (sigLocalMoran[cnt] <= 0.001) sigCat[cnt] = 3;
		else if (sigLocalMoran[cnt] <= 0.01) sigCat[cnt] = 2;
		else if (sigLocalMoran[cnt] <= 0.05) sigCat[cnt]= 1;
		else sigCat[cnt]= 0;

		// observations with no neighbors get marked as isolates
        // NOTE: undefined should be marked as well, however, since undefined_cat has covered undefined category, we don't need to handle here
		if (numNeighbors == 0) {
			sigCat[cnt] = 5;
		}
	}
}

void LisaCoordinator::SetSignificanceFilter(int filter_id)
{
	// 0: >0.05 1: 0.05, 2: 0.01, 3: 0.001, 4: 0.0001
	if (filter_id < 1 || filter_id > 4) return;
	significance_filter = filter_id;
	if (filter_id == 1) significance_cutoff = 0.05;
	if (filter_id == 2) significance_cutoff = 0.01;
	if (filter_id == 3) significance_cutoff = 0.001;
	if (filter_id == 4) significance_cutoff = 0.0001;
}

void LisaCoordinator::update(WeightsManState* o)
{
    if (w_man_int) {
        weight_name = w_man_int->GetLongDispName(w_id);
    }
}

int LisaCoordinator::numMustCloseToRemove(boost::uuids::uuid id) const
{
	return id == w_id ? observers.size() : 0;
}

void LisaCoordinator::closeObserver(boost::uuids::uuid id)
{
	if (numMustCloseToRemove(id) == 0) return;
	std::list<LisaCoordinatorObserver*> obs_cpy = observers;
	for (std::list<LisaCoordinatorObserver*>::iterator i=obs_cpy.begin();
		 i != obs_cpy.end(); ++i) {
		(*i)->closeObserver(this);
	}
}

void LisaCoordinator::registerObserver(LisaCoordinatorObserver* o)
{
	observers.push_front(o);
}

void LisaCoordinator::removeObserver(LisaCoordinatorObserver* o)
{
	LOG_MSG("Entering LisaCoordinator::removeObserver");
	observers.remove(o);
	LOG(observers.size());
	if (observers.size() == 0) {
		delete this;
	}
	LOG_MSG("Exiting LisaCoordinator::removeObserver");
}

void LisaCoordinator::notifyObservers()
{
	for (std::list<LisaCoordinatorObserver*>::iterator  it=observers.begin();
		 it != observers.end(); ++it) {
		(*it)->update(this);
	}
}


#define PY_ARRAY_UNIQUE_SYMBOL pbcvt_ARRAY_API

#define NUMPY_IMPORT_ARRAY_RETVAL NULL

#include <boost/python.hpp>
#include <pyboostcvconverter/pyboostcvconverter.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/highgui.hpp>
#include "sift.simd.hpp"
#include <iostream>

#include <algorithm>

const float sigma = 1.6;
const int nOctaveLayers = 3;

std::string getImageType(int number)
{
    // find type
    int imgTypeInt = number%8;
    std::string imgTypeString;

    switch (imgTypeInt)
    {
        case 0:
            imgTypeString = "8U";
            break;
        case 1:
            imgTypeString = "8S";
            break;
        case 2:
            imgTypeString = "16U";
            break;
        case 3:
            imgTypeString = "16S";
            break;
        case 4:
            imgTypeString = "32S";
            break;
        case 5:
            imgTypeString = "32F";
            break;
        case 6:
            imgTypeString = "64F";
            break;
        default:
            break;
    }

    // find channel
    int channel = (number/8) + 1;

    std::stringstream type;
    type<<"CV_"<<imgTypeString<<"C"<<channel;

    return type.str();
}

static cv::Mat createInitialImage( const cv::Mat& img, bool doubleImageSize, float sigma )
{
    CV_TRACE_FUNCTION();

    cv::Mat gray, gray_fpt;
    if( img.channels() == 3 || img.channels() == 4 )
    {
        cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
        gray.convertTo(gray_fpt, cv::DataType<cv::sift_wt>::type, cv::SIFT_FIXPT_SCALE, 0);
    }
    else
        img.convertTo(gray_fpt, cv::DataType<cv::sift_wt>::type, cv::SIFT_FIXPT_SCALE, 0);

    float sig_diff;

    if( doubleImageSize )
    {
        sig_diff = sqrtf( std::max(sigma * sigma - cv::SIFT_INIT_SIGMA * cv::SIFT_INIT_SIGMA * 4, 0.01f) );
        cv::Mat dbl;
#if DoG_TYPE_SHORT
        cv::resize(gray_fpt, dbl, cv::Size(gray_fpt.cols*2, gray_fpt.rows*2), 0, 0, cv::INTER_LINEAR_EXACT);
#else
        cv::resize(gray_fpt, dbl, cv::Size(gray_fpt.cols*2, gray_fpt.rows*2), 0, 0, cv::INTER_LINEAR);
#endif
        cv::Mat result;
        cv::GaussianBlur(dbl, result, cv::Size(), sig_diff, sig_diff);
        return result;
    }
    else
    {
        sig_diff = sqrtf( std::max(sigma * sigma - cv::SIFT_INIT_SIGMA * cv::SIFT_INIT_SIGMA, 0.01f) );
        cv::Mat result;
        cv::GaussianBlur(gray_fpt, result, cv::Size(), sig_diff, sig_diff);
        return result;
    }
}

std::vector<cv::Mat>
build_image_pyramid(cv::Mat im, int firstOctave, int nOctaves) {

    // Copyright (c) 2006-2010, Rob Hess <hess@eecs.oregonstate.edu>
    // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
    // Copyright (C) 2020, Intel Corporation, all rights reserved.
    cv::Mat base = createInitialImage(im, firstOctave<0, sigma);
    printf("%i, %i\n", base.rows, base.cols);
    CV_TRACE_FUNCTION();
    std::vector<cv::Mat> gauss_pyr;

    std::vector<double> sig(nOctaveLayers + 3);
    gauss_pyr.resize(nOctaves*(nOctaveLayers + 3));

    // precompute Gaussian sigmas using the following formula:
    //  \sigma_{total}^2 = \sigma_{i}^2 + \sigma_{i-1}^2
    sig[0] = sigma;
    double k = std::pow( 2., 1. / nOctaveLayers );
    for( int i = 1; i < nOctaveLayers + 3; i++ )
    {
        double sig_prev = std::pow(k, (double)(i-1))*sigma;
        double sig_total = sig_prev*k;
        sig[i] = std::sqrt(sig_total*sig_total - sig_prev*sig_prev);
    }

    for( int o = 0; o < nOctaves; o++ )
    {
        for( int i = 0; i < nOctaveLayers + 3; i++ )
        {
            cv::Mat& dst = gauss_pyr[o*(nOctaveLayers + 3) + i];
            if( o == 0  &&  i == 0 )
                dst = base;
            // base of new octave is halved image from end of previous octave
            else if( i == 0 )
            {
                const cv::Mat& src = gauss_pyr[(o-1)*(nOctaveLayers + 3) + nOctaveLayers];
                cv::resize(src, dst, cv::Size(src.cols/2, src.rows/2),
                       0, 0, cv::INTER_NEAREST);
            }
            else
            {
                const cv::Mat& src = gauss_pyr[o*(nOctaveLayers + 3) + i-1];
                cv::GaussianBlur(src, dst, cv::Size(), sig[i], sig[i]);
            }
        }
    }
    return gauss_pyr;
}

namespace pbcvt {

  using namespace boost::python;
  PyObject *dog_pyramid(PyObject *impy, int firstOctave, int nOctaves) {
    cv::Mat im;
    im = pbcvt::fromNDArrayToMat(impy);
    std::vector<cv::Mat> dogpyr;
    dogpyr.resize(nOctaves * (nOctaveLayers + 2));
    std::vector<cv::Mat> gauss_pyr = build_image_pyramid(im, firstOctave, nOctaves);
    for( int a=0; a < nOctaves * (nOctaveLayers + 2); a++ ) {
      const int o = a / (nOctaveLayers + 2);
      const int i = a % (nOctaveLayers + 2);

      const cv::Mat& src1 = gauss_pyr[o*(nOctaveLayers + 3) + i];
      const cv::Mat& src2 = gauss_pyr[o*(nOctaveLayers + 3) + i + 1];
      cv::Mat& dst = dogpyr[o*(nOctaveLayers + 2) + i];
      cv::subtract(src2, src1, dst, noArray(), DataType<sift_wt>::type);
    }
    PyObject *ret = PyList_New(dogpyr.size());
    for (int i=0; i<dogpyr.size(); i++) {
      PyObject* layer = pbcvt::fromMatToNDArray(dogpyr.at(i));
      PyList_SetItem(ret, i, layer);
    }
    return ret;
  }

  PyObject *image_pyramid(PyObject *impy, int firstOctave, int nOctaves) {
    cv::Mat im;
    im = pbcvt::fromNDArrayToMat(impy);
    std::vector<cv::Mat> gauss_pyr = build_image_pyramid(im, firstOctave, nOctaves);
    // cv::Mat out_kpts_t;
    // cv::vconcat(out_kpts, out_kpts_t);
    // PyObject *ret = pbcvt::fromMatToNDArray(out_kpts_t);
    PyObject *ret = PyList_New(gauss_pyr.size());
    for (int i=0; i<gauss_pyr.size(); i++) {
      PyObject* layer = pbcvt::fromMatToNDArray(gauss_pyr.at(i));
      PyList_SetItem(ret, i, layer);
    }
    return ret;
  }

  PyObject *sift_desc(PyObject *impy, PyObject *keypoints, int firstOctave, int nOctaves) {
    // IDK if this is clear but there is a lot of opencv code in here

    cv::Mat im, kpts;
    im = pbcvt::fromNDArrayToMat(impy);
    kpts = pbcvt::fromNDArrayToMat(keypoints);
    // First, build a gauss_pyr
    std::vector<cv::Mat> gauss_pyr = build_image_pyramid(im, firstOctave, nOctaves);

    std::vector<cv::Mat> out_kpts;

    // Compute orientation of keypoints
    for (int i=0; i<kpts.rows; i++) {
      // Get attributes of keypoint
      float x = kpts.at<float>(i, 0);
      float y = kpts.at<float>(i, 1);
      float size = kpts.at<float>(i, 2);
      int layer = kpts.at<float>(i, 3);
      int octave = kpts.at<float>(i, 4) - firstOctave;
      float c = x / (1 << octave);
      float r = y / (1 << octave);
      if (firstOctave < 0) {
        size *= (1 << -firstOctave);
        c *= (1 << -firstOctave);
        r *= (1 << -firstOctave);
      }

      // Init variables
      int n = cv::SIFT_ORI_HIST_BINS;
      float hist[n];
      float scl_octv = size*0.5f/(1 << octave);

      // std::cout << gauss_pyr[octave*(nOctaveLayers+3) + layer].depth() << std::endl;
      // cv::imshow("HI", gauss_pyr[octave*(nOctaveLayers+3) + layer]);
      // cv::waitKey(0);
      float omax = cv::opt_CV_CPU_DISPATCH_MODE::calcOrientationHist(
          gauss_pyr[octave*(nOctaveLayers+3) + layer],
          cv::Point(c, r),
          cvRound(cv::SIFT_ORI_RADIUS * scl_octv),
          cv::SIFT_ORI_SIG_FCTR * scl_octv,
          hist, n);

      float mag_thr = (float)(omax * cv::SIFT_ORI_PEAK_RATIO);
      for( int j = 0; j < n; j++ )
      {
        int l = j > 0 ? j - 1 : n - 1;
        int r2 = j < n-1 ? j + 1 : 0;

        if( hist[j] > hist[l]  &&  hist[j] > hist[r2]  &&  hist[j] >= mag_thr )
        {
          float bin = j + 0.5f * (hist[l]-hist[r2]) / (hist[l] - 2*hist[j] + hist[r2]);
          bin = bin < 0 ? n + bin : bin >= n ? bin - n : bin;
          float angle = 360.f - (float)((360.f/n) * bin);
          if(std::abs(angle - 360.f) < FLT_EPSILON)
            angle = 0.f;

          cv::Mat kpt = cv::Mat::zeros(1, 6, CV_32F);
          kpt.at<float>(0, 0) = x;
          kpt.at<float>(0, 1) = y;
          kpt.at<float>(0, 2) = size;
          kpt.at<float>(0, 3) = layer;
          kpt.at<float>(0, 4) = octave;
          kpt.at<float>(0, 5) = angle;
          if (firstOctave < 0) {
            kpt.at<float>(0, 2) /= (1 << -firstOctave);
            kpt.at<float>(0, 4) += firstOctave;
          }
          out_kpts.push_back(kpt);
        }
      }
    }

    cv::Mat out_kpts_t;
    cv::vconcat(out_kpts, out_kpts_t);
    PyObject *ret = pbcvt::fromMatToNDArray(out_kpts_t);
    return ret;
  }

#if (PY_VERSION_HEX >= 0x03000000)

  static void *init_ar() {
#else
    static void init_ar(){
#endif
      Py_Initialize();

      import_array();
      return NUMPY_IMPORT_ARRAY_RETVAL;
    }

    BOOST_PYTHON_MODULE (sift_ori) {
      //using namespace XM;
      init_ar();

      //initialize converters
      to_python_converter<cv::Mat,pbcvt::matToNDArrayBoostConverter>();
      matFromNDArrayBoostConverter();

      //expose module-level functions
      def("sift_desc", sift_desc);
      def("image_pyramid", image_pyramid);
      def("dog_pyramid", dog_pyramid);
    }

  } //end namespace pbcvt


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson, John B. Mains
//
// 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.

#ifndef SMOOTH__FEEDBACK__QP_SOLVER_HPP_
#define SMOOTH__FEEDBACK__QP_SOLVER_HPP_

/**
 * @file
 * @brief Quadratic Program solver.
 */

#include <Eigen/Cholesky>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>

#include <chrono>
#include <iomanip>
#include <iostream>
#include <limits>
#include <optional>

#include "qp.hpp"
#include "utils/sparse.hpp"

namespace smooth::feedback {

/**
 * @brief Options for solve_qp
 */
struct QPSolverParams
{
  /// print solver info to stdout
  bool verbose = false;

  /// relaxation parameter
  float alpha = 1.6;
  /// first dual step size
  float rho = 0.1;
  /// second dual step length
  float sigma = 1e-6;

  /// scale problem
  bool scaling = true;

  /// absolute threshold for convergence
  float eps_abs = 1e-3;
  /// relative threshold for convergence
  float eps_rel = 1e-3;
  /// threshold for primal infeasibility
  float eps_primal_inf = 1e-4;
  /// threshold for dual infeasibility
  float eps_dual_inf = 1e-4;

  /// max number of iterations (default no limit)
  std::optional<uint32_t> max_iter = {};

  /// max solution time (default no limit)
  std::optional<std::chrono::nanoseconds> max_time = {};

  /// iterations between checking stopping criterion
  uint32_t stop_check_iter = 25;

  /// run solution polishing (uses dynamic memory)
  bool polish = true;
  /// number of iterations to refine polish
  uint32_t polish_iter = 5;
  /// regularization parameter for polishing
  float delta = 1e-6;
};

namespace detail {

template<typename Pbm>
using qp_solution_t = QPSolution<
  decltype(Pbm::A)::RowsAtCompileTime,
  decltype(Pbm::A)::ColsAtCompileTime,
  typename decltype(Pbm::A)::Scalar>;

/**
 * @brief Polish solution of quadratic program
 *
 * @tparam Pbm problem type
 *
 * @param[in] pbm problem formulation
 * @param[in, out] sol solution to polish
 * @param[in] prm solver options
 * @param[in] c cost scaling
 * @param[in] sx variable scaling
 * @param[in] sy constraint scaling
 *
 * @warning This function allocates dynamic memory even for static-sized problems.
 */
template<typename Pbm, typename D1, typename D2>
bool polish_qp(
  const Pbm & pbm,
  qp_solution_t<Pbm> & sol,
  const QPSolverParams & prm,
  const typename decltype(Pbm::A)::Scalar c,
  const Eigen::MatrixBase<D1> & sx,
  const Eigen::MatrixBase<D2> & sy)
{
  using AmatT                  = decltype(Pbm::A);
  using Scalar                 = typename AmatT::Scalar;
  static constexpr bool sparse = std::is_base_of_v<Eigen::SparseMatrixBase<AmatT>, AmatT>;

  static constexpr Scalar inf = std::numeric_limits<Scalar>::infinity();
  static constexpr Scalar eps = std::numeric_limits<Scalar>::epsilon();

  static constexpr Eigen::Index N = AmatT::ColsAtCompileTime;
  const Eigen::Index n = pbm.A.cols(), m = pbm.A.rows();

  // FIND ACTIVE CONSTRAINT SETS

  Eigen::Index nl = 0, nu = 0;
  for (Eigen::Index idx = 0; idx < m; ++idx) {
    if (sol.dual[idx] < -100 * eps && pbm.l[idx] != -inf) { nl++; }
    if (sol.dual[idx] > 100 * eps && pbm.u[idx] != inf) { nu++; }
  }

  Eigen::VectorXi LU_idx(nl + nu);
  for (Eigen::Index idx = 0, lcntr = 0, ucntr = 0; idx < m; ++idx) {
    if (sol.dual[idx] < -100 * eps && pbm.l[idx] != -inf) { LU_idx(lcntr++) = idx; }
    if (sol.dual[idx] > 100 * eps && pbm.u[idx] != inf) { LU_idx(nl + ucntr++) = idx; }
  }

  // FORM REDUCED SYSTEMS (27) AND (30)

  // square symmetric system matrix
  using HT = std::conditional_t<sparse, Eigen::SparseMatrix<Scalar>, Eigen::Matrix<Scalar, -1, -1>>;
  HT H(n + nl + nu, n + nl + nu), Hp(n + nl + nu, n + nl + nu);

  // fill up H
  if constexpr (sparse) {
    // preallocate nonzeros
    Eigen::VectorXi nnz(n + nl + nu);
    for (auto i = 0u; i != n; ++i) {
      nnz(i) = pbm.P.outerIndexPtr()[i + 1] - pbm.P.outerIndexPtr()[i];
    }
    for (auto i = 0u; i != nl + nu; ++i) {
      nnz(n + i) = pbm.A.outerIndexPtr()[LU_idx(i) + 1] - pbm.A.outerIndexPtr()[LU_idx(i)];
    }
    H.reserve(nnz);
    Hp.reserve(nnz + Eigen::VectorXi::Ones(n + nl + nu));

    // fill P in top left block
    for (auto k = 0u; k < n; ++k) {
      for (Eigen::InnerIterator it(pbm.P, k); it; ++it) {
        const Scalar pij              = c * sx(it.col()) * sx(it.row()) * it.value();
        H.insert(it.row(), it.col())  = pij;
        Hp.insert(it.row(), it.col()) = pij;
      }
    }

    // fill selected rows of A in top right block
    for (auto a_row = 0u; a_row != nl + nu; ++a_row) {
      for (Eigen::InnerIterator it(pbm.A, LU_idx(a_row)); it; ++it) {
        const Scalar Aij               = sy(it.row()) * sx(it.col()) * it.value();
        H.insert(it.col(), n + a_row)  = Aij;
        Hp.insert(it.col(), n + a_row) = Aij;
      }
    }
  } else {
    H.setZero();
    H.topLeftCorner(n, n) = c * sx.asDiagonal() * pbm.P * sx.asDiagonal();
    for (auto i = 0u; i != nl + nu; ++i) {
      H.col(n + i).template head<N>(n) = sy(LU_idx(i)) * pbm.A.row(LU_idx(i)) * sx.asDiagonal();
    }
    Hp = H;
  }

  // add perturbing diagonal elements to Hp
  if constexpr (sparse) {
    for (auto i = 0u; i != n; ++i) { Hp.coeffRef(i, i) += prm.delta; }
    for (auto i = 0u; i != nl + nu; ++i) { Hp.coeffRef(n + i, n + i) -= prm.delta; }
    H.makeCompressed();
    Hp.makeCompressed();
  } else {
    Hp.topLeftCorner(n, n) += Eigen::VectorX<Scalar>::Constant(n, prm.delta).asDiagonal();
    Hp.bottomRightCorner(nl + nu, nl + nu) -=
      Eigen::VectorX<Scalar>::Constant(nl + nu, prm.delta).asDiagonal();
  }

  Eigen::VectorX<Scalar> h(n + nl + nu);
  h.head(n) = -c * sx.cwiseProduct(pbm.q);
  for (auto i = 0u; i != nl; ++i) { h(n + i) = sy(LU_idx(i)) * pbm.l(LU_idx(i)); }
  for (auto i = 0u; i != nu; ++i) { h(n + nl + i) = sy(LU_idx(nl + i)) * pbm.u(LU_idx(nl + i)); }

  // ITERATIVE REFINEMENT

  // factorize Hp
  std::conditional_t<
    sparse,
    Eigen::SimplicialLDLT<decltype(H), Eigen::Upper>,
    Eigen::LDLT<decltype(H), Eigen::Upper>>
    ldlt(Hp);

  if (ldlt.info()) { return false; }

  Eigen::VectorX<Scalar> t_hat = Eigen::VectorX<Scalar>::Zero(n + nl + nu);
  for (auto i = 0u; i != prm.polish_iter; ++i) {
    t_hat += ldlt.solve(h - H.template selfadjointView<Eigen::Upper>() * t_hat);
  }

  // UPDATE SOLUTION

  sol.primal = t_hat.template head<N>(n);
  for (auto i = 0u; i < nl; ++i) { sol.dual(LU_idx(i)) = t_hat(n + i); }
  for (auto i = 0u; i < nu; ++i) { sol.dual(LU_idx(nl + i)) = t_hat(n + nl + i); }

  return true;
}

/**
 * @brief Copy-able wrapper around Eigen LDLT types.
 */
template<typename LDLTt>
struct LDLTWrapper
{
  bool first{true};  // true if ldlt object needs factorization
  LDLTt ldlt{};

  LDLTWrapper() = default;
  LDLTWrapper(const LDLTWrapper &) : first{true}, ldlt{} {}
  LDLTWrapper(LDLTWrapper &&) : first{true}, ldlt{} {}
  LDLTWrapper & operator=(const LDLTWrapper &)
  {
    first = true;
    return *this;
  }
  LDLTWrapper & operator=(LDLTWrapper &)
  {
    first = true;
    return *this;
  }
  ~LDLTWrapper() = default;
};

}  // namespace detail

/**
 * @brief Solver for quadratic programs
 *
 * Use this class to efficiently solve many QPs with the same problem structure.
 *
 * For one-off QPs, see solve_qp().
 */
template<typename Pbm>
class QPSolver
{
  using AmatT                  = decltype(Pbm::A);
  using Scalar                 = typename AmatT::Scalar;
  static constexpr bool sparse = std::is_base_of_v<Eigen::SparseMatrixBase<AmatT>, AmatT>;

  // static sizes
  static constexpr Eigen::Index M = AmatT::RowsAtCompileTime;
  static constexpr Eigen::Index N = AmatT::ColsAtCompileTime;
  static constexpr Eigen::Index K = (N == -1 || M == -1) ? Eigen::Index(-1) : N + M;

  // typedefs
  using Rn = Eigen::Vector<Scalar, N>;
  using Rm = Eigen::Vector<Scalar, M>;
  using Rk = Eigen::Vector<Scalar, K>;
  using Ht = std::conditional_t<sparse, Eigen::SparseMatrix<Scalar>, Eigen::Matrix<Scalar, K, K>>;
  using LDLTt = std::
    conditional_t<sparse, Eigen::SimplicialLDLT<Ht, Eigen::Upper>, Eigen::LDLT<Ht, Eigen::Upper>>;

  static inline const Scalar inf = std::numeric_limits<Scalar>::infinity();

public:
  /**
   * @brief Default constructor.
   */
  QPSolver(const QPSolverParams & prm = {}) : prm_(prm) {}

  /**
   * @brief Construct and allocate working memory.
   *
   * @param pbm template problem.
   *
   * Memory is allocated for solving problems with same structure as pbm.
   */
  QPSolver(const Pbm & pbm, const QPSolverParams & prm = {}) : prm_(prm) { analyze(pbm); }

  /// @brief Default copy constructor
  QPSolver(const QPSolver &) = default;
  /// @brief Default move constructor
  QPSolver(QPSolver &&) = default;
  /// @brief Default copy assignment
  QPSolver & operator=(const QPSolver &) = default;
  /// @brief Default move assignment
  QPSolver & operator=(QPSolver &&) = default;

  /**
   * @brief Access most recent QP solution.
   */
  const QPSolution<M, N, Scalar> & sol() const { return sol_; }

  /**
   * @brief Prepare for solving problems.
   */
  void analyze(const Pbm & pbm)
  {
    const Eigen::Index n = pbm.A.cols(), m = pbm.A.rows(), k = n + m;

    // solution
    sol_.primal.setZero(n);
    sol_.dual.setZero(m);

    // scaling variables and working memory
    c_ = 1;
    sx_.setOnes(n);
    sy_.setOnes(m);
    sx_inc_.setZero(n);
    sy_inc_.setZero(m);

    // solve working memory
    z_.resize(m);
    z_next_.resize(m);
    rho_.resize(m);
    p_.resize(k);
    pt_.resize(k);

    // stopping working memory
    x_us_.resize(n);
    dx_us_.resize(n);
    y_us_.resize(m);
    dy_us_.resize(m);
    z_us_.resize(m);
    Px_.resize(n);
    Aty_.resize(n);
    Ax_.resize(m);

    // preallocate nonzeros in H
    H_.setZero();
    H_.resize(k, k);
    if constexpr (sparse) {
      Eigen::VectorXi nnz(k);
      for (auto i = 0u; i < n; ++i) {
        nnz(i) = pbm.P.outerIndexPtr()[i + 1] - pbm.P.outerIndexPtr()[i] + 1;
      }
      for (auto i = 0u; i < m; ++i) {
        nnz(n + i) = pbm.A.outerIndexPtr()[i + 1] - pbm.A.outerIndexPtr()[i] + 1;
      }
      H_.reserve(nnz);
    }
  }

  /**
   * @brief Solve quadratic program.
   */
  const QPSolution<M, N, Scalar> & solve(
    const Pbm & pbm,
    std::optional<std::reference_wrapper<const QPSolution<M, N, Scalar>>> warmstart = {})
  {
    // update problem scaling
    if (prm_.scaling) { scale(pbm); }

    // dynamic sizes
    const Eigen::Index n = pbm.A.cols(), m = pbm.A.rows();

    // cast parameters to scalar type
    const Scalar rho_bar    = static_cast<Scalar>(prm_.rho);
    const Scalar alpha      = static_cast<Scalar>(prm_.alpha);
    const Scalar alpha_comp = Scalar(1) - alpha;
    const Scalar sigma      = static_cast<Scalar>(prm_.sigma);

    // return code: when set algorithm is finished
    std::optional<QPSolutionStatus> ret_code = std::nullopt;

    for (auto i = 0u; i != m; ++i) {
      if (pbm.l(i) == inf || pbm.u(i) == -inf || pbm.u(i) - pbm.l(i) < Scalar(0.)) {
        ret_code = QPSolutionStatus::PrimalInfeasible;  // feasible set trivially empty
      }

      // set rho depending on constraint type
      if (pbm.l(i) == -inf && pbm.u(i) == inf) {
        rho_(i) = Scalar(1e-6);  // unbounded
      } else if (sy_(i) * abs(pbm.l(i) - pbm.u(i)) < 1e-5) {
        rho_(i) = Scalar(1e3) * rho_bar;  // equality
      } else {
        rho_(i) = rho_bar;  // inequality
      }
    }

    const auto t0 = std::chrono::high_resolution_clock::now();

    // fill square symmetric system matrix H = [P A'; A 0]
    if constexpr (sparse) {
      if (H_.isCompressed()) { H_.coeffs().setZero(); }

      for (auto i = 0u; i < pbm.P.outerSize(); ++i) {
        for (Eigen::InnerIterator it(pbm.P, i); it; ++it) {
          if (it.col() >= it.row()) {
            H_.coeffRef(it.row(), it.col()) = c_ * sx_(it.row()) * sx_(it.col()) * it.value();
          }
        }
      }
      block_add_identity(H_, 0, 0, n, sigma);
      for (auto i = 0u; i < pbm.A.outerSize(); ++i) {
        for (Eigen::InnerIterator it(pbm.A, i); it; ++it) {
          H_.coeffRef(it.col(), n + it.row()) = sy_(it.row()) * sx_(it.col()) * it.value();
        }
      }
      for (auto row = 0u; row < m; ++row) {
        H_.coeffRef(n + row, n + row) = Scalar(-1) / rho_(row);
      }

      if (!H_.isCompressed()) { H_.makeCompressed(); }
    } else {
      H_.setZero();

      H_.template topLeftCorner<N, N>(n, n) = c_ * sx_.asDiagonal() * pbm.P * sx_.asDiagonal();
      H_.template topLeftCorner<N, N>(n, n) +=
        Eigen::Vector<Scalar, N>::Constant(n, sigma).asDiagonal();
      H_.template topRightCorner<N, M>(n, m) =
        (sy_.asDiagonal() * pbm.A * sx_.asDiagonal()).transpose();
      H_.template bottomRightCorner<M, M>(m, m) = (-rho_).cwiseInverse().asDiagonal();
    }

    const auto t_fill = std::chrono::high_resolution_clock::now();

    if (prm_.verbose) {
      using std::cout, std::left, std::setw, std::right;
      // clang-format off
      cout << "========================= QP Solver =========================" << '\n';
      cout << "Solving " << (sparse ? "sparse" : "dense") << " QP with n=" << n << ", m=" << m << '\n';
      cout << setw(8)  << right << "ITER"
           << setw(14) << right << "OBJ"
           << setw(14) << right << "PRI_RES"
           << setw(14) << right << "DUA_RES"
           << setw(10) << right << "TIME" << '\n';
      // clang-format on
    }

    // factorize H
    if constexpr (sparse) {
      if (ldlt_.first) { ldlt_.ldlt.analyzePattern(H_); }
      ldlt_.ldlt.factorize(H_);  // makes copy for permuted matrix..
      ldlt_.first = false;
    } else {
      ldlt_.ldlt.compute(H_);
    }

    const auto t_factor = std::chrono::high_resolution_clock::now();

    if (ldlt_.ldlt.info()) { ret_code = QPSolutionStatus::Unknown; }

    // initialize solver variables
    if (warmstart.has_value()) {
      // warmstart variables must be scaled
      sol_.primal  = sx_.cwiseInverse().cwiseProduct(warmstart.value().get().primal);
      sol_.dual    = c_ * sy_.cwiseInverse().cwiseProduct(warmstart.value().get().dual);
      z_.noalias() = sy_.asDiagonal() * pbm.A * warmstart.value().get().primal;
    } else {
      sol_.primal.setZero();
      sol_.dual.setZero();
      z_.setZero();
    }

    // main optimization loop
    auto iter = 0u;
    for (; (!prm_.max_iter || iter != prm_.max_iter.value()) && !ret_code; ++iter) {
      p_.template segment<N>(0, n) = sigma * sol_.primal - c_ * sx_.asDiagonal() * pbm.q;
      p_.template segment<M>(n, m) = z_ - rho_.cwiseInverse().cwiseProduct(sol_.dual);
      if constexpr (sparse) {
        // p_ = ldlt_.solve(p_);
        // manual solve because ldlt_.solve() uses temporaries...
        // Pinv L D L' P x = b  <==> x = Pinv * (L' \ Dinv(L \ (P * b)))
        pt_.noalias() = ldlt_.ldlt.permutationP() * p_;
        ldlt_.ldlt.matrixL().solveInPlace(pt_);
        pt_.applyOnTheLeft(ldlt_.ldlt.vectorD().cwiseInverse().asDiagonal());
        ldlt_.ldlt.matrixU().solveInPlace(pt_);
        p_.noalias() = ldlt_.ldlt.permutationPinv() * pt_;
      } else {
        ldlt_.ldlt.solveInPlace(p_);
      }

      if (iter % prm_.stop_check_iter == 1) {
        // termination checking requires difference, store old scaled values
        dx_us_ = sol_.primal, dy_us_ = sol_.dual;
      }

      sol_.primal = alpha * p_.template segment<N>(0, n) + alpha_comp * sol_.primal;
      z_next_     = (alpha * rho_.cwiseInverse().cwiseProduct(p_.template segment<M>(n, m))
                 + alpha_comp * rho_.cwiseInverse().cwiseProduct(sol_.dual) + z_)
                  .cwiseMax(sy_.cwiseProduct(pbm.l))
                  .cwiseMin(sy_.cwiseProduct(pbm.u));
      sol_.dual = alpha_comp * sol_.dual + alpha * p_.template segment<M>(n, m)
                + rho_.cwiseProduct(z_) - rho_.cwiseProduct(z_next_);
      std::swap(z_, z_next_);

      if (iter % prm_.stop_check_iter == 1) {
        // unscale solution
        x_us_  = sx_.cwiseProduct(sol_.primal);
        y_us_  = sy_.cwiseProduct(sol_.dual) / c_;
        z_us_  = sy_.cwiseInverse().cwiseProduct(z_);
        dx_us_ = sx_.cwiseProduct(sol_.primal - dx_us_);
        dy_us_ = sy_.cwiseProduct(sol_.dual - dy_us_) / c_;

        // check stopping criteria for unscaled problem and unscaled variables
        ret_code = check_stopping(pbm);

        if (prm_.verbose) {
          using std::cout, std::setw, std::right, std::chrono::microseconds;
          // clang-format off
          cout << setw(7) << right << iter << ":"
            << std::scientific
            << setw(14) << right << (0.5 * pbm.P * x_us_ + pbm.q).dot(x_us_)
            << setw(14) << right << (pbm.A * x_us_ - z_us_).template lpNorm<Eigen::Infinity>()
            << setw(14) << right << (pbm.P * x_us_ + pbm.q + pbm.A.transpose() * y_us_).template lpNorm<Eigen::Infinity>()
            << setw(10) << right << duration_cast<microseconds>(std::chrono::high_resolution_clock::now() - t0).count()
            << '\n';
          // clang-format on
        }

        // check for timeout
        if (!ret_code) {
          if (
            prm_.max_time
            && std::chrono::high_resolution_clock::now() > t0 + prm_.max_time.value()) {
            ret_code = QPSolutionStatus::MaxTime;
          }
        }
      }
    }

    const auto t_iter = std::chrono::high_resolution_clock::now();

    // polish solution if optimal
    if (ret_code.has_value() && ret_code.value() == QPSolutionStatus::Optimal && prm_.polish) {
      if (detail::polish_qp(pbm, sol_, prm_, c_, sx_, sy_)) {
        if (prm_.verbose) {
          // unscale solution
          x_us_ = sx_.cwiseProduct(sol_.primal);
          y_us_ = sy_.cwiseProduct(sol_.dual) / c_;
          z_us_ = sy_.cwiseInverse().cwiseProduct(z_);

          using std::cout, std::setw, std::right, std::chrono::microseconds;
          // clang-format off
          cout << setw(8) << right << "polish:"
            << std::scientific
            << setw(14) << right << (0.5 * pbm.P * x_us_ + pbm.q).dot(x_us_)
            << setw(14) << right << (pbm.A * x_us_ - z_us_).template lpNorm<Eigen::Infinity>()
            << setw(14) << right << (pbm.P * x_us_ + pbm.q + pbm.A.transpose() * y_us_).template lpNorm<Eigen::Infinity>()
            << setw(10) << right << duration_cast<microseconds>(std::chrono::high_resolution_clock::now() - t0).count()
            << '\n';
          // clang-format on
        }

      } else {
        if (prm_.verbose) { std::cout << "Polish failed" << '\n'; }
        sol_.code = QPSolutionStatus::PolishFailed;
      }
    }

    const auto t_polish = std::chrono::high_resolution_clock::now();

    // unscale solution
    sol_.code      = ret_code.value_or(QPSolutionStatus::MaxIterations);
    sol_.primal    = sx_.cwiseProduct(sol_.primal);
    sol_.dual      = sy_.cwiseProduct(sol_.dual) / c_;
    sol_.objective = sol_.primal.dot(0.5 * pbm.P * sol_.primal + pbm.q);
    sol_.iter      = iter;

    if (prm_.verbose) {
      using std::cout, std::left, std::right, std::setw, std::chrono::microseconds;

      // clang-format off
      cout << "QP solver summary:" << '\n';
      cout << "Result " << static_cast<int>(sol_.code) << '\n';

      cout << setw(25) << left << "Iterations"        << setw(10) << right << iter - 1                                               << '\n';
      cout << setw(26) << left << "Total time (µs)"   << setw(10) << right << duration_cast<microseconds>(t_polish - t0).count()     << '\n';
      cout << setw(25) << left << "  Matrix filling"  << setw(10) << right << duration_cast<microseconds>(t_fill - t0).count()       << '\n';
      cout << setw(25) << left << "  Factorization"   << setw(10) << right << duration_cast<microseconds>(t_factor - t_fill).count() << '\n';
      cout << setw(25) << left << "  Iteration"       << setw(10) << right << duration_cast<microseconds>(t_iter - t_factor).count() << '\n';
      cout << setw(25) << left << "  Polish"          << setw(10) << right << duration_cast<microseconds>(t_polish - t_iter).count() << '\n';
      cout << "=============================================================" << '\n';
      // clang-format on
    }

    return sol_;
  }

protected:
  /**
   * @brief Check stopping criteria for solver.
   */
  std::optional<QPSolutionStatus> check_stopping(const Pbm & pbm)
  {
    const Eigen::Index m = pbm.A.rows();

    // norm function
    static const auto norm = [](auto && t) -> Scalar {
      return t.template lpNorm<Eigen::Infinity>();
    };

    // OPTIMALITY

    // check primal
    Ax_.noalias()        = pbm.A * x_us_;
    const Scalar Ax_norm = norm(Ax_);
    Ax_ -= z_us_;
    if (norm(Ax_) <= prm_.eps_abs + prm_.eps_rel * std::max<Scalar>(Ax_norm, norm(z_us_))) {
      // primal succeeded, check dual
      Px_.noalias()           = pbm.P * x_us_;
      Aty_.noalias()          = pbm.A.transpose() * y_us_;
      const Scalar dual_scale = std::max<Scalar>({norm(Px_), norm(pbm.q), norm(Aty_)});
      Px_ += pbm.q + Aty_;
      if (norm(Px_) <= prm_.eps_abs + prm_.eps_rel * dual_scale) {
        return QPSolutionStatus::Optimal;
      }
    }

    // PRIMAL INFEASIBILITY

    Aty_.noalias()        = pbm.A.transpose() * dy_us_;  // NOTE new value A' * dy
    const Scalar Edy_norm = norm(dy_us_);

    Scalar u_dyp_plus_l_dyn = Scalar(0);
    for (auto i = 0u; i != m; ++i) {
      if (pbm.u(i) != inf) {
        u_dyp_plus_l_dyn += pbm.u(i) * std::max<Scalar>(Scalar(0), dy_us_(i));
      } else if (dy_us_(i) > prm_.eps_primal_inf * Edy_norm) {
        // contributes +inf to sum --> no certificate
        u_dyp_plus_l_dyn = inf;
        break;
      }
      if (pbm.l(i) != -inf) {
        u_dyp_plus_l_dyn += pbm.l(i) * std::min<Scalar>(Scalar(0), dy_us_(i));
      } else if (dy_us_(i) < -prm_.eps_primal_inf * Edy_norm) {
        // contributes +inf to sum --> no certificate
        u_dyp_plus_l_dyn = inf;
        break;
      }
    }

    if (std::max<Scalar>(norm(Aty_), u_dyp_plus_l_dyn) < prm_.eps_primal_inf * Edy_norm) {
      return QPSolutionStatus::PrimalInfeasible;
    }

    // DUAL INFEASIBILITY

    Ax_.noalias()        = pbm.A * dx_us_;  // note new value A * dx
    const Scalar dx_norm = norm(dx_us_);
    Px_.noalias()        = pbm.P * dx_us_;

    bool dual_infeasible = (norm(Px_) <= prm_.eps_dual_inf * dx_norm)
                        && (pbm.q.dot(dx_us_) <= prm_.eps_dual_inf * dx_norm);
    for (auto i = 0u; i != m && dual_infeasible; ++i) {
      if (pbm.u(i) == inf) {
        dual_infeasible &= (Ax_(i) >= -prm_.eps_dual_inf * dx_norm);
      } else if (pbm.l(i) == -inf) {
        dual_infeasible &= (Ax_(i) <= prm_.eps_dual_inf * dx_norm);
      } else {
        dual_infeasible &= std::abs(Ax_(i)) < prm_.eps_dual_inf * dx_norm;
      }
    }

    if (dual_infeasible) { return QPSolutionStatus::DualInfeasible; }

    return std::nullopt;
  }

  /**
   * @brief Re-scale QP.
   *
   * The scaled problem is defined as
   *
   * * \f$ P_s = c S_x P S_x \f$,
   * * \f$ q_s = c q S_x \f$,
   * * \f$ A_s = S_y A S_x \f$,
   * * \f$ l_s = S_y l \f$,
   * * \f$ u_s = S_y u \f$,
   *
   * where Sx = diag(sx), Sy = diag(sy).
   *
   * The relation between scaled variables and original variables are
   *
   * * Primal: \f$ x_s = S_x^{-1} x \f$,
   * * Dual: \f$ y_s = c S_y^{-1} y \f$.
   *
   * The objective of the rescaling is the make the columns of
   * \f[
   *   \begin{bmatrix} \bar P & \bar A^T \\ \bar A & 0 \end{bmatrix}
   * \f]
   * have similar \f$ l_\infty \f$ norm, and similarly for the columns of
   * \f[
   *  \begin{bmatrix} \bar P & \bar q \end{bmatrix}.
   * \f]
   */
  void scale(const Pbm & pbm)
  {
    sx_.setOnes();
    sy_.setOnes();

    sx_inc_.setZero();

    // find "norm" of cost function
    for (auto i = 0u; i < pbm.P.outerSize(); ++i) {
      for (Eigen::InnerIterator it(pbm.P, i); it; ++it) {
        sx_inc_(it.col()) = std::max(sx_inc_(it.col()), std::abs(it.value()));
      }
    }

    // if there are "zero cols"
    for (auto i = 0u; i != sx_.size(); ++i) {
      if (sx_inc_(i) == 0) { sx_inc_(i) = 1; }
    }

    // scale cost function
    c_ = Scalar(1) / std::max({1e-6, sx_inc_.mean(), pbm.q.template lpNorm<Eigen::Infinity>()});

    int iter = 0;

    // calculate inf-norm for every column of [Ps As' ; As 0]
    do {
      sx_inc_.setZero();
      sy_inc_.setZero();
      for (auto k = 0u; k < pbm.P.outerSize(); ++k) {
        for (Eigen::InnerIterator it(pbm.P, k); it; ++it) {
          // upper left block of H
          sx_inc_(it.col()) = std::max({
            sx_inc_(it.col()),
            std::abs(c_ * sx_(it.row()) * sx_(it.col()) * it.value()),
          });
        }
      }
      for (auto k = 0u; k < pbm.A.outerSize(); ++k) {
        for (Eigen::InnerIterator it(pbm.A, k); it; ++it) {
          const Scalar Aij  = std::abs(sy_(it.row()) * sx_(it.col()) * it.value());
          sx_inc_(it.col()) = std::max(sx_inc_(it.col()), Aij);  // bottom left block of H
          sy_inc_(it.row()) = std::max(sy_inc_(it.row()), Aij);  // upper right block of H
        }
      }

      // if there are "zero cols" we don't scale
      for (auto i = 0u; i < sx_.size(); ++i) {
        if (sx_inc_(i) == 0) { sx_inc_(i) = 1; }
      }
      for (auto i = 0u; i < sy_.size(); ++i) {
        if (sy_inc_(i) == 0) { sy_inc_(i) = 1; }
      }

      sx_.applyOnTheLeft(sx_inc_.cwiseMax(1e-8).cwiseInverse().cwiseSqrt().asDiagonal());
      sy_.applyOnTheLeft(sy_inc_.cwiseMax(1e-8).cwiseInverse().cwiseSqrt().asDiagonal());
    } while (
      iter++ < 10
      && std::max((sx_inc_.array() - 1).abs().maxCoeff(), (sy_inc_.array() - 1).abs().maxCoeff())
           > 0.1);
  }

private:
  // solver parameters
  QPSolverParams prm_{};

  // solution
  QPSolution<M, N, Scalar> sol_{};

  // scaling variables and working memory
  Scalar c_{0};
  Rn sx_{}, sx_inc_{};
  Rm sy_{}, sy_inc_{};

  // solve working memory
  Rm z_{}, z_next_{}, rho_{};
  Rk p_{}, pt_{};

  // stopping working memory
  Rn x_us_{}, dx_us_{};
  Rn Px_{}, Aty_{};
  Rm Ax_{};
  Rm y_us_{}, z_us_{}, dy_us_{};

  // system matrix and decomposition
  Ht H_{};
  detail::LDLTWrapper<LDLTt> ldlt_{};
};

/**
 * @brief Solve a quadratic program using the operator splitting approach.
 *
 * @tparam Pbm problem type (QuadraticProgram or QuadraticProgramSparse)
 *
 * @param pbm problem formulation
 * @param prm solver options
 * @param warmstart provide initial guess for primal and dual variables
 * @return solution as QuasraticProgramSolution<M, N>
 *
 * @note dynamic problem sizes (`M == -1 || N == -1`) are supported
 *
 * This is a third-party implementation of the algorithm described in the following paper:
 * * Stellato, B., Banjac, G., Goulart, P. et al.
 * **OSQP: an operator splitting solver for quadratic programs.**
 * *Math. Prog. Comp.* 12, 637–672 (2020).
 * https://doi.org/10.1007/s12532-020-00179-2
 *
 * For the official C implementation, see https://osqp.org/.
 */
template<typename Pbm>
detail::qp_solution_t<Pbm> solve_qp(
  const Pbm & pbm,
  const QPSolverParams & prm,
  std::optional<std::reference_wrapper<const detail::qp_solution_t<Pbm>>> warmstart = {})
{
  QPSolver solver(pbm, prm);
  return solver.solve(pbm, warmstart);
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__QP_SOLVER_HPP_


// Boost.Geometry (aka GGL, Generic Geometry Library)
// This file is manually converted from PROJ4

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Geometry Library by Barend Gehrels (Geodan, Amsterdam)

// Original copyright notice:

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

#ifndef BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP
#define BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP

#include <string>
#include <vector>

#include <boost/geometry/formulas/eccentricity_sqr.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/srs/projections/impl/pj_ellps.hpp>
#include <boost/geometry/srs/projections/impl/pj_param.hpp>
#include <boost/geometry/srs/projections/proj4.hpp>

namespace boost { namespace geometry { namespace projections {

namespace detail {

/* set ellipsoid parameters a and es */
template <typename T>
inline T SIXTH() { return .1666666666666666667; } /* 1/6 */
template <typename T>
inline T RA4() { return .04722222222222222222; } /* 17/360 */
template <typename T>
inline T RA6() { return .02215608465608465608; } /* 67/3024 */
template <typename T>
inline T RV4() { return .06944444444444444444; } /* 5/72 */
template <typename T>
inline T RV6() { return .04243827160493827160; } /* 55/1296 */

/* initialize geographic shape parameters */
template <typename BGParams, typename T>
inline void pj_ell_set(BGParams const& /*bg_params*/, std::vector<pvalue<T> >& parameters, T &a, T &es)
{
    T b = 0.0;
    T e = 0.0;
    std::string name;

    /* check for varying forms of ellipsoid input */
    a = es = 0.;

    /* R takes precedence */
    if (pj_param(parameters, "tR").i)
        a = pj_param(parameters, "dR").f;
    else { /* probable elliptical figure */

        /* check if ellps present and temporarily append its values to pl */
        name = pj_param(parameters, "sellps").s;
        if (! name.empty())
        {
            const int n = sizeof(pj_ellps) / sizeof(pj_ellps[0]);
            int index = -1;
            for (int i = 0; i < n && index == -1; i++)
            {
                if(pj_ellps[i].id == name)
                {
                    index = i;
                }
            }

            if (index == -1) {
                BOOST_THROW_EXCEPTION( projection_exception(-9) );
            }

            parameters.push_back(pj_mkparam<T>(pj_ellps[index].major));
            parameters.push_back(pj_mkparam<T>(pj_ellps[index].ell));
        }
        a = pj_param(parameters, "da").f;
        if (pj_param(parameters, "tes").i) /* eccentricity squared */
            es = pj_param(parameters, "des").f;
        else if (pj_param(parameters, "te").i) { /* eccentricity */
            e = pj_param(parameters, "de").f;
            es = e * e;
        } else if (pj_param(parameters, "trf").i) { /* recip flattening */
            es = pj_param(parameters, "drf").f;
            if (!es) {
                BOOST_THROW_EXCEPTION( projection_exception(-10) );
            }
            es = 1./ es;
            es = es * (2. - es);
        } else if (pj_param(parameters, "tf").i) { /* flattening */
            es = pj_param(parameters, "df").f;
            es = es * (2. - es);
        } else if (pj_param(parameters, "tb").i) { /* minor axis */
            b = pj_param(parameters, "db").f;
            es = 1. - (b * b) / (a * a);
        }     /* else es == 0. and sphere of radius a */
        if (!b)
            b = a * sqrt(1. - es);
        /* following options turn ellipsoid into equivalent sphere */
        if (pj_param(parameters, "bR_A").i) { /* sphere--area of ellipsoid */
            a *= 1. - es * (SIXTH<T>() + es * (RA4<T>() + es * RA6<T>()));
            es = 0.;
        } else if (pj_param(parameters, "bR_V").i) { /* sphere--vol. of ellipsoid */
            a *= 1. - es * (SIXTH<T>() + es * (RV4<T>() + es * RV6<T>()));
            es = 0.;
        } else if (pj_param(parameters, "bR_a").i) { /* sphere--arithmetic mean */
            a = .5 * (a + b);
            es = 0.;
        } else if (pj_param(parameters, "bR_g").i) { /* sphere--geometric mean */
            a = sqrt(a * b);
            es = 0.;
        } else if (pj_param(parameters, "bR_h").i) { /* sphere--harmonic mean */
            a = 2. * a * b / (a + b);
            es = 0.;
        } else {
            int i = pj_param(parameters, "tR_lat_a").i;
            if (i || /* sphere--arith. */
                pj_param(parameters, "tR_lat_g").i) { /* or geom. mean at latitude */
                T tmp;

                tmp = sin(pj_param(parameters, i ? "rR_lat_a" : "rR_lat_g").f);
                if (geometry::math::abs(tmp) > geometry::math::half_pi<T>()) {
                    BOOST_THROW_EXCEPTION( projection_exception(-11) );
                }
                tmp = 1. - es * tmp * tmp;
                a *= i ? .5 * (1. - es + tmp) / ( tmp * sqrt(tmp)) :
                    sqrt(1. - es) / tmp;
                es = 0.;
            }
        }
    }

    /* some remaining checks */
    if (es < 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(-12) );
    }
    if (a <= 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(-13) );
    }
}

template <BOOST_GEOMETRY_PROJECTIONS_DETAIL_TYPENAME_PX, typename T>
inline void pj_ell_set(srs::static_proj4<BOOST_GEOMETRY_PROJECTIONS_DETAIL_PX> const& bg_params,
                       std::vector<pvalue<T> >& /*parameters*/, T &a, T &es)
{
    typedef srs::static_proj4<BOOST_GEOMETRY_PROJECTIONS_DETAIL_PX> static_parameters_type;
    typedef typename srs::par4::detail::pick_ellps
        <
            static_parameters_type
        > pick_ellps;

    typename pick_ellps::model_type model = pick_ellps::model(bg_params);

    a = geometry::get_radius<0>(model);
    T b = geometry::get_radius<2>(model);
    es = 0.;
    if (a != b)
    {
        es = formula::eccentricity_sqr<T>(model);

        // Ignore all other parameters passed in string, at least for now
    }

    /* some remaining checks */
    if (es < 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(-12) );
    }
    if (a <= 0.) {
        BOOST_THROW_EXCEPTION( projection_exception(-13) );
    }
}

} // namespace detail
}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_IMPL_PJ_ELL_SET_HPP