/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#ifndef TURI_NN_DISTANCE_FUNCTIONS_H_
#define TURI_NN_DISTANCE_FUNCTIONS_H_

#include <string>
#include <Eigen/SparseCore>
#include <Eigen/Core>
#include <memory>
#include <util/logit_math.hpp>
#include <unity/toolkits/util/algorithmic_utils.hpp>
#include <flexible_type/flexible_type_base_types.hpp>
#include <unity/lib/toolkit_function_macros.hpp>
#include <boost/algorithm/string.hpp>

namespace turi {
namespace nearest_neighbors {

typedef Eigen::VectorXd DenseVector;
typedef Eigen::MatrixXd DenseMatrix;
typedef Eigen::SparseVector<double> SparseVector;
typedef Eigen::SparseMatrix<double, 1> SparseMatrix; // row-major

/**
 * Compute the Euclidean distance between the Cartesian product of rows in two
 * matrices.
 */
void inline all_pairs_squared_euclidean(const DenseMatrix& A,
                                        const DenseMatrix& B,
                                        DenseMatrix& dists) {

  DASSERT_EQ(A.cols(), B.cols());
  DASSERT_EQ(A.rows(), dists.rows());
  DASSERT_EQ(B.rows(), dists.cols());

  dists = -2 * A * B.transpose();

  for (size_t i = 0; i < (size_t)A.rows(); ++i) {
    dists.row(i).array() += A.row(i).squaredNorm();
  }

  for (size_t j = 0; j < (size_t)B.rows(); ++j) {
    dists.col(j).array() += B.row(j).squaredNorm();
  }
}

void inline all_pairs_cosine(const DenseMatrix& A, const DenseMatrix& B,
                             DenseMatrix& dists) {

  DASSERT_EQ(A.cols(), B.cols());
  DASSERT_EQ(A.rows(), dists.rows());
  DASSERT_EQ(B.rows(), dists.cols());

  dists = -1 * A * B.transpose();

  for (size_t i = 0; i < (size_t)A.rows(); ++i) {
    double row_norm = std::max(1e-16, A.row(i).norm());
    dists.row(i).array() /= row_norm;
  }

  for (size_t j = 0; j < (size_t)B.rows(); ++j) {
    double col_norm = std::max(1e-16, B.row(j).norm());
    dists.col(j).array() /= col_norm;
  }

  dists.array() += 1;
}

void inline all_pairs_dot_product(const DenseMatrix& A, const DenseMatrix& B,
                                  DenseMatrix& dists) {

  DASSERT_EQ(A.cols(), B.cols());
  DASSERT_EQ(A.rows(), dists.rows());
  DASSERT_EQ(B.rows(), dists.cols());

  dists = A * B.transpose();
  dists = dists.cwiseMax(1e-10).cwiseInverse();
}

void inline all_pairs_transformed_dot_product(const DenseMatrix& A,
                                              const DenseMatrix& B,
                                              DenseMatrix& dists) {

  DASSERT_EQ(A.cols(), B.cols());
  DASSERT_EQ(A.rows(), dists.rows());
  DASSERT_EQ(B.rows(), dists.cols());

  dists = A * B.transpose();
  dists = dists.unaryExpr([](double x) { return log1pen(x); });
}

struct distance_metric {

  virtual ~distance_metric() = default;

  // factory methods
  static inline std::shared_ptr<distance_metric> make_dist_instance(const std::string& dist_name);

  static inline std::shared_ptr<distance_metric> make_distance_metric(
    function_closure_info fn);

  virtual double distance(const DenseVector& a, const DenseVector& b) const {
    ASSERT_MSG(false, "Dense vector type not supported by this distance metric.");
    ASSERT_UNREACHABLE();
  }

  virtual double distance(const SparseVector& a, const SparseVector& b) const {
    ASSERT_MSG(false, "Sparse vector type not supported by this distance metric.");
    ASSERT_UNREACHABLE();
  }

  virtual double distance(const std::string& a, const std::string& b) const {
    ASSERT_MSG(false, "String type not supported by this distance metric.");
    ASSERT_UNREACHABLE();
  }

  virtual double distance(const std::vector<double>& a, const std::vector<double>& b) const {
    ASSERT_MSG(false, "Vector of double type not supported by this distance metric.");
    ASSERT_UNREACHABLE();
  }

};

/** squared_euclidean distance.
 */
struct gaussian_kernel final : public distance_metric {

  double distance(const DenseVector& a, const DenseVector& b) const  {
    return 1 - std::exp( - ( (a - b).squaredNorm() ) );
  }

  double distance(const SparseVector& a, const SparseVector& b) const  {
    return 1 - std::exp(-( (a.squaredNorm() + b.squaredNorm() - 2 * (a.dot(b))) ));
  }

};

/** squared_euclidean distance.
 */
struct squared_euclidean final : public distance_metric {

  double distance(const DenseVector& a, const DenseVector& b) const  {
    return (a - b).squaredNorm();
  }

  double distance(const SparseVector& a, const SparseVector& b) const  {
    return (a.squaredNorm() + b.squaredNorm() - 2 * (a.dot(b)));
  }

};

/** euclidean distance.
 */
struct euclidean final : public distance_metric {

  double distance(const DenseVector& a, const DenseVector& b) const {
    DASSERT_TRUE(a.size() == b.size());
    DASSERT_TRUE(a.size() > 0);
    return std::sqrt((a - b).squaredNorm());
  }

  double distance(const SparseVector& a, const SparseVector& b) const {
    return std::sqrt(a.squaredNorm() + b.squaredNorm() - 2 * (a.dot(b)));
  }

};

/** manhattan distance.
 */
struct manhattan final : public distance_metric {

  double distance(const DenseVector& a, const DenseVector& b) const {
    DASSERT_TRUE(a.size() == b.size());
    DASSERT_TRUE(a.size() > 0);
    return (a - b).cwiseAbs().sum();
  }

  double distance(const SparseVector& a, const SparseVector& b) const {
    return (a - b).cwiseAbs().sum();
  }

};

/** cosine distance.
 */
struct cosine final : public distance_metric {

  double distance(const DenseVector& a, const DenseVector& b) const {
    DASSERT_TRUE(a.size() == b.size());
    DASSERT_TRUE(a.size() > 0);

    double similarity = (double)a.dot(b) / std::max(1e-16, a.norm() * b.norm());
    return  1 - similarity;
  }

  double distance(const SparseVector& a, const SparseVector& b) const {
    double similarity = (double)a.dot(b) / std::max(1e-16, a.norm() * b.norm());
    return  1 - similarity;
  }

};

/* dot_product distance
 */
struct dot_product final : public distance_metric {

  double distance(const DenseVector& a, const DenseVector& b) const {
    DASSERT_TRUE(a.size() == b.size());
    DASSERT_TRUE(a.size() > 0);

    double dot_product = (double)a.dot(b);
    return 1.0 / std::max(dot_product, 1e-10);
  }

  double distance(const SparseVector& a, const SparseVector& b) const {
    double dot_product = (double)a.dot(b);
    return 1.0 / std::max(dot_product, 1e-10);
  }
};

/* transformed_dot_product distance
 */
struct transformed_dot_product final : public distance_metric {

  double distance(const DenseVector& a, const DenseVector& b) const {
    DASSERT_TRUE(a.size() == b.size());
    DASSERT_TRUE(a.size() > 0);

    double dot_product = (double)a.dot(b);
    return log1pen(dot_product);
  }

  double distance(const SparseVector& a, const SparseVector& b) const {
    double dot_product = (double)a.dot(b);
    return log1pen(dot_product);
  }
};

/* jaccard distance
 */
struct jaccard final : public distance_metric {
  using distance_metric::distance;

  double distance(const DenseVector& a, const DenseVector& b) const {
    DASSERT_EQ(a.size(), b.size());
    double intersection_size = 0.;
    double union_size = 0.;
    for(size_t idx = 0; idx < std::min<size_t>(a.size(), b.size()); ++idx) {
      if (a(idx) > 0. || b(idx) > 0.) {
        union_size += 1.;
        if (a(idx) > 0. && b(idx) > 0.) {
          intersection_size += 1.;
        }
      }
    }
    if (union_size == 0.) return 1.;
    return 1. - intersection_size / union_size;
  }

  double distance(const SparseVector& a, const SparseVector& b) const GL_HOT_FLATTEN {

    size_t intersection_size = 0;

    SparseVector::InnerIterator it_a(a, 0);
    SparseVector::InnerIterator it_b(b, 0);

    while(it_a && it_b) {
      if(it_a.index() < it_b.index()) {
        ++it_a;
      } else if(it_a.index() > it_b.index()) {
        ++it_b;
      } else {
        ++intersection_size;
        ++it_a, ++it_b;
      }
    }

    size_t d = a.nonZeros() + b.nonZeros() - intersection_size;

    return 1.0 - double(intersection_size) / d;
  }

  double distance(std::vector<size_t>& av,
                  std::vector<size_t>& bv) const {
    DASSERT_TRUE(av.size() > 0);
    DASSERT_TRUE(bv.size() > 0);

    std::sort(av.begin(), av.end());
    std::sort(bv.begin(), bv.end());

    // Use an efficient accumulate function
    size_t n = count_intersection(av.begin(), av.end(),
                                  bv.begin(), bv.end());
    size_t d = av.size() + bv.size() - n;
    DASSERT_TRUE(d > 0);

    return 1 - double(n) / d;
  }

};

/* weighted jaccard distance
 */
struct weighted_jaccard final : public distance_metric {

  double distance(const SparseVector& a, const SparseVector& b) const {

    SparseVector::InnerIterator it_a(a, 0);
    SparseVector::InnerIterator it_b(b, 0);

    double cwise_min_sum = 0;
    double cwise_max_sum = 0;

    while(it_a && it_b) {
      if(it_a.index() < it_b.index()) {
        cwise_max_sum += it_a.value();
        ++it_a;
      } else if(it_a.index() > it_b.index()) {
        cwise_max_sum += it_b.value();
        ++it_b;
      } else {
        cwise_min_sum += std::min(it_a.value(), it_b.value());
        cwise_max_sum += std::max(it_a.value(), it_b.value());
        ++it_a, ++it_b;
      }
    }

    while(it_a) {
      cwise_max_sum += it_a.value();
      ++it_a;
    }

    while(it_b) {
      cwise_max_sum += it_b.value();
      ++it_b;
    }

    double similarity = cwise_min_sum / cwise_max_sum;
    return 1 - similarity;
  }

};

/* levenshtein distance
 */
struct levenshtein final : public distance_metric {

  double distance(const std::string& a, const std::string& b) const {

    std::string s;
    std::string t;
    size_t len_s;
    size_t len_t;

    // if 't' is not the longer string, switch them so 't' is the longer string
    if (a.length() > b.length()) {
      t = a;
      s = b;
    } else {
      s = a;
      t = b;
    }

    // trim common prefix - these cannot add anything to the distance
    size_t idx_start = 0;
    while ((s[idx_start] == t[idx_start]) && (idx_start < s.length())) {
      idx_start++;
    }

    if (idx_start == t.length()) {  // if the entire strings match, distance is 0
      return 0;
    }

    s = s.substr(idx_start);
    t = t.substr(idx_start);

    len_s = s.length();
    len_t = t.length();

    // if either trimmed string has length 0, the distance is the length of the
    // other string. Since 's' is the shorter string, this should capture all
    // cases.
    if (len_s == 0)
      return len_t;

    // initialize the rows
    std::vector<size_t> v0(len_t + 1, 0);
    std::vector<size_t> v1(len_t + 1, 0);

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

    size_t cost;

    // For each letter in the shorter string...
    for (size_t i = 0; i < len_s; i++) {
      v1[0] = i + 1;

      // Fill in the second row
      for (size_t j = 0; j < len_t; j++) {
        cost = (s[i] == t[j]) ? 0 : 1;
        v1[j+1] = std::min({v0[j] + cost, v0[j+1] + 1, v1[j] + 1});
      }

      // Copy the second row into the first row
      for (size_t j = 0; j < v0.size(); j++) {
        v0[j] = v1[j];
      }
    }

    return v1[t.length()];
  }
};

struct custom_distance final : public distance_metric {

