/**
    @file Project_2.cpp

    @author Terence Henriod

    Project 2: Bayesion Minimum Error Classification

    @brief The driver program for use of a Bayesian Minimum Error Classifier to
           both classify randomly generated data and detect face (or at least
           skin-colored) regions in images.

    @version Original Code 1.00 (3/26/2014) - T. Henriod

    UNOFFICIALLY:
    This program 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.

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

Compilation notes:
g++ -I /home/thenriod/Desktop/cpp_libs/Eigen_lib/ Project_1.cpp

*/

/*==============================================================================
=======     HEADER FILES     ===================================================
==============================================================================*/
#include <cmath>
#include <iostream>

#include "bayes_classifier.h"
#include "strict_gaussian_classifier.h"
#include "my_ppm.h"
#include <Eigen/Dense>  // -I /home/thenriod/Desktop/cpp_libs/Eigen_lib

using namespace std;

/*==============================================================================
=======     USER DEFINED TYPES     =============================================
==============================================================================*/
typedef struct
{
  string input_file_name;
  string output_file_base;
  string class_one_name;
  string class_two_name;
} BayesProblemData;

typedef struct
{
  string training_photo;
  string training_reference;
  string training_output;
  string photo_one;
  string reference_one;
  string output_one;
  string photo_two;
  string reference_two;
  string output_two;
} SkinProblemData;

/*==============================================================================
=======     CONSTANTS / MACROS     =============================================
==============================================================================*/
const string SKIN = "SKIN";
const string NOT_SKIN = "INANIMATE OBJECT";

/*==============================================================================
=======     GLOBAL VARIABLES     ===============================================
==============================================================================*/
  // none

/*==============================================================================
=======     FUNCTION PROTOTYPES     ============================================
==============================================================================*/

void solveBayesClassificationProblem( BayesProblemData& info );

int readData( vector<DataItem>& data, const string& input_file_name );

void solveImageProblem( SkinProblemData& info);

void trainStrictGaussianClassifiers( StrictGaussianClassifier& classifier_one,
                                     StrictGaussianClassifier& classifier_two,
                                     string& photo_file_name,
                                     string& reference_file_name );

void readPpmToDataVectors( vector<DataItem>& rb_picture_data,
                           vector<DataItem>& cb_cr_picture_data,
                          string& photo_file_name,
                          string& reference_file_name );

void classifyPhotoPixels( StrictGaussianClassifier& classifier,
                          vector<DataItem>& data,
                          string& output_file_name );

/*==============================================================================
=======     MAIN FUNCTION     ==================================================
==============================================================================*/

/**
main

The main driver

@param

@return

@pre
-#

@post
-#

@code
@endcode
*/

int main( int argc, char** argv )
{
  // variables
  BayesProblemData problem_info;
  SkinProblemData face_detection_info;

  string photo_file_name = "Training_1.ppm";
  string reference_file_name = "ref1.ppm";
  StrictGaussianClassifier skin_finder;

  // solve problem 1
  problem_info.input_file_name = "P1_data.txt";
  problem_info.output_file_base = "P1_solution";
  problem_info.class_one_name = "ONE";
  problem_info.class_two_name = "TWO";
  solveBayesClassificationProblem( problem_info );

  // solve problem 2
  problem_info.input_file_name = "P2_data.txt";
  problem_info.output_file_base = "P2_solution";
  solveBayesClassificationProblem( problem_info );

  // solve image problem
  face_detection_info.training_photo     = "Training_1.ppm";
  face_detection_info.training_reference = "ref1.ppm";
  face_detection_info.training_output    = "test_output.txt";
  face_detection_info.photo_one          = "Training_3.ppm";
  face_detection_info.reference_one      = "ref3.ppm";
  face_detection_info.output_one         = "output_1.txt";
  face_detection_info.photo_two          = "Training_6.ppm";
  face_detection_info.reference_two      = "ref6.ppm";
  face_detection_info.output_two         = "output_2.txt";

  skin_finder.set_class_name( SKIN );

  solveImageProblem( face_detection_info );

  // end program
  return 0;
}

/*==============================================================================
=======     FUNCTION IMPLEMENTATIONS     =======================================
==============================================================================*/
void solveBayesClassificationProblem( BayesProblemData& info )
{
  // variables
  vector<BayesClassifier> classifiers( 2 );
  vector<DataItem> problem_data;

  // read in the data
  readData( problem_data, info.input_file_name );

  // set the classifier class names
  classifiers[0].set_class_name( info.class_one_name );
  classifiers[1].set_class_name( info.class_two_name );

  // find the means of the training data
  classifiers[0].set_mean( problem_data );
  classifiers[1].set_mean( problem_data );

  // find the covariances of the training data
  classifiers[0].set_covariance( problem_data, classifiers[0].mean_vector() );
  classifiers[1].set_covariance( problem_data, classifiers[1].mean_vector() );

  // set the prior probabilities
  classifiers[0].set_prior_probability( 0.5 );
  classifiers[1].set_prior_probability( 0.5 );

  // perform the classifications with the equal prior probabilities
  BayesClassifier::performAnalysis( classifiers, problem_data,
                                    info.output_file_base + "_part1.txt" );

  // perform the classifications with the differing priors
  classifiers[0].set_prior_probability( 0.3 );
  classifiers[1].set_prior_probability( 0.7 );
  BayesClassifier::performAnalysis( classifiers, problem_data,
                                    info.output_file_base + "_part2.txt" );

  // no return - void
}

int readData( vector<DataItem>& data, const string& input_file_name )
{
  // variables
  fstream file;
  DataItem temp;
  char delimiter;

  // clear file stream object and open the file
  file.clear();
  file.open( input_file_name.c_str(), fstream::in );

  // prime the reading loop
  file >> temp.feature_vector(0) >> delimiter
       >> temp.feature_vector(1) >> delimiter
       >> temp.actual_class;

  // continue to read from the file while possible
  while( file.good() )
  {
    // store the recently read data
    data.push_back( temp );

    // attempt to read more data
    file >> temp.feature_vector(0) >> delimiter
         >> temp.feature_vector(1) >> delimiter
         >> temp.actual_class;
  }

  // return the data vector by reference
}

void solveImageProblem( SkinProblemData& info)
{
  // variables
  StrictGaussianClassifier rb_skin_finder;
  StrictGaussianClassifier cb_cr_skin_finder;
  string dummy = "CbCr_output.txt";

  vector<DataItem> rb_data;
  vector<DataItem> cb_cr_data;

  rb_skin_finder.set_class_name( SKIN );
  cb_cr_skin_finder.set_class_name( SKIN );

  // train the classifier
  trainStrictGaussianClassifiers( rb_skin_finder, cb_cr_skin_finder,
                                 info.training_photo,
                                 info.training_reference );

/*
readPpmToDataVectors( rb_data, cb_cr_data, info.training_photo, info.training_reference );

classifyPhotoPixels( rb_skin_finder, rb_data, info.training_output );

classifyPhotoPixels( cb_cr_skin_finder, cb_cr_data, dummy );
 */

  // perform classification on the first data set
  readPpmToDataVectors( rb_data, cb_cr_data, info.photo_one, info.reference_one );
  classifyPhotoPixels( rb_skin_finder, rb_data, info.output_one );
  dummy = "CbCr";
  dummy += info.output_one;
  classifyPhotoPixels( cb_cr_skin_finder, cb_cr_data, dummy );

  // clear the data vectors between runs
  rb_data.clear();
  cb_cr_data.clear();

  // perform classification on the second data set
  readPpmToDataVectors( rb_data, cb_cr_data, info.photo_two, info.reference_two );
  classifyPhotoPixels( rb_skin_finder, rb_data, info.output_two );
  dummy = "CbCr";
  dummy += info.output_two;
  classifyPhotoPixels( cb_cr_skin_finder, cb_cr_data, dummy );

  // no return - void
}

void trainStrictGaussianClassifiers( StrictGaussianClassifier& classifier_one,
                                     StrictGaussianClassifier& classifier_two,
                                     string& photo_file_name,
                                     string& reference_file_name )
{
  // variables
  vector<DataItem> rb_data;
  vector<DataItem> cb_cr_data;

  // read in the training data
  readPpmToDataVectors( rb_data, cb_cr_data, photo_file_name,
                        reference_file_name );

  // find the training mean
  classifier_one.set_mean( rb_data );
  classifier_two.set_mean( cb_cr_data );

  // find the covariance
  classifier_one.set_covariance( rb_data, classifier_one.mean_vector() );
  classifier_two.set_covariance( cb_cr_data, classifier_two.mean_vector() );

cout << "RB Classifier" << endl;
classifier_one.reportClassifierInfo();

cout << "CbCr Classifier" << endl;
classifier_two.reportClassifierInfo();

  // return the trained classifier by reference
}

void readPpmToDataVectors( vector<DataItem>& rb_picture_data,
                            vector<DataItem>& cb_cr_picture_data,
                            string& photo_file_name,
                            string& reference_file_name )
{
  // variables
  PpmImageData photo_image;
  PpmImageData reference_image;
  DataItem temp;
  int i = 0;
  int j = 0;
  int k = 0;
  double color_sum = 0;

  // read in the image file
  readPpmFile( &photo_image, photo_file_name.c_str() );

  // read in the reference file
  readPpmFile( &reference_image, reference_file_name.c_str() );

  // visit each row of pixels in the images
  for( i = 0, k = 0; i < photo_image.height; i++ )
  {
    // visit each pixel of the rows
    for( j = 0; j < photo_image.width; j++, k++ )
    {
      // convert the pixel color vector to a two dimensional one
      color_sum = photo_image.data[i][j].red + photo_image.data[i][j].green +
                  photo_image.data[i][j].blue;
      if( color_sum > 0 )
      {
        temp.feature_vector( 0 ) = photo_image.data[i][j].red / color_sum;
        temp.feature_vector( 1 ) = photo_image.data[i][j].green / color_sum;
      }
      else  // to not divide by 0
      {
        temp.feature_vector( 0 ) = 0;
        temp.feature_vector( 1 ) = 0;
      }

      // case: the reference image indicates the pixel in the photo is skin
      if( (reference_image.data[i][j].red + reference_image.data[i][j].green +
           reference_image.data[i][j].blue) > 0 )
      {
        // tag the data
        temp.actual_class = SKIN;
      }
      // case: the photo pixel is non-skin
      else
      {
        // tag the data
        temp.actual_class = NOT_SKIN;
      }

      // add the new item to the vector
      rb_picture_data.push_back( temp );

      // compute the Cb and Cr values for the alternative
      temp.feature_vector( 0 ) = ( -0.169 * photo_image.data[i][j].red ) +
                                 ( -0.332 * photo_image.data[i][j].green ) +
                                 ( 0.5 * photo_image.data[i][j].blue );
      temp.feature_vector( 1 ) = ( 0.5 * photo_image.data[i][j].red ) +
                                 ( -0.419 * photo_image.data[i][j].green ) +
                                 ( -0.081 * photo_image.data[i][j].blue );

      // add the new item to the new vector
      cb_cr_picture_data.push_back( temp );
    }
  }

  // deconstruct the image structs
  deconstructPpmImage( &photo_image );
  deconstructPpmImage( &reference_image );

  // return the data vector by reference
}