  // std::function<double(const std::vector<double>, const std::vector<double>)> fn;
  std::function<double(const flexible_type, const flexible_type)> fn;

  double distance(const std::vector<double>& a, const std::vector<double>& b) const {
    return fn(a, b);
  }

  double distance(const std::string& a, const std::string& b) const {
    return fn(a, b);
  }
};

std::shared_ptr<distance_metric> inline distance_metric::make_distance_metric(
    function_closure_info fn) {
  auto fn_name = fn.native_fn_name;
  distance_metric d;
  if (boost::algorithm::ends_with(fn_name, ".euclidean")) {
    return  distance_metric::make_dist_instance("euclidean");
  } else if (boost::algorithm::ends_with(fn_name, ".squared_euclidean")) {
    return distance_metric::make_dist_instance("squared_euclidean");
  } else if (boost::algorithm::ends_with(fn_name, ".gaussian_kernel")) {
    return distance_metric::make_dist_instance("gaussian_kernel");
  } else if (boost::algorithm::ends_with(fn_name, ".manhattan")) {
    return distance_metric::make_dist_instance("manhattan");
  } else if (boost::algorithm::ends_with(fn_name, ".cosine")) {
    return distance_metric::make_dist_instance("cosine");
  } else if (boost::algorithm::ends_with(fn_name, ".dot_product")) {
    return distance_metric::make_dist_instance("dot_product");
  } else if (boost::algorithm::ends_with(fn_name, ".transformed_dot_product")) {
    return distance_metric::make_dist_instance("transformed_dot_product");
  } else if (boost::algorithm::ends_with(fn_name, ".jaccard")) {
    return distance_metric::make_dist_instance("jaccard");
  } else if (boost::algorithm::ends_with(fn_name, ".weighted_jaccard")) {
    return distance_metric::make_dist_instance("weighted_jaccard");
  } else if (boost::algorithm::ends_with(fn_name, ".levenshtein")) {
    return distance_metric::make_dist_instance("levenshtein");
  } else {
    // Create a distance metric that uses the user-provided function.
    // Only functions that take dense vectors are currently supported.
    auto actual_fn = variant_get_value< std::function<double(const std::vector<double>, const std::vector<double>)> >(fn);
    auto d = nearest_neighbors::custom_distance();
    d.fn = actual_fn;
    auto sp = std::make_shared<distance_metric>(d);
    return sp;
  }
}

std::shared_ptr<distance_metric> inline distance_metric::make_dist_instance(
  const std::string& dist_name) {

  std::shared_ptr<distance_metric> dist_ptr;

  if (dist_name == "euclidean")
    dist_ptr.reset(new euclidean);
  else if(dist_name == "squared_euclidean")
    dist_ptr.reset(new squared_euclidean);
  else if(dist_name == "gaussian_kernel")
    dist_ptr.reset(new gaussian_kernel);
  else if (dist_name == "manhattan")
    dist_ptr.reset(new manhattan);
  else if (dist_name == "cosine")
    dist_ptr.reset(new cosine);
  else if (dist_name == "dot_product")
    dist_ptr.reset(new dot_product);
  else if (dist_name == "transformed_dot_product")
    dist_ptr.reset(new transformed_dot_product);
  else if (dist_name == "jaccard")
    dist_ptr.reset(new jaccard);
  else if (dist_name == "weighted_jaccard")
    dist_ptr.reset(new weighted_jaccard);
  else if (dist_name == "levenshtein")
    dist_ptr.reset(new levenshtein);
  else
    log_and_throw("Unrecognized distance: " + dist_name);

  return dist_ptr;
}

}}

#endif  // TURI_NN_DISTANCE_FUNCTIONS_H_


/*
 * Copyright (c) 2018 Nicholas Corgan (n.corgan@gmail.com)
 *
 * Distributed under the MIT License (MIT) (See accompanying file LICENSE.txt
 * or copy at http://opensource.org/licenses/MIT)
 */

#include <pkmn/config.hpp>

// http://stackoverflow.com/q/6884093
#ifdef PKMN_PLATFORM_WIN32
#define NOMINMAX
#include <windows.h>
#endif

#include "exception_internal.hpp"
#include "utils/misc.hpp"
#include "types/rng.hpp"
#include "utils/floating_point_comparison.hpp"

#include <pkmn/calculations/personality.hpp>

#include <pkmn/calculations/shininess.hpp>
#include <pkmn/database/pokemon_entry.hpp>
#include <pkmn/enums/gender.hpp>

#include <boost/assign/list_of.hpp>

#include <algorithm>
#include <stdexcept>
#include <vector>

namespace pkmn { namespace calculations {

    static uint32_t get_gender_threshold(
        float chance_male
    )
    {
        uint32_t ret = 0;

        if(pkmn::fp_compare_equal(chance_male, 0.875f))
        {
            ret = 31;
        }
        else if(pkmn::fp_compare_equal(chance_male, 0.75f))
        {
            ret = 64;
        }
        else if(pkmn::fp_compare_equal(chance_male, 0.5f))
        {
            ret = 127;
        }
        else
        {
            ret = 191;
        }

        return ret;
    }

    uint32_t generate_personality(
        pkmn::e_species species,
        uint32_t trainer_id,
        bool shiny,
        pkmn::e_ability ability,
        pkmn::e_gender gender,
        pkmn::e_nature nature
    )
    {
        pkmn::enforce_value_in_vector(
            "Gender",
            gender,
            {pkmn::e_gender::MALE, pkmn::e_gender::FEMALE, pkmn::e_gender::GENDERLESS}
        );

        if((species == pkmn::e_species::NONE) || (species == pkmn::e_species::INVALID))
        {
            throw std::invalid_argument("Species cannot be None or Invalid.");
        }
        if(nature == pkmn::e_nature::NONE)
        {
            throw std::invalid_argument("Nature cannot be None.");
        }

        uint32_t ret = 0;

        pkmn::database::pokemon_entry entry(species, pkmn::e_game::OMEGA_RUBY, "");
        pkmn::ability_pair_t abilities = entry.get_abilities();
        pkmn::e_ability hidden_ability = entry.get_hidden_ability();
        float chance_male = entry.get_chance_male();
        float chance_female = entry.get_chance_female();

        // Validate ability input.
        uint32_t ability_modulo = 0; // If first or hidden ability, keep this
        if(ability == abilities.second)
        {
            if(ability != pkmn::e_ability::NONE)
            {
                ability_modulo = 1;
            }
            else
            {
                throw std::invalid_argument("You cannot use NONE.");
            }
        }
        else if((ability != abilities.first) && (ability != hidden_ability))
        {
            throw std::invalid_argument("Invalid ability.");
        }

        // Validate gender input.
        if(pkmn::fp_compare_equal((chance_male + chance_female), 0.0f))
        {
            if(gender != pkmn::e_gender::GENDERLESS)
            {
                throw std::invalid_argument("This Pokémon is genderless.");
            }
        }
        else if(pkmn::fp_compare_equal(chance_male, 1.0f))
        {
            if(gender != pkmn::e_gender::MALE)
            {
                throw std::invalid_argument("This Pokémon is male-only.");
            }
        }
        else if(pkmn::fp_compare_equal(chance_female, 1.0f))
        {
            if(gender != pkmn::e_gender::FEMALE)
            {
                throw std::invalid_argument("This Pokémon is female-only.");
            }
        }
        else if(gender != pkmn::e_gender::MALE and gender != pkmn::e_gender::FEMALE)
        {
            throw std::invalid_argument("Valid genders: Male, Female");
        }

        // TODO: validate
        uint32_t nature_index = static_cast<uint32_t>(nature) - 1;
        static const size_t NUM_NATURES = 25;

        // Start trying to find a valid value.
        uint32_t gender_threshold = get_gender_threshold(chance_male);
        bool found = false;
        pkmn::rng<uint32_t> rng;
        do
        {
            ret = rng.rand();

            // Set the gender if applicable.
            if(gender == pkmn::e_gender::MALE)
            {
                ret &= ~0xFF;
                ret |= (rng.rand() % (0xFF - gender_threshold) + gender_threshold);
            }
            else if(gender == pkmn::e_gender::FEMALE)
            {
                ret &= ~0xFF;
                ret |= (rng.rand() % gender_threshold);
            }

            if((modern_shiny(ret, trainer_id) == shiny) &&
               ((ret % NUM_NATURES) == nature_index) &&
               ((ret % 2) == ability_modulo)
            )
            {
                found = true;
            }
        }
        while(!found);

        return ret;
    }

}}


// Copyright 2015 National ICT Australia Limited (NICTA)
//
// 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 "KluSolver.h"

#include <klu.h>

#include <armadillo>

using namespace arma;

bool kluSolve(const arma::SpMat<double>& a, const arma::Col<double>& b, arma::Col<double>& result)
{
    auto n = b.size();
    auto nnz = a.n_nonzero;

    int* ap = new int[n + 1];
    for (unsigned int i = 0; i <= n; ++i) ap[i] = static_cast<int>(a.col_ptrs[i]);

    int* ai = new int[nnz];
    for (unsigned int i = 0; i < nnz; ++i) ai[i] = static_cast<int>(a.row_indices[i]);

    double* ax = new double[nnz];
    for (unsigned int i = 0; i < nnz; ++i) ax[i] = a.values[i];

    double* b1 = new double[n];
    for (unsigned int i = 0; i < n; ++i) b1[i] = b(i);

    klu_symbolic *Symbolic;
    klu_numeric *Numeric;
    klu_common Common;

    klu_defaults (&Common);
    Symbolic = klu_analyze (static_cast<int>(n), ap, ai, &Common);
    Numeric = klu_factor (ap, ai, ax, Symbolic, &Common);
    bool ok = klu_solve(Symbolic, Numeric, static_cast<int>(n), 1, b1, &Common) == 1;

    if (!ok)
    {
        std::cerr << "KLU failed." << std::endl;
        std::cerr << "Status = " << Common.status << std::endl;
    }

    klu_free_symbolic (&Symbolic, &Common);
    klu_free_numeric (&Numeric, &Common);

    result = arma::Col<double>(n, fill::none);
    for (arma::uword i = 0; i < n; ++i)
    {
        result(i) = b1[i];
    }

    delete[] ap;
    delete[] ai;
    delete[] ax;
    delete[] b1;

    return ok;
}


/*
  Copyright (c) <2014> <Thomas Mörwald, Vienna University of Technology>

  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted (subject to the limitations in the disclaimer
  below) provided that the following conditions are met:

   * Redistributions of source code must retain the above copyright
     notice, this list of conditions and the following disclaimer.

   * Redistributions in binary form must reproduce the above copyright
     notice, this list of conditions and the following disclaimer in the
     documentation and/or other materials provided with the distribution.

   * Neither the name of <Owner Organization> nor the names of its
     contributors may be used to endorse or promote products derived from this
     software without specific prior written permission.

  NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
  LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
  THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

// This file contains mathematical definitions derifed from CGAL and Eigen
// It implements some helper functions like arithmetic mean, principal component
// analysis (PCA) and comparison of floating point values and vectors

#ifndef _TSPLINE_MATH_H_
#define _TSPLINE_MATH_H_

#include <vector>
#include <stdio.h>
#include <stdexcept>
#include <limits>

#undef Success
#include <Eigen/Eigen>

#include <CGAL/Cartesian.h>
#include <CGAL/MP_Float.h>
#include <CGAL/Quotient.h>
#include <CGAL/Arr_segment_traits_2.h>
#include <CGAL/Arrangement_2.h>
#include <CGAL/Arr_extended_dcel.h>

#define SIZE_T_MAX std::numeric_limits<std::size_t>::max()

namespace tspline
{

// derive algebraic objects from CGAL types
typedef CGAL::Cartesian<double> Kernel;
typedef CGAL::Arr_segment_traits_2<Kernel> Traits_2;
typedef Kernel::Point_2 Point2d;
typedef Kernel::Point_3 Point3d;
typedef Kernel::Vector_2 Vector2d;
typedef Kernel::Vector_3 Vector3d;
typedef Kernel::Ray_3 Ray;
typedef Kernel::Direction_3 Direction3d;
typedef Traits_2::X_monotone_curve_2 Segment2;

// numerical limits
static double epsilon = 10.0 * std::numeric_limits<float>::epsilon(); // float because tgModel using OpenGL is float
static double digits = 1e10;
static double div_digits = 1e-10;

/** @brief extends Point3d by the weight entry for control points  (weight is 1.0 by default) */
class Point4d : public Point3d
{
protected:
  double weight;

public:
  Point4d() : Point3d(), weight(1.0) { }
  Point4d(const Point3d& a, const double& w=1.0) : Point3d(a), weight(w) { }
  Point4d(const double& x, const double& y, const double& z, const double& w) :
    Point3d(x,y,z), weight(w) { }
  Point4d(const double& x, const double& y, const double& z) :
    Point3d(x,y,z), weight(1.0) { }

  void operator=(const Point3d& a)
  {
    *this = Point4d(a);
  }

  double w() const { return weight; }

};

/** @brief aligned Eigen::Vector classes */
typedef std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > vector_vec4d;
typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > vector_vec3d;
typedef std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > vector_vec2d;

/** @brief compute the arithmetic mean of a vector of Vector3d
 *  @param data input data */
inline Eigen::Vector3d compute_mean (const vector_vec3d &data)
{
  Eigen::Vector3d u (0.0, 0.0, 0.0);

  unsigned s = unsigned (data.size ());
  double ds = 1.0 / s;

  for (unsigned i = 0; i < s; i++)
    u += (data[i] * ds);

  return u;
}

/** @brief compute the principal components of a vector of Vector3d
 *  @param data in: input data
 *  @param mean out: the arithmetic mean of the data
 *  @param eigenvectors out: the eigenvectors sorted descending by their eigenvalues
 *  @param eigenvalues out: descending eigenvalues */
inline void pca (const vector_vec3d &data, Eigen::Vector3d &mean, Eigen::Matrix3d &eigenvectors,
                 Eigen::Vector3d &eigenvalues)
{
  if (data.empty ())
    throw std::runtime_error ("[Math::pca] Error, data is empty\n");

  mean = compute_mean (data);

  unsigned s = unsigned (data.size ());

  Eigen::MatrixXd Q (3, s);

  for (unsigned i = 0; i < s; i++)
    Q.col (i) << (data[i] - mean);

  Eigen::Matrix3d C = Q * Q.transpose ();

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver (C);
  if (eigensolver.info () != Eigen::Success)
    throw std::runtime_error ("[Math::pca] Can not find eigenvalues.\n");

  // reverse ordering of eigenvalues and eigenvectors
  for (int i = 0; i < 3; ++i)
  {
    eigenvalues (i) = eigensolver.eigenvalues () (2 - i);
    if (i == 2)
      eigenvectors.col (2) = eigenvectors.col (0).cross (eigenvectors.col (1));
    else
      eigenvectors.col (i) = eigensolver.eigenvectors ().col (2 - i);
  }
}

// comparison functions for floating point data types

// ==
static bool equal(const double& a, const double& b)
{
  if(std::abs<double>(a-b) < epsilon)
    return true;
  else
    return false;
}

// >
static bool greater(const double &a, const double &b)
{
  if(equal(a,b)) // equal
    return false;
  else if(a < b)
    return false;
  else
    return true;
}

// <
static bool smaller(const double &a, const double &b)
{
  if(equal(a,b)) // equal
    return false;
  else if(a > b)
    return false;
  else
    return true;
}

// >=
static bool gequal(const double &a, const double &b)
{
  if(equal(a,b)) // equal
    return true;
  else if(a > b)
    return true;
  else
    return false;
}

// <=
static bool sequal(const double &a, const double &b)
{
  if(equal(a,b))
    return true;
  else if(a < b)
    return true;
  else
    return false;
}

// ==
static bool equal( const Point2d& a, const Point2d& b)
{
  if(equal(a.x(), b.x()) && equal(a.y(), b.y()))
    return true;
  else
    return false;
}

// ==
static bool equal( const Point3d& a, const Point3d& b)
{
  if(equal(a.x(), b.x()) && equal(a.y(), b.y()) && equal(a.z(), b.z()))
    return true;
  else
    return false;
}

// ==
static bool equal( const Eigen::Vector3d& a, const Eigen::Vector3d& b)
{
  if(equal(a(0), b(0)) && equal(a(1), b(1)) && equal(a(2),b(2)))
    return true;
  else
    return false;
}

/** @brief adjust a double value to a grid of minimal resolution */
static double adjust(const double &a)
{
  return a;
//  return double(float(a));
  double b = round(a * digits) * div_digits;
//  if(equal(b,0.0))
//    b = 0.0;
  return b;
}

/** @brief adjust a Point2D value to a grid of minimal resolution */
static Point2d adjust(const Point2d &p)
{
  return Point2d(adjust(p.x()), adjust(p.y()));
}

/** @brief adjust a Point3D value to a grid of minimal resolution */
static Point3d adjust(const Point3d &p)
{
  return Point3d(adjust(p.x()), adjust(p.y()), adjust(p.z()));
}

/** @brief dot product of CGAL::Vector3d (NOT Eigen)*/
static double dot( const Vector3d& a, const Vector3d& b )
{
  return ( a.x()*b.x() + a.y()*b.y() + a.z()*b.z() );
}

/** @brief L2 norm of CGAL::Vector3d (NOT Eigen) */
static double norm( const Vector3d& a )
{
  return sqrt(a.squared_length());
}

/** @brief angle between two CGAL::Vector3d (NOT Eigen) */
static double angle( const Vector3d& a, const Vector3d& b )
{
  return acos( dot(a,b) / (norm(a) * norm(b)) );
}

}

#endif


#ifndef SVD_HPP

#define SVD_HPP

#include "print.hpp"
#include "modify.hpp"
#include "sub.hpp"
#include "multiply.hpp"
#include "special.hpp"
#include "../workspace.hpp"
#include "../tools.hpp"
#include <mkl.h>

#include <boost/timer/timer.hpp>
#include <boost/chrono.hpp>

#include <vector>
#include <utility>

inline bool comparator ( const max_pair& l, const max_pair& r) { return l.first > r.first; }

namespace dqmc {
    namespace la {

	// extern "C" {
	//     void dgejsv_( char* joba, char* jobu, char* jobv, char* jobr, char* jobt,
	// 		  char* jobp,
	// 		  const int* m, const int* n, double* a, const int* lda,
	// 		  double* sva, double* u, const int* ldu,
	// 		  double* v, const int* ldv, double* work,
	// 		  const int* lwork, int* iwork, int* info );

	//     int LAPACKE_dgejsv( int matrix_layout, char joba, char jobu, char jobv,
	//     			char jobr, char jobt, char jobp, int m,
	//     			int n, const double* a,
	//     			int lda, double* sva, double* u,
	//     			int ldu, double* v, int ldv,
	//     			double* stat, int* istat );

	//     void dgesvj_( const char* joba, const char* jobu, const char* jobv,
	// 		  const int* m, const int* n, double* a,
	// 		  const int* lda, double* sva, const int* mv, double* v,
	// 		  const int* ldv, double* work, int* lwork,
	// 		  int* info );
	// }

	template <typename M, typename V>
	inline void close_svd(int sites, M&__restrict__ U, V&__restrict__ D,
			      M&__restrict__ T,
			      M&__restrict__ out) {
	    using namespace std;

	    for (int col = sites - 1; col >= 0; col--) {
		for (int row = sites - 1; row >= 0; row--) {
		    out(row, col) = 0.;
		    for (int k = sites - 1; k >= 0; k--) {
			if (U(row, k) != 0 && T(k, col) != 0 && D(k) != 0)
			    out(row, col) += ( U(row, k) * T(k, col)) * D(k);
		    }
		}
	    }
	}

	template <typename M, typename V>
	inline void close_svd_inv_diag(int sites, M&__restrict__ U, V&__restrict__ D,
				       M&__restrict__ T,
				       M& out) {
	    using namespace std;

	    for (int col = sites - 1; col >= 0; col--) {
		for (int row = sites - 1; row >= 0; row--) {
		    out(row, col) = 0.;
		    for (int k = sites - 1; k >= 0; k--) {
			if (U(row, k) != 0 && T(k, col) != 0 && D(k) != 0)
			    out(row, col) += ( U(row, k) * T(k, col)) / D(k);
		    }
		}
	    }
	}

	inline int decompose_dgejsv_nt(pmat_t&__restrict__ in, pmat_t&__restrict__ U,
				       pvec_t&__restrict__ D, pmat_t&__restrict__ Tt,
				       pvec_t& work, ivec& ivywork) {
	    using namespace std;

	    int info;
	    int lwork = work.size() - 10;

	    char f = 'F';
	    char j = 'J';
	    char n = 'N';
	    char p = 'P';
	    int rows = U.rows();
	    int cols = U.cols();
	    int T_rows = Tt.rows();
	    // arma::vec work(6);
	    ivec iwork(in.rows() + 3 * in.cols());
	    work.setZero();
	    iwork.setZero();

	    dgejsv_( &f, &f, &j, &n, &n, &p,
	    	     &rows, &cols, in.data(), &rows,
	    	     D.data(), U.data(), &rows,
	    	     Tt.data(), &T_rows, work.data(),
	    	     &lwork, iwork.data(), &info);
	    return info;
	    // info = LAPACKE_dgejsv_work( 102, f, f, j, n, n, p,
	    // 				rows, cols, in.data(), rows,
	    // 				D.data(), U.data(), rows,
	    // 				Tt.data(), T_rows, work.data(),
	    // 				work.size(),
	    // 				iwork.data());

	    // return info;
	}

	inline int decompose_dgejsv_col_nt(pmat_t&__restrict__ in, pmat_t&__restrict__ U,
				       pvec_t&__restrict__ D, pmat_t&__restrict__ Tt,
				       pvec_t& work, ivec& iwork) {
	    using namespace std;

	    int info;
	    int lwork = work.size() - 10; //std::max(1024, int(in.rows()*in.rows()*in.rows()));
	    // pvec_t work(lwork);
	    // ivec iwork(std::max(1024, int(in.rows() * in.cols())));

	    char c = 'C';
	    char u = 'U';
	    char j = 'J';
	    char n = 'N';
	    char p = 'P';
	    int rows = U.rows();
	    int cols = U.cols();
	    int T_rows = Tt.rows();

	    work.setZero();
	    iwork.setZero();
	    // info = LAPACKE_dgejsv_work( 102, c, j, j, n, n, p,
	    // 			   rows, cols, in.data(), rows,
	    // 			   D.data(), U.data(), rows,
	    // 			   Tt.data(), T_rows, work.data(),
	    // 				work.size(),
	    // 			   iwork.data());

	    dgejsv_( &c, &u, &j, &n, &n, &p, // 6
	    	     &rows, &cols, in.data(), &rows, //10
	    	     D.data(), U.data(), &rows, // 13
	    	     Tt.data(), &T_rows, work.data(),
	    	     &lwork, iwork.data(), &info); // 17
	    // std::cout << info << std::endl;
	    return info;
	}

	inline int decompose_jsvd_nt(pmat_t&__restrict__ in, pmat_t&__restrict__ U,
				     pvec_t&__restrict__ D, pmat_t&__restrict__ Tt) {
	    using namespace std;

	    char joba = 'G';
	    char jobu = 'U';
	    char jobv = 'V';

	    U = in;

	    int rows = in.rows();
	    int cols = in.cols();
	    int U_rows = in.rows();
	    int T_rows = Tt.rows();

	    int lwork = max(1024, rows * cols);
	    pvec_t work(lwork);
	    work.setOnes();
	    int mv = 0;
	    int info;

	    dgesvj_(&joba, &jobu, &jobv, &rows, &cols, U.data(), &rows,
		    D.data(), &mv, Tt.data(), &T_rows, work.data(), &lwork, &info);

	    if (work(0) != 1.) {
		cout << "MKL ERROR: SVD HAVE TO BE SCALED! " << work(0) << endl;
		throw std::runtime_error("SVD Error");
	    }

	    return info;
	}