void classifyPhotoPixels( StrictGaussianClassifier& classifier,
                          vector<DataItem>& data,
                          string& output_file_name )
{
  // variables
  vector<double> skin_likelihood;
  int i = 0;
  int num_skin = 0;
  int num_other = 0;
  int num_correct_skin = 0;
  int num_correct_other = 0;
  int false_acceptance = 0;
  int false_rejection = 0;
  double threshold = 0;
  double overall_correct_rate = 0;
  double skin_correct_rate = 0;
  double other_correct_rate = 0;
  double false_acceptance_rate = 0;
  double false_rejection_rate = 0;
  fstream file;

  // prepare the file for the output summary
  file.clear();
  file.open( output_file_name.c_str(), fstream::out );
  file << "Threshold Used, "
       << "Number of items, "
       << "Correct Classifications, "
       << "Correct Classification Rate, "
       << "Number of Skin Items, "
       << "Correct Skin Classifications, "
       << "Correct Skin Classification Rate, "
       << "Number of Other Items, "
       << "Correct Other Classifications, "
       << "Correct Other Classification Rate, "
       << "False Acceptance Rate, "
       << "False Rejection Rate"<< endl;

  // find the likelihood of each test vector being a skin one
  for( i = 0; i < data.size(); i++ )
  {
     // compute the likelihood that the pixel is skin
     skin_likelihood.push_back(
         classifier.getGaussianProbability( data[i].feature_vector ) );
  }

  // perform the classifications at various acceptance thresholds
  for( threshold = 0.01; threshold < 1.0; threshold += 0.01 )
  {
    // set the decision threshold
    classifier.set_decision_threshold( threshold );

    // reset the counters
    num_skin = 0;
    num_other = 0;
    num_correct_skin = 0;
    num_correct_other = 0;
    false_acceptance = 0;
    false_rejection = 0;

    // for every skin-pixel likelihood
    for( i = 0; i < data.size(); i++ )
    {
       // case: the pixel is likely skin
       if( skin_likelihood[i] > threshold )
       {
          // mark the pixel as skin
          data[i].classified_as = SKIN;

          // case: the pixel was not correctly classified
          if( data[i].actual_class != SKIN  )
          {
            // count it
            false_acceptance++;
          }
          else
          {
             num_correct_skin++;
          }
       }
       // case: the skin pixel is not likely skin
       else
       {
          // mark the pixel as skin
          data[i].classified_as = NOT_SKIN;

          // case: the pixel was actually skin
          if( data[i].actual_class == SKIN )
          {
            // count it
            false_rejection++;
          }
          else
          {
            num_correct_other++;
          }
       }

       // case: the pixel was in fact skin
       if( data[i].actual_class == SKIN )
       {
         // count it
         num_skin++;
       }
       else
       {
         num_other++;
       }
    }

    // compute the summary statistics
    overall_correct_rate = (double)(num_correct_skin + num_correct_other) /
                           (double) data.size();
    skin_correct_rate = (double)((double)num_correct_skin / (double)num_skin);
    other_correct_rate = (double)((double)num_correct_other /
                         (double)num_other);
    false_acceptance_rate = (double) false_acceptance / (double) (data.size() - num_skin);  //false positive rate = false positive / negative
    false_rejection_rate = (double) false_rejection / (double) num_skin;    // false negative rate = false negative / positive

    // output the result data to the output file
    file << classifier.decision_threshold() << ", "
         << data.size() << ", "
         << (num_correct_skin + num_correct_other) << ", "
         << overall_correct_rate << ", "
         << num_skin << ", "
         << num_correct_skin << ", "
         << skin_correct_rate << ", "
         << num_other << ", "
         << num_correct_other << ", "
         << other_correct_rate << ", "
         << false_acceptance_rate << ", "
         << false_rejection_rate
         << endl;
  }

  // close the file
  file.close();

  // no return - void
}