	inline int decompose_jsvd_nt(pmat_t&__restrict__ in, pmat_t&__restrict__ U,
				     pvec_t&__restrict__ D, pmat_t&__restrict__ Tt,
				     pvec_t&__restrict__ work) {
	    using namespace std;

	    char joba = 'G';
	    char jobu = 'U';
	    char jobv = 'V';

	    U = in;

	    int rows = in.rows();
	    int cols = in.cols();
	    int U_rows = in.rows();
	    int T_rows = Tt.rows();

	    int lwork = work.size();
	    //pvec_t work(lwork);
	    work.setOnes();
	    int mv = 0;
	    int info;

	    dgesvj_(&joba, &jobu, &jobv, &rows, &cols, U.data(), &rows,
		    D.data(), &mv, Tt.data(), &T_rows, work.data(), &lwork, &info);

	    if (work(0) != 1.) {
		cout << "MKL ERROR: SVD HAVE TO BE SCALED! " << work(0) << endl;
		throw std::runtime_error("SVD Error");
	    }

	    return info;
	}

	inline perm_mat row_sort(mat&__restrict in, mat&__restrict out) {
	    using namespace std;
	    using namespace Eigen;
	    vector<int> max_index(in.rows());
	    vector<max_pair> max_indices;
	    vec max_vals(in.rows());
	    for(int i = 0; i < in.rows(); ++i) {
		max_vals(i) = in.cwiseAbs().row(i).maxCoeff( &max_index[i] );
		max_indices.push_back(max_pair(max_vals(i), i));
	    }
	    std::sort(max_indices.begin(), max_indices.end(), comparator);

	    ivec row_ids(in.rows());
	    for (std::vector<max_pair>::iterator it=max_indices.begin(); it != max_indices.end(); ++it) {
		row_ids((*it).second) = std::distance(max_indices.begin(), it);
		// cout << row_ids((*it).second) <<  " ";
	    }
	    // cout << endl;
	    perm_mat row_perm(row_ids);

	    out = row_perm * in;
	    return row_perm;
	}

	inline perm_mat col_sort(mat&__restrict in, mat&__restrict out) {
	    using namespace std;
	    using namespace Eigen;
	    vector<int> max_index(in.cols());
	    vector<max_pair> max_indices;
	    vec max_vals(in.cols());
	    for(int i = 0; i < in.cols(); ++i) {
		max_vals(i) = in.cwiseAbs().col(i).maxCoeff( &max_index[i] );
		max_indices.push_back(max_pair(max_vals(i), i));
	    }
	    std::sort(max_indices.begin(), max_indices.end(), comparator);

	    ivec col_ids(in.cols());
	    for (std::vector<max_pair>::iterator it=max_indices.begin();
		 it != max_indices.end(); ++it) {
		col_ids((*it).second) = std::distance(max_indices.begin(), it);
		// cout << row_ids((*it).second) <<  " ";
	    }
	    // cout << endl;
	    perm_mat col_perm(col_ids);

	    out = in * col_perm;
	    return col_perm;
	}

	inline void decompose_udt_full_piv(mat&__restrict__ in, mat&__restrict__ U, vec&__restrict__ D, mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;

	    if (U.rows() != in.rows() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of U are wrong");

	    if (D.size() != in.cols()) {
		cout << D.size() << endl;
		throw std::runtime_error("dimensions of D are wrong");
	    }

	    if (T.rows() != in.cols() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of T are wrong");

	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.5);
	    Eigen::FullPivHouseholderQR<mat> qr(in);
	    in /= scale;

	    qr.compute(in);
	    U = qr.matrixQ().block(0, 0, in.rows(), in.cols());
	    mat upper = qr.matrixQR().triangularView<Upper>();// .block(0, 0, in.cols(), in.cols());
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    std::cout << "culprit " << D(i) << " was "
			      << upper(i, i) << std::endl;
		    throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) {
		    D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }

	    T = (d_inv.asDiagonal() * upper.block(0, 0, in.cols(), in.cols()))
		* qr.colsPermutation().transpose();
	    D *= scale;
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}

	inline void decompose_udt_full_sort(mat&__restrict__ in, mat&__restrict__ U, vec&__restrict__ D, mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;

	    if (U.rows() != in.rows() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of U are wrong");

	    if (D.size() != in.cols()) {
		cout << D.size() << endl;
		throw std::runtime_error("dimensions of D are wrong");
	    }

	    if (T.rows() != in.cols() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of T are wrong");

	    mat scaled_in = mat::Zero(in.rows(), in.cols());
	    perm_mat row_perm = dqmc::la::row_sort(in, scaled_in);
	    perm_mat col_perm = dqmc::la::col_sort(scaled_in, in);

	    // Very crude scaling...
	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.5);

	    vec taus(in.cols());
	    ivec iwork(in.cols());

	    // vec work(in.rows() * in.rows() * in.rows());
	    int lwork = -1;
	    int info = 0;
	    int M = in.rows();
	    int N = in.cols();

	    scaled_in.block(0, 0, in.rows(), in.cols()) = in/scale;
	    iwork.setZero();
	    // cout << iwork.size() << " " << work.size() << " " << taus.size() << endl;
	    lwork = -1;
	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), taus.data(),
		    &lwork, &info);
	    lwork = taus(0);
	    vec work(lwork);
	    work.setZero();
	    taus.setZero();

	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), work.data(),
		    &lwork, &info);

	    mat upper = scaled_in.triangularView<Upper>();
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    std::cout << "culprit " << D(i) << " was "
			      << upper(i, i) << std::endl;
		    throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) { D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }
	    lwork = -1;
	    dorgqr_(&M, &N, &N, scaled_in.data(), &M, taus.data(),
		    work.data(), &lwork, &info);
	    // if (work(0) > work.size()) {
	    // 	cout << "Need more lwork for dorgqr" << endl;
	    // }
	    lwork = work(0);
	    work.resize(lwork);
	    // cout << "in" << endl << scaled_in << endl << endl;
	    dorgqr_(&M, &N, &N, scaled_in.data(), &M, taus.data(),
		    work.data(), &lwork, &info);
	    // cout << "out" << endl << scaled_in << endl << endl;
	    U = row_perm.inverse() * scaled_in.block(0, 0, in.rows(), in.cols());
	    ivec col_ids(in.cols());
	    for (int i = 0; i < in.cols(); ++i) {
		col_ids(i) = iwork(i) - 1;
	    }

	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> lapack_col_perm(col_ids);
	    T = (d_inv.asDiagonal() * upper.block(0, 0, in.cols(), in.cols()))
		* lapack_col_perm.transpose() * col_perm.transpose();
	    D *= scale;
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}

	inline void decompose_udt_row_sort(mat&__restrict__ in, mat&__restrict__ U, vec&__restrict__ D, mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;

	    if (U.rows() != in.rows() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of U are wrong");

	    if (D.size() != in.cols()) {
		cout << D.size() << endl;
		throw std::runtime_error("dimensions of D are wrong");
	    }

	    if (T.rows() != in.cols() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of T are wrong");

	    vector<int> max_index(in.rows());
	    vector<max_pair> max_indices;
	    vec max_vals(in.rows());
	    for(int i = 0; i < in.rows(); ++i) {
		max_vals(i) = in.cwiseAbs().row(i).maxCoeff( &max_index[i] );
		max_indices.push_back(max_pair(max_vals(i), i));
	    }
	    std::sort(max_indices.begin(), max_indices.end(), comparator);

	    ivec row_ids(in.rows());
	    for (std::vector<max_pair>::iterator it=max_indices.begin(); it != max_indices.end(); ++it) {
		row_ids((*it).second) = std::distance(max_indices.begin(), it);
		// cout << row_ids((*it).second) <<  " ";
	    }
	    // cout << endl;
	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> row_perm(row_ids);

	    // Very crude scaling...
	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.5);

	    vec taus(in.cols());
	    ivec iwork(in.cols());

	    // vec work(in.rows() * in.rows() * in.rows());
	    int lwork = -1;
	    int info = 0;
	    int M = in.rows();
	    int N = in.cols();
	    mat scaled_in = mat::Zero(in.rows(), in.cols());

	    scaled_in.block(0, 0, in.rows(), in.cols()) = (row_perm * in)/scale;
	    iwork.setZero();
	    // cout << iwork.size() << " " << work.size() << " " << taus.size() << endl;
	    lwork = -1;
	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), taus.data(),
		    &lwork, &info);
	    lwork = taus(0);
	    vec work(lwork);
	    work.setZero();
	    taus.setZero();

	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), work.data(),
		    &lwork, &info);

	    mat upper = scaled_in.triangularView<Upper>();
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    std::cout << "culprit " << D(i) << " was "
			      << upper(i, i) << std::endl;
		    throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) { D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }
	    lwork = -1;
	    dorgqr_(&M, &N, &N, scaled_in.data(), &M, taus.data(),
		    work.data(), &lwork, &info);
	    // if (work(0) > work.size()) {
	    // 	cout << "Need more lwork for dorgqr" << endl;
	    // }
	    lwork = work(0);
	    work.resize(lwork);
	    // cout << "in" << endl << scaled_in << endl << endl;
	    dorgqr_(&M, &N, &N, scaled_in.data(), &M, taus.data(),
		    work.data(), &lwork, &info);
	    // cout << "out" << endl << scaled_in << endl << endl;
	    U = row_perm.inverse() * scaled_in.block(0, 0, in.rows(), in.cols());
	    ivec col_ids(in.cols());
	    for (int i = 0; i < in.cols(); ++i) {
		col_ids(i) = iwork(i) - 1;
	    }

	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> col_perm(col_ids);
	    T = (d_inv.asDiagonal() * upper.block(0, 0, in.cols(), in.cols()))
		* col_perm.transpose();
	    D *= scale;
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}

	inline void decompose_udt_row_sort_sqr_q(mat&__restrict__ in, mat&__restrict__ U, vec&__restrict__ D, mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;

	    vector<int> max_index(in.rows());
	    vector<max_pair> max_indices;
	    vec max_vals(in.rows());
	    for(int i = 0; i < in.rows(); ++i) {
		max_vals(i) = in.cwiseAbs().row(i).maxCoeff( &max_index[i] );
		max_indices.push_back(max_pair(max_vals(i), i));
	    }
	    std::sort(max_indices.begin(), max_indices.end(), comparator);

	    ivec row_ids(in.rows());
	    for (std::vector<max_pair>::iterator it=max_indices.begin(); it != max_indices.end(); ++it) {
		row_ids((*it).second) = std::distance(max_indices.begin(), it);
		// cout << row_ids((*it).second) <<  " ";
	    }
	    // cout << endl;
	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> row_perm(row_ids);

	    // Very crude scaling...
	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.3);

	    vec taus(in.rows());
	    // vec work(in.rows() * in.rows() * in.rows());
	    int lwork = -1;
	    ivec iwork(in.rows());
	    int info = 0;
	    int M = in.rows();
	    int N = in.cols();
	    mat scaled_in = mat::Zero(in.rows(), in.rows());

	    scaled_in.block(0, 0, in.rows(), in.cols()) = (row_perm * in)/scale;
	    iwork.setZero();
	    // cout << iwork.size() << " " << work.size() << " " << taus.size() << endl;
	    lwork = -1;
	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), taus.data(),
		    &lwork, &info);
	    lwork = taus(0);
	    vec work(lwork);
	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), work.data(),
		    &lwork, &info);

	    {
		{
		    mat upper = scaled_in.triangularView<Upper>();
		    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
		    vec d_inv = D;

		    for (int i = 0; i < in.cols(); ++i) {
			if (D(i) == 0.) {
			    std::cout << "culprit " << D(i) << " was "
				      << upper(i, i) << std::endl;
			    throw std::runtime_error("Invalid upper triangle"); }
			d_inv(i) = 1./D(i);
			if (D(i) < 0) { D(i) *= -1.;
			    d_inv(i) *= -1.; }
		    }
		    dorgqr_(&M, &M, &N, scaled_in.data(), &M, taus.data(),
			    work.data(), &lwork, &info);
		    U = row_perm.inverse() * scaled_in.block(0, 0, in.rows(), in.cols());
		    ivec col_ids(in.cols());
		    for (int i = 0; i < in.cols(); ++i) {
			col_ids(i) = iwork(i) - 1;
		    }

		    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> col_perm(col_ids);
		    T = (d_inv.asDiagonal() * upper.block(0, 0, in.cols(), in.cols()))
			* col_perm.transpose();
		    D *= scale;
		    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
		}
	    }
	}

#ifdef USE_DD
	inline void decompose_udt_row_sort_dd(mat&__restrict__ in, mat&__restrict__ U, vec&__restrict__ D, mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;

	    vector<int> max_index(in.rows());
	    vector<max_pair> max_indices;
	    vec max_vals(in.rows());
	    for(int i = 0; i < in.rows(); ++i) {
		max_vals(i) = in.cwiseAbs().row(i).maxCoeff( &max_index[i] );
		max_indices.push_back(max_pair(max_vals(i), i));
	    }
	    std::sort(max_indices.begin(), max_indices.end(), comparator);

	    ivec row_ids(in.rows());
	    for (std::vector<max_pair>::iterator it=max_indices.begin(); it != max_indices.end(); ++it) {
		row_ids((*it).second) = std::distance(max_indices.begin(), it);
		// cout << row_ids((*it).second) <<  " ";
	    }
	    // cout << endl;
	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> row_perm(row_ids);

	    // Very crude scaling...
	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.5);

	    vec taus(in.rows());
	    // vec work(in.rows() * in.rows() * in.rows());
	    int lwork = -1;
	    ivec iwork(in.rows());
	    int info = 0;
	    int M = in.rows();
	    int N = in.cols();
	    mat temp = mat::Zero(in.rows(), in.cols());
	    dd_mat dd_temp = dd_mat::Zero(in.cols(), in.cols());
	    dd_mat scaled_in = dd_mat::Zero(in.rows(), in.cols());
	    temp = ((row_perm * in)/scale);
	    scaled_in.block(0, 0, in.rows(), in.cols()) = temp.cast<ddouble>();
	    Eigen::FullPivHouseholderQR<dd_mat> qr(scaled_in);
	    // cout << scaled_in << endl << endl;
	    qr.compute(scaled_in);
	    dd_temp = qr.matrixQR().block(0, 0, in.cols(), in.cols()).triangularView<Upper>();
	    mat upper = mat::Zero(in.cols(), in.cols());
	    // cout << "from dd_temp" << endl << endl << upper << endl << endl;
	    // cout << dd_temp.rows() << " x " << dd_temp.cols() << " - " <<
	    // 	upper.rows() << " x " << upper.cols() << endl;
	    dqmc::la::copy_from_dd(dd_temp, upper);
	    // cout << "from dd_temp" << endl << endl << upper << endl << endl;
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    std::cout << "culprit " << D(i) << " was "
			      << upper(i, i) << std::endl;
		    throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) { D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }
	    //dd_mat qrQ  = qr.householderQ();
	    dd_mat qrQ  = qr.matrixQ();
	    dd_temp = qrQ.block(0, 0, in.rows(), in.cols());

	    dqmc::la::copy_from_dd(dd_temp, temp);
	    // temp = dd_temp.cast<double>();
	    U = row_perm.inverse() * temp;

	    T = (d_inv.asDiagonal() * upper.block(0, 0, in.cols(), in.cols()))
		* qr.colsPermutation().transpose();
	    D *= scale;
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}
#endif

	inline void decompose_udt_row_sort(mat &__restrict__ in, mat &__restrict__ U, vec &__restrict__ D, mat &__restrict__ T,
					   mat &__restrict__ scaled_in, mat &__restrict__ T_temp,
					   ivec &__restrict__ row_ids, ivec &__restrict__ col_ids,
					   ivec &__restrict__ max_index, vec &__restrict__ max_vals, std::vector<max_pair>& max_pairs,
					   vec &__restrict__ taus, vec &__restrict__ work_old, ivec &__restrict__ iwork_old ) {
	    using namespace std;
	    using namespace Eigen;

	    max_pairs.clear();
	    for(int i = 0; i < in.rows(); ++i) {
		max_vals(i) = in.cwiseAbs().row(i).maxCoeff( &max_index(i) );
		max_pairs.push_back(max_pair(max_vals(i), i));
		// cout << i << endl;
	    }

	    // for (std::vector<max_pair>::iterator it=max_pairs.begin(); it != max_pairs.end(); ++it) {
	    // 	row_ids((*it).second) = std::distance(max_pairs.begin(), it);
	    // 	cout << (*it).second << " | " << std::distance(max_pairs.begin(), it) << "   ";
	    // }
	    // cout << endl;

	    std::sort(max_pairs.begin(), max_pairs.end(), comparator);
	    // cout << "Sorted" << endl;
	    // cout << row_ids.size();
	    for (std::vector<max_pair>::iterator it=max_pairs.begin(); it != max_pairs.end(); ++it) {
		row_ids((*it).second) = std::distance(max_pairs.begin(), it);
		// cout << (*it).second << " | " << std::distance(max_pairs.begin(), it) << "   ";
	    }
	    // cout << endl;
	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> row_perm(row_ids);
	    // cout << "permutation" << endl;

	    // Very crude scaling...
	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.3);
	    int lwork; // = work.size();
	    // vec work(in.rows() * in.rows() * in.rows());
	    // int lwork = work.size();

	    int info = 0;
	    int M = in.rows();
	    int N = in.cols();
	    ivec iwork(in.cols());

	    // cout << "Scaled" << endl;
	    // cout << row_perm.indices().transpose() << endl << endl;
	    // cout << row_perm.toDenseMatrix() << endl << endl;
	    // cout << in << endl << endl;
	    {
		{
		    scaled_in = (row_perm * in)/scale;
		    // cout << scaled_in << endl << endl;
		    iwork.setZero();
		    // cout << "QRing" << endl;
		    // cout << iwork.size() << " " << work.size() << " " << taus.size() << endl;
		    // ivec iwork(4*in.rows());
		    lwork = -1;
		    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
			    taus.data(), taus.data(),
			    &lwork, &info);
		    lwork = taus(0);
		    vec work(lwork);

		    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
			    taus.data(), work.data(),
			    &lwork, &info);
		}
	    }
	    {
		{

		    // cout << "To zero!" << endl;
		    // cout << "Getting diagonal" << endl;
		    D = scaled_in.diagonal();
		    vec d_inv = D;
		    // cout << "Gitgotthat" << endl;

		    for (int i = 0; i < in.cols(); ++i) {
			if (D(i) == 0.) {
			    std::cout << "culprit " << D(i) << std::endl;
			    throw std::runtime_error("Invalid upper triangle"); }

			d_inv(i) = 1./D(i);
			if (D(i) < 0) { D(i) *= -1.;
			    d_inv(i) *= -1.; }
		    }

		    for (int i = 0; i < in.cols(); ++i) col_ids(i) = iwork(i) - 1;
		    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> col_perm(col_ids);
		    T_temp = scaled_in.triangularView<Upper>();
		    T = (d_inv.asDiagonal() * T_temp) * col_perm.transpose();
		    // cout << T << endl << endl;

		    D *= scale;
		    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
		    lwork = -1;
		    dorgqr_(&M, &M, &N, scaled_in.data(), &M, taus.data(), taus.data(), &lwork, &info);
		    lwork = taus(0);
		    vec work(lwork);
		    dorgqr_(&M, &M, &N, scaled_in.data(), &M, taus.data(), work.data(), &lwork, &info);
		    U = row_perm.inverse() * scaled_in;
		}
	    }
	    return;
	}

	inline void decompose_udt_col_piv(mat&__restrict__ in,
					  mat&__restrict__ U,
					  vec&__restrict__ D,
					  mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;
	    if (U.rows() != in.rows() || U.cols() != in.cols() || U.cols() == 0) {
		cout << U.rows() << " " << U.cols() << endl;
		throw std::runtime_error("dimensions of U are wrong");
	    }

	    if (D.size() != in.cols() || D.size() == 0) {
		cout << D.size() << endl;
		throw std::runtime_error("dimensions of D are wrong");
	    }

	    if (T.rows() != in.cols() || T.cols() == 0)
		throw std::runtime_error("dimensions of T are wrong");

	    mat_t scaled_in = mat_t::Zero(in.rows(), in.cols());

	    vec_t taus(in.cols());
	    ivec iwork(in.cols());

	    int lwork = -1;
	    int M = in.rows();
	    int N = in.cols();

	    perm_mat row_sort_perm = row_sort(in, scaled_in);
	    // scaled_in.block(0, 0, in.rows(), in.cols()) = in;
	    iwork.setZero();
	    lapack_int info = LAPACKE_dgeqp3( LAPACK_COL_MAJOR, M, N, scaled_in.data(),
					      M, iwork.data(), taus.data() );

	    mat_t upper = scaled_in.triangularView<Upper>();
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    // std::cout << "culprit " << D(i) << " was "
		    // 	      << upper(i, i) << std::endl;
		    throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) { D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }
	    lwork = -1;

	    info = LAPACKE_dorgqr( LAPACK_COL_MAJOR, M, N, N,
				   scaled_in.data(), M, taus.data() );

	    U = (row_sort_perm.inverse() * scaled_in).block(0, 0, in.rows(), in.cols());
	    ivec col_ids(in.cols());
	    for (int i = 0; i < in.cols(); ++i) {
		col_ids(i) = iwork(i) - 1;
		if (col_ids(i) == -1) {
		    dqmc::tools::abort("Permutation fails");
		}
	    }
	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> lapack_col_perm(col_ids);

	    T = (d_inv.asDiagonal()
		 * upper.block(0, 0, in.cols(), in.cols()))
		* lapack_col_perm.transpose();
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}

    }
}
#endif


/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * 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 <iomanip>
#include <cassert>

#include "NPY.hpp"
#include "GLMPrint.hpp"
#include "GLMFormat.hpp"

#include "Camera.hh"
#include "View.hh"

#include "OPTICKS_LOG.hh"
#include "Opticks.hh"
#include "Composition.hh"

#include <boost/math/constants/constants.hpp>

void test_rotate()
{
    glm::vec3 X(1,0,0);
    glm::vec3 Y(0,1,0);
    glm::vec3 Z(0,0,1);

    float angle = 0.f ;
    for(unsigned int i=0 ; i < 6 ; i++)
    {
        switch(i)
        {
            case 0:angle = 0.f ; break;
            case 1:angle = 30.f ; break;
            case 2:angle = 45.f ; break;
            case 3:angle = 60.f ; break;
            case 4:angle = 90.f ; break;
            case 5:angle = 180.f ; break;
        }

        float pi = boost::math::constants::pi<float>() ;
        float a = angle*pi/180. ;
        printf(" angle %10.4f a %10.4f \n", angle, a );

        glm::mat4 rotX = glm::rotate(glm::mat4(1.0), a, X );
        glm::mat4 rotY = glm::rotate(glm::mat4(1.0), a, Y );
        glm::mat4 rotZ = glm::rotate(glm::mat4(1.0), a, Z );

        glm::mat4 irotX = glm::transpose(rotX);
        glm::mat4 irotY = glm::transpose(rotY);
        glm::mat4 irotZ = glm::transpose(rotZ);

        print(rotX, "rotX");
        print(irotX, "irotX");

        print(rotY, "rotY");
        print(irotY, "irotY");

        print(rotZ, "rotZ");
        print(irotZ, "irotZ");

   }
}

void test_center_extent(Opticks* ok)
{
   NPY<float>* dom = NPY<float>::load("domain", "1", "dayabay");
   if(!dom) return ;

   dom->dump();
   glm::vec4 ce = dom->getQuad(0,0);
   print(ce, "ce");

   Composition c(ok) ;
   c.setCenterExtent(ce);
   c.update();
   c.dumpAxisData();
}