/*
 * Copyright 2015 Christoph Jud (christoph.jud@unibas.ch)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <fstream>
#include <iostream>
#include <iomanip>

//#include <boost/filesystem.hpp>
//namespace fs = boost::filesystem;

#include <Eigen/SVD>
#include <Eigen/Eigenvalues>

#include "GaussianProcess.h"
//#include "KernelFactory.h"
#include "MatrixIO.h"
#include "LAPACKUtils.h"

namespace gpr{

template< class TScalarType >
void GaussianProcess<TScalarType>::AddSample(const typename GaussianProcess<TScalarType>::VectorType &x,
                                             const typename GaussianProcess<TScalarType>::VectorType &y){
    if(m_SampleVectors.size() == 0){ // first call of AddSample defines dimensionality of input space
        m_InputDimension = x.size();
    }
    if(m_LabelVectors.size() == 0){ // first call of AddSample defines dimensionality of output space
        m_OutputDimension = y.size();
    }

    CheckInputDimension(x, "GaussianProcess::AddSample: ");
    CheckOutputDimension(y, "GaussianProcess::AddSample: ");

    m_SampleVectors.push_back(x);
    m_LabelVectors.push_back(y);
    m_Initialized = false;
}

template< class TScalarType >
typename GaussianProcess<TScalarType>::VectorType
GaussianProcess<TScalarType>::Predict(const typename GaussianProcess<TScalarType>::VectorType &x){
    Initialize();
    CheckInputDimension(x, "GaussianProcess::Predict: ");
    VectorType Kx;
    ComputeKernelVector(x, Kx);
    return (Kx.adjoint() * m_RegressionVectors).adjoint();
}

template< class TScalarType >
typename GaussianProcess<TScalarType>::VectorType
GaussianProcess<TScalarType>::PredictDerivative(const typename GaussianProcess<TScalarType>::VectorType &x,
                                                           typename GaussianProcess<TScalarType>::MatrixType &D){
    Initialize();
    CheckInputDimension(x, "GaussianProcess::PredictDerivative: ");
    VectorType Kx;
    ComputeKernelVector(x, Kx);
    MatrixType X;
    ComputeDifferenceMatrix(x, X);

    unsigned d = m_InputDimension;
    unsigned m = m_OutputDimension;
    D.resize(m_InputDimension, m_OutputDimension);
    for(unsigned i=0; i<m_OutputDimension; i++){
        D.col(i) = -X.transpose() * Kx.cwiseProduct(m_RegressionVectors.col(i));
    }
    return (Kx.adjoint() * m_RegressionVectors).adjoint(); // return point prediction
}

template< class TScalarType >
TScalarType
GaussianProcess<TScalarType>::operator()(const typename GaussianProcess<TScalarType>::VectorType & x,
                                         const typename GaussianProcess<TScalarType>::VectorType & y){
    Initialize();
    CheckInputDimension(x, "GaussianProcess::(): ");
    CheckInputDimension(y, "GaussianProcess::(): ");
    VectorType Kx;
    ComputeKernelVector(x, Kx);
    VectorType Ky;
    ComputeKernelVector(y, Ky);

    if(m_CoreMatrix.diagonalSize() == 0){
        ComputeCoreMatrix(m_CoreMatrix);
    }
    return (*m_Kernel)(x, y) - Kx.adjoint() * m_CoreMatrix * Ky;
}

template< class TScalarType >
TScalarType
GaussianProcess<TScalarType>::GetCredibleInterval(const typename GaussianProcess<TScalarType>::VectorType& x){
    Initialize();
    CheckInputDimension(x, "GaussianProcess::GetCredibleIntervall: ");

    // due to nummerical instabilities of the inversion of the kernel matrix
    // gp(x,x) might return negative values
    // therefore the maximum of zero and c is taken.
    TScalarType c = (*this)(x, x);
    if(debug && c<0) std::cout << "GaussianProcess::GetCredibleIntervall: prediction is instable. gp(x,x) = " << c << "." << std::endl;
    c = 2*std::sqrt(std::max(static_cast<TScalarType>(0.0),c));
    return c;
}

template< class TScalarType >
void GaussianProcess<TScalarType>::Initialize(){
    if(m_Initialized){
        return;
    }
    if(!(m_SampleVectors.size() > 0)){
        throw std::string("GaussianProcess::Initialize: no input samples defined during initialization");
    }
    if(!(m_LabelVectors.size() > 0)){
        throw std::string("GaussianProcess::Initialize: no ouput labels defined during initialization");
    }
    ComputeRegressionVectors();
    m_Initialized = true;
}

template< class TScalarType >
void GaussianProcess<TScalarType>::Save(std::string prefix){
    if(!m_Initialized){
        throw std::string("GaussianProcess::Save: gaussian process is not initialized.");
    }

    if(debug){
        std::cout << "GaussianProcess::Save: writing gaussian process: " << std::endl;
        std::cout << "\t " << prefix+"-RegressionVectors.txt" << std::endl;
        std::cout << "\t " << prefix+"-CoreMatrix.txt" << std::endl;
        std::cout << "\t " << prefix+"-SampleVectors.txt" << std::endl;
        std::cout << "\t " << prefix+"-LabelVectors.txt" << std::endl;
        std::cout << "\t " << prefix+"-ParameterFile.txt" << std::endl;
    }

    // save regression vectors
    WriteMatrix<MatrixType>(m_RegressionVectors, prefix+"-RegressionVectors.txt");

    // save regression vectors
    if(m_EfficientStorage) m_CoreMatrix.setZero(0,0);
    WriteMatrix<MatrixType>(m_CoreMatrix, prefix+"-CoreMatrix.txt");

    // save sample vectors
    MatrixType X = MatrixType::Zero(m_SampleVectors[0].size(), m_SampleVectors.size());
    for(unsigned i=0; i<m_SampleVectors.size(); i++){
        X.block(0,i,m_SampleVectors[0].size(),1) = m_SampleVectors[i];
    }
    WriteMatrix<MatrixType>(X, prefix+"-SampleVectors.txt");

    // save label vectors
    MatrixType Y = MatrixType::Zero(m_LabelVectors[0].size(), m_LabelVectors.size());
    for(unsigned i=0; i<m_LabelVectors.size(); i++){
        Y.block(0,i,m_LabelVectors[0].size(),1) = m_LabelVectors[i];
    }
    WriteMatrix<MatrixType>(Y, prefix+"-LabelVectors.txt");

    // save parameters
    // KernelType, #KernelParameters, KernelParameters, noise, InputDimension, OutputDimension
    std::ofstream parameter_outfile;
    parameter_outfile.open(std::string(prefix+"-ParameterFile.txt").c_str());

    parameter_outfile << m_Sigma << " " << m_InputDimension << " " << m_OutputDimension << " " << m_EfficientStorage << " " << debug << " ";

    parameter_outfile << std::setprecision(std::numeric_limits<TScalarType>::digits10 +1);
    parameter_outfile << m_Kernel->ToString();

    parameter_outfile.close();
}

template< class TScalarType >
void GaussianProcess<TScalarType>::ToString() const{
    std::cout << "---------------------------------------" << std::endl;
    std::cout << "Gaussian Process" << std::endl;
    std::cout << " - initialized:\t\t" << m_Initialized << std::endl;
    std::cout << " - # samples:\t\t" << m_SampleVectors.size() << std::endl;
    std::cout << " - # labels:\t\t" << m_LabelVectors.size() << std::endl;
    std::cout << " - noise:\t\t" << m_Sigma << std::endl;
    std::cout << " - input dimension:\t" << m_InputDimension << std::endl;
    std::cout << " - output dimension:\t" << m_OutputDimension << std::endl;
    std::cout << std::endl;
    std::cout << " - Kernel:" << std::endl;
    std::cout << "       - Type:\t\t" << m_Kernel->ToString() << std::endl;
    std::cout << "       - Parameter:\t";
    for(unsigned i=0; i<m_Kernel->GetStringParameters().size(); i++){
        std::cout << m_Kernel->GetStringParameters()[i] << ", ";
    }
    std::cout << std::endl;
    std::cout << "---------------------------------------" << std::endl;
}

template< class TScalarType >
bool GaussianProcess<TScalarType>::operator ==(const GaussianProcess<TScalarType> &b) const{
    if(this->debug) std::cout << "GaussianProcess::comparison: " << std::flush;

    if((this->m_RegressionVectors - b.m_RegressionVectors).norm() > 0){
        if(this->debug) std::cout << "regression vectors not equal." << std::endl;
        return false;
    }

    if(this->m_CoreMatrix.diagonalSize() != b.m_CoreMatrix.diagonalSize()){
        if(this->debug) std::cout << "core matrices not equal."  << std::endl;
        return false;
    }
    else{
        if(this->debug && (this->m_CoreMatrix - b.m_CoreMatrix).norm() > 0) std::cout << "core matrices error is " << (this->m_CoreMatrix - b.m_CoreMatrix).norm() << std::endl;
    }

    if(this->m_SampleVectors.size() != b.m_SampleVectors.size()){
        if(this->debug) std::cout << "number of sample vectors not equal." << std::endl;
        return false;
    }
    for(unsigned i=0; i<this->m_SampleVectors.size(); i++){
        if((this->m_SampleVectors[i] - b.m_SampleVectors[i]).norm()>0){
            if(this->debug) std::cout << "sample vectors not equal." << std::endl;
            return false;
        }
    }

    if(this->m_LabelVectors.size() != b.m_LabelVectors.size()){
        if(this->debug) std::cout << "number of label vectors not equal." << std::endl;
        return false;
    }
    for(unsigned i=0; i<this->m_LabelVectors.size(); i++){
        if((this->m_LabelVectors[i] - b.m_LabelVectors[i]).norm()>0) {
            if(this->debug) std::cout << "label vectors not equal." << std::endl;
            return false;
        }
    }
    if(*this->m_Kernel.get() != *b.m_Kernel.get()){
        if(this->debug) std::cout << "kernel not equal." << std::endl;
        return false;
    }
    if(this->m_Sigma != b.m_Sigma){
        if(this->debug) std::cout << "sigma not equal." << std::endl;
        return false;
    }
    if(this->m_Initialized != b.m_Initialized){
        if(this->debug) std::cout << "initialization state not equal." << std::endl;
        return false;
    }
    if(this->m_InputDimension != b.m_InputDimension){
        if(this->debug) std::cout << "input dimension not equal." << std::endl;
        return false;
    }
    if(this->m_OutputDimension != b.m_OutputDimension){
        if(this->debug) std::cout << "output dimension not equal." << std::endl;
        return false;
    }
    if(this->m_EfficientStorage!= b.m_EfficientStorage){
        if(this->debug) std::cout << "efficient storage setting not equal." << std::endl;
        return false;
    }
    if(this->debug != b.debug){
        if(this->debug) std::cout << "debug state not equal." << std::endl;
        return false;
    }
    if(this->debug) std::cout << "is equal!" << std::endl;
    return true;
}

template< class TScalarType >
void GaussianProcess<TScalarType>::ComputeKernelMatrix(typename GaussianProcess<TScalarType>::MatrixType &M) const{
    if(debug){
        std::cout << "GaussianProcess::ComputeKernelMatrix: building kernel matrix... ";
        std::cout.flush();
    }

    ComputeKernelMatrixInternal(M, m_SampleVectors);

    if(debug) std::cout << "[done]" << std::endl;
}

template< class TScalarType >
void GaussianProcess<TScalarType>::AddNoiseToKernelMatrix(typename GaussianProcess<TScalarType>::MatrixType &M) const{
    // add noise variance to diagonal
    if(M.rows() != M.cols()) throw std::string("GaussianProcess::AddNoiseToKernelMatrix: square matrix required.");
    for(unsigned i=0; i<M.rows(); i++){
        M(i,i) += m_Sigma*m_Sigma;
    }
}

template< class TScalarType >
void GaussianProcess<TScalarType>::ComputeKernelMatrixInternal(typename GaussianProcess<TScalarType>::MatrixType &M,
                                                               const typename GaussianProcess<TScalarType>::VectorListType& samples) const{
    unsigned n = samples.size();
    M.resize(n,n);

#pragma omp parallel for
    for(unsigned i=0; i<n; i++){
        for(unsigned j=i; j<n; j++){
            TScalarType v = (*m_Kernel)(samples[i],samples[j]);
            M(i,j) = v;
            M(j,i) = v;
        }
    }

    // check if matrix entries are finite
    if(!((M - M).array() == (M - M).array()).all()){
        throw std::string("GaussianProcess::ComputeKernelMatrixInternal: kernel matrix contains entries which are not finite.");
    }
}

template< class TScalarType >
TScalarType GaussianProcess<TScalarType>::ComputeKernelMatrixTrace() const{
    if(debug){
        std::cout << "GaussianProcess::ComputeKernelMatrixTrace: sum up diagonal elements of kernel matrix... ";
        std::cout.flush();
    }

    TScalarType trace = ComputeKernelMatrixTraceInternal(m_SampleVectors);

    if(debug) std::cout << "[done]" << std::endl;

    return trace;
}

template< class TScalarType >
TScalarType GaussianProcess<TScalarType>::ComputeKernelMatrixTraceInternal(const typename GaussianProcess<TScalarType>::VectorListType& samples) const{
    unsigned n = samples.size();
    TScalarType trace = 0;

    for(unsigned i=0; i<n; i++){
            trace += (*m_Kernel)(samples[i],samples[i]);
    }
    return trace;
}

template< class TScalarType >
typename GaussianProcess<TScalarType>::VectorType
GaussianProcess<TScalarType>::ComputeDerivativeKernelMatrixTrace() const{
    if(debug){
        std::cout << "GaussianProcess::ComputeDerivativeKernelMatrixTrace: sum up diagonal elements of derivative kernel matrix... ";
        std::cout.flush();
    }

    typename GaussianProcess<TScalarType>::VectorType trace = ComputeDerivativeKernelMatrixTraceInternal(m_SampleVectors);

    if(debug) std::cout << "[done]" << std::endl;

    return trace;
}

template< class TScalarType >
typename GaussianProcess<TScalarType>::VectorType
GaussianProcess<TScalarType>::ComputeDerivativeKernelMatrixTraceInternal(const typename GaussianProcess<TScalarType>::VectorListType& samples) const{
    typedef typename GaussianProcess<TScalarType>::VectorType VectorType;
    unsigned num_params = m_Kernel->GetNumberOfParameters();
    VectorType trace = VectorType::Zero(num_params);

    unsigned n = samples.size();
    for(unsigned i=0; i<n; i++){
            trace += m_Kernel->GetDerivative(samples[i], samples[i]);
    }
    return trace;
}

template< class TScalarType >
void GaussianProcess<TScalarType>::ComputeDerivativeKernelMatrix(typename GaussianProcess<TScalarType>::MatrixType &M) const{
    if(debug){
        std::cout << "GaussianProcess::ComputeDerivativeKernelMatrix: building kernel matrix... ";
        std::cout.flush();
    }

    ComputeDerivativeKernelMatrixInternal(M, m_SampleVectors);

    if(debug) std::cout << "[done]" << std::endl;
}

template< class TScalarType >
void GaussianProcess<TScalarType>::ComputeDerivativeKernelMatrixInternal(typename GaussianProcess<TScalarType>::MatrixType &M,
                                                                         const typename GaussianProcess<TScalarType>::VectorListType& samples) const{
    unsigned num_params = m_Kernel->GetNumberOfParameters();

    unsigned n = samples.size();
    M.resize(n*num_params,n);

#pragma omp parallel for
    for(unsigned i=0; i<n; i++){
        for(unsigned j=i; j<n; j++){
            typename GaussianProcess<TScalarType>::VectorType v;
            v = m_Kernel->GetDerivative(samples[i], samples[j]);

            if(v.rows() != num_params) throw std::string("GaussianProcess::ComputeDerivativeKernelMatrixInternal: dimension missmatch in derivative.");
            for(unsigned p=0; p<num_params; p++){

                //if(i+p*n >= M.rows() || j+p*n >= M.rows())  throw std::string("GaussianProcess::ComputeDerivativeKernelMatrix: dimension missmatch in derivative.");

                M(i + p*n, j) = v[p];
                M(j + p*n, i) = v[p];
            }
        }
    }
}

template< class TScalarType >
void GaussianProcess<TScalarType>::ComputeCoreMatrix(typename GaussianProcess<TScalarType>::MatrixType &C) const{
    MatrixType K;
    ComputeKernelMatrix(K);

    // add noise variance to diagonal
    AddNoiseToKernelMatrix(K);

    C = InvertKernelMatrix(K, m_InvMethod);

    if(debug){
        std::cout << "GaussianProcess::ComputeCoreMatrix: inversion error: " << (K*C - MatrixType::Identity(K.rows(),K.cols())).norm() << std::endl;
    }
}

template< class TScalarType >
typename GaussianProcess<TScalarType>::HighPrecisionType GaussianProcess<TScalarType>::ComputeCoreMatrixWithDeterminant(typename GaussianProcess<TScalarType>::MatrixType &C) const{
    typedef typename GaussianProcess<TScalarType>::HighPrecisionType HighPrecisionType;
    MatrixType K;
    ComputeKernelMatrix(K);

    // add noise variance to diagonal
    AddNoiseToKernelMatrix(K);

    C = InvertKernelMatrix(K, m_InvMethod);

    if(debug){
        std::cout << "GaussianProcess::ComputeCoreMatrix: inversion error: " << (K*C - MatrixType::Identity(K.rows(),K.cols())).norm() << std::endl;
        std::cout << "GaussianProcess::ComputeCoreMatrix: determinant of K: " << K.template cast<HighPrecisionType>().determinant() << std::endl;
    }
    return K.template cast<HighPrecisionType>().determinant();
}

template< class TScalarType >
typename GaussianProcess<TScalarType>::MatrixType GaussianProcess<TScalarType>::InvertKernelMatrix(const typename GaussianProcess<TScalarType>::MatrixType &K,
                                                      typename GaussianProcess<TScalarType>::InversionMethod inv_method,
                                                                                bool stable) const{
    // compute core matrix
    if(debug){
        std::cout << "GaussianProcess::InvertKernelMatrix: inverting kernel matrix... ";
        std::cout.flush();
    }

    typename GaussianProcess<TScalarType>::MatrixType core;

    switch(inv_method){
    // standard method: fast but not that accurate
    // Uses the LU decomposition with full pivoting for the inversion
    case FullPivotLU:{
        if(debug) std::cout << " (inversion method: FullPivotLU) " << std::flush;
        try{
            if(stable){
                core = K.inverse();
            }
            else{
                if(debug) std::cout << " (using lapack) " << std::flush;
                core = lapack::lu_invert<TScalarType>(K);
            }
        }
        catch(lapack::LAPACKException& e){
            core = K.inverse();
        }
    }
    break;

    // very accurate and very slow method, use it for small problems
    // Uses the two-sided Jacobi SVD decomposition
    case JacobiSVD:{
        if(debug) std::cout << " (inversion method: JacobiSVD) " << std::flush;
        Eigen::JacobiSVD<MatrixType> jacobisvd(K, Eigen::ComputeThinU | Eigen::ComputeThinV);
        if((jacobisvd.singularValues().real().array() < 0).any() && debug){
            std::cout << "GaussianProcess::InvertKernelMatrix: warning: there are negative eigenvalues.";
            std::cout.flush();
        }
        core = jacobisvd.matrixV() * VectorType(1/jacobisvd.singularValues().array()).asDiagonal() * jacobisvd.matrixU().transpose();
    }
    break;

    // accurate method and faster than Jacobi SVD.
    // Uses the bidiagonal divide and conquer SVD
    case BDCSVD:{
        if(debug) std::cout << " (inversion method: BDCSVD) " << std::flush;
#ifdef EIGEN_BDCSVD_H
        Eigen::BDCSVD<MatrixType> bdcsvd(K, Eigen::ComputeThinU | Eigen::ComputeThinV);
        if((bdcsvd.singularValues().real().array() < 0).any() && debug){
            std::cout << "GaussianProcess::InvertKernelMatrix: warning: there are negative eigenvalues.";
            std::cout.flush();
        }
        core = bdcsvd.matrixV() * VectorType(1/bdcsvd.singularValues().array()).asDiagonal() * bdcsvd.matrixU().transpose();
#else
        // this is checked, since BDCSVD is currently not in the newest release
        throw std::string("GaussianProcess::InvertKernelMatrix: BDCSVD is not supported by the provided Eigen library.");
#endif

    }
    break;

    // faster than the SVD method but less stable
    // computes the eigenvalues/eigenvectors of selfadjoint matrices
    case SelfAdjointEigenSolver:{
        if(debug) std::cout << " (inversion method: SelfAdjointEigenSolver) " << std::flush;
        try{
            core = lapack::chol_invert<TScalarType>(K);
        }
        catch(lapack::LAPACKException& e){
            Eigen::SelfAdjointEigenSolver<MatrixType> es;
            es.compute(K);
            VectorType eigenValues = es.eigenvalues().reverse();
            MatrixType eigenVectors = es.eigenvectors().rowwise().reverse();
            if((eigenValues.real().array() < 0).any() && debug){
                std::cout << "GaussianProcess::InvertKernelMatrix: warning: there are negative eigenvalues.";
                std::cout.flush();
            }
            core = eigenVectors * VectorType(1/eigenValues.array()).asDiagonal() * eigenVectors.transpose();
        }
    }
    break;
    }

    if(debug) std::cout << "[done]" << std::endl;
    return core;
}

template< class TScalarType >
void GaussianProcess<TScalarType>::ComputeLabelMatrix(typename GaussianProcess<TScalarType>::MatrixType &Y) const{
    ComputeLabelMatrixInternal(Y, m_LabelVectors);
}

template< class TScalarType >
void GaussianProcess<TScalarType>::ComputeLabelMatrixInternal(typename GaussianProcess<TScalarType>::MatrixType &Y,
                                                              const typename GaussianProcess<TScalarType>::VectorListType& labels) const{
    unsigned n = labels.size();
    if(!(n > 0)){
        throw std::string("GaussianProcess::ComputeLabelMatrixInternal: no ouput labels defined.");
    }
    unsigned d = labels[0].size();
    Y.resize(n,d);

#pragma omp parallel for
    for(unsigned i=0; i<n; i++){
        Y.block(i,0,1,d) = labels[i].adjoint();
    }
}

template< class TScalarType >
void GaussianProcess<TScalarType>::ComputeRegressionVectors(){

    // Computation of kernel matrix
    if(debug){
        std::cout << "GaussianProcess::ComputeRegressionVectors: calculating regression vectors... " << std::endl;
    }

    // compute the core matrix which is inv(K + sigma2 I)
    // This is a separate function call because it is also used if the core matrix
    // is not stored due to the efficient storage setting
    ComputeCoreMatrix(m_CoreMatrix);

    // calculate label matrix
    // TODO: if a mean support is implemented, the mean has to be subtracted from the labels!
    MatrixType Y;
    ComputeLabelMatrix(Y);

    // calculate regression vectors
    m_RegressionVectors = m_CoreMatrix * Y ; // inv(K + sigma2)*Y

    // deleting core matrix if the storage has to be handled efficiently
    // - it is not used for regression
    // - but it is needed to compute the credible interval
    if(m_EfficientStorage){
        m_CoreMatrix.setZero(0,0);
    }
    if(debug){
        std::cout << "GaussianProcess::ComputeRegressionVectors: calculating regression vectors [done]" << std::endl;
    }
}

template< class TScalarType >
void GaussianProcess<TScalarType>::ComputeKernelVector(const typename GaussianProcess<TScalarType>::VectorType &x,
                                                       typename GaussianProcess<TScalarType>::VectorType &Kx) const{
    if(!m_Initialized){
        throw std::string("GaussianProcess::ComputeKernelVectorInternal: gaussian process is not initialized.");
    }
    ComputeKernelVectorInternal(x, Kx, m_SampleVectors);
}

template< class TScalarType >
void GaussianProcess<TScalarType>::ComputeKernelVectorInternal(const typename GaussianProcess<TScalarType>::VectorType &x,
                                                               typename GaussianProcess<TScalarType>::VectorType &Kx,
                                                               const typename GaussianProcess<TScalarType>::VectorListType& samples) const{
    Kx.resize(samples.size());

#pragma omp parallel for
    for(unsigned i=0; i<Kx.size(); i++){
        Kx(i) = (*m_Kernel)(x, samples[i]);
    }
}

template< class TScalarType >
void GaussianProcess<TScalarType>::ComputeDifferenceMatrix(const typename GaussianProcess<TScalarType>::VectorType &x,
                                                           typename GaussianProcess<TScalarType>::MatrixType &X) const{
    unsigned n = m_SampleVectors.size();
    unsigned d = x.size();
    X.resize(n,d);

    for(unsigned i=0; i<n; i++){
        X.block(i,0,1,d) = (x - m_SampleVectors[i]).adjoint();
    }
}

template< class TScalarType >
void GaussianProcess<TScalarType>::CheckInputDimension(const typename GaussianProcess<TScalarType>::VectorType &x, std::string msg_prefix) const{
    if(x.size()!=m_InputDimension){
        std::stringstream error_msg;
        error_msg << msg_prefix << "dimension of input vector ("<< x.size() << ") does not correspond to the input dimension (" << m_InputDimension << ").";
        throw std::string(error_msg.str());
    }
}

template< class TScalarType >
void GaussianProcess<TScalarType>::CheckOutputDimension(const typename GaussianProcess<TScalarType>::VectorType &y, std::string msg_prefix) const{
    if(y.size()!=m_OutputDimension){
        std::stringstream error_msg;
        error_msg << msg_prefix << "dimension of output vector ("<< y.size() << ") does not correspond to the output dimension (" << m_OutputDimension << ").";
        throw std::string(error_msg.str());
    }
}

}

template class gpr::GaussianProcess<float>;
template class gpr::GaussianProcess<double>;


// Copyright 2020 Jan Feitsma (Falcons)
// SPDX-License-Identifier: Apache-2.0
/*
 * falconsCommonLegacy.hpp (previously cFalconsCommon.cpp)
 *
 *  Created on: Sep 11, 2014
 *      Author: Jan Feitsma
 */