void test_depth(Opticks* ok)
{
   Composition* comp = new Composition(ok) ;
   View* view = comp->getView();
   Camera* cam = comp->getCamera();

   float s = 100.0 ;

   glm::vec4 ce(0.,0.,0.,s);

    // extent normalized inputs to view
   view->setEye(-1,0,0) ;
   view->setLook(0,0,0) ;
   view->setUp(0,1,0) ;

   bool autocam ;
   comp->setCenterExtent(ce, autocam=true );

   cam->Summary("test_depth cam");
   view->Summary("test_depth view");

   glm::vec4 vp = comp->getViewpoint();
   glm::vec4 lp = comp->getLookpoint();
   glm::vec4 gaze = comp->getGaze();
   glm::vec4 front = glm::normalize(gaze);

   print(vp, "viewpoint");
   print(lp, "lookpoint");
   print(gaze, "gaze");
   print(front, "front");

   comp->update();

   glm::vec4 zproj ;
   cam->fillZProjection(zproj);

   print(zproj, "zproj");

   //unsigned int ix = cam->getWidth()/2 ;
   //unsigned int iy = cam->getHeight()/2 ;
   float near = cam->getNear();
   float far  = cam->getFar();

   std::cout
        << " near " << near
        << " far " << far
        << std::endl ;

   std::cout << " step along the gaze direction, from near to far  " << std::endl ;
   // (world frame : X axis from -100 to 0)
   // (eye frame   : Z axis from    0 to -100 )

   int N = 20 ;
   for(int i=0 ; i <= N ; i++)
   {
       float t = near + (far - near)*float(i)/float(N)  ;

       const glm::vec4 p_world = vp + front*t ;
       const glm::vec4 p_eye = comp->transformWorldToEye(p_world);
       const glm::vec3 p_ndc = comp->getNDC(p_world);
       const glm::vec3 p_ndc2 = comp->getNDC2(p_world);

       float eyeDist = p_eye.z ;   // from eye frame definition, this is general
       assert(eyeDist <= 0.f );
       float ndc_z = -zproj.z - zproj.w/eyeDist ;     // range -1:1 for visibles
       float ndc_z2 = comp->getNDCDepth(p_world);

       float clip_z = 0.5f*ndc_z + 0.5f ;             // range  0:1
       float depth = clip_z ;
       float clip_z2 = comp->getClipDepth(p_world);

       glm::vec3 unp = comp->unProject(0,0,-depth); // ix,iy ??

       std::cout << " t " << std::setw(5) << t
                 << " p_world " << std::setw(30) << gformat(p_world)
                 << " p_eye   " << std::setw(30) << gformat(p_eye)
                 << " p_ndc " << std::setw(30) << gformat(p_ndc)
                 << " p_ndc2 " << std::setw(30) << gformat(p_ndc2)
                 << " eyeDist " << std::setw(8) << eyeDist
                 << " ndc_z " << std::setw(8) << ndc_z
                 << " ndc_z2 " << std::setw(8) << ndc_z2
                 << " clip_z " << std::setw(8) << clip_z
                 << " clip_z2 " << std::setw(8) << clip_z2
                 << " unp   " << std::setw(20) << gformat(unp)
                 << std::endl ;

   }
}

/*

w2m
   scale+translate 3D object of arbitrary center/extent into unit box

      m2w 500.000   0.000   0.000   0.000
            0.000 500.000   0.000   0.000
            0.000   0.000 500.000   0.000
          100.000 100.000 100.000   1.000

      w2m   0.002   0.000   0.000   0.000
            0.000   0.002   0.000   0.000
            0.000   0.000   0.002   0.000
           -0.200  -0.200  -0.200   1.000

*/

void test_setCenterExtent()
{
    glm::vec4 ce(100.,100.,100.,500.);
    glm::vec3 sc(ce.w);              // scale factor from 1 to extent
    glm::vec3 tr(ce.x, ce.y, ce.z);  // translation from origin to center

    glm::vec3 isc(1.f/ce.w);

    glm::mat4 m_model_to_world = glm::scale( glm::translate(glm::mat4(1.0), tr), sc);

    glm::mat4 m_world_to_model = glm::translate( glm::scale(glm::mat4(1.0), isc), -tr);

    glm::mat4 check = m_world_to_model * m_model_to_world ;

    print(m_model_to_world, "m_model_to_world");
    print(m_world_to_model, "m_world_to_model");
    print(check, "check");

   std::cout << gpresent("m2w", m_model_to_world ) << std::endl ;
   std::cout << gpresent("w2m", m_world_to_model ) << std::endl ;

   std::vector<glm::vec4> world ;
   world.push_back( { ce.x       , ce.y        , ce.z       , 1.0 } );
   world.push_back( { ce.x + ce.w, ce.y + ce.w,  ce.z + ce.w, 1.0 } );
   world.push_back( { ce.x - ce.w, ce.y - ce.w,  ce.z - ce.w, 1.0 } );

   for(unsigned i=0 ; i < world.size() ; i++)
   {
       const glm::vec4& wpos = world[i] ;

       glm::vec4 mpos = m_world_to_model * wpos ;

       std::cout
               << gpresent("w", wpos )
               << gpresent("m", mpos )
               << std::endl ;

   }

}

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

    Opticks ok(argc, argv);
    ok.configure();

   //test_rotate();
   //test_center_extent(&ok);
   test_setCenterExtent();
   //test_depth(&ok);

   return 0 ;
}


// Std includes
#include <cmath>
#include <iostream>
#include <memory>
// Thirdparties includes
#include <Eigen/Dense>
// Lib includes
#include "s0s/runge_kutta_fehlberg.h"
#include "sl0/point.h"
#include "sl0/chain/dynamic.h"
// Simple includes
#include "flow.h"

using TypeScalar = double;
// State
template<int Size>
using TypeVector = Eigen::Matrix<TypeScalar, Size, 1>;
// Space
constexpr unsigned int DIM = 2;
using TypeSpaceVector = Eigen::Matrix<TypeScalar, DIM, 1>;
// Ref and View
template<typename ...Args>
using TypeRef = Eigen::Ref<Args...>;
template<typename ...Args>
using TypeView = Eigen::Map<Args...>;
// Group Parameters
using TypeStepPoint = sl0::StepPoint<TypeVector, DIM, TypeView, Flow>;
// Solver
using TypeSolver = s0s::SolverRungeKuttaFehlberg<TypeVector<-1>, TypeView>;

const unsigned int np = 11;

int main () {
    // Parameters
    TypeSpaceVector x0 = TypeSpaceVector::Zero();
    TypeScalar t0 = 0.0;
    TypeScalar dt = 1e-3;
    unsigned int nt = std::round(1.0 / dt);
    double dl = 0.1;
    double l = 1.0;
    // Create chain
    std::shared_ptr<TypeStepPoint> sStepPoint = std::make_shared<TypeStepPoint>(std::make_shared<Flow>());
    sl0::ChainDynamic<TypeVector, DIM, TypeView, TypeRef, TypeStepPoint, TypeSolver> chain(sStepPoint, dl, 4);
    // Init
    for(std::size_t i = 0; i < np; i++) {
        chain.sStep->addMember(chain.state);
        sStepPoint->x(chain.sStep->memberState(chain.state.data(), i)) = x0;
        sStepPoint->x(chain.sStep->memberState(chain.state.data(), i))[0] += i * dl;
    }
    std::cout << "Init Length : " << "\n" << chain.sStep->length(chain.state.data()) << "\n";
    std::cout << "Init Size : " << "\n" << chain.sStep->size() << "\n";
    chain.t = t0;
    // Computation
    std::cout << "Computing" << "\n";
    for(std::size_t i = 0; i < nt; i++) {
        chain.update(dt);
    }
    // out
    std::cout << "\n";
    std::cout << "Chain advected following a an exponential flow, exp(" << chain.t << ") = " << "\n";
    std::cout << "\n";
    std::cout << "Final Length : " << "\n" << chain.sStep->length(chain.state.data()) << "\n";
    std::cout << "Final Size : " << "\n" << chain.sStep->size() << "\n";
    std::cout << std::endl;
}


//#ifndef AMPAR_GILLESPIE_CLASS_HPP_INCLUDED
//#define AMPAR_GILLESPIE_CLASS_HPP_INCLUDED

#include <iostream>
#include <stdlib.h>
#include <cmath>
#include <vector>
#include <ctime>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <boost/numeric/odeint.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/random/linear_congruential.hpp>

#include "astron_utility_functions.hpp"
#include "stl_vector_operation_functions.hpp"
#include "boost_vector_matrix_operation_functions.hpp"

namespace bno=boost::numeric::odeint;
namespace bnu=boost::numeric::ublas;

typedef double value_type;
typedef bnu::matrix< value_type > matrix_type;

class stochastic_ampar
{
   private:
      unsigned int seed;
      boost::minstd_rand rng;   //Boost random number generator
      boost::uniform_real<double> uni_dist;   //Uniform random number distribution which produces values b/w 0 and 1 (0 inclusive, 1 exlusive).
      boost::variate_generator<boost::minstd_rand&, boost::uniform_real< double> > uni_rand;// variate generator
      //--------------
      double kf1 = 4.59E06;    //M^-1 S^-1
      double kb1 = 4.26E03;    //S^-1
      double kf2 = 28.4E06;    //M^-1 * S^-1
      double kb2 = 3.26E03;    //S^-1
      double kf3 = 1.27E06;    //M^-1 S^-1
      double kb3 = 45.7;       //S^-1
      double a0 = 4.24E03;     //S^-1
      double b0 = 900.0;       //S^-1
      double a1 = 2.89E03;     //S^-1
      double b1 = 39.2;        //S^-1
      double a2 = 172.0;       //S^-1
      double b2 = 0.727;       //S^-1
      double a3 = 17.7;        //S^-1
      double b3 = 4.0;         //S^-1
      double a4 = 16.8;        //S^-1
      double b4 = 190.4;       //S^-1
      double Volume = 1.0;
   public:
      //Construct vectors
      matrix_type X;       // A matrix to hold the states
      matrix_type P;       // A matrix that holds the probability at time t
      matrix_type T;       // The transition probability matrix
      matrix_type Q;       // The infinitly small matrix, Q matrix
      /* stochastic_ampar class constructor */
      stochastic_ampar(matrix_type X_, unsigned int seed_=std::time(0)):   seed(seed_),
                                                                        uni_dist(boost::uniform_real<double>(0,1)),
                                                                        uni_rand(boost::variate_generator<boost::minstd_rand&, boost::uniform_real<double> >(rng, uni_dist)),
                                                                        X(X_)
      {
         rng.seed(seed);
         std::vector<double> Tv;
         Tv = {
      };
      //------------------ Function declarations
      void operator() (  matrix_type &p, matrix_type &dpdt , const double  t )
      {
         dpdt = bnu::prod(p,Q);
      };
};
int main(int argc, char **argv)
{
   matrix_type mat(3,2);
   mat(2,1) = 32;
   stochastic_ampar test(mat);
   std::cout << test.X << std::endl;
}


#include <QApplication>

#include <boost/qvm/all.hpp>
#include "qvm_eigen.h"
#include "qvm_osg.h"

//#include "rtcvImage.h"

#include "Roi3DF_osg.h"

#include "QtOsgWidget.h"
#include "OsgGeometryManager.h"
#include "osg_utils.h"

#include "DisplayableAreaLights.h"
#include "DisplayableCameras.h"
#include "DisplayableCoordinateSystem.h"
#include "DisplayableGrid.h"

#include "CoordinateTransform.h"
#include "CoordinateSystem.h"

#include "Camera.h"
#include "Rotation.h"

#include "commonTypes.h"

struct UnrealTag {};

using UnrealCoordinateSystem = CoordinateSystem<3, ScalarType, UnrealTag>;

using UnrealVector = NamedType<UnrealCoordinateSystem::Vector, UnrealCoordinateSystem, VectorTypeSkills>;
using UnrealTransform = NamedType<UnrealCoordinateSystem::AffineTransform, UnrealCoordinateSystem>;

using UnrealToWorld = CoordinateTransform<UnrealCoordinateSystem, WorldCoordinateSystem>;

using UnrealRotation = Rotation<UnrealCoordinateSystem>;

UnrealTransform getCameraTransformation_ue(Degrees yaw, Degrees pitch, Degrees roll)
{
	const Radians angle_rotZ = deg2rad<ScalarType>(yaw);
	const Radians angle_rotY = deg2rad<ScalarType>(pitch);
	const Radians angle_rotX = deg2rad<ScalarType>(roll);

	const float cZ = cos(angle_rotZ.get());
	const float cY = cos(angle_rotY.get());
	const float cX = cos(angle_rotX.get());

	const float sZ = sin(angle_rotZ.get());
	const float sY = sin(angle_rotY.get());
	const float sX = sin(angle_rotX.get());

	Matrix33 mat_rotZ;
	Matrix33 mat_rotX;
	Matrix33 mat_rotY;

	mat_rotZ <<
		cZ , -sZ , 0 ,
		sZ , cZ , 0 ,
		0 , 0 , 1;

	mat_rotY <<
		cY , 0 , -sY ,
		0 , 1 , 0 ,
		sY , 0 , cY;

	mat_rotX <<
		1 , 0 , 0 ,
		0 , cX , sX ,
		0 , -sX , cX;

	Matrix33 mat_rot = mat_rotZ * mat_rotY * mat_rotX;

	return UnrealTransform(Transform(mat_rot));
}

CameraGeometry::Info makeInfo(const Camera<WorldCoordinateSystem> & cam)
{
	using namespace boost::qvm;

	using Size = Camera<WorldCoordinateSystem>::Size;

	const osg::Vec3 origin = convert_to<osg::Vec3>(cam.getOrigin().get());

	const Size imageSize = cam.getImageSize();

	const ImagePoint topLeft = make_named<ImagePoint>(0, 0);
	const ImagePoint topRight = make_named<ImagePoint>(imageSize(0), 0);
	const ImagePoint bottomLeft = make_named<ImagePoint>(0, imageSize(1));
	const ImagePoint bottomRight = make_named<ImagePoint>(imageSize(0), imageSize(1));

	const ViewRay<WorldCoordinateSystem> dir_topLeft = cam.imageToWorld(topLeft);
	const ViewRay<WorldCoordinateSystem> dir_topRight = cam.imageToWorld(topRight);
	const ViewRay<WorldCoordinateSystem> dir_bottomLeft = cam.imageToWorld(bottomLeft);
	const ViewRay<WorldCoordinateSystem> dir_bottomRight = cam.imageToWorld(bottomRight);

	return CameraGeometry::Info
	{
		origin,
		convert_to<osg::Vec3>(dir_topLeft.direction.get()),
		convert_to<osg::Vec3>(dir_topRight.direction.get()),
		convert_to<osg::Vec3>(dir_bottomLeft.direction.get()),
		convert_to<osg::Vec3>(dir_bottomRight.direction.get())
	};
}

class DisplayablePoints : public OsgDisplayable
{
public:
	DisplayablePoints(const std::vector<osg::Vec3> & points, float size) :
		m_points(points),
		m_size(size)
	{
		// empty
	}