#include "ext/falconsCommonLegacy.hpp"

#include <stdio.h>
#include <math.h>
#include <float.h>
#include <pwd.h>
#include <cstdio>
#include <string.h>
#include <arpa/inet.h>
#include <ifaddrs.h>
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <unistd.h>

// TODO most of this stuff should be distributed to dedicated packages,
// for instance angle utilities, Position2D, Velocity2D etc. to geometry

// Source: http://math.stackexchange.com/questions/1201337/finding-the-angle-between-two-points
double angle_between_two_points_0_2pi(double x1, double y1, double x2, double y2)
{

    double angle = std::atan2(y2 - y1, x2 - x1);
    return project_angle_0_2pi(angle);
}

double project_angle_0_2pi(double angle)
{
    // sanity checks
    if ((angle > 100) || (angle < -100))
    {
        throw std::runtime_error("angle out of bounds");
    }
    while (angle < 0) angle += 2*M_PI;
    while (angle > 2*M_PI) angle -= 2*M_PI;
    return angle;
}

float project_angle_0_2pi(float angle)
{
    // sanity checks
    if ((angle > 100) || (angle < -100))
    {
        throw std::runtime_error("angle out of bounds");
    }
    while (angle < 0) angle += 2*M_PI;
    while (angle > 2*M_PI) angle -= 2*M_PI;
    return angle;
}

double project_angle_mpi_pi(double angle)
{
    // sanity checks
    if ((angle > 100) || (angle < -100))
    {
        throw std::runtime_error("angle out of bounds");
    }
    while (angle < -M_PI) angle += 2*M_PI;
    while (angle > M_PI) angle -= 2*M_PI;
    return angle;
}

float project_angle_mpi_pi(float angle)
{
    // sanity checks
    if ((angle > 100) || (angle < -100))
    {
        throw std::runtime_error("angle out of bounds");
    }
    while (angle < -M_PI) angle += 2*M_PI;
    while (angle > M_PI) angle -= 2*M_PI;
    return angle;
}

// position type transformations
Position2D getPosition2D( const geometry::Pose2D& pose)
{
    Position2D myPosition;
    myPosition.x= pose.x;
    myPosition.y= pose.y;
    return ( myPosition );
}

Position2D getPosition2D( const Point3D& point3d)
{
    Position2D myPosition;
    myPosition.x= (double) point3d.x;
    myPosition.y= (double) point3d.y;
    return ( myPosition );
}

// coordinate transformations
Velocity2D& Velocity2D::transform_fcs2rcs(const Position2D& robotpos)
{
    double angle = (M_PI_2 - robotpos.phi);
    Vector2D xynew = Vector2D(x, y).rotate(angle);
    x = xynew.x;
    y = xynew.y;
    // do not update vphi
    return (*this);
}

Velocity2D& Velocity2D::transform_rcs2fcs(const Position2D& robotpos)
{
    double angle = -(M_PI_2 - robotpos.phi);
    Vector2D xynew = Vector2D(x, y).rotate(angle);
    x = xynew.x;
    y = xynew.y;
    // do not update vphi
    return (*this);
}

Position2D& Position2D::transform_fcs2rcs(const Position2D& robotpos)
{
    // first ttranslate, then rotate
    double angle = (M_PI_2 - robotpos.phi);
    Vector2D xynew = (Vector2D(x, y) - robotpos.xy()).rotate(angle);
    x = xynew.x;
    y = xynew.y;
    phi = phi + angle;
    phi = project_angle_0_2pi(phi);
    return (*this);
}

Position2D& Position2D::transform_rcs2fcs(const Position2D& robotpos)
{
    // first rotate, then translate
    double angle = - (M_PI_2 - robotpos.phi);
    Vector2D xyrot = (Vector2D(x, y)).rotate(angle);
    x = xyrot.x + robotpos.x;
    y = xyrot.y + robotpos.y;
    phi = phi + angle;
    phi = project_angle_0_2pi(phi);
    return (*this);
}

Position2D& Position2D::transform_fcs2acs(bool playing_left_to_right)
{
    // no change in case we are playing left to right
    if (!playing_left_to_right)
    {
        // rotate by half a circle
        x = -x;
        y = -y;
        phi = project_angle_0_2pi(phi + M_PI);
    }
    return (*this);
}

Position2D& Position2D::transform_acs2fcs(bool playing_left_to_right)
{
    // no change in case we are playing left to right
    if (!playing_left_to_right)
    {
        // rotate by half a circle
        x = -x;
        y = -y;
        phi = project_angle_0_2pi(phi + M_PI);
    }
    return (*this);
}

Velocity2D& Velocity2D::transform_fcs2acs(bool playing_left_to_right)
{
    // no change in case we are playing left to right
    if (!playing_left_to_right)
    {
        // rotate by half a circle, xy only
        x = -x;
        y = -y;
    }
    return (*this);
}

Velocity2D& Velocity2D::transform_acs2fcs(bool playing_left_to_right)
{
    // no change in case we are playing left to right
    if (!playing_left_to_right)
    {
        // rotate by half a circle, xy only
        x = -x;
        y = -y;
    }
    return (*this);
}

// handy function to return output of a command
// source: http://stackoverflow.com/questions/478898/how-to-execute-a-command-and-get-output-of-command-within-c
std::string exec(const char* cmd) {
    boost::shared_ptr<FILE> pipe(popen(cmd, "r"), pclose);
    if (!pipe) return "ERROR";
    char buffer[128];
    std::string result = "";
    while (!feof(pipe.get())) {
        if (fgets(buffer, 128, pipe.get()) != NULL)
            result += buffer;
    }
    boost::algorithm::trim(result);
    return result;
}

std::string getProcessId()
{
    return exec("getProcessId");
}

float restrictValue( float a, float minA, float maxA )  //returns restricted value position within given limits
{
    if( a > maxA )
        return maxA;
    if( a < minA )
        return minA;
    return a;
}

bool isValueInRange( float a, float minA, float maxA )  //check if value falls in min-max range
{
    if( a > maxA )
        return false;
    if( a < minA )
        return false;
    return true;
}

double calc_angle(double dX, double dY)
{
    // calculates the absolute angle
    // looking from X1,Y1 towards X2,Y2
    double S = 0.0;
    double th = 0.0;
    S = calc_hypothenusa(dX, dY);

    if (dY < 0) {
        if (dX == 0) {
            th = M_PI_2;
        } else {
            th = acos(-dX / S);
        }
    } else if (dY > 0) {
        if (dX == 0) {
            th = 3.0 / 2.0 * M_PI;
        } else {
            th = 2 * M_PI - acos(-dX / S);
        }
    } else if (dY == 0) {
        if (dX > 0) {
            th = 0;
        } else {
            th = M_PI;
        }
    }
    return th;
}

float calc_angle(float dX, float dY)
{
    // calculates the absolute angle
    // looking from X1,Y1 towards X2,Y2
    float S = 0.0;
    float th = 0.0;

    S = calc_hypothenusa(dX, dY);

    if (dY < 0) {
        if (dX == 0) {
            th = M_PI_2;
        } else {
            th = acos(-dX / S);
        }
    } else if (dY > 0) {
        if (dX == 0) {
            th = 3.0 / 2.0 * M_PI;
        } else {
            th = 2 * M_PI - acos(-dX / S);
        }
    } else if (dY == 0) {
        if (dX > 0) {
            th = 0;
        } else {
            th = M_PI;
        }
    }
    return th;
}

// transform a line segment (v1,v2)
// to a line equation a*x + b*y + c = 0
void calculate_line_equation(Vector2D v1, Vector2D v2, double &a, double &b, double &c)
{
    double dx = v2.x - v1.x;
    double dy = v2.y - v1.y;
    // dx*y - dx*v1.y - dy*x - dy*v1.x = 0
    a = -dy;
    b = dx;
    c = -1.0 * (dx*v1.y - dy*v1.x);
}