	osg::ref_ptr<osg::Group> getGeometry() const override
	{
		osg::ref_ptr<osg::Group> camGroup = new osg::Group();
		for (const auto & p : m_points)
		{
			osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable(osg::ref_ptr<osg::Sphere>(new osg::Sphere(p, m_size)));
			sd->setColor(osg::Vec4(1, 0, 0, 1));
			camGroup->addChild(sd);
		}

		return camGroup;
	}

private:
	std::vector<osg::Vec3> m_points;
	float m_size;
};

int main(int argc, char ** argv)
{
	UnrealCoordinateSystem::setDirections({ Vector3(1, 0, 0), Vector3(0, -1, 0), Vector3(0, 0, 1) });
	WorldCoordinateSystem::setDirections({ Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1) });

	Camera<WorldCoordinateSystem> camera;

	using Vector = WorldCoordinateSystem::Vector;
	using FocalLength = decltype(camera)::FocalLength;
	using Size = decltype(camera)::Size;

	camera.setBaseDirections(
		{
			make_named<WorldVector>(0.f, -1.f, 0.f),
			make_named<WorldVector>(0.f, 0.f, -1.f),
			make_named<WorldVector>(1.f, 0.f, 0.f)
		}
	);

	const Degrees yaw(223.12f);
	const Degrees pitch(316.78f);
	const Degrees roll(0.f);

	const UnrealVector ue_cameraPos(Vector(31.61f, 28.6f, 23.54f));
	const UnrealRotation ue_cameraRotation = UnrealRotation(yaw, pitch, roll);

	std::cout << ue_cameraPos(0) << ", " << ue_cameraPos(1) << ", " << ue_cameraPos(2) << std::endl;

	WorldVector w_cameraPos = UnrealToWorld::sourceToTarget(ue_cameraPos);
	WorldVector w_cameraPos2 = convertTo<WorldCoordinateSystem>(ue_cameraPos);
	WorldRotation w_cameraRotation(ue_cameraRotation);

	camera.extrinsicTransform(w_cameraRotation);
	camera.extrinsicTransform(make_named<WorldTransform>(Translation(w_cameraPos.get())));

	camera.setFocalLength(FocalLength(713.2f));
	camera.setOpticalCenter(make_named<ImagePoint>(400, 300));
	camera.setImageSize(make_named<Size>(800, 600));

	std::ofstream("C:/TEMP/cam.txt") << camera;
	decltype(camera) loaded;
	std::ifstream("C:/TEMP/cam.txt") >> loaded;

	std::cout << std::boolalpha << (camera == camera) << std::endl;
	std::cout << std::boolalpha << (camera == Camera<WorldCoordinateSystem>()) << std::endl;
	std::cout << std::boolalpha << (camera == loaded) << std::endl;

	std::vector<UnrealVector> ue_points =
	{
		make_named<UnrealVector>(0.f, 0.f, 0.f),
		make_named<UnrealVector>(0.f, -15.f, 0.f),
		make_named<UnrealVector>(-10.f, -10.f, 0.f),
		make_named<UnrealVector>(19.f, 13.f, 0.f),
		make_named<UnrealVector>(-8.f, 16.f, 0.f)
	};

	std::vector<WorldVector> w_points;
	std::transform(ue_points.begin(), ue_points.end(), std::back_inserter(w_points),
	               [](const UnrealVector & ue_v)
	               {
		               return UnrealToWorld::sourceToTarget(ue_v);
	               }
	);

	for (const auto & w_p : w_points)
	{
		std::cout << w_p.get() << std::endl << std::endl;
	}

	std::cout << std::endl << std::endl;

	std::vector<decltype(camera)::CameraVector> cam_points;
	std::transform(w_points.begin(), w_points.end(), std::back_inserter(cam_points),
	               [&camera](const WorldVector & w_v)
	               {
		               return camera.worldToCamera(w_v);
	               });

	for (const auto & cam_p : cam_points)
	{
		std::cout << cam_p.get() << std::endl << std::endl;
	}

	std::cout << std::endl << std::endl;

	std::vector<ImagePoint> img_points;
	std::transform(w_points.begin(), w_points.end(), std::back_inserter(img_points),
	               [&camera](const WorldVector & w_v)
	               {
		               return camera.worldToImage(w_v);
	               });

	for (const auto & img_p : img_points)
	{
		std::cout << img_p.get() << std::endl << std::endl;
	}

	std::cout << std::endl << std::endl;

	std::vector<decltype(camera)::CameraVector> cam_points_reconstructed;
	std::transform(img_points.begin(), img_points.end(), std::back_inserter(cam_points_reconstructed),
		[&camera](const ImagePoint & p)
	{
		return camera.imageToCamera(p);
	});

	for (const auto & cam_p : cam_points_reconstructed)
	{
		std::cout << cam_p.get() << std::endl << std::endl;
	}

	std::cout << std::endl << std::endl;

	std::vector<ViewRay<WorldCoordinateSystem>> viewRays;
	std::transform(img_points.begin(), img_points.end(), std::back_inserter(viewRays),
		[&camera](const ImagePoint & p)
	{
		return camera.imageToWorld(p);
	});

	for (int i = 0; i < viewRays.size(); ++i)
	{
		const auto & v = viewRays[i];
		const float dist = (w_points[i].get() - v.origin.get()).norm();
		std::cout << v.origin.get() + dist * v.direction.get() << std::endl << std::endl;
	}

	/*rtcvImageRgba img;
	img.resize(800, 600);
	img.fill(rtcvRgbaValue(20, 20, 20));

	for(const auto & p : img_points)
	{
		img.drawRoi(rtcvRoi(rtcvPoint(p.get()(0), p.get()(1)), 10, 10), rtcvRed);
	}

	img.writeToFile("C:/TEMP/testImg.ppm");*/

	QApplication app(argc, argv);

	Roi3DF area(osg::Vec3(-30, -30, 0), osg::Vec3(30, 30, 20));

	QtOsgWidget widget(nullptr);

	osg::StateSet * rootStateSet = widget.getRoot()->getOrCreateStateSet();
	rootStateSet->setMode(GL_LIGHTING, osg::StateAttribute::ON);
	rootStateSet->setMode(GL_DEPTH_TEST, osg::StateAttribute::ON);

	//widget.addNodeToScene(createLightsFromUsableArea(area));

	GeometryManager manager(&widget);

	manager.switchGeometry(std::make_unique<DisplayableAreaLight>(area));
	manager.switchGeometry(std::make_unique<DisplayableGrid>(area, osg::Vec3(1, 1, 1)));
	manager.switchGeometry(std::make_unique<DisplayableCoordinateSystem>(osg::Vec3(0, 0, 0), 0.5f, 5.f));

	std::vector<CameraGeometry::Info> camerasToDisplay = { makeInfo(camera) };
	manager.switchGeometry(std::make_unique<DisplayableCameras>(camerasToDisplay, 1.f, 5.f));

	std::vector<osg::Vec3> pointsToDisplay;
	std::transform(w_points.begin(), w_points.end(), std::back_inserter(pointsToDisplay),
		[](const WorldVector & v)
	{
		return boost::qvm::convert_to<osg::Vec3>(v.get());
	}
	);

	manager.switchGeometry(std::make_unique<DisplayablePoints>(pointsToDisplay, 1.f));

	widget.show();

	return app.exec();
}


/*
 * Copyright (c) Huawei Technologies Co., Ltd. 2020-2030. All rights reserved.
 * Description:  Implementation of different 3D Plane Detection Algorithm.
 * Author: Created by huangjingwei 589411
 * Create date: 2021-08-13
 */
// STL includes.
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <iterator>
#include <string>
#include <vector>

// CGAL includes.
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/pca_estimate_normals.h>
#include <CGAL/Point_set_3.h>
#include <CGAL/Point_set_3/IO.h>
#include <CGAL/Random.h>
#include <CGAL/Shape_detection/Region_growing/Region_growing.h>
#include <CGAL/Shape_detection/Region_growing/Region_growing_on_point_set.h>
#include <CGAL/Timer.h>

#include <Eigen/Core>
#include <Eigen/Dense>

// Type declarations.
using Kernel = CGAL::Exact_predicates_inexact_constructions_kernel;
using FT = typename Kernel::FT;
using Point_3 = typename Kernel::Point_3;
using Vector_3 = typename Kernel::Vector_3;
using Input_range = CGAL::Point_set_3<Point_3>;
using Point_map = typename Input_range::Point_map;
using Normal_map = typename Input_range::Vector_map;
using Neighbor_query =
    CGAL::Shape_detection::Point_set::K_neighbor_query<Kernel, Input_range,
                                                       Point_map>;
// using Neighbor_query =
// CGAL::Shape_detection::Point_set::Sphere_neighbor_query<Kernel, Input_range,
// Point_map>;
using Region_type =
    CGAL::Shape_detection::Point_set::Least_squares_plane_fit_region<
        Kernel, Input_range, Point_map, Normal_map>;
using Region_growing =
    CGAL::Shape_detection::Region_growing<Input_range, Neighbor_query,
                                          Region_type>;
using Indices = std::vector<std::size_t>;
using Output_range = CGAL::Point_set_3<Point_3>;
using Points_3 = std::vector<Point_3>;