double calc_distance_point_line(Vector2D p, double a, double b, double c)
{
    if (a != 0 || b != 0)
    {
        return (a * p.x +  b * p.y + c) / sqrt(a*a + b*b);
    }
    else
    {
        return 0;
    }
}

// based on http://stackoverflow.com/a/385355
bool intersect(Vector2D const &a1, Vector2D const &a2, Vector2D const &b1, Vector2D const &b2, Vector2D &result)
{
    float x12 = a1.x - a2.x;
    float x34 = b1.x - b2.x;
    float y12 = a1.y - a2.y;
    float y34 = b1.y - b2.y;

    float c = x12 * y34 - y12 * x34;

    if (fabs(c) < 0.01)
    {
        // No intersection
        return false;
    }
    // Intersection
    float a = a1.x * a2.y - a1.y * a2.x;
    float b = b1.x * b2.y - b1.y * b2.x;

    result.x = (a * x34 - b * x12) / c;
    result.y = (a * y34 - b * y12) / c;

    return true;
}

double calc_hypothenusa(double dX, double dY)
{
    // Function to calculate the Hypothenusa (Schuine zijde)
    double S = 0.0;
    S = sqrt(pow(dX, 2) + pow(dY, 2));
    return S;
}

float calc_hypothenusa(float dX, float dY)
{
    // Function to calculate the Hypothenusa (Schuine zijde)
    float S = 0.0;
    S = sqrt(pow(dX, 2) + pow(dY, 2));
    return S;
}

double calc_distance( double x1, double y1, double x2, double y2)
{
    double dX=x2-x1;
    double dY=y2-y1;

    return calc_hypothenusa( dX, dY );
};

double calc_distance( Position2D p1, Position2D p2 )
{
    double dX=p2.x - p1.x;
    double dY=p2.y - p1.y;

    return calc_hypothenusa( dX, dY );
}

double calc_distance( Point2D p1, Point2D p2 )
{
    double dX=p2.x - p1.x;
    double dY=p2.y - p1.y;

    return calc_hypothenusa( dX, dY );
}

bool ignoreIfaName(std::string ifa_name)
{
    if (ifa_name == "enp0s31f6") return true; // the port on top-side of CPU-box, located most inward, is reserved for multiCam
    return false;
}

connectionType GetPrimaryIp(char* buffer, size_t buflen)
{
    struct ifaddrs * ifAddrStruct=NULL;
    struct ifaddrs * ifa=NULL;
    void * tmpAddrPtr=NULL;
    bool LANFound = false;
    connectionType retVal = connectionType::INVALID;

    assert(buflen >= INET_ADDRSTRLEN);
    getifaddrs(&ifAddrStruct);

    if(!LANFound)
    {
        for (ifa = ifAddrStruct; ((ifa != NULL) && (!LANFound)); ifa = ifa->ifa_next)
        {
            /* Filter IPV4 addresses */
            if (ifa->ifa_addr->sa_family == AF_INET)
            {

                tmpAddrPtr=&((struct sockaddr_in *)ifa->ifa_addr)->sin_addr;
                char addressBuffer[INET_ADDRSTRLEN];
                inet_ntop(AF_INET, tmpAddrPtr, addressBuffer, INET_ADDRSTRLEN);

                if((strstr(ifa->ifa_name, "en") != NULL) ||
                    (strstr(ifa->ifa_name, "eth") != NULL))
                {
                    if (!ignoreIfaName(ifa->ifa_name))
                    {
                        /* Copy IP address to output */
                        inet_ntop(AF_INET, tmpAddrPtr, buffer, buflen);

                        /* Alright meow! It is time to stop now Mack */
                        LANFound = true;
                        retVal = connectionType::LAN;
                    }
                }
            }
        }
    }

    for (ifa = ifAddrStruct; ((ifa != NULL) && (!LANFound)); ifa = ifa->ifa_next)
    {
        /* Filter IPV4 addresses */
        if (ifa->ifa_addr->sa_family == AF_INET)
        {

            tmpAddrPtr=&((struct sockaddr_in *)ifa->ifa_addr)->sin_addr;
            char addressBuffer[INET_ADDRSTRLEN];
            inet_ntop(AF_INET, tmpAddrPtr, addressBuffer, INET_ADDRSTRLEN);

            if(strstr(ifa->ifa_name, "wl") != NULL)
            {
                /* Copy IP address to output */
                inet_ntop(AF_INET, tmpAddrPtr, buffer, buflen);

                /* Alright meow! It is time to stop now Mack */
                LANFound = true;
                retVal = connectionType::WAN;
            }
        }
    }

    for (ifa = ifAddrStruct; ((ifa != NULL) && (!LANFound)); ifa = ifa->ifa_next)
    {
        /* Filter IPV4 addresses */
        if (ifa->ifa_addr->sa_family == AF_INET)
        {

            tmpAddrPtr=&((struct sockaddr_in *)ifa->ifa_addr)->sin_addr;
            char addressBuffer[INET_ADDRSTRLEN];
            inet_ntop(AF_INET, tmpAddrPtr, addressBuffer, INET_ADDRSTRLEN);

            if(strstr(ifa->ifa_name, "usb") != NULL)
            {
                /* Copy IP address to output */
                inet_ntop(AF_INET, tmpAddrPtr, buffer, buflen);

                /* Alright meow! It is time to stop now Mack */
                LANFound = true;
                retVal = connectionType::USB;
            }
        }
    }

    if(!LANFound)
    {
        for (ifa = ifAddrStruct; ((ifa != NULL) && (!LANFound)); ifa = ifa->ifa_next)
        {
            /* Filter IPV4 addresses */
            if (ifa->ifa_addr->sa_family == AF_INET)
            {

                tmpAddrPtr=&((struct sockaddr_in *)ifa->ifa_addr)->sin_addr;
                char addressBuffer[INET_ADDRSTRLEN];
                inet_ntop(AF_INET, tmpAddrPtr, addressBuffer, INET_ADDRSTRLEN);

                if(strstr(ifa->ifa_name, "lo") != NULL)
                {
                    /* Copy IP address to output */
                    inet_ntop(AF_INET, tmpAddrPtr, buffer, buflen);

                    /* Alright meow! It is time to stop now Mack */
                    LANFound = true;
                    retVal = connectionType::LOOPBACK;
                }
            }
        }
    }

    if(!LANFound)
    {
        printf("No adapter found for fetching an IP address");
    }

    if (ifAddrStruct!=NULL)
    {
        freeifaddrs(ifAddrStruct);
    }

    return retVal;
}

connectionType GetPrimaryConnectionType()
{
    struct ifaddrs * ifAddrStruct=NULL;
    struct ifaddrs * ifa=NULL;
    void * tmpAddrPtr=NULL;
    connectionType retVal = connectionType::INVALID;

    getifaddrs(&ifAddrStruct);

    if(retVal == connectionType::INVALID)
    {
        for (ifa = ifAddrStruct; ((ifa != NULL) && (retVal == connectionType::INVALID)); ifa = ifa->ifa_next)
        {
            /* Filter IPV4 addresses */
            if (ifa->ifa_addr->sa_family == AF_INET)
            {

                tmpAddrPtr=&((struct sockaddr_in *)ifa->ifa_addr)->sin_addr;
                char addressBuffer[INET_ADDRSTRLEN];
                inet_ntop(AF_INET, tmpAddrPtr, addressBuffer, INET_ADDRSTRLEN);

                if((strstr(ifa->ifa_name, "en") != NULL) ||
                    (strstr(ifa->ifa_name, "eth") != NULL))
                {
                    if (!ignoreIfaName(ifa->ifa_name))
                    {
                        /* Alright meow! It is time to stop now Mack */
                        retVal = connectionType::LAN;
                    }
                }
            }
        }
    }

    for (ifa = ifAddrStruct; ((ifa != NULL) && (retVal == connectionType::INVALID)); ifa = ifa->ifa_next)
    {
        /* Filter IPV4 addresses */
        if (ifa->ifa_addr->sa_family == AF_INET)
        {

            tmpAddrPtr=&((struct sockaddr_in *)ifa->ifa_addr)->sin_addr;
            char addressBuffer[INET_ADDRSTRLEN];
            inet_ntop(AF_INET, tmpAddrPtr, addressBuffer, INET_ADDRSTRLEN);

            if(strstr(ifa->ifa_name, "wl") != NULL)
            {
                /* Alright meow! It is time to stop now Mack */
                retVal = connectionType::WAN;
            }
        }
    }

    for (ifa = ifAddrStruct; ((ifa != NULL) && (retVal == connectionType::INVALID)); ifa = ifa->ifa_next)
    {
        /* Filter IPV4 addresses */
        if (ifa->ifa_addr->sa_family == AF_INET)
        {

            tmpAddrPtr=&((struct sockaddr_in *)ifa->ifa_addr)->sin_addr;
            char addressBuffer[INET_ADDRSTRLEN];
            inet_ntop(AF_INET, tmpAddrPtr, addressBuffer, INET_ADDRSTRLEN);

            if(strstr(ifa->ifa_name, "usb") != NULL)
            {
                /* Alright meow! It is time to stop now Mack */
                retVal = connectionType::USB;
            }
        }
    }

    if(retVal == connectionType::INVALID)
    {
        for (ifa = ifAddrStruct; ((ifa != NULL) && (retVal == connectionType::INVALID)); ifa = ifa->ifa_next)
        {
            /* Filter IPV4 addresses */
            if (ifa->ifa_addr->sa_family == AF_INET)
            {

                tmpAddrPtr=&((struct sockaddr_in *)ifa->ifa_addr)->sin_addr;
                char addressBuffer[INET_ADDRSTRLEN];
                inet_ntop(AF_INET, tmpAddrPtr, addressBuffer, INET_ADDRSTRLEN);

                if(strstr(ifa->ifa_name, "lo") != NULL)
                {
                    /* Alright meow! It is time to stop now Mack */
                    retVal = connectionType::LOOPBACK;
                }
            }
        }
    }

    if(retVal == connectionType::INVALID)
    {
        printf("No adapter found for fetching an IP address");
    }

    if (ifAddrStruct!=NULL)
    {
        freeifaddrs(ifAddrStruct);
    }

    return retVal;
}

/* Double comparison */
bool doubleIsEqual(double a, double b)
{
    return fabs(a - b) < FLT_EPSILON;
}

bool floatIsEqual(float a, float b)
{
    return fabs(a - b) < DBL_EPSILON;
}

// handy function to return output of a command
// source: http://stackoverflow.com/questions/478898/how-to-execute-a-command-and-get-output-of-command-within-c
std::string systemStdout(std::string cmd, int bufferLimit)
{
    std::shared_ptr<FILE> pipe(popen(cmd.c_str(), "r"), pclose);
    if (!pipe) return "ERROR";
    char buffer[128];
    std::string result = "";
    while (!feof(pipe.get())) {
        if (fgets(buffer, 128, pipe.get()) != NULL)
            result += buffer;
    }
    boost::algorithm::trim(result);
    // limit string length
    if ((int)result.size() > bufferLimit)
    {
        result = result.substr(0, bufferLimit);
    }
    return result;
}


// Copyright 2020 ICLUE @ UIUC. All rights reserved.
#include <coxeter_properness/search.h>

#include <algorithm>
#include <climits>
#include <deque>
#include <numeric>
#include <ostream>
#include <cctype>
#include <stack>
#include <utility>
#include <vector>

#include <coxeter_properness/search.h>

#include <coxeter/constants.h>
#include <coxeter/coxtypes.h>
#include <coxeter/interactive.h>
#include <coxeter/type.h>

#include <boost/math/special_functions/binomial.hpp>

namespace coxeter_properness {

using interactive::CoxGroup;
using coxeter::CoxWord;

auto numLDescents(const CoxGroup& W, const CoxWord& w) -> int32_t {
  auto numDescents = 0;
  for (auto f1 = W.ldescent(w); f1 != 0; f1 &= (f1-1)) {
    // const auto s = bits::firstBit(f1);
    ++numDescents;
  }
  return numDescents;
}

auto choose(const int n, const int k) -> int32_t {
  if (n < k) return 0;
  return static_cast<int32_t>(boost::math::binomial_coefficient<double>(n, k));
}

auto maxw0(const CoxGroup& W, const CoxWord& w) -> int32_t {
  const auto n = W.rank();
  const auto dw = numLDescents(W, w);
  const auto& t = W.type();
  const auto& typeName = t.name();

  if (typeName == io::String("A"))  return choose(dw+1, 2);
  if (typeName == io::String("B")) return dw * dw;
  if (typeName == io::String("D")) return dw > 3 ? dw*(dw-1) : choose(dw+1, 2);
  if (typeName == io::String("E")) {
    const static std::vector<int32_t> m_e = {0, 1, 3, 6, 12, 20, 36, 63, 120};
    return m_e.at(dw);
  }
  if (typeName == io::String("F")) {
    const static std::vector<int32_t> m_f = {0, 1, 4, 9, 24};
    return m_f[dw];
  }
  if (typeName == io::String("G")) {
    const static std::vector<int32_t> m_g = {0, 1, 6};
    return m_g[dw];
  }
  if (typeName == io::String("H")) {
    const static std::vector<int32_t> m_h = {0, 1, 5, 15, 60};
    return m_h[dw];
  }
  if (typeName == io::String("I")) return dw == 2 ? n : dw;

  throw std::invalid_argument("no type matching");
}

bool proper(const CoxGroup& W, const CoxWord& w) {
  const auto n = W.rank();
  return w.length() <= n + maxw0(W, w);
}

Transformer::Transformer(const std::shared_ptr<CoxGroup>& W) : W(W) { }

bool Transformer::parse(const std::string& line, CoxWord* w) {
  elements.clear();
  for (const auto& x : line) {
    // This assumes that the rank of the group is < 10.
    if (std::isdigit(x)) {
      elements.push_back(x-'0');
    }
  }

  if (elements.empty()) return false;

  // Don't trust the coxeter code to handle dynamic resource allocation.
  rawWord.setLength(elements.size());
  for (int i = 0; i < elements.size(); ++i) {
    rawWord[i] = elements[i];
  }

  // W->reduced(*w, rawWord);
  // We can assume that the word is already reduced.
  *w = rawWord;
  return true;
}

}  // namespace coxeter_properness


//
// Created by erik on 9/23/16.
//

#include <cmath>
#include <boost/optional.hpp>
#include "HitTests.h"

using namespace spatacs;

boost::optional<physics::time_t> physics::intersect_origin(const MovingSphere& s)
{
    // x²(t) = r²(t): v² t² + 2 vo t + o² = a²t² + 2 ar t + r²
    // (v² - a²) t² + 2 (vo - ar)t + o² - r² = 0
    // t² + 2 (vo - ar)/(v²-a²) t + (o²-r²)/(v²-a²) = 0
    auto vo = dot(s.vel, s.pos);
    auto ar = s.exp * s.rad;
    auto v2 = dot(s.vel, s.vel);
    auto a2 = s.exp * s.exp;
    auto o2 = dot(s.pos, s.pos);
    auto r2 = s.rad * s.rad;

    // extreme case, need to consider to prevent div by zero
    if(v2 == a2)
    {
        if(vo == ar)
            return boost::none;

        return (r2 - o2) / (vo - ar);
    }

    auto p = 2.0 * (vo - ar) / (v2 - a2);
    auto q = (o2 - r2) / (v2 - a2);
    auto det = p*p/4.0 - q;
    if(det < decltype(det)(0))
    {
        return boost::none;
    }
    auto w = sqrt(det);
    if(-p/2.0 < w)
    {
        return -p/2.0 + w;
    } else
    {
        return -p/2.0 - w;
    }
}

boost::optional<physics::time_t> physics::intersect(MovingSphere s1, const MovingSphere& s2)
{
    s1.pos -= s2.pos;
    s1.vel -= s2.vel;
    s1.rad += s2.rad;
    s1.exp += s2.exp;
    return intersect_origin(s1);
}


/* -*- c++ -*- */
/*
 * Copyright 2013 Jiří Pinkava <j-pi@seznam.cz>.
 *
 * This 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, or (at your option)
 * any later version.
 *
 * This software 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.
 */

#include "noise_level_estimator2_impl.h"
#include <algorithm>
#include <boost/foreach.hpp>
#include <cmath>
#include <stdexcept>

#include <stdio.h>

namespace gr {
  namespace rstt {

    noise_level_estimator2::sptr
    noise_level_estimator2::make(float coverage, float chunk_size)
    {
      if (!(coverage > 0.))
        throw std::out_of_range("noise_level_estimator2 coverage must be > 0.");
      if (!(coverage <= 1.))
        throw std::out_of_range("noise_level_estimator2 coverage must be <= 1.");
      if (!(chunk_size > 0.))
        throw std::out_of_range("noise_level_estimator2 chunk_size must be > 0.");
      if (!(chunk_size <= coverage))
        throw std::out_of_range("noise_level_estimator2 chunk_size must be <= coverage");
      return sptr(new noise_level_estimator2_impl(coverage, chunk_size));
    }

    noise_model
    noise_level_estimator2_impl::estimate(const float *data, int data_items) const
    {
      const int coverage = data_items * this->coverage;
      const int chunk_size = data_items * this->chunk_size;

      const int ms_size = data_items - chunk_size + 1;
      float mean[ms_size];
      float mean2[ms_size];

      // moving average, first point
      mean[0] = 0.;
      mean2[0] = 0.;
      for (int i = 0; i < chunk_size; ++i) {
        mean[0] += data[i];
        mean2[0] += data[i] * data[i];
      }

      // moving average, other points
      for (int i = chunk_size, j = 1; i < data_items; ++i, ++j) {
       mean[j] = mean[j-1] - data[j-1] + data[i];
       mean2[j] = mean2[j-1] - (data[j-1] * data[j-1]) + (data[i] * data[i]);
      }

      for (int i = 0; i < ms_size; ++i) {
        mean[i] /= chunk_size;
        mean2[i] /= chunk_size;
      }

      // moving standard deviation (s^2)
      float s2[ms_size];
      for (int i = 0; i < ms_size; ++i) {
        s2[i] = mean2[i] - mean[i] * mean[i];
      }

      // get the average of lower bound values for mean and deviation
      std::sort(s2, s2 + ms_size);
      float s = 0.;
      for (int i = 0; i < coverage; ++i) {
        s += std::sqrt(s2[i]);
      }
      s /= coverage;

      std::sort(mean, mean + ms_size);
      float m = 0.;
      for (int i = 0; i < coverage; ++i) {
        m += mean[i];
      }
      m /= coverage;

      return noise_model(m, s);
    }

  } // namespace rstt
} // namespace gr


/**
  *
  * @file SpatialPoint.cpp
  * @author Naoki Takahashi
  *
  **/

#include "SpatialPoint.hpp"

#include <sstream>

#include <Eigen/Geometry>
#include <unsupported/Eigen/EulerAngles>

namespace Kinematics {
	namespace Quantity {
		template <typename Scalar>
		void SpatialPoint<Scalar>::reset() {
			reset_point();
			reset_angle();
		}

		template <typename Scalar>
		void SpatialPoint<Scalar>::reset_point() {
			point_vector = Point::Zero();
		}

		template <typename Scalar>
		void SpatialPoint<Scalar>::reset_angle() {
			q_rotation.vec() = Vector3::Zero();
			q_rotation.w() = 0;
		}

		template <typename Scalar>
		const typename SpatialPoint<Scalar>::Point &SpatialPoint<Scalar>::point() const {
			return point_vector;
		}

		template <typename Scalar>
		SpatialPoint<Scalar> &SpatialPoint<Scalar>::point(const Point &new_point) {
			point_vector = new_point;

			return *this;
		}

		template <typename Scalar>
		SpatialPoint<Scalar> &SpatialPoint<Scalar>::point(const Scalar &x, const Scalar &y, const Scalar &z) {
			point_vector << x, y, z;

			return *this;
		}

		template <typename Scalar>
		const typename SpatialPoint<Scalar>::EulerAngles &SpatialPoint<Scalar>::angle() const {
			static Eigen::EulerAngles<Scalar, Eigen::EulerSystem<Eigen::EULER_X, Eigen::EULER_Y, Eigen::EULER_Z>> euler_angle_system;

			euler_angle_system = q_rotation;

			return euler_angle_system.angles();
		}

		template <typename Scalar>
		SpatialPoint<Scalar> &SpatialPoint<Scalar>::angle(const EulerAngles &new_angle) {
			q_rotation = Eigen::AngleAxis<Scalar>(new_angle.x(), EulerAngles::UnitX());
			q_rotation = q_rotation * Eigen::AngleAxis<Scalar>(new_angle.y(), EulerAngles::UnitY());
			q_rotation = q_rotation * Eigen::AngleAxis<Scalar>(new_angle.z(), EulerAngles::UnitZ());

			return *this;
		}

		template <typename Scalar>
		SpatialPoint<Scalar> &SpatialPoint<Scalar>::angle(const Scalar &roll, const Scalar &pitch, const Scalar &yaw) {
			q_rotation = Eigen::AngleAxis<Scalar>(roll, EulerAngles::UnitX());
			q_rotation = q_rotation * Eigen::AngleAxis<Scalar>(pitch, EulerAngles::UnitY());
			q_rotation = q_rotation * Eigen::AngleAxis<Scalar>(yaw, EulerAngles::UnitZ());

			return *this;
		}

		template <typename Scalar>
		const typename SpatialPoint<Scalar>::Quaternion &SpatialPoint<Scalar>::quaternion() {
			return q_rotation;
		}

		template <typename Scalar>
		SpatialPoint<Scalar> &SpatialPoint<Scalar>::quaternion(const Quaternion &quaternion) {
			q_rotation = quaternion;

			return *this;
		}

		template <typename Scalar>
		const typename SpatialPoint<Scalar>::Matrix3x3 &SpatialPoint<Scalar>::rotation() {
			static Matrix3x3 rotation_matrix;

			rotation_matrix = q_rotation.matrix();

			return rotation_matrix;

		}

		template <typename Scalar>
		SpatialPoint<Scalar> &SpatialPoint<Scalar>::rotation(const Matrix3x3 &rotation_matrix) {
			const auto rrt = rotation_matrix * rotation_matrix.transpose();

			if(1e-5 < (Matrix3x3::Identity() - rrt).norm()) {
				std::stringstream ss;

				ss << "Input Matrix is\n" << rotation_matrix << "\n";
				ss << "\nM*MT is\n" << rrt << "\n";
				ss << (Matrix3x3::Identity() - rrt).norm() << "\n";
				ss << "\n";

				throw std::runtime_error("Unknown type matrix from Kinematics::SpatialPoint\n" + ss.str());
			}

			q_rotation = rotation_matrix;

			return *this;
		}

		template <typename Scalar>
		void SpatialPoint<Scalar>::operator ()(const Vector6 &new_point_with_angle_vector) {
			point(new_point_with_angle_vector.block(0, 0, Rank3, 1));
			angle(new_point_with_angle_vector.block(Rank3, 0, Rank3, 1));
		}

		template <typename Scalar>
		const typename SpatialPoint<Scalar>::Vector6 SpatialPoint<Scalar>::operator ()() {
			static Vector6 point_with_angle_vector;

			point_with_angle_vector << point(), angle();

			return point_with_angle_vector;
		}

		template <typename Scalar>
		SpatialPoint<Scalar> &SpatialPoint<Scalar>::operator += (const SpatialPoint<Scalar> &spatial_point) {
			if(this != &spatial_point) {
				static Eigen::EulerAngles<Scalar, Eigen::EulerSystem<Eigen::EULER_X, Eigen::EULER_Y, Eigen::EULER_Z>> euler_angle_system;

				euler_angle_system = spatial_point.q_rotation;

				this->angle(this->angle() + euler_angle_system.angles());

				this->point_vector += spatial_point.point_vector;
			}

			return *this;
		}

		template <typename Scalar>
		SpatialPoint<Scalar> &SpatialPoint<Scalar>::operator -= (const SpatialPoint<Scalar> &spatial_point) {
			if(this != &spatial_point) {
				static Eigen::EulerAngles<Scalar, Eigen::EulerSystem<Eigen::EULER_X, Eigen::EULER_Y, Eigen::EULER_Z>> euler_angle_system;

				euler_angle_system = spatial_point.q_rotation;

				this->angle(this->angle() - euler_angle_system.angles());

				this->point_vector -= spatial_point.point_vector;
			}

			return *this;
		}