typedef std::pair<Point_3, Vector_3> PointVectorPair;
typedef std::vector<PointVectorPair> PointList;

// Concurrency
typedef CGAL::Parallel_if_available_tag Concurrency_tag;

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
    MatrixD;
typedef Eigen::Matrix<FT, 3, 1> Vector3;

struct Pointcloud {
  MatrixD P, N, C;
};

void ComputePointNormals(
    Pointcloud& pc,                         // input points + output normals
    unsigned int nb_neighbors_pca_normals)  // number of neighbors
{
  PointList points(pc.P.rows());
  for (int i = 0; i < points.size(); ++i) {
    auto p = pc.P.row(i);
    points[i].first = Point_3(p[0], p[1], p[2]);
  }
  CGAL::Timer task_timer;
  task_timer.start();

  // Estimates normals direction.
  // Note: pca_estimate_normals() requires an iterator over points
  // as well as property maps to access each point's position and normal.
  CGAL::pca_estimate_normals<Concurrency_tag>(
      points, nb_neighbors_pca_normals,
      CGAL::parameters::point_map(
          CGAL::First_of_pair_property_map<PointVectorPair>())
          .normal_map(CGAL::Second_of_pair_property_map<PointVectorPair>()));

  std::size_t memory = CGAL::Memory_sizer().virtual_size();
  pc.N.conservativeResize(points.size(), 3);
  for (int i = 0; i < points.size(); ++i) {
    pc.P.row(i) =
        Vector3(points[i].first.x(), points[i].first.y(), points[i].first.z());
    pc.N.row(i) = Vector3(points[i].second.x(), points[i].second.y(),
                          points[i].second.z());
    if (pc.N.row(i).dot(Vector3(1, 1, 1)) < 0) pc.N.row(i) = -pc.N.row(i);
  }
}

// Define an insert iterator.
int PlaneDetectRegion(Pointcloud& pc,
                      std::vector<std::pair<Vector3, FT> >& plane_parameters,
                      std::vector<int>& new_instances,
                      double dist_thres, double angle_thres, int min_points,
                      int num_neigbhors) {
  const bool with_normal_map = true;
  Input_range input_range(with_normal_map);
  for (int i = 0; i < pc.P.rows(); ++i) {
    input_range.insert(Kernel::Point_3(pc.P(i, 0), pc.P(i, 1), pc.P(i, 2)));
  }
  auto it = input_range.begin();
  for (int i = 0; i < pc.N.rows(); ++i) {
    input_range.normal(*(it++)) =
        Kernel::Vector_3(pc.N(i, 0), pc.N(i, 1), pc.N(i, 2));
  }
  // Default parameter values for the data file point_set_3.xyz.
  const std::size_t k = num_neigbhors;
  const FT max_distance_to_plane = dist_thres;
  const FT max_accepted_angle = angle_thres;
  const std::size_t min_region_size = min_points;

  // Create instances of the classes Neighbor_query and Region_type.
  Neighbor_query neighbor_query(input_range, k, input_range.point_map());
  Region_type region_type(input_range, max_distance_to_plane,
                          max_accepted_angle, min_region_size,
                          input_range.point_map(), input_range.normal_map());
  // Create an instance of the region growing class.
  Region_growing region_growing(input_range, neighbor_query, region_type);
  // Run the algorithm.
  Output_range output_range;
  std::size_t number_of_regions = 0;

  std::vector<std::vector<std::size_t> > regions;
  region_growing.detect(std::back_inserter(regions));
  plane_parameters.resize(regions.size());
  new_instances.resize(pc.P.rows(), -1);

  for (int i = 0; i < regions.size(); ++i) {
    Vector3 c(0, 0, 0);
    for (auto& idx : regions[i]) {
      new_instances[idx] = i;
      c += pc.P.row(idx);
    }
    c /= (double)regions[i].size();

    MatrixD C = MatrixD::Zero(3, 3);
    for (auto& idx : regions[i]) {
      Vector3 diff = pc.P.row(idx);
      diff -= c;
      C += diff * diff.transpose();
    }
    Eigen::JacobiSVD<MatrixD> svd(C, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Vector3 N = svd.matrixU().col(2);
    plane_parameters[i] = std::make_pair(N, -N.dot(c));
  }
  return regions.size();
}

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

  const int kNeighbors = 16;
  const double kDistThres = 1e-1;
  const double kAngleThres = 20;
  const int kMinPoints = 200;

  // simulate a box
  std::vector<Vector3> points;
  for (int i = 0; i < 100; ++i) {
    for (int j = 0; j < 100; ++j) {
      points.push_back(Vector3(i * 0.01, j * 0.01, 0));
      points.push_back(Vector3(i * 0.01, j * 0.01, 1));
      points.push_back(Vector3(i * 0.01, 0, j * 0.01));
      points.push_back(Vector3(i * 0.01, 1, j * 0.01));
      points.push_back(Vector3(0, i * 0.01, j * 0.01));
      points.push_back(Vector3(1, i * 0.01, j * 0.01));
    }
  }
  for (auto& p : points) {
    double dx = rand() % 256 / 256.0 - 0.5;
    double dy = rand() % 256 / 256.0 - 0.5;
    double dz = rand() % 256 / 256.0 - 0.5;
    p += Vector3(dx * 0.01, dy * 0.01, dz * 0.01);
  }

  Pointcloud pc;
  pc.P.resize(points.size(), 3);
  memcpy(pc.P.data(), points.data(), sizeof(Vector3) * points.size());

  ComputePointNormals(pc, kNeighbors);

  std::vector<std::pair<Vector3, double> > plane_parameters;
  std::vector<int> plane_instances;
  int num_inst = PlaneDetectRegion(pc, plane_parameters, plane_instances,
    kDistThres, kAngleThres, kMinPoints, kNeighbors);

  printf("Num inst: %d\n", num_inst);
  printf("Params:\n");
  for (auto& p : plane_parameters) {
    printf("%f %f %f %f\n", p.first[0], p.first[1], p.first[2], p.second);
  }
  std::vector<Vector3> colors(num_inst);
  for (auto& c : colors) {
    c = Vector3(rand() % 256 / 256.0,
      rand() % 256 / 256.0, rand() % 256 / 256.0);
  }

  std::ofstream os("result.obj");
  for (int i = 0; i < points.size(); ++i) {
    auto p = points[i];
    auto c = Vector3(0, 0, 0);
    if (plane_instances[i] >= 0)
      c = colors[plane_instances[i]];
    os << "v " << p[0] << " " << p[1] << " " << p[2] << " "
      << c[0] << " " << c[1] << " " << c[2] << "\n";
  }
  os.close();
  return 0;
}


#pragma once

#include "geometry/any_rect2d.hpp"
#include "geometry/point2d.hpp"

#include "base/exception.hpp"
#include "base/math.hpp"

#include <vector>

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wcomma"
#include <boost/geometry.hpp>
#pragma clang diagnostic pop
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

namespace geometry
{
double constexpr kPenaltyScore = -1.0;
DECLARE_EXCEPTION(NotAPolygonException, RootException);

namespace impl
{
using PointXY = boost::geometry::model::d2::point_xy<double>;
using Polygon = boost::geometry::model::polygon<PointXY>;
using MultiPolygon = boost::geometry::model::multi_polygon<Polygon>;
}  // namespace impl

template <typename Container>
impl::Polygon PointsToPolygon(Container const & points);
template <typename Container>
impl::MultiPolygon TrianglesToPolygon(Container const & points);

// The return value is a real number from [-1.0, 1.0].
// * Returns positive value when the geometries intersect (returns intersection area divided by union area).
//   In particular, returns 1.0 when the geometries are equal.
// * Returns zero when the geometries do not intersect.
// * Returns kPenaltyScore as penalty. It is possible when any of the geometries is empty or invalid.
/// |lhs| and |rhs| are any areal boost::geometry types.
template <typename LGeometry, typename RGeometry>
double GetIntersectionScore(LGeometry const & lhs, RGeometry const & rhs)
{
  if (!boost::geometry::is_valid(lhs) || !boost::geometry::is_valid(rhs) ||
      boost::geometry::is_empty(lhs) || boost::geometry::is_empty(rhs))
  {
    return kPenaltyScore;
  }

  auto const lhsArea = boost::geometry::area(lhs);
  auto const rhsArea = boost::geometry::area(rhs);
  impl::MultiPolygon result;
  boost::geometry::intersection(lhs, rhs, result);
  auto const intersectionArea = boost::geometry::area(result);
  auto const unionArea = lhsArea + rhsArea - intersectionArea;

  auto const score = intersectionArea / unionArea;

  return score;
}

/// Throws NotAPolygonException exception.
/// For detailed info see comment for
/// double GetIntersectionScore(LPolygon const & lhs, RPolygon const & rhs).
/// |lhs| and |rhs| are any standard container of m2::Point with random access iterator.
/// |toPolygonConverter| is a method which converts |lhs| and |rhs| to boost::geometry areal type.
template <typename Container, typename Converter>
double GetIntersectionScore(Container const & lhs, Container const & rhs,
                            Converter const & toPolygonConverter)
{
  auto const lhsPolygon = toPolygonConverter(lhs);
  if (boost::geometry::is_empty(lhsPolygon))
    return kPenaltyScore;

  auto const rhsPolygon = toPolygonConverter(rhs);
  if (boost::geometry::is_empty(rhsPolygon))
    return kPenaltyScore;

  return GetIntersectionScore(lhsPolygon, rhsPolygon);
}

/// Throws NotAPolygonException exception.
/// For detailed info see comment for
/// double GetIntersectionScore(LPolygon const & lhs, RPolygon const & rhs).
/// |lhs| and |rhs| are any standard containers of m2::Point with random access iterator.
template <typename Container>
double GetIntersectionScoreForPoints(Container const & lhs, Container const & rhs)
{
  return GetIntersectionScore<Container>(lhs, rhs, PointsToPolygon<Container>);
}

/// Throws NotAPolygonException exception.
/// For detailed info see comment for
/// double GetIntersectionScore(LPolygon const & lhs, RPolygon const & rhs).
/// |lhs| and |rhs| are any standard containers of m2::Point with random access iterator.
template <typename Container>
double GetIntersectionScoreForTriangulated(Container const & lhs, Container const & rhs)
{
  return GetIntersectionScore<Container>(lhs, rhs, TrianglesToPolygon<Container>);
}

/// |points| is any standard container of m2::Point with random access iterator.
template <typename Container>
impl::Polygon PointsToPolygon(Container const & points)
{
  impl::Polygon polygon;
  for (auto const & point : points)
    polygon.outer().push_back(impl::PointXY(point.x, point.y));
  boost::geometry::correct(polygon);
  if (!boost::geometry::is_valid(polygon))
    MYTHROW(geometry::NotAPolygonException, ("The points is not valid polygon"));

  return polygon;
}

/// |points| is any standard container of m2::Point with random access iterator.
template <typename Container>
impl::MultiPolygon TrianglesToPolygon(Container const & points)
{
  size_t const kTriangleSize = 3;
  if (points.size() % kTriangleSize != 0)
    MYTHROW(geometry::NotAPolygonException, ("Count of points must be multiple of", kTriangleSize));

  std::vector<impl::MultiPolygon> polygons;
  for (size_t i = 0; i < points.size(); i += kTriangleSize)
  {
    impl::MultiPolygon polygon;
    polygon.resize(1);
    auto & p = polygon[0];
    auto & outer = p.outer();
    for (size_t j = i; j < i + kTriangleSize; ++j)
      outer.push_back(impl::PointXY(points[j].x, points[j].y));
    boost::geometry::correct(p);
    if (!boost::geometry::is_valid(polygon))
      MYTHROW(geometry::NotAPolygonException, ("The triangle is not valid"));
    polygons.push_back(polygon);
  }

  if (polygons.empty())
    return {};

  auto & result = polygons[0];
  for (size_t i = 1; i < polygons.size(); ++i)
  {
    impl::MultiPolygon u;
    boost::geometry::union_(result, polygons[i], u);
    u.swap(result);
  }
  return result;
}
}  // namespace geometry