		template <typename Scalar>
		bool SpatialPoint<Scalar>::operator == (const SpatialPoint &spatial_point) {
			return
				this->point_vector == spatial_point.point_vector
				&& this->q_rotation.vec() == spatial_point.q_rotation.vec()
				&& this->q_rotation.w() == spatial_point.q_rotation.w();
		}

		template <typename Scalar>
		bool SpatialPoint<Scalar>::operator != (const SpatialPoint &spatial_point) {
			return !this->operator == (spatial_point);
		}

		template class SpatialPoint<float>;
		template class SpatialPoint<double>;
		template class SpatialPoint<long double>;
	}
}


//
// Copyright Fabien Dekeyser, Quoc-Cuong Pham 2008
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_HEGV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_HEGV_HPP

#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>
// #include <boost/numeric/bindings/traits/std_vector.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

#include <cassert>

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // hegv
    //
    ///////////////////////////////////////////////////////////////////

    /*
     *  hegv() computes all the eigenvalues, and optionally, the eigenvectors
     *  of a real generalized symmetric-definite eigenproblem, of the form
     *  A*x=(lambda)*B*x,  A*Bx=(lambda)*x,  or B*A*x=(lambda)*x.
     *  Here A and B are assumed to be symmetric and B is also
     *  positive definite.
     * TYPE   (input) INTEGER
     *          Specifies the problem type to be solved:
     *          = 1:  A*x = (lambda)*B*x
     *          = 2:  A*B*x = (lambda)*x
     *          = 3:  B*A*x = (lambda)*x
     *
     *  JOBZ    (input) CHARACTER*1
     *          = 'N':  Compute eigenvalues only;
     *          = 'V':  Compute eigenvalues and eigenvectors.
     *
     *  UPLO    (input) CHARACTER*1
     *          = 'U':  Upper triangles of A and B are stored;
     *          = 'L':  Lower triangles of A and B are stored.
     *
     *  N       (input) INTEGER
     *          The order of the matrices A and B.  N >= 0.
     *
     *  A       (input/output) DOUBLE PRECISION array, dimension (LDA, N)
     *          On entry, the symmetric matrix A.  If UPLO = 'U', the
     *          leading N-by-N upper triangular part of A contains the
     *          upper triangular part of the matrix A.  If UPLO = 'L',
     *          the leading N-by-N lower triangular part of A contains
     *          the lower triangular part of the matrix A.
     *
     *          On exit, if JOBZ = 'V', then if INFO = 0, A contains the
     *          matrix Z of eigenvectors.  The eigenvectors are normalized
     *          as follows:
     *          if ITYPE = 1 or 2, Z**T*B*Z = I;
     *          if ITYPE = 3, Z**T*inv(B)*Z = I.
     *          If JOBZ = 'N', then on exit the upper triangle (if UPLO='U')
     *          or the lower triangle (if UPLO='L') of A, including the
     *          diagonal, is destroyed.
     *
     *  LDA     (input) INTEGER
     *          The leading dimension of the array A.  LDA >= max(1,N).
     *
     *  B       (input/output) DOUBLE PRECISION array, dimension (LDB, N)
     *          On entry, the symmetric positive definite matrix B.
     *          If UPLO = 'U', the leading N-by-N upper triangular part of B
     *          contains the upper triangular part of the matrix B.
     *          If UPLO = 'L', the leading N-by-N lower triangular part of B
     *          contains the lower triangular part of the matrix B.
     *
     *          On exit, if INFO <= N, the part of B containing the matrix is
     *          overwritten by the triangular factor U or L from the Cholesky
     *          factorization B = U**T*U or B = L*L**T.
     *
     *  LDB     (input) INTEGER
     *          The leading dimension of the array B.  LDB >= max(1,N).
     *
     *  W       (output) DOUBLE PRECISION array, dimension (N)
     *          If INFO = 0, the eigenvalues in ascending order.
     *
     *  WORK    (workspace/output) DOUBLE PRECISION array, dimension (MAX(1,LWORK))
     *          On exit, if INFO = 0, WORK(1) returns the optimal LWORK.
     *
     *  LWORK   (input) INTEGER
     *          The length of the array WORK.  LWORK >= max(1,3*N-1).
     *          For optimal efficiency, LWORK >= (NB+2)*N,
     *          where NB is the blocksize for DSYTRD returned by ILAENV.
     *
     *          If LWORK = -1, then a workspace query is assumed; the routine
     *          only calculates the optimal size of the WORK array, returns
     *          this value as the first entry of the WORK array, and no error
     *          message related to LWORK is issued by XERBLA.
     *
     *  INFO    (output) INTEGER
     *          = 0:  successful exit
     *          < 0:  if INFO = -i, the i-th argument had an illegal value
     *          > 0:  DPOTRF or DSYEV returned an error code:
     *             <= N:  if INFO = i, DSYEV failed to converge;
     *                    i off-diagonal elements of an intermediate
     *                    tridiagonal form did not converge to zero;
     *             > N:   if INFO = N + i, for 1 <= i <= N, then the leading
     *                    minor of order i of B is not positive definite.
     *                    The factorization of B could not be completed and
     *                    no eigenvalues or eigenvectors were computed.
     *
     */

    namespace detail {

      inline
      void hegv (int const itype, char const jobz, char const uplo, int const n,
                 float *a, int const lda, float *b, int const ldb,
                 float *w, float *work, int const lwork, int& info)
      {
        LAPACK_SSYGV (&itype, &jobz, &uplo, &n, a, &lda, b, &ldb, w, work, &lwork, &info);
      }

      inline
      void hegv (int const itype, char const jobz, char const uplo, int const n,
                 double *a, int const lda, double *b, int const ldb,
                 double *w, double *work, int const lwork, int& info)
      {
        LAPACK_DSYGV (&itype, &jobz, &uplo, &n, a, &lda, b, &ldb, w, work, &lwork, &info);
      }

      inline
      void hegv (int const itype, char const jobz, char const uplo, int const n,
                 traits::complex_f *a, int const lda, traits::complex_f *b, int const ldb,
                 float *w, traits::complex_f *work, int const lwork, float *rwork, int& info)
      {
        LAPACK_CHEGV (&itype, &jobz, &uplo, &n, traits::complex_ptr(a), &lda,
          traits::complex_ptr(b), &ldb, w, traits::complex_ptr(work), &lwork, rwork, &info);
      }

      inline
      void hegv (int const itype, char const jobz, char const uplo, int const n,
                 traits::complex_d *a, int const lda, traits::complex_d *b, int const ldb,
                 double *w, traits::complex_d *work, int const lwork, double *rwork, int& info)
      {
        LAPACK_ZHEGV (&itype, &jobz, &uplo, &n, traits::complex_ptr(a), &lda,
          traits::complex_ptr(b), &ldb, w, traits::complex_ptr(work), &lwork, rwork, &info);
      }
    }  // namespace detail

    namespace detail {

      template <int N>
      struct Hegv{};

      /// Handling of workspace in the case of one workarray.
      template <>
      struct Hegv< 1 > {
        // Function that allocates work arrays
        template <typename T, typename R>
        void operator() (
                 int const itype, char const jobz, char const uplo, int const n,
                 T *a, int const lda, T *b, int const ldb,
                 R *w, optimal_workspace, int& info ) {

           traits::detail::array<T> work( std::max<int>(1,34*n) );

           detail::hegv(itype, jobz, uplo, n, a, lda, b, ldb, w,
             traits::vector_storage (work), traits::vector_size (work), info);
        }
        // Function that allocates work arrays
        template <typename T, typename R>
        void operator() (
                 int const itype, char const jobz, char const uplo, int const n,
                 T *a, int const lda, T *b, int const ldb,
                 R *w, minimal_workspace, int& info ) {

           traits::detail::array<T> work( std::max<int>(1,3*n-1) );

           detail::hegv(itype, jobz, uplo, n, a, lda, b, ldb, w,
             traits::vector_storage (work), traits::vector_size (work), info);
        }
        // Function that uses given workarrays
        template <typename T, typename R, typename W>
        void operator() (
                 int const itype, char const jobz, char const uplo, int const n,
                 T *a, int const lda, T *b, int const ldb,
                 R *w, detail::workspace1<W> work, int& info ) {

           assert (traits::vector_size (work.w_) >= 3*n-1);

           detail::hegv(itype, jobz, uplo, n, a, lda, b, ldb, w,
             traits::vector_storage (work.w_), traits::vector_size (work.w_), info);
        }
      };

      /// Handling of workspace in the case of two workarrays.
      template <>
      struct Hegv< 2 > {
        // Function that allocates work arrays
        template <typename T, typename R>
        void operator() (
                 int const itype, char const jobz, char const uplo, int const n,
                 T *a, int const lda, T *b, int const ldb,
                 R *w, optimal_workspace, int& info ) {

           traits::detail::array<T> work( std::max<int>(1,34*n) );
           traits::detail::array<R> rwork( std::max<int>(1,3*n-2) );

           detail::hegv(itype, jobz, uplo, n, a, lda, b, ldb, w,
             traits::vector_storage (work), traits::vector_size (work),
             traits::vector_storage (rwork), info);
        }
        // Function that allocates work arrays
        template <typename T, typename R>
        void operator() (
                 int const itype, char const jobz, char const uplo, int const n,
                 T *a, int const lda, T *b, int const ldb,
                 R *w, minimal_workspace, int& info ) {

           traits::detail::array<T> work( std::max<int>(1,2*n-1) );
           traits::detail::array<R> rwork( std::max<int>(1,3*n-2) );

           detail::hegv(itype, jobz, uplo, n, a, lda, b, ldb, w,
             traits::vector_storage (work), traits::vector_size (work),
             traits::vector_storage (rwork), info);
        }
        // Function that uses given workarrays
        template <typename T, typename R, typename WC, typename WR>
        void operator() (
                 int const itype, char const jobz, char const uplo, int const n,
                 T *a, int const lda, T *b, int const ldb,
                 R *w, detail::workspace2<WC,WR> work, int& info ) {

           assert (traits::vector_size (work.w_) >= 2*n-1);
           assert (traits::vector_size (work.wr_) >= 3*n-2);

           detail::hegv(itype, jobz, uplo, n, a, lda, b, ldb, w,
             traits::vector_storage (work.w_), traits::vector_size (work.w_),
             traits::vector_storage (work.wr_), info);
        }
      };    } // namespace detail

    template <typename A, typename B, typename W, typename Work>
    int hegv (int itype, char jobz, char uplo, A& a, B& b, W& w, Work work = optimal_workspace()) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<A>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert ( n>0 );
      assert (traits::matrix_size2 (a)==n);
      assert (traits::leading_dimension (a)>=n);
      assert (traits::vector_size (w)==n);

      int const nb = traits::matrix_size1 (b);
      assert ( nb>0 );
      assert (traits::matrix_size2 (b)==nb);
      assert (traits::leading_dimension (b)>=nb);
      assert ( n== nb);

      assert ( uplo=='U' || uplo=='L' );
      assert ( jobz=='N' || jobz=='V' );

      assert( itype==1 || itype==2 || itype==3);

      int info;
      detail::Hegv< n_workspace_args<typename A::value_type>::value >() (
                   itype, jobz, uplo, n,
                   traits::matrix_storage (a),
                   traits::leading_dimension (a),
                   traits::matrix_storage (b),
                   traits::leading_dimension (b),
                   traits::vector_storage (w),
                   work,
                   info);
      return info;
    }
  }

}}}

#endif


/*=============================================================================
 *
 *  Copyright (c) 2021 Sunnybrook Research Institute
 *
 *  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.
 *
 *=============================================================================*/

#include "StainVectorMath.h"
#include "ODConversion.h"

//Boost includes
#include <boost/qvm/vec.hpp>
#include <boost/qvm/mat.hpp>
#include <boost/qvm/vec_access.hpp>
#include <boost/qvm/mat_access.hpp>
#include <boost/qvm/vec_operations.hpp>
#include <boost/qvm/mat_operations.hpp>
#include <boost/qvm/vec_mat_operations.hpp>
#include <boost/qvm/map_mat_mat.hpp>

///Compute the inverse of a 3x3 matrix using Boost qvm: ensure matrix is unitary before using
void StainVectorMath::Compute3x3MatrixInverse(const double (&inputMat)[9], double (&inversionMat)[9]) {
    //Clear the inversionMat (output) array
    for (int i = 0; i < 9; i++) { inversionMat[i] = 0.0; }
    //Define an output matrix
    boost::qvm::mat<double, 3, 3> outputMatrix;

    //Reshape to a 3x3 matrix
    double reshapedInput[3][3] = { 0.0 };
    int i, j = 0;
    for (int x = 0; x < 9; x++) {
        i = x / 3; //integer division
        j = x % 3;
        reshapedInput[i][j] = inputMat[x];
    }

    //Check the determinant of the matrix: is it 0? Thus is the matrix singular?
    double theDeterminant = boost::qvm::determinant(reshapedInput);
    //Get the inverse of the reshapedInput matrix if the determinant is not zero
    //Return matrix of all zeros if the determinant is zero
    if (abs(theDeterminant) < ODConversion::GetODMinValue()) {
        outputMatrix = boost::qvm::zero_mat<double, 3, 3>();
    }
    else {
        outputMatrix = boost::qvm::transposed(boost::qvm::inverse(reshapedInput));
    }

    //Assign output values (first index is row), no looping
    inversionMat[0] = boost::qvm::A<0, 0>(outputMatrix);
    inversionMat[1] = boost::qvm::A<0, 1>(outputMatrix);
    inversionMat[2] = boost::qvm::A<0, 2>(outputMatrix);
    inversionMat[3] = boost::qvm::A<1, 0>(outputMatrix);
    inversionMat[4] = boost::qvm::A<1, 1>(outputMatrix);
    inversionMat[5] = boost::qvm::A<1, 2>(outputMatrix);
    inversionMat[6] = boost::qvm::A<2, 0>(outputMatrix);
    inversionMat[7] = boost::qvm::A<2, 1>(outputMatrix);
    inversionMat[8] = boost::qvm::A<2, 2>(outputMatrix);
    //void return
}//end Compute3x3MatrixInverse

void StainVectorMath::Make3x3MatrixUnitary(const double (&inputMat)[9], double (&unitaryMat)[9]) {
    //Bundle the input values in rows of three
    std::vector<std::array<double, 3>> inputRows;
    inputRows.push_back(std::array<double, 3>({ inputMat[0], inputMat[1], inputMat[2] }));
    inputRows.push_back(std::array<double, 3>({ inputMat[3], inputMat[4], inputMat[5] }));
    inputRows.push_back(std::array<double, 3>({ inputMat[6], inputMat[7], inputMat[8] }));
    //Get the norm values
    std::vector<double> normVals;
    normVals.push_back(StainVectorMath::Norm<std::array<double, 3>::iterator, double>(inputRows[0].begin(), inputRows[0].end()));
    normVals.push_back(StainVectorMath::Norm<std::array<double, 3>::iterator, double>(inputRows[1].begin(), inputRows[1].end()));
    normVals.push_back(StainVectorMath::Norm<std::array<double, 3>::iterator, double>(inputRows[2].begin(), inputRows[2].end()));
    //Create output rows
    std::vector<std::array<double, 3>> outputRows;
    for (auto it = normVals.begin(); it != normVals.end(); ++it) {
        bool smallValue = (*it < 10.0*ODConversion::GetODMinValue());
        std::array<double, 3> addRow = inputRows[it - normVals.begin()];
        if (smallValue) {
            //do not modify
        }
        else {
            for (auto p = addRow.begin(); p != addRow.end(); ++p) {
                *p = *p / *it;
            }
        }
        outputRows.push_back(addRow);
    }
    //Assign to the unitary (output) matrix
    for (int x = 0; x < 9; x++) {
        int i = x / 3;
        int j = x % 3;
        unitaryMat[x] = outputRows[i][j];
    }
}//end Make3x3MatrixUnitary

void StainVectorMath::ConvertZeroRowsToUnitary(const double (&inputMat)[9], double (&unitaryMat)[9]) {
    double replacementVals[3] = { 1.0,1.0,1.0 };
    StainVectorMath::ConvertZeroRowsToUnitary(inputMat, unitaryMat, replacementVals);
}//end ConvertZeroRowsToUnitary

void StainVectorMath::ConvertZeroRowsToUnitary(const double (&inputMat)[9], double (&unitaryMat)[9], const double (&replacementVals)[3]) {
    //Bundle the input values in rows of three
    std::vector<std::array<double, 3>> inputRows;
    inputRows.push_back(std::array<double, 3>({ inputMat[0], inputMat[1], inputMat[2] }));
    inputRows.push_back(std::array<double, 3>({ inputMat[3], inputMat[4], inputMat[5] }));
    inputRows.push_back(std::array<double, 3>({ inputMat[6], inputMat[7], inputMat[8] }));
    //Get the norm values
    std::vector<double> normVals;
    normVals.push_back(StainVectorMath::Norm<std::array<double, 3>::iterator, double>(inputRows[0].begin(), inputRows[0].end()));
    normVals.push_back(StainVectorMath::Norm<std::array<double, 3>::iterator, double>(inputRows[1].begin(), inputRows[1].end()));
    normVals.push_back(StainVectorMath::Norm<std::array<double, 3>::iterator, double>(inputRows[2].begin(), inputRows[2].end()));
    //Compare against 10xGetODMinValue, set to unitary row if smaller
    std::array<double, 3> replacementArray = { replacementVals[0], replacementVals[1], replacementVals[2] };
    auto unitaryRow = StainVectorMath::NormalizeArray(replacementArray);
    std::vector<std::array<double, 3>> outputRows;
    for (auto it = normVals.begin(); it != normVals.end(); ++it) {
        bool smallValue = (*it < 10.0*ODConversion::GetODMinValue());
        auto addRow = smallValue ? unitaryRow : inputRows[it-normVals.begin()];
        outputRows.push_back(addRow);
    }
    //Assign to the unitary (output) matrix
    for (int x = 0; x < 9; x++) {
        int i = x / 3;
        int j = x % 3;
        unitaryMat[x] = outputRows[i][j];
    }
}//end ConvertZeroRowsToUnitary

std::array<bool, 3> StainVectorMath::RowSumZeroCheck(const double (&inputMat)[9]) {
    std::array<bool, 3> returnVals;
    //Bundle the input values in rows of three
    std::vector<std::array<double, 3>> inputRows;
    inputRows.push_back(std::array<double, 3>({ inputMat[0], inputMat[1], inputMat[2] }));
    inputRows.push_back(std::array<double, 3>({ inputMat[3], inputMat[4], inputMat[5] }));
    inputRows.push_back(std::array<double, 3>({ inputMat[6], inputMat[7], inputMat[8] }));
    //Get the sums
    std::vector<double> rowSums;
    rowSums.push_back(std::accumulate(inputRows[0].begin(), inputRows[0].end(), 0.0));
    rowSums.push_back(std::accumulate(inputRows[1].begin(), inputRows[1].end(), 0.0));
    rowSums.push_back(std::accumulate(inputRows[2].begin(), inputRows[2].end(), 0.0));
    //Get the norm values
    std::vector<double> normVals;
    normVals.push_back(StainVectorMath::Norm<std::array<double, 3>::iterator, double>(inputRows[0].begin(), inputRows[0].end()));
    normVals.push_back(StainVectorMath::Norm<std::array<double, 3>::iterator, double>(inputRows[1].begin(), inputRows[1].end()));
    normVals.push_back(StainVectorMath::Norm<std::array<double, 3>::iterator, double>(inputRows[2].begin(), inputRows[2].end()));

    //For each, if rowSums is zero and normVals is non-zero, returnVals is true
    for (int i = 0; i < 3; i++) {
        bool checkVal = ((abs(rowSums[i]) < ODConversion::GetODMinValue()) && (normVals[i] > 0.0));
        returnVals[i] = checkVal ? true : false;
    }
    return returnVals;
}//end RowSumZeroCheck

void StainVectorMath::Multiply3x3MatrixAndVector(const double (&inputMat)[9], const double (&inputVec)[3], double (&outputVec)[3]) {
    //Clear the output vector
    for (int i = 0; i < 3; i++) { outputVec[i] = 0.0; }
    //Reshape to a 3x3 matrix
    double reshapedMatrix[3][3] = { 0.0 };
    int i, j = 0;
    for (int x = 0; x < 9; x++) {
        i = x / 3;
        j = x % 3;
        reshapedMatrix[i][j] = inputMat[x];
    }

    //Assign matrix and input vector to boost::qvm types
    boost::qvm::mat<double, 3, 3> inputQVMMatrix = boost::qvm::mref(reshapedMatrix);
    boost::qvm::vec<double, 3> inputQVMVector = boost::qvm::zero_vec<double, 3>();
    boost::qvm::A<0>(inputQVMVector) = inputVec[0];
    boost::qvm::A<1>(inputQVMVector) = inputVec[1];
    boost::qvm::A<2>(inputQVMVector) = inputVec[2];

    //Multiplication
    boost::qvm::vec<double, 3> outputQVMVector;
    outputQVMVector = inputQVMMatrix * inputQVMVector;

    //Assign to the output vector
    outputVec[0] = boost::qvm::A<0>(outputQVMVector);
    outputVec[1] = boost::qvm::A<1>(outputQVMVector);
    outputVec[2] = boost::qvm::A<2>(outputQVMVector);
    //void return
}//end Multiply3x3MatrixAndVector

void StainVectorMath::SortStainVectors(const double(&inputMat)[9], double(&outputMat)[9],
    const int &sortOrder /*= SortOrder::ASCENDING */) {
    //Define lambdas to set how to compare two stain vectors (as 3-element arrays)
    auto ascLambda = [](const std::array<double, 3> a, const std::array<double, 3> b) {
        double prec = 1e-3;
        //Always put (0,0,0) stain vectors at the end
        double aSum = std::abs(std::accumulate(a.begin(), a.end(), 0.0));
        double bSum = std::abs(std::accumulate(b.begin(), b.end(), 0.0));
        if (aSum < prec) { return false; }
        if (bSum < prec) { return true; }
        //If first element is the same within error, sort by second element
        if (std::abs(a[0] - b[0]) > prec) { return a[0] < b[0]; }
        //If second element is the same within error, sort by third element
        else if (std::abs(a[1] - b[1]) > prec) { return a[1] < b[1]; }
        else { return a[2] < b[2]; }
    };

    auto descLambda = [](const std::array<double, 3> a, const std::array<double, 3> b) {
        double prec = 1e-3;
        //Always put (0,0,0) stain vectors at the end
        double aSum = std::abs(std::accumulate(a.begin(), a.end(), 0.0));
        double bSum = std::abs(std::accumulate(b.begin(), b.end(), 0.0));
        if (aSum < prec) { return false; }
        if (bSum < prec) { return true; }
        //If first element is the same within error, sort by second element
        if (std::abs(a[0] - b[0]) > prec) { return a[0] > b[0]; }
        //If second element is the same within error, sort by third element
        else if (std::abs(a[1] - b[1]) > prec) { return a[1] > b[1]; }
        else { return a[2] >= b[2]; }
    };

    //Bundle the input values in rows of three
    std::vector<std::array<double, 3>> inputRows;
    inputRows.push_back(std::array<double, 3>({ inputMat[0], inputMat[1], inputMat[2] }));
    inputRows.push_back(std::array<double, 3>({ inputMat[3], inputMat[4], inputMat[5] }));
    inputRows.push_back(std::array<double, 3>({ inputMat[6], inputMat[7], inputMat[8] }));
    //Create output rows
    std::vector<std::array<double, 3>> outputRows = inputRows;

    //Sort using the appropriate lambda
    if (sortOrder == SortOrder::ASCENDING) {
        std::sort(outputRows.begin(), outputRows.end(), ascLambda);
    }
    else if (sortOrder == SortOrder::DESCENDING) {
        std::sort(outputRows.begin(), outputRows.end(), descLambda);
    }
    else {
        //do not fill the output matrix
        return;
    }

    //Assign to the output matrix
    for (int x = 0; x < 9; x++) {
        int i = x / 3;
        int j = x % 3;
        outputMat[x] = outputRows[i][j];
    }
}//end SortStainVectors