//

#include <string>
#include <vector>
//#include <tuple>
#include <cstdio>

#include <Eigen/Sparse>

#include "SparseMatrix.hpp"
#include "SparseVector.hpp"
#include "DenseVector.hpp"
#include "KernelComposer.hpp"
#include "KernelComposerHelpers.hpp"

#include "gtest/gtest.h"

class KernelComposerTest: public ::testing::Test{
    protected:
    //KernelComposerTest(){};
    //~KernelComposerTest(){};

    void SetUp() override{
        std::string dnv1name("dnv_name_1");
        std::string dnv2name("dnv_name_2");
        std::string spvname("spv_name");
        std::string spmname("spm_name");
        std::vector<double> dnv1;
        std::vector<double> dnv2;
        Eigen::SparseVector<double> spv;
        Eigen::SparseMatrix<double,1> spm;

        for(int k=0;k<Nv;k++){
            dnv1.push_back(10.0*((double)(k)));
        };
        DnV1.setName(dnv1name);
        DnV1.setVector(dnv1);

        for(int k=0;k<Nv;k++){
            dnv2.push_back(10.0);
        };
        DnV2.setName(dnv2name);
        DnV2.setVector(dnv2);

        spv.resize(Nv);
        for(int k=Nv/4;k<(3*Nv/4);k++){
            spv.coeffRef(k)=10.0;
        };
        SpV.setName(spvname);
        SpV.vec=spv;

        spm.resize(Nv,Nv);
        for(int k=0;k<Nv;k++){
            spm.coeffRef(k,k)=1.0;
        };
        spm.makeCompressed();
        SpM.setName(spmname);
        SpM.mat=spm;

        for(int k=0;k<NtSig;k++){
            ft.push_back(1.0);
        };
    };
    //void TearDown() override{};
    swSim::DenseVector DnV1;
    swSim::DenseVector DnV2;
    swSim::SparseVector SpV;
    swSim::SparseMatrix SpM;
    std::vector<double> ft;

    int Nv=20;
    int NtSig=10;
};

TEST_F(KernelComposerTest,committed){
    swSim::KernelComposer KC;
    EXPECT_FALSE(KC.isCommitted());
};

TEST_F(KernelComposerTest,setDnV){
    swSim::KernelComposer KC;
    EXPECT_EQ(KC.setDenseVector(&DnV1),swSim::KC_OK);
};

TEST_F(KernelComposerTest,setSpV){
    swSim::KernelComposer KC;
    EXPECT_EQ(KC.setSparseVector(&SpV),swSim::KC_OK);
};

TEST_F(KernelComposerTest,setSpM){
    swSim::KernelComposer KC;
    EXPECT_EQ(KC.setSparseMatrix(&SpM),swSim::KC_OK);
};

TEST_F(KernelComposerTest,setSpM2){
    swSim::KernelComposer KC;
    EXPECT_EQ(KC.setSparseMatrix(&SpM),swSim::KC_OK);
    ASSERT_FALSE(KC.isCommitted());
    ASSERT_EQ(KC.kernelCommit(),swSim::KC_OK);
    ASSERT_TRUE(KC.isCommitted());
    ASSERT_EQ(KC.kernelDecommit(),swSim::KC_OK);
    ASSERT_FALSE(KC.isCommitted());
};

TEST_F(KernelComposerTest,Commit){
    swSim::KernelComposer KC;
    ASSERT_FALSE(KC.isCommitted());
    ASSERT_EQ(KC.kernelCommit(),swSim::KC_OK);
    ASSERT_TRUE(KC.isCommitted());
    ASSERT_EQ(KC.kernelDecommit(),swSim::KC_OK);
    ASSERT_FALSE(KC.isCommitted());
};

TEST_F(KernelComposerTest,SpMVstaging){
    swSim::KernelComposer KC;
    KC.setSparseMatrix(&SpM);
    KC.setDenseVector(&DnV1);
    KC.setDenseVector(&DnV2);

    //Configure test, ensure configuration is correct
    ASSERT_FALSE(KC.isCommitted());
    ASSERT_EQ(KC.setSPMV(SpM.getName(),DnV1.getName(),DnV2.getName(),1.0,1.0),
              swSim::KC_OK);
    ASSERT_EQ(KC.kernelCommit(),swSim::KC_OK);
    ASSERT_TRUE(KC.isCommitted());
    ASSERT_EQ(KC.run(1),swSim::KC_OK);
    ASSERT_EQ(KC.kernelDecommit(),swSim::KC_OK);
    ASSERT_FALSE(KC.isCommitted());
};

TEST_F(KernelComposerTest,SPMV){
    swSim::KernelComposer KC;
    KC.setSparseMatrix(&SpM);
    KC.setDenseVector(&DnV1);
    KC.setDenseVector(&DnV2);

    //Configure test, ensure configuration is correct
    ASSERT_FALSE(KC.isCommitted());
    ASSERT_EQ(KC.setSPMV(SpM.getName(),DnV1.getName(),DnV2.getName(),1.0,1.0),
              swSim::KC_OK);
    ASSERT_EQ(KC.kernelCommit(),swSim::KC_OK);
    ASSERT_TRUE(KC.isCommitted());
    ASSERT_EQ(KC.run(1),swSim::KC_OK);
    ASSERT_EQ(KC.getDeviceData(DnV2.getName()),swSim::KC_OK);
    ASSERT_EQ(KC.kernelDecommit(),swSim::KC_OK);
    ASSERT_FALSE(KC.isCommitted());

    //test values
    for(int k=0;k<Nv;k++){
        EXPECT_FLOAT_EQ(DnV2.getValueArray()[k],10.0+((double)(k))*10.0);
    };
};
TEST_F(KernelComposerTest,SPMV2){
    swSim::KernelComposer KC;
    KC.setSparseMatrix(&SpM);
    KC.setDenseVector(&DnV1);
    KC.setDenseVector(&DnV2);

    //Configure test, ensure configuration is correct
    ASSERT_FALSE(KC.isCommitted());
    ASSERT_EQ(KC.setSPMV(SpM.getName(),DnV1.getName(),DnV2.getName(),1.0,1.0),
              swSim::KC_OK);
    ASSERT_EQ(KC.kernelCommit(),swSim::KC_OK);
    ASSERT_TRUE(KC.isCommitted());
    ASSERT_EQ(KC.run(0,1),swSim::KC_OK);
    ASSERT_EQ(KC.getDeviceData(DnV2.getName()),swSim::KC_OK);
    ASSERT_EQ(KC.kernelDecommit(),swSim::KC_OK);
    ASSERT_FALSE(KC.isCommitted());

    //test values
    for(int k=0;k<Nv;k++){
        EXPECT_FLOAT_EQ(DnV2.getValueArray()[k],10.0+((double)(k))*10.0);
    };
};

TEST_F(KernelComposerTest,SPaXPY){
    swSim::KernelComposer KC;
    KC.setDenseVector(&DnV2);
    KC.setSparseVector(&SpV);

    //Configure test, ensure configuration is correct
    ASSERT_FALSE(KC.isCommitted());
    ASSERT_EQ(KC.setSPaXPY(SpV.getName(),DnV2.getName(),&ft),
              swSim::KC_OK);
    ASSERT_EQ(KC.kernelCommit(),swSim::KC_OK);
    ASSERT_TRUE(KC.isCommitted());
    ASSERT_EQ(KC.run(1),swSim::KC_OK);
    ASSERT_EQ(KC.getDeviceData(DnV2.getName()),swSim::KC_OK);
    ASSERT_EQ(KC.kernelDecommit(),swSim::KC_OK);
    ASSERT_FALSE(KC.isCommitted());

    //test values
    for(int k=0;k<Nv;k++){
        EXPECT_FLOAT_EQ(DnV2.getValueArray()[k],10.0
                    +10.0*((double)((k>=(Nv/4)) && (k<(3*Nv/4)))));
    };
};
TEST_F(KernelComposerTest,SPaXPY2){
    swSim::KernelComposer KC;
    KC.setDenseVector(&DnV2);
    KC.setSparseVector(&SpV);

    //Configure test, ensure configuration is correct
    ASSERT_FALSE(KC.isCommitted());
    ASSERT_EQ(KC.setSPaXPY(SpV.getName(),DnV2.getName(),&ft),
              swSim::KC_OK);
    ASSERT_EQ(KC.kernelCommit(),swSim::KC_OK);
    ASSERT_TRUE(KC.isCommitted());
    ASSERT_EQ(KC.run(0,1),swSim::KC_OK);
    ASSERT_EQ(KC.getDeviceData(DnV2.getName()),swSim::KC_OK);
    ASSERT_EQ(KC.kernelDecommit(),swSim::KC_OK);
    ASSERT_FALSE(KC.isCommitted());

    //test values
    for(int k=0;k<Nv;k++){
        EXPECT_FLOAT_EQ(DnV2.getValueArray()[k],10.0
                    +10.0*((double)((k>=(Nv/4)) && (k<(3*Nv/4)))));
    };
};

TEST_F(KernelComposerTest,AXPY){
    swSim::KernelComposer KC;
    KC.setDenseVector(&DnV1);
    KC.setDenseVector(&DnV2);

    //Configure test, ensure configuration is correct
    ASSERT_FALSE(KC.isCommitted());
    ASSERT_EQ(KC.setAXPY(DnV1.getName(),DnV2.getName(),1.0),
              swSim::KC_OK);
    ASSERT_EQ(KC.kernelCommit(),swSim::KC_OK);
    ASSERT_TRUE(KC.isCommitted());
    ASSERT_EQ(KC.run(1),swSim::KC_OK);
    ASSERT_EQ(KC.getDeviceData(DnV2.getName()),swSim::KC_OK);
    ASSERT_EQ(KC.kernelDecommit(),swSim::KC_OK);
    ASSERT_FALSE(KC.isCommitted());

    //test values
    for(int k=0;k<Nv;k++){
        EXPECT_FLOAT_EQ(DnV2.getValueArray()[k],10.0+10.0*((double)(k)));
    };
};

TEST_F(KernelComposerTest,SPMVandSPaXPY){
    swSim::KernelComposer KC;
    KC.setSparseMatrix(&SpM);
    KC.setDenseVector(&DnV1);
    KC.setDenseVector(&DnV2);
    KC.setSparseVector(&SpV);

    //Configure test, ensure configuration is correct
    ASSERT_FALSE(KC.isCommitted());
    ASSERT_EQ(KC.setSPMV(SpM.getName(),DnV1.getName(),DnV2.getName(),1.0,1.0),
              swSim::KC_OK);
    ASSERT_EQ(KC.setSPaXPY(SpV.getName(),DnV2.getName(),&ft),
              swSim::KC_OK);
    ASSERT_EQ(KC.kernelCommit(),swSim::KC_OK);
    ASSERT_TRUE(KC.isCommitted());
    ASSERT_EQ(KC.run(1),swSim::KC_OK);
    ASSERT_EQ(KC.getDeviceData(DnV2.getName()),swSim::KC_OK);
    ASSERT_EQ(KC.kernelDecommit(),swSim::KC_OK);
    ASSERT_FALSE(KC.isCommitted());

    //test values
    for(int k=0;k<Nv;k++){
        EXPECT_FLOAT_EQ(DnV2.getValueArray()[k],10.0+((double)(k))*10.0
                        +10.0*((double)((k>=(Nv/4)) && (k<(3*Nv/4)))));
    };
};

TEST_F(KernelComposerTest,SPMVandSPaXPYandAXPY){
    swSim::KernelComposer KC;
    KC.setSparseMatrix(&SpM);
    KC.setDenseVector(&DnV1);
    KC.setDenseVector(&DnV2);
    KC.setSparseVector(&SpV);

    //Configure test, ensure configuration is correct
    ASSERT_FALSE(KC.isCommitted());
    ASSERT_EQ(KC.setSPMV(SpM.getName(),DnV1.getName(),DnV2.getName(),1.0,1.0),
              swSim::KC_OK);
    ASSERT_EQ(KC.setSPaXPY(SpV.getName(),DnV2.getName(),&ft),
              swSim::KC_OK);
    ASSERT_EQ(KC.setAXPY(DnV1.getName(),DnV2.getName(),1.0),
              swSim::KC_OK);
    ASSERT_EQ(KC.kernelCommit(),swSim::KC_OK);
    ASSERT_TRUE(KC.isCommitted());
    ASSERT_EQ(KC.run(1),swSim::KC_OK);
    ASSERT_EQ(KC.getDeviceData(DnV2.getName()),swSim::KC_OK);
    ASSERT_EQ(KC.kernelDecommit(),swSim::KC_OK);
    ASSERT_FALSE(KC.isCommitted());

    //test values
    for(int k=0;k<Nv;k++){
        EXPECT_FLOAT_EQ(DnV2.getValueArray()[k],10.0+((double)(k))*10.0
                        +((double)(k))*10.0
                        +10.0*((double)((k>=(Nv/4)) && (k<(3*Nv/4)))));
    };
};

TEST_F(KernelComposerTest,updateSpVfromHost){
    swSim::KernelComposer KC;
    KC.setSparseMatrix(&SpM);
    KC.setDenseVector(&DnV1);
    KC.setDenseVector(&DnV2);
    KC.setSparseVector(&SpV);

    //Configure test, ensure configuration is correct
    ASSERT_FALSE(KC.isCommitted());
    //ASSERT_EQ(KC.setUpdateSparseVectorFromHost(SpV.getName()),swSim::KC_OK);
    ASSERT_EQ(KC.setSPaXPY(SpV.getName(),DnV2.getName(),&ft),
              swSim::KC_OK);
    ASSERT_EQ(KC.kernelCommit(),swSim::KC_OK);
    ASSERT_TRUE(KC.isCommitted());
    ASSERT_EQ(KC.run(1),swSim::KC_OK);
    ASSERT_EQ(KC.getDeviceData(DnV2.getName()),swSim::KC_OK);
    ASSERT_EQ(KC.kernelDecommit(),swSim::KC_OK);
    ASSERT_FALSE(KC.isCommitted());

    //test values
    for(int k=0;k<Nv;k++){
        EXPECT_FLOAT_EQ(DnV2.getValueArray()[k],
                10.0+(10.0)*(k>=Nv/4 && k<3*Nv/4));
    };
};

TEST_F(KernelComposerTest,setDeviceData){
    swSim::KernelComposer KC;
    KC.setSparseMatrix(&SpM);
    KC.setDenseVector(&DnV1);
    KC.setDenseVector(&DnV2);
    ASSERT_EQ(KC.setSPMV(SpM.getName(),DnV1.getName(),DnV2.getName(),1.0,1.0),
              swSim::KC_OK);

    ASSERT_EQ(KC.kernelCommit(),swSim::KC_OK);
    ASSERT_TRUE(KC.isCommitted());
    for(int k=0;k<Nv;k++){
        DnV1.getValueArray()[k]=1.0;
    };
    ASSERT_EQ(KC.setDeviceData(DnV1.getName()),swSim::KC_OK);
    ASSERT_TRUE(KC.isCommitted());
    ASSERT_EQ(KC.run(1),swSim::KC_OK);
    ASSERT_EQ(KC.getDeviceData(DnV2.getName()),swSim::KC_OK);
    ASSERT_EQ(KC.kernelDecommit(),swSim::KC_OK);
    ASSERT_FALSE(KC.isCommitted());

    //test values
    for(int k=0;k<Nv;k++){
        EXPECT_FLOAT_EQ(DnV2.getValueArray()[k],11.0);
    };
};


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <stdint.h>
#include <string>
#include <vector>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/QR>
#include <unsupported/Eigen/MatrixFunctions>

// CUDA runtime
#include <cuda_runtime.h>
// Utilities and system includes
//#include <mmf/defines.h>
#include <nvidia/helper_cuda.h>

#include <jsCore/clDataGpu.hpp>
#include <jsCore/timer.hpp>

#include <mmf/optimizationSO3.hpp>
#include <manifold/SO3.h>

using namespace Eigen;
using namespace std;

extern void directSquaredAngleCostFctGPU(float *h_cost, float *d_cost,
    float *d_x, float* d_weights, uint32_t *d_z, float *d_mu,
    int N);

extern void directSquaredAngleCostFctAssignmentGPU(float *h_cost, float *d_cost,
  uint32_t *h_W, uint32_t *d_W, float *d_x, float* d_weights,
  uint32_t *d_z, float *d_mu, int N);

extern void directSquaredAngleCostFctJacobianGPU(float *h_J, float *d_J,
    float *d_x, float *d_weights, uint32_t *d_z, float *d_mu, int N);

extern void meanInTpS2GPU(float *h_p, float *d_p, float *h_mu_karch,
    float *d_mu_karch, float *d_q, uint32_t *d_z, float* d_weights, int N);

extern void sufficientStatisticsOnTpS2GPU(float *h_p, float *d_p, float
    *h_Rnorths, float *d_Rnorths, float *d_q, uint32_t *d_z ,int N, float
    *h_SSs, float *d_SSs);

extern void loadRGBvaluesForMFaxes();

namespace mmf{

/// Implements Conjugate Gradient Optimization on SO3 using GPU
/// optimizations
class OptSO3GD : public OptSO3
{
public:
  OptSO3GD(float *d_weights =NULL)
    : OptSO3(1.,1.,1.,d_weights), thr_(1.e-6), c_(0.1), ddelta_(0.1), tauR_(1.)
  {};

  virtual ~OptSO3GD() {};

protected:
  SO3f theta_;
  SO3f thetaPrev_;
  float thr_; // threshold for gradient descent
  float c_;
  float ddelta_;
  float tauR_;

  void ComputeJacobian(const SO3f& thetaPrev, const SO3f& theta,
      uint32_t N, Eigen::Vector3f* J, float* f);
  void LineSearch(uint32_t N, Eigen::Vector3f* J, float* f);

  virtual float conjugateGradientPreparation_impl(Matrix3f& R, uint32_t& N);
  virtual float conjugateGradientCUDA_impl(Matrix3f& R, float res0, uint32_t N, uint32_t maxIter=100);
  virtual void conjugateGradientPostparation_impl(Matrix3f& R){;};
  /* recompute assignment based on rotation R and return residual as well */
  virtual float computeAssignment(Matrix3f& R, uint32_t& N);
  /* mainly init GPU arrays */
  virtual void init();
};
}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__INTERNAL__TN_HPP_
#define SMOOTH__INTERNAL__TN_HPP_

#include <Eigen/Core>

#include "common.hpp"

namespace smooth {

/**
 * @brief T(n) Lie Group represented as R^n
 *
 * Memory layout
 * -------------
 * Group:    x1 x2 ... xn
 * Tangent:  v1 v2 ... vn
 *
 * Lie group Matrix form
 * ---------------------
 * [ I T ]
 * [ 0 1 ]
 *
 * where T = [x1 ... xn]'
 *
 * Lie algebra Matrix form
 * -----------------------
 * [ 0 V ]
 * [ 0 0 ]
 *
 * where V = [v1 ... vn]'
 */
template<int N, typename _Scalar>
struct TnImpl
{
  using Scalar = _Scalar;

  static constexpr Eigen::Index Dim     = N + 1;
  static constexpr Eigen::Index Dof     = N;
  static constexpr Eigen::Index RepSize = N;
  static constexpr bool IsCommutative   = true;

  SMOOTH_DEFINE_REFS;

  static void setIdentity(GRefOut g_out) { g_out.setZero(); }

  static void setRandom(GRefOut g_out) { g_out.setRandom(); }

  static void matrix(GRefIn g_in, MRefOut m_out)
  {
    m_out.setIdentity();
    m_out.template topRightCorner<Dof, 1>() = g_in;
  }
  static void composition(GRefIn g_in1, GRefIn g_in2, GRefOut g_out) { g_out = g_in1 + g_in2; }

  static void inverse(GRefIn g_in, GRefOut g_out) { g_out = -g_in; }

  static void log(GRefIn g_in, TRefOut a_out) { a_out = g_in; }

  static void exp(TRefIn a_in, GRefOut g_out) { g_out = a_in; }

  static void hat(TRefIn a_in, MRefOut A_out)
  {
    A_out.setZero();
    A_out.template topRightCorner<N, 1>() = a_in;
  }

  static void vee(MRefIn A_in, TRefOut a_out) { a_out = A_in.template topRightCorner<N, 1>(); }

  static void ad(TRefIn, TMapRefOut A_out) { A_out.setZero(); }
};

}  // namespace smooth

#endif  // SMOOTH__INTERNAL__TN_HPP_


// This file is part of OpenMVG, an Open Multiple View Geometry C++ library.

// Copyright (c) 2018 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef OPENMVG_SFM_STELLAR_STELLAR_DEFINITIONS_HPP
#define OPENMVG_SFM_STELLAR_STELLAR_DEFINITIONS_HPP

#include "openMVG/sfm/pipelines/stellar/relative_scale.hpp"
#include "openMVG/numeric/l1_solver_admm.hpp"

#include <Eigen/Sparse>

namespace openMVG{
namespace sfm{

/// Mode that can be used to solve to rescale the relative translation of a stellar pod
enum class Stellar_Translation_Averaging_Solver_Type
{
  SCALING_SOLVER_L1,
  SCALING_SOLVER_L2,
  SCALING_SOLVER_L2_FULL
};

/// Check if a stellar pod defined by some relative scale is one CC or not
inline bool Relative_scales_are_one_cc
(
  const std::vector<Relative_Scale> & relative_scales
)
{
  // Iterate along the pairs:
  // - check if everytime at least one of the pair id was listed before
  std::set<uint32_t> previous_ids;
  const Pair_Set used_pairs = Relative_Scale::Get_pairs(relative_scales);
  for (const Pair & pair_it : used_pairs)
  {
    if (previous_ids.empty())
    {
      previous_ids = {pair_it.first, pair_it.second};
    }
    else
      if (previous_ids.count(pair_it.first) == 0 &&
          previous_ids.count(pair_it.second) == 0)
        return false;
  }
  return !relative_scales.empty();
}

/// Solve the relative scale to a common coordinate system
/// Since ratios of depth are used, the found solution is an approximation
/// Solving equation (4) from “Global Structure-from-Motion by Similarity Averaging"
/// Zhaopeng Cui and Ping Tan. (ICCV 2015).”
///
inline
bool
Solve_stellar_translation_scales_averaging
(
  const size_t node_id, // Central node
  const std::vector<Relative_Scale> & vec_relative_scales,
  const Hash_Map<Pair, geometry::Pose3> & relative_poses,
  Hash_Map<IndexT, geometry::Pose3> & triplet_pose,
  const Stellar_Translation_Averaging_Solver_Type e_used_solver =
    Stellar_Translation_Averaging_Solver_Type::SCALING_SOLVER_L2_FULL
)
{
  const Pair_Set used_pairs = Relative_Scale::Get_pairs(vec_relative_scales);
  std::cout << "Stellar reconstruction with center node: " << node_id << "\n"
    << "#relative scales: " << vec_relative_scales.size() << "\n"
    << "#pairs : " << used_pairs.size() << std::endl;

  // Assert that the relative scales pose ids defined a unique connected component
  if (!Relative_scales_are_one_cc(vec_relative_scales))
  {
    std::cerr << "The stellar edges are not giving a single connected component." << std::endl;
    return false;
  }

  // Compute a contiguous indexes mapping
  std::map<Pair, unsigned int> pair_to_index;
  {
    unsigned int i = 0;
    for (const Pair & pair_it: used_pairs)
    {
      pair_to_index[pair_it] = i++;
    }
  }

  // Solve the scale to put all the pair in a common global coordinate system
  Vec x_scales;
  switch (e_used_solver)
  {
    case Stellar_Translation_Averaging_Solver_Type::SCALING_SOLVER_L2_FULL:
    {
      // Setup the linear system: lhx X = rhs
      std::vector< Eigen::Triplet<double> > vec_triplets;
      Vec rhs(vec_relative_scales.size());
      unsigned int i = 0;
      for (const Relative_Scale & relative_scales : vec_relative_scales)
      {
        // Write:
        // S_ij / S_il = factor_ijl
        // as the following linear constraint:
        // S_ij - S_il = log(factor_ijl)
        vec_triplets.emplace_back(i, pair_to_index.at(relative_scales.pairs[0]), 1); // row, col, value
        vec_triplets.emplace_back(i, pair_to_index.at(relative_scales.pairs[1]), -1);
        rhs[i] = std::log(relative_scales.ratio);
        ++i;
      }

      sMat lhs;
      lhs.resize(vec_relative_scales.size(), pair_to_index.size());
      lhs.setFromTriplets(vec_triplets.cbegin(), vec_triplets.cend());

      // Use QR
      Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int> > solver(lhs);
      if (solver.info() != Eigen::Success)
      {
        std::cerr << "Sparse matrix cannot be factorized" << std::endl;
        return false;
      }
      const Vec x = solver.solve(rhs);
      if (solver.info() != Eigen::Success)
      {
        std::cerr << "Sparse system cannot be solved" << std::endl;
        return false;
      }

      // Make the log of the distances negative, such that all distances are <= 1
      const double maxLogDistance = x.maxCoeff();
      x_scales = (x.array() - maxLogDistance).array().exp();
    }
    break;

    case Stellar_Translation_Averaging_Solver_Type::SCALING_SOLVER_L2:
    {
      // Setup the linear system: lhx X = rhs
      std::vector< Eigen::Triplet<double> > vec_triplets;
      Vec rhs(vec_relative_scales.size());
      unsigned int i = 0;
      for (const Relative_Scale & relative_scales : vec_relative_scales)
      {
        // Write:
        // S_ij / S_il = factor_ijl
        // as the following linear constraint:
        // S_ij - S_il = log(factor_ijl)
        if (pair_to_index[relative_scales.pairs[0]]!=0)
          vec_triplets.emplace_back(i, pair_to_index.at(relative_scales.pairs[0])-1, 1); // row, col, value
        if (pair_to_index[relative_scales.pairs[1]]!=0)
          vec_triplets.emplace_back(i, pair_to_index.at(relative_scales.pairs[1])-1, -1);
        rhs[i] = std::log(relative_scales.ratio);
        ++i;
      }

      sMat lhs;
      lhs.resize(vec_relative_scales.size(), pair_to_index.size()-1);
      lhs.setFromTriplets(vec_triplets.cbegin(), vec_triplets.cend());

      // Use QR
      Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int> > solver(lhs);
      if (solver.info() != Eigen::Success)
      {
        std::cerr << "Sparse matrix cannot be factorized" << std::endl;
        return false;
      }
      const Vec x = solver.solve(rhs);
      if (solver.info() != Eigen::Success)
      {
        std::cerr << "Sparse system cannot be solved" << std::endl;
        return false;
      }

      // Make the log of the distances negative, such that all distances are <= 1
      Vec xCpy(x.size()+1);
      xCpy << 0.0, x;
      const double maxLogDistance = xCpy.maxCoeff();
      x_scales = (xCpy.array() - maxLogDistance).array().exp();
    }
      break;

    case Stellar_Translation_Averaging_Solver_Type::SCALING_SOLVER_L1:
    {
      // Setup the linear system: lhx X = rhs
      std::vector< Eigen::Triplet<double> > vec_triplets;
      Vec rhs(vec_relative_scales.size());
      unsigned int i = 0;
      for (const Relative_Scale & relative_scales : vec_relative_scales)
      {
        // Write:
        // S_ij / S_il = factor_ijl
        // as the following linear constraint:
        // S_ij - S_il = log(factor_ijl)
        if (pair_to_index[relative_scales.pairs[0]]!=0)
          vec_triplets.emplace_back(i, pair_to_index.at(relative_scales.pairs[0])-1, 1); // row, col, value
        if (pair_to_index[relative_scales.pairs[1]]!=0)
          vec_triplets.emplace_back(i, pair_to_index.at(relative_scales.pairs[1])-1, -1);
        rhs[i] = std::log(relative_scales.ratio);
        ++i;
      }

      sMat lhs;
      lhs.resize(vec_relative_scales.size(), pair_to_index.size()-1);
      lhs.setFromTriplets(vec_triplets.cbegin(), vec_triplets.cend());

      // Try to solve under L1 norm
      openMVG::L1Solver<sMat>::Options opt;
      openMVG::L1Solver<sMat> l1solver(opt, lhs);

      Vec l1solution_vec(pair_to_index.size());
      l1solution_vec.setZero();
      if (l1solver.Solve(rhs, &l1solution_vec))
      {
        // Make the log of the distances negative, such that all distances are <= 1
        Vec x(l1solution_vec.size()+1);
        x << 0.0, l1solution_vec;
        const double maxLogDistance = x.maxCoeff();
        x_scales = (x.array() - maxLogDistance).array().exp();
      }
      else
      {
        std::cerr << "Sparse system cannot be solved" << std::endl;
        return false;
      }
    }
    break;
    default:
      std::cerr << "Unsupported solver" << std::endl;
      return false;
  }

  if (x_scales.size() == 0)
  {
    return false;
  }

  // Upgrade the found stellar reconstruction
  triplet_pose.clear();
  triplet_pose[node_id] = geometry::Pose3(); // Identity

  std::set<unsigned int> stellar_ids;
  for (unsigned int i = 0 ; i < x_scales.size(); ++i)
  {
    // Retrieve the original pair (inverse mapping)
    Pair selected_pair;
    for (const Pair & pair_it: used_pairs)
    {
      if (pair_to_index[pair_it] == i)
      {
        selected_pair = pair_it;
        break;
      }
    }
    std::cout << "Pair: " << selected_pair.first << "," << selected_pair.second << "; scaling: " << x_scales[i] << std::endl;
    stellar_ids.insert(selected_pair.first);
    stellar_ids.insert(selected_pair.second);

    if (relative_poses.count(selected_pair) == 0)
    {
      return false;
    }

    if (selected_pair.first == node_id)
    {
      geometry::Pose3 relative_pose = relative_poses.at(selected_pair);
      relative_pose.center() /= x_scales[i];
      triplet_pose[selected_pair.second] = relative_pose;
    }
    else
    {
      geometry::Pose3 relative_pose = relative_poses.at(selected_pair).inverse();
      relative_pose.center() /= x_scales[i];
      triplet_pose[selected_pair.first] = relative_pose;
    }
  }
  return true;
}

} // namespace sfm
} // namespace openMVG

#endif // OPENMVG_SFM_STELLAR_STELLAR_DEFINITIONS_HPP


/**
 * \ file TanFilter.cpp
 */

#include <array>
#include <fstream>

#include <ATK/config.h>

#include <ATK/Tools/TanFilter.h>
#include <ATK/Mock/SimpleSinusGeneratorFilter.h>

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

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

constexpr gsl::index PROCESSSIZE = 1000;
constexpr gsl::index SAMPLING_RATE = 1024*64;

BOOST_AUTO_TEST_CASE( TanFilter_const_sin1k )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLING_RATE);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::TanFilter<double> filter;
  filter.set_input_sampling_rate(SAMPLING_RATE);
  filter.set_output_sampling_rate(SAMPLING_RATE);

  filter.set_input_port(0, generator, 0);
  filter.process(PROCESSSIZE);

  auto sin = generator.get_output_array(0);
  auto array = filter.get_output_array(0);

  for(size_t i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_CHECK_CLOSE(array[i], std::tan(sin[i] * boost::math::constants::pi<double>() / SAMPLING_RATE), 0.00001);
  }
}


//#include "ps/ps.h"
#include <iostream>
#include <vector>
#include "softmax.h"
#include "prox_l1.h"
#include "prox_l2.h"

#include <Eigen/Dense>

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::VectorXi;
using Eigen::ArrayXd;

using namespace std;
//using namespace ps;

int main() {
  MatrixXd X(3, 4);
  VectorXi y(3);
  vector<double> a = {1, -1, 1};
  X << 1,2,3,4,5,6,7,8,9,10,11,12;
  X = X / 10;
  for (int i = 0; i < a.size(); i++) {
    y(i) = a[i];
  }
  cout << X << endl;
  cout << y << endl;

  // test data copy
  vector<double> vecx(X.rows()*X.cols());
  for (int i = 0; i < X.cols(); i++) {
    VectorXd::Map(&vecx[i*X.rows()], X.rows()) = X.col(i);
  }
  cout << "---------" << endl;
  for (int i = 0; i < vecx.size(); i++) {
    cout << vecx[i] << " ";
  }
  cout << endl;

  // test data copy back
  cout << "---------" << endl;
  for (int i = 0; i < vecx.size(); i++) {
    vecx[i] = vecx[i] * 2;
  }
  for (int i = 0; i < X.cols(); i++) {
    X.col(i) = VectorXd::Map(&vecx[i*X.rows()], X.rows());
  }
  cout << X << endl;

  // test replicate
  cout << "---------" << endl;
  cout << X.rowwise().sum() << endl;
  cout << X.rowwise().sum().replicate(1, X.cols()) << endl;

  // test transpose
  cout << "---------" << endl;
  cout << X.transpose() << endl;
  cout << "---------" << endl;
  cout << X << endl;

  // test softmax
  lrprox::SOFTMAX softmax = lrprox::SOFTMAX(4, 2);
  // one-hot
  MatrixXi y_onehot = softmax.onehot_encoder(y);
//  y_onehot << 0, 1, 0, 1, 0, 1;
//  y_onehot.transposeInPlace();
  cout << "---------" << endl;
  X << 1,2,3,4,5,6,7,8,9,10,11,12;
  X = X / 10;
  cout << X << endl;
  cout << y_onehot << endl;
  for (int i = 1; i <= 3; i++) {
    cout << "--" << endl;
    MatrixXd w(4, 2);
    w.col(0) << 1, 2, 3, 4;
    w.col(1) << 5, 6, 7, 8;
    w = (-w.eval().array()/10 *i).matrix();
    softmax.updateWeight(w);
    cout << softmax.getWeight() << endl;
    cout << softmax.cost(X, y_onehot) << endl;
    cout << softmax.grad(X, y_onehot) << endl;
  }

  // test proximal
  lrprox::prox_l1 prox_op1 = lrprox::prox_l1(0.1);
  lrprox::prox_l2 prox_op2 = lrprox::prox_l2(0.1);
  double gamma = 0.3;
  cout << "---------" << endl;
  VectorXd x(Eigen::Map<VectorXd>(X.data(), X.cols()*X.rows()));
  cout << prox_op1.cost(X) << endl;
  cout << prox_op1.cost(x) << endl;
  cout << prox_op1.proximal(X, gamma) << endl;
  cout << prox_op1.proximal(x, gamma) << endl;
  cout << prox_op2.cost(X) << endl;
  cout << prox_op2.cost(x) << endl;
  cout << prox_op2.proximal(X, gamma) << endl;
  cout << prox_op2.proximal(x, gamma) << endl;

  // test outputWeight
  vector<double> w_vec;
  cout << "---------" << endl;
  softmax.outputWeight(*(&w_vec));
  cout << softmax.getWeight() << endl;
  for (int i = 0; i < w_vec.size(); i++) {
    cout << w_vec[i] << " ";
  }
  cout << endl;
  w_vec[2] = 100;
  softmax.updateWeight(w_vec);
  cout << softmax.getWeight() << endl;
  for (int i = 0; i < w_vec.size(); i++) {
    cout << w_vec[i] << " ";
  }
  cout << endl;
  MatrixXd xtmp = softmax.getWeight();
  xtmp.data()[0] = -100;
  for (int i = 0; i < w_vec.size(); i++) {
    cout << xtmp.data()[i] << " ";
  }
  cout << endl;

  return 0;
}


//  ================================================================
//  Created by Gregory Kramida on 11/2/18.
//  Copyright (c) 2018 Gregory Kramida
//  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.
//  ================================================================
#pragma once

//stdlib
#include <vector>

//libraries
#include <Eigen/Eigen>
#include <boost/property_tree/ptree.hpp>

//local
#include "../../math/stacking.hpp"
#include "../../math/typedefs.hpp"
#include "../../telemetry/convergence_report.hpp"
#include "optimizer2d.hpp"

namespace pt = boost::property_tree;
namespace eig = Eigen;

namespace nonrigid_optimization {
namespace slavcheva{
class SobolevOptimizer2d:
		public Optimizer2d {
public:
	class SobolevParameters {
	public:
		static SobolevParameters& get_instance() {
			static SobolevParameters instance;
			return instance;
		}

		SobolevParameters(const SobolevParameters&) = delete;
		void operator=(SobolevParameters const&) = delete;

		void set_from_json(pt::ptree root);

		eig::VectorXf sobolev_kernel = [] {
			eig::VectorXf sobolev_kernel(7);
			sobolev_kernel << 2.995900285895913839e-04f,
			4.410949535667896271e-03f,
			6.571318954229354858e-02f,
			9.956527948379516602e-01f,
			6.571318954229354858e-02f,
			4.410949535667896271e-03f,
			2.995900285895913839e-04f;
			return sobolev_kernel;
		}();

		eig::VectorXf get_sobolev_kernel();
		void set_sobolev_kernel(eig::VectorXf);

		float smoothing_term_weight = 0.2f;

	private:
		SobolevParameters() = default;
	};

	// *** parameters ***
	static SobolevParameters& sobolev_parameters();
	static SharedParameters& shared_parameters();

	virtual eig::MatrixXf optimize(const eig::MatrixXf& live_field, const eig::MatrixXf& canonical_field) override;
	telemetry::ConvergenceReport2d get_convergence_report();
	eig::MatrixXf get_warp_statistics_as_matrix();

private:
	float perform_optimization_iteration_and_return_max_warp(eig::MatrixXf& warped_live_field,
			math::Vector2i& max_warp_location, const eig::MatrixXf& canonical_field, math::MatrixXv2f& warp_field);
	// *** Logging ***
	telemetry::ConvergenceReport2d convergence_report;
	std::vector<telemetry::WarpDeltaStatistics2d> warp_statistics;

	void clean_out_logs();
};
} //namespace slavcheva
} //namespace nonrigid_optimization


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#include <geometry_test_common.hpp>

#include <boost/concept/requires.hpp>
#include <boost/concept_check.hpp>

#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/strategies/spherical/distance_haversine.hpp>
#include <boost/geometry/strategies/concepts/distance_concept.hpp>

#include <boost/geometry/geometries/point.hpp>

#ifdef HAVE_TTMATH
#  include <boost/geometry/extensions/contrib/ttmath_stub.hpp>
#endif

double const average_earth_radius = 6372795.0;

template <typename Point, typename LatitudePolicy>
struct test_distance
{
    typedef bg::strategy::distance::haversine<double> haversine_type;
    typedef typename bg::strategy::distance::services::return_type<haversine_type, Point, Point>::type return_type;

    BOOST_CONCEPT_ASSERT
        (
            (bg::concept::PointDistanceStrategy<haversine_type, Point, Point>)
        );

    static void test(double lon1, double lat1, double lon2, double lat2,
                       double radius, double expected, double tolerance)
    {
        haversine_type strategy(radius);

        Point p1, p2;
        bg::assign_values(p1, lon1, LatitudePolicy::apply(lat1));
        bg::assign_values(p2, lon2, LatitudePolicy::apply(lat2));
        return_type d = strategy.apply(p1, p2);

        BOOST_CHECK_CLOSE(d, expected, tolerance);
    }
};

template <typename Point, typename LatitudePolicy>
void test_all()
{
    // earth to unit-sphere -> divide by earth circumference, then it is from 0-1,
    // then multiply with 2 PI, so effectively just divide by earth radius
    double e2u = 1.0 / average_earth_radius;

    // ~ Amsterdam/Paris, 467 kilometers
    double const a_p = 467.2704 * 1000.0;
    test_distance<Point, LatitudePolicy>::test(4, 52, 2, 48, average_earth_radius, a_p, 1.0);
    test_distance<Point, LatitudePolicy>::test(2, 48, 4, 52, average_earth_radius, a_p, 1.0);
    test_distance<Point, LatitudePolicy>::test(4, 52, 2, 48, 1.0, a_p * e2u, 0.001);

    // ~ Amsterdam/Barcelona
    double const a_b = 1232.9065 * 1000.0;
    test_distance<Point, LatitudePolicy>::test(4, 52, 2, 41, average_earth_radius, a_b, 1.0);
    test_distance<Point, LatitudePolicy>::test(2, 41, 4, 52, average_earth_radius, a_b, 1.0);
    test_distance<Point, LatitudePolicy>::test(4, 52, 2, 41, 1.0, a_b * e2u, 0.001);
}

template <typename P1, typename P2, typename CalculationType, typename LatitudePolicy>
void test_services()
{
    namespace bgsd = bg::strategy::distance;
    namespace services = bg::strategy::distance::services;

    {

        // Compile-check if there is a strategy for this type
        typedef typename services::default_strategy<bg::point_tag, P1, P2>::type haversine_strategy_type;
    }

    P1 p1;
    bg::assign_values(p1, 4, 52);

    P2 p2;
    bg::assign_values(p2, 2, 48);

    // ~ Amsterdam/Paris, 467 kilometers
    double const km = 1000.0;
    double const a_p = 467.2704 * km;
    double const expected = a_p;

    double const expected_lower = 460.0 * km;
    double const expected_higher = 470.0 * km;

    // 1: normal, calculate distance:

    typedef bgsd::haversine<double, CalculationType> strategy_type;
    typedef typename bgsd::services::return_type<strategy_type, P1, P2>::type return_type;

    strategy_type strategy(average_earth_radius);
    return_type result = strategy.apply(p1, p2);
    BOOST_CHECK_CLOSE(result, return_type(expected), 0.001);

    // 2: the strategy should return the same result if we reverse parameters
    result = strategy.apply(p2, p1);
    BOOST_CHECK_CLOSE(result, return_type(expected), 0.001);

    // 3: "comparable" to construct a "comparable strategy" for P1/P2
    //    a "comparable strategy" is a strategy which does not calculate the exact distance, but
    //    which returns results which can be mutually compared (e.g. avoid sqrt)

    // 3a: "comparable_type"
    typedef typename services::comparable_type<strategy_type>::type comparable_type;

    // 3b: "get_comparable"
    comparable_type comparable = bgsd::services::get_comparable<strategy_type>::apply(strategy);

    // Check vice versa:
    // First the result of the comparable strategy
    return_type c_result = comparable.apply(p1, p2);
    // Second the comparable result of the expected distance
    return_type c_expected = services::result_from_distance<comparable_type, P1, P2>::apply(comparable, expected);
    // And that one should be equa.
    BOOST_CHECK_CLOSE(c_result, return_type(c_expected), 0.001);

    // 4: the comparable_type should have a distance_strategy_constructor as well,
    //    knowing how to compare something with a fixed distance
    return_type c_dist_lower = services::result_from_distance<comparable_type, P1, P2>::apply(comparable, expected_lower);
    return_type c_dist_higher = services::result_from_distance<comparable_type, P1, P2>::apply(comparable, expected_higher);

    // If this is the case:
    BOOST_CHECK(c_dist_lower < c_result && c_result < c_dist_higher);

    // Calculate the Haversine by hand here:
    return_type c_check = return_type(2.0) * asin(sqrt(c_result)) * average_earth_radius;
    BOOST_CHECK_CLOSE(c_check, expected, 0.001);

    // This should also be the case
    return_type dist_lower = services::result_from_distance<strategy_type, P1, P2>::apply(strategy, expected_lower);
    return_type dist_higher = services::result_from_distance<strategy_type, P1, P2>::apply(strategy, expected_higher);
    BOOST_CHECK(dist_lower < result && result < dist_higher);
}

/****
template <typename P, typename Strategy>
void time_compare_s(int const n)
{
    boost::timer t;
    P p1, p2;
    bg::assign_values(p1, 1, 1);
    bg::assign_values(p2, 2, 2);
    Strategy strategy;
    typename Strategy::return_type s = 0;
    for (int i = 0; i < n; i++)
    {
        for (int j = 0; j < n; j++)
        {
            s += strategy.apply(p1, p2);
        }
    }
    std::cout << "s: " << s << " t: " << t.elapsed() << std::endl;
}

template <typename P>
void time_compare(int const n)
{
    time_compare_s<P, bg::strategy::distance::haversine<double> >(n);
    time_compare_s<P, bg::strategy::distance::comparable::haversine<double> >(n);
}

#include <time.h>
double time_sqrt(int n)
{
    clock_t start = clock();

    double v = 2.0;
    double s = 0.0;
    for (int i = 0; i < n; i++)
    {
        for (int j = 0; j < n; j++)
        {
            s += sqrt(v);
            v += 1.0e-10;
        }
    }
    clock_t end = clock();
    double diff = double(end - start) / CLOCKS_PER_SEC;

    std::cout << "Check: " << s << " Time: " << diff << std::endl;
    return diff;
}

double time_normal(int n)
{
    clock_t start = clock();

    double v = 2.0;
    double s = 0.0;
    for (int i = 0; i < n; i++)
    {
        for (int j = 0; j < n; j++)
        {
            s += v;
            v += 1.0e-10;
        }
    }
    clock_t end = clock();
    double diff = double(end - start) / CLOCKS_PER_SEC;

    std::cout << "Check: " << s << " Time: " << diff << std::endl;
    return diff;
}
***/

int test_main(int, char* [])
{
    test_all<bg::model::point<int, 2, bg::cs::spherical_equatorial<bg::degree> >, geographic_policy>();
    test_all<bg::model::point<float, 2, bg::cs::spherical_equatorial<bg::degree> >, geographic_policy>();
    test_all<bg::model::point<double, 2, bg::cs::spherical_equatorial<bg::degree> >, geographic_policy>();

    // NYI: haversine for mathematical spherical coordinate systems
    // test_all<bg::model::point<double, 2, bg::cs::spherical<bg::degree> >, mathematical_policy>();

    //double t1 = time_sqrt(20000);
    //double t2 = time_normal(20000);
    //std::cout << "Factor: " << (t1 / t2) << std::endl;
    //time_compare<bg::model::point<double, 2, bg::cs::spherical<bg::radian> > >(10000);

#if defined(HAVE_TTMATH)
    typedef ttmath::Big<1,4> tt;
    test_all<bg::model::point<tt, 2, bg::cs::spherical_equatorial<bg::degree> >, geographic_policy>();
#endif

    test_services
        <
            bg::model::point<double, 2, bg::cs::spherical_equatorial<bg::degree> >,
            bg::model::point<double, 2, bg::cs::spherical_equatorial<bg::degree> >,
            double,
            geographic_policy
        >();

    return 0;
}


/*
 *  Simulate the real boundary of the region of attraction (ROA)
 *  for the reversed VDP example.
 *
 *  Created by Yinan Li on May 19, 2018.
 *
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>

#include "src/definitions.h"
#include "src/matlabio.h"

/* user defined dynamics */
void vdp(const rocs::Rn &x, rocs::Rn &dxdt, double t) {
    dxdt[0] = -x[1];
    dxdt[1] = x[0] + x[1]*(x[0]*x[0]-1);
}

int main()
{
    double xlb[] = {-4, -4};
    double xub[] = {4, 4};
    double eta[] = {0.001, 0.001};
    double T = 10.0;
    double dt = 0.01;

    rocs::grid xg(2, eta, xlb, xub);
    xg.gridding();

    /* ROA computation by running simulations */
    boost::numeric::odeint::runge_kutta_cash_karp54<rocs::Rn> rk45;
    std::vector<bool> roa(xg._nv);
    rocs::Rn x;
    bool stable;
    for (int i = 0; i < xg._nv; ++i) {

	x = xg._data[i];

	// boost::numeric::odeint::integrate(vdp, x, 0.0, T);
	// boost::numeric::odeint::integrate_adaptive(make_controlled(1E-12, 1E-12, dopri5()), vdp, x, 0.0, T, dt);
	boost::numeric::odeint::integrate_const(rk45, vdp, x, 0.0, T, dt);

	stable = true;
	for (int j = 0; j < xg._dim; ++j) {
	    // std::cout << x[j] << ", ";
	    stable = stable & (x[j] < 0.5) & (x[j] > -0.5);
	}
	// std::cout <<'\n';

	if (stable)
	    roa[i] = true;
    }

    /* write roa boundary to a mat file */
    rocs::matWriter wtr("data_roavdp_real.mat");
    wtr.open();

    wtr.write_boundary(xg, roa, "bd0");
    wtr.close();

    return 0;
}


// This example is heavily based on the tutorial at https://open.gl

// OpenGL Helpers to reduce the clutter
#include "Helpers.h"

// GLFW is necessary to handle the OpenGL context
#include <GLFW/glfw3.h>

// Linear Algebra Library
#include <Eigen/Core>
#include <Eigen/Dense>

// Timer
#include <chrono>

// VertexBufferObject wrapper
VertexBufferObject VBO;

// Contains the vertex positions
Eigen::MatrixXf V(2,3);

// Contains the view transformation
Eigen::Matrix4f view(4,4);

void mouse_button_callback(GLFWwindow* window, int button, int action, int mods)
{
    // Get the position of the mouse in the window
    double xpos, ypos;
    glfwGetCursorPos(window, &xpos, &ypos);

    // Get the size of the window
    int width, height;
    glfwGetWindowSize(window, &width, &height);

    // Convert screen position to world coordinates
    Eigen::Vector4f p_screen(xpos,height-1-ypos,0,1);
    Eigen::Vector4f p_canonical((p_screen[0]/width)*2-1,(p_screen[1]/height)*2-1,0,1);
    Eigen::Vector4f p_world = view.inverse()*p_canonical;

    // Update the position of the first vertex if the left button is pressed
    if (button == GLFW_MOUSE_BUTTON_LEFT && action == GLFW_PRESS)
        V.col(0) << p_world[0], p_world[1];

    // Upload the change to the GPU
    VBO.update(V);
}

void key_callback(GLFWwindow* window, int key, int scancode, int action, int mods)
{
    // Update the position of the first vertex if the keys 1,2, or 3 are pressed
    switch (key)
    {
        case  GLFW_KEY_1:
            V.col(0) << -0.5,  0.5;
            break;
        case GLFW_KEY_2:
            V.col(0) << 0,  0.5;
            break;
        case  GLFW_KEY_3:
            V.col(0) << 0.5,  0.5;
            break;
        default:
            break;
    }

    // Upload the change to the GPU
    VBO.update(V);
}

int main(void)
{
    GLFWwindow* window;

    // Initialize the library
    if (!glfwInit())
        return -1;

    // Activate supersampling
    glfwWindowHint(GLFW_SAMPLES, 8);

    // Ensure that we get at least a 3.2 context
    glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
    glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 2);

    // On apple we have to load a core profile with forward compatibility
#ifdef __APPLE__
    glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
    glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
#endif

    // Create a windowed mode window and its OpenGL context
    window = glfwCreateWindow(640, 480, "Hello World", NULL, NULL);
    if (!window)
    {
        glfwTerminate();
        return -1;
    }

    // Make the window's context current
    glfwMakeContextCurrent(window);

    #ifndef __APPLE__
      glewExperimental = true;
      GLenum err = glewInit();
      if(GLEW_OK != err)
      {
        /* Problem: glewInit failed, something is seriously wrong. */
       fprintf(stderr, "Error: %s\n", glewGetErrorString(err));
      }
      glGetError(); // pull and savely ignonre unhandled errors like GL_INVALID_ENUM
      fprintf(stdout, "Status: Using GLEW %s\n", glewGetString(GLEW_VERSION));
    #endif

    int major, minor, rev;
    major = glfwGetWindowAttrib(window, GLFW_CONTEXT_VERSION_MAJOR);
    minor = glfwGetWindowAttrib(window, GLFW_CONTEXT_VERSION_MINOR);
    rev = glfwGetWindowAttrib(window, GLFW_CONTEXT_REVISION);
    printf("OpenGL version recieved: %d.%d.%d\n", major, minor, rev);
    printf("Supported OpenGL is %s\n", (const char*)glGetString(GL_VERSION));
    printf("Supported GLSL is %s\n", (const char*)glGetString(GL_SHADING_LANGUAGE_VERSION));

    // Initialize the VAO
    // A Vertex Array Object (or VAO) is an object that describes how the vertex
    // attributes are stored in a Vertex Buffer Object (or VBO). This means that
    // the VAO is not the actual object storing the vertex data,
    // but the descriptor of the vertex data.
    VertexArrayObject VAO;
    VAO.init();
    VAO.bind();

    // Initialize the VBO with the vertices data
    // A VBO is a data container that lives in the GPU memory
    VBO.init();

    V.resize(2,3);
    V << 0,  0.5, -0.5, 0.5, -0.5, -0.5;
    VBO.update(V);

    // Initialize the OpenGL Program
    // A program controls the OpenGL pipeline and it must contains
    // at least a vertex shader and a fragment shader to be valid
    Program program;
    const GLchar* vertex_shader =
            "#version 150 core\n"
                    "in vec2 position;"
                    "uniform mat4 view;"
                    "void main()"
                    "{"
                    "    gl_Position = view * vec4(position, 0.0, 1.0);"
                    "}";
    const GLchar* fragment_shader =
            "#version 150 core\n"
                    "out vec4 outColor;"
                    "uniform vec3 triangleColor;"
                    "void main()"
                    "{"
                    "    outColor = vec4(triangleColor, 1.0);"
                    "}";

    // Compile the two shaders and upload the binary to the GPU
    // Note that we have to explicitly specify that the output "slot" called outColor
    // is the one that we want in the fragment buffer (and thus on screen)
    program.init(vertex_shader,fragment_shader,"outColor");
    program.bind();

    // The vertex shader wants the position of the vertices as an input.
    // The following line connects the VBO we defined above with the position "slot"
    // in the vertex shader
    program.bindVertexAttribArray("position",VBO);

    // Save the current time --- it will be used to dynamically change the triangle color
    auto t_start = std::chrono::high_resolution_clock::now();

    // Register the keyboard callback
    glfwSetKeyCallback(window, key_callback);

    // Register the mouse callback
    glfwSetMouseButtonCallback(window, mouse_button_callback);

    // Loop until the user closes the window
    while (!glfwWindowShouldClose(window))
    {
        // Bind your VAO (not necessary if you have only one)
        VAO.bind();

        // Bind your program
        program.bind();

        // Set the uniform value depending on the time difference
        auto t_now = std::chrono::high_resolution_clock::now();
        float time = std::chrono::duration_cast<std::chrono::duration<float>>(t_now - t_start).count();
        glUniform3f(program.uniform("triangleColor"), (float)(sin(time * 4.0f) + 1.0f) / 2.0f, 0.0f, 0.0f);

        // Get size of the window
        int width, height;
        glfwGetWindowSize(window, &width, &height);
        float aspect_ratio = float(height)/float(width); // corresponds to the necessary width scaling

        view <<
        aspect_ratio,0, 0, 0,
        0,           1, 0, 0,
        0,           0, 1, 0,
        0,           0, 0, 1;

        glUniformMatrix4fv(program.uniform("view"), 1, GL_FALSE, view.data());

        // Clear the framebuffer
        glClearColor(0.5f, 0.5f, 0.5f, 1.0f);
        glClear(GL_COLOR_BUFFER_BIT);

        // Draw a triangle
        glDrawArrays(GL_TRIANGLES, 0, 3);

        // Swap front and back buffers
        glfwSwapBuffers(window);

        // Poll for and process events
        glfwPollEvents();
    }

    // Deallocate opengl memory
    program.free();
    VAO.free();
    VBO.free();

    // Deallocate glfw internals
    glfwTerminate();
    return 0;
}


#pragma once

// Some standard functions for dealing with rotation matrices and quaternions

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

using namespace Eigen;

const MatrixXd H = (MatrixXd() << MatrixXd::Zero(1, 3), MatrixXd::Identity(3, 3)).finished();
const MatrixXd T = (MatrixXd()  << 1, MatrixXd::Zero(1, 3), MatrixXd::Zero(3, 1), -MatrixXd::Identity(3, 3)).finished();

MatrixXd hat(VectorXd omega);
MatrixXd L(VectorXd Q);
MatrixXd R(VectorXd Q);
MatrixXd G(VectorXd Q);
MatrixXd G_tilde(VectorXd q);
MatrixXd rho(VectorXd phi);


//#########################################################//
//#                                                       #//
//# gaussian_mixture_models  gaussian.cpp                 #//
//# Roberto Capobianco  <capobianco@dis.uniroma1.it>      #//
//#                                                       #//
//#########################################################//

#include <Eigen/Eigenvalues>
#include <cmath>

#include <particle_filter/gaussian.h>

namespace gmms {
double Gaussian::evaluate_point(const Eigen::VectorXd& pt) const {
    if (pt.size() != dimensionality_) {
        throw std::runtime_error(dimensionality_mismatch());
    }

    Eigen::VectorXd dist = pt - mean_;

    double factor = std::sqrt(
            std::pow(2 * M_PI, dimensionality_) * cov_abs_determinant_);
    double exp = std::exp(-0.5 * dist.transpose() * inv_covariance_ * dist);

    double result = exp / factor;

    if (result == 0) {
        srand(time(NULL));
        double random = ((double) rand() / (RAND_MAX));
        result = random * 1e-15;
    }

    return result;
}

void Gaussian::setCovariance(const Eigen::MatrixXd& covariance) {
    if (covariance.rows() != covariance.cols() ||
            covariance.rows() != dimensionality_) {
        throw std::runtime_error(dimensionality_mismatch());
    }

    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(covariance.cols());
    solver.compute(covariance);

    Eigen::VectorXd eigenvalues = solver.eigenvalues();
    Eigen::MatrixXd eigenvectors = solver.eigenvectors();

    for (int i = 0; i < eigenvalues.rows(); ++i) {
        if (eigenvalues(i) < M_EPS) {
            eigenvalues(i) = M_EPS;
        }
    }

    covariance_ =
            eigenvectors * eigenvalues.asDiagonal() * eigenvectors.inverse();
    double abs_determinant = std::fabs(covariance_.determinant());

    inv_covariance_ = covariance_.inverse();
    cov_abs_determinant_ = abs_determinant;
}

void Gaussian::setMeanCovariance(const Eigen::VectorXd& mean,
        const Eigen::MatrixXd& covariance) {
    int dimensionality = mean.size();
    dimensionality_ = dimensionality;
    setMean(mean);
    setCovariance(covariance);
}

std::string Gaussian::toString() const {
    Eigen::IOFormat mean_format(
            5, 0, ", ", "\n", "[", "]", "Mean:       ", "\n");
    Eigen::IOFormat cov_format(
            5, 0, ", ", "\n", "[", "]", "Covariance: ", "\n");
    std::stringstream gaussian_string;
    gaussian_string << "Gaussian: \n";
    gaussian_string << mean_.format(mean_format);
    gaussian_string << covariance_.format(cov_format);
    return gaussian_string.str();
}

void Gaussian::addToEigenMatrix(Eigen::MatrixXd& matrix,
        float x0,
        float y0,
        float x1,
        float y1,
        int stepcount) const {
    assert(x0 < x1);
    assert(y0 < y1);
    float x_delta = std::abs(x1 - x0);
    float y_delta = std::abs(y1 - y0);
    float x, y;
    float x_mean = mean_(0, 0);
    float y_mean = mean_(1, 0);
    // TODO: save stepsize

    for (int y_step = 0; y_step < stepcount; y_step++) {
        y = y0 + (y_delta / stepcount * y_step);
        for (int x_step = 0; x_step < stepcount; x_step++) {
            x = x0 + (x_delta / stepcount * x_step);
            // Function taken from
            // https://en.wikipedia.org/wiki/Gaussian_function#Two-dimensional_Gaussian_function
            // matrix(y_step, x_step) += 1 * std::exp(-(std::pow(x - x_mean, 2)
            // / covariance_(0, 0)  + 2 * (x - x_mean) * (y - y_mean) /
            // covariance_(1, 0) + std::pow(y - y_mean, 2) / covariance_(1,
            // 1)));
            matrix(y_step, x_step) +=
                    1 * std::exp(-((std::pow(x - x_mean, 2) /
                                           (2 * covariance_(0, 0))) +
                                   (std::pow(y - y_mean, 2) /
                                           (2 * covariance_(1, 1)))));
        }
    }
}

double Gaussian::calcDistance(const Eigen::VectorXd& mean) const {
    if (mean.size() != dimensionality_) {
        throw std::runtime_error(dimensionality_mismatch());
    }
    Eigen::VectorXd diff_vec = mean_ - mean;
    return diff_vec.cwiseAbs().sum() / static_cast<double>(dimensionality_);
}

}  // namespace gmms


#ifndef MCMC_UTILS_CPP_UPDATES_HPP
#define MCMC_UTILS_CPP_UPDATES_HPP

#include <algorithm>
#include <cmath>
#include <numeric>
#include <tuple>
#include <vector>

#include <Eigen/Dense>

namespace updates {
/*
 * Computes the posterior parameters for a model
 *     x_1, ... x_n ~ N(mu, 1 / tau)
 *     \mu | tau ~ N(mu0, 1 / (lambda * tau))
 *     \tau ~ Gamma(a, b)
 *
 * returns an std::vector{mu0_post, a_post, b_post, lambda_post}
 */
std::vector<double> normalGammaUpdate(
    std::vector<double> data, double priorMean, double priorA,
    double priorB, double priorLambda);

/*
 * Computes the posterior parameters of the parameter \beta
 * for a linear regression model (univariate)
 *
 *          y_i = x_i^T \beta + \epsilon_i
 *          epsilon_i ~ N(0, \sigma^2)
 *          \beta ~ N(\mu0, \Lambda)
 *
 * where \Lambda is the precision matrix
 *
 * Then \beta | \sigma^2, y ~ N(mu_post, Lambda_post)
 * where
 *          Lambda_post = \Lambda + X^t X
 *          mu_post = Lambda_post^{-1} (X^t y + \Lambda \mu0)
 */
std::tuple<Eigen::VectorXd, Eigen::MatrixXd> simpleLinearRegressionUpdate(
    Eigen::VectorXd y, Eigen::MatrixXd X, Eigen::VectorXd betaMean,
    Eigen::MatrixXd betaPrec);

/*
 * Computes the posterior parameters of the parameter \beta
 * for a linear regression model (univariate)
 *
 *          y_i = x_i^T \beta + \epsilon_i
 *          epsilon_i ~ N(\mu_i, \sigma_i^2)
 *          \beta ~ N(\mu0, \Lambda)
 *
 * where \Lambda is the precision matrix
 *
 * Then \beta | \sigma^2, y ~ N(mu_post, Lambda_post)
 * where, being V = diag(sigma^2_i)^{-1}
 *
 *          Lambda_post = \Lambda + X^t V X
 *          mu_post = Lambda_post^{-1} (X^t y + \Lambda \mu0)
 */
std::tuple<Eigen::VectorXd, Eigen::MatrixXd> heteroSchedLinearRegressionUpdate(
    Eigen::VectorXd y, Eigen::MatrixXd X, Eigen::VectorXd betaMean,
    Eigen::MatrixXd betaPrec, Eigen::VectorXd mu,
    Eigen::VectorXd V);

}

#endif


#include "setup_modes.h"
#include "to_triplets.h"
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>
#include <igl/writeDMAT.h>
#include <unsupported/Eigen/KroneckerProduct>
#include <json.hpp>

// #include <MatOp/SparseSymMatProd.h>
// #include <MatOp/SparseCholesky.h>
// #include <SymGEigsSolver.h>
// #include <GenEigsSolver.h>
#include <igl/cotmatrix.h>

using namespace Eigen;
using namespace std;
typedef Eigen::Triplet<double> Trip;

using json = nlohmann::json;

void setup_modes(json& j_input, int nummodes, bool reduced, SparseMatrix<double>& mP, SparseMatrix<double>& mA, SparseMatrix<double> mConstrained, SparseMatrix<double> mFree, SparseMatrix<double> mY, MatrixXd& mV, const MatrixXi& mT, VectorXd& mmass_diag, MatrixXd& mG){
        if(nummodes==0 && reduced==false){
            //Unreduced just dont use G
            return;
        }
        if(nummodes==0){
            //reduced, but no modes
            cout<<"reduced, but no modes?"<<endl;
            mG = MatrixXd::Identity(3*mV.rows(), 3*mV.rows());
            return;
        }
        nummodes = std::min(nummodes+25, mA.cols());
        // SparseMatrix<double> L;
        // igl::cotmatrix(mV, mT, L);
        // Eigen::kroneckerProduct(L, Matrix3d::Identity());

        cout<<"+EIG SOLVE"<<endl;
        SparseMatrix<double> K = (mP*mA).transpose()*mP*mA;
        SparseMatrix<double> M(3*mV.rows(), 3*mV.rows());
        for(int i=0; i<mmass_diag.size(); i++){
            M.coeffRef(i,i) = mmass_diag[i];
        }

        cout<<"     eig1"<<endl;
        //Spectra seems to freak out if you use row storage, this copy just ensures everything is setup the way the solver likes
        Eigen::SparseMatrix<double> A = mY.transpose()*K*mY;
        Eigen::SparseMatrix<double> B = mY.transpose()*M*mY;

        cout<<"here1"<<endl;
        double shift = 1e-6;
        Eigen::SparseMatrix<double> K1 = A + shift*B;
        Eigen::SparseMatrix<double> M1 = B;
        cout<<"here2"<<endl;

        Spectra::SparseSymMatProd<double>Aop(M1);
        SparseMatrix<double> Kt = K1.transpose();
        // SparseMatrix<double> symK = -.5*(K1+Kt);
        Spectra::SparseCholesky<double> Bop(K1);
        cout<<"here3"<<endl;

        Spectra::SymGEigsSolver<double, Spectra::LARGEST_MAGN, Spectra::SparseSymMatProd<double>, Spectra::SparseCholesky<double>, Spectra::GEIGS_CHOLESKY>geigs(&Aop, &Bop, nummodes, std::min(5*nummodes, A.rows()));
        geigs.init();
        cout<<"     eig2"<<endl;
        int nconv = geigs.compute();
        cout<<"     eig3"<<endl;

        VectorXd eigsCorrected;
        eigsCorrected.resize(geigs.eigenvalues().rows());
        MatrixXd evsCorrected = geigs.eigenvectors();
        cout<<"     eig3.5"<<endl;
        if(geigs.info() == Spectra::SUCCESSFUL)
        {
            for(unsigned int ii=0; ii<geigs.eigenvalues().rows(); ++ii) {
                eigsCorrected[ii] = -(static_cast<double>(1)/(geigs.eigenvalues()[ii]) + shift);
                evsCorrected.col(ii) /= sqrt(geigs.eigenvectors().col(ii).transpose()*M1*geigs.eigenvectors().col(ii));
            }

        }
        else
        {
            cout<<"EIG SOLVE FAILED: "<<endl<<geigs.info()<<endl;
            exit(0);
        }

        cout<<"     eig4"<<endl;
        // eigenvalues.head(eigenvalues.size() - 3));
        mG = evsCorrected.leftCols(nummodes-25);
        std::string outputfile = j_input["output"];
        igl::writeDMAT(outputfile+"/"+to_string((int)j_input["number_modes"])+"modes.dmat", evsCorrected);
        cout<<"-EIG SOLVE"<<endl;
        return;

        // //############handle modes KKT solve#####
        // cout<<"+ModesForHandles"<<endl;
        // SparseMatrix<double> C = mConstrained.transpose();
        // SparseMatrix<double> HandleModesKKTmat(K.rows()+C.rows(), K.rows()+C.rows());
        // HandleModesKKTmat.setZero();
        // std::vector<Trip> KTrips = to_triplets(K);
        // std::vector<Trip> CTrips = to_triplets(C);
        // cout<<"     eig5"<<endl;
        // for(int i=0; i<CTrips.size(); i++){
        //     int row = CTrips[i].row();
        //     int col = CTrips[i].col();
        //     int val = CTrips[i].value();
        //     KTrips.push_back(Trip(row+K.rows(), col, val));
        //     KTrips.push_back(Trip(col, row+K.cols(), val));
        // }
        // KTrips.insert(KTrips.end(),CTrips.begin(), CTrips.end());
        // HandleModesKKTmat.setFromTriplets(KTrips.begin(), KTrips.end());

        // cout<<"     eig6"<<endl;
        // SparseMatrix<double>eHconstrains(K.rows()+C.rows(), C.rows());
        // eHconstrains.setZero();
        // std::vector<Trip> eHTrips;
        // for(int i=0; i<C.rows(); i++){
        //     eHTrips.push_back(Trip(i+K.rows(), i, 1));
        // }
        // eHconstrains.setFromTriplets(eHTrips.begin(), eHTrips.end());

        // cout<<"     eig7"<<endl;
        // SparseLU<SparseMatrix<double>> solver;
        // solver.compute(HandleModesKKTmat);
        // SparseMatrix<double> eHsparse = solver.solve(eHconstrains);
        // MatrixXd eH = MatrixXd(eHsparse).topRows(K.rows());
        // cout<<"-ModesForHandles"<<endl;

        // //###############QR get orth basis of Modes, eH#######
        // MatrixXd eHeV(eH.rows(), eH.cols()+eV.cols());
        // eHeV<<eV,eH;
        // igl::writeDMAT("TOQR.dmat", eHeV);
        // HouseholderQR<MatrixXd> QR(eHeV);
        // cout<<"     eig8"<<endl;
        // MatrixXd thinQ = MatrixXd::Identity(eHeV.rows(), eHeV.cols());
        // //SET Q TO G
        // mG = QR.householderQ()*thinQ;
        // return;
        // cout<<"     eig9"<<endl;
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
namespace boost_int = boost::multiprecision;
using booint = boost_int::cpp_int;
using namespace std;
int main() {
    long long int N, A, B; cin >> N >> A >> B;
    booint ans = (B - A) * (N - 2) + 1;
    if ((N == 1 && A != B) || A > B) cout << 0 << endl;
    else if (N == 1) cout << 1 << endl;
    else cout << ans << endl;
}


/*******************************************************************************
 *         Copyright 2003-2012 LASMEA UMR 6602 CNRS/U.B.P
 *         Copyright 2011-2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#ifndef NT2_TOOLBOX_LINALG_FUNCTIONS_DETAILS_LU_HPP_INCLUDED
#define NT2_TOOLBOX_LINALG_FUNCTIONS_DETAILS_LU_HPP_INCLUDED

#include <nt2/sdk/error/warning.hpp>
#include <nt2/include/functions/triu.hpp>
#include <nt2/include/functions/tri1l.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/width.hpp>
#include <nt2/include/functions/numel.hpp>
#include <nt2/include/functions/diag_of.hpp>
#include <nt2/include/functions/eye.hpp>
#include <nt2/include/functions/expand.hpp>
#include <nt2/include/functions/rif.hpp>
#include <nt2/include/functions/max.hpp>
#include <nt2/include/functions/rec.hpp>
#include <nt2/include/functions/norm.hpp>
#include <nt2/include/functions/prod.hpp>
#include <nt2/include/functions/eps.hpp>
#include <nt2/include/functions/frexp.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/sb2b.hpp>
#include <nt2/include/functions/is_eqz.hpp>
#include <nt2/include/functions/isempty.hpp>
#include <nt2/include/functions/issquare.hpp>
#include <nt2/include/functions/if_one_else_zero.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/toolbox/linalg/details/utility/options.hpp>
#include <nt2/toolbox/linalg/details/utility/workspace.hpp>
#include <nt2/toolbox/linalg/details/lapack/getrf.hpp>
#include <nt2/toolbox/linalg/details/lapack/gecon.hpp>
#include <nt2/toolbox/linalg/details/lapack/getri.hpp>
#include <nt2/toolbox/linalg/details/lapack/gesvx.hpp>
#include <nt2/toolbox/linalg/details/lapack/lange.hpp>
#include <boost/dispatch/details/ignore_unused.hpp>
#include <nt2/core/container/table/table.hpp>

// TODO:
// these are the kind of syntaxes to be enforced by nt2::chol
//  lu     lu factorization.
//     [l,u] = lu(a) stores an upper triangular matrix in u and a
//     "psychologically lower triangular matrix" (i.e. a product of lower
//     triangular and permutation matrices) in l, so that a = l*u. a can be
//     rectangular.
// -> {pl, u]

//     [l,u,p] = lu(a) returns unit lower triangular matrix l, upper
//     triangular matrix u, and permutation matrix p so that p*a = l*u.
// -> {l, u, p]

//     [l,u,p] = lu(a,'vector') returns the permutation information as a
//     vector instead of a matrix.  that is, p is a row vector such that
//     a(p,:) = l*u.  similarly, [l,u,p] = lu(a,'matrix') returns a
//     permutation matrix p.  this is the default behavior.
// -> {l, u, ip]

////////////////////////////////////////////////////////////////////////////////////////
// The class provides:
// constructor from an expression or matrix a
//
// accesors to l, u, p, pl and ip
//
// l is lower unittriangular part of lu_
// u is upper triangular part of lu_
// p is a permutation matrix
// pl is l permuted by p
// ip is the permutation index vector defining p
// we have l*u =  p*a
//         pl*u =  a
/////////////////////////////////////////////////// TODO  perhaps l and u could return an expresiion rather than a matrix
//
// the class allow to compute
//
// status the plu status from lapack
//
// rank the matrix rank
//
// When a is square
// det and absdet determinant and absolute value of the determinant
// They have two syntaxes
//  1 - d =  f.det()   or d =  f.absdet()
//  2 - m =  f.det(e)  or m =  f.absdet(e)
// In the second case m is a mantissa and e and exponent
// If there is no overflow or undeflow in e the determinant is then equal to ldexp(m, e)
//
// rcond the reciprocal condition number of a
//
// Two solvers
// solve and inplace_solve
//
// a matrix inversion
// inv
// inv() can emit a warning if rcond is bad
// inv(false) never warns
//     please avoid using inv if you need not the inverse coefficients.

namespace nt2 { namespace details
{
  template<class T> struct lu_result
  {
    typedef typename meta::strip<T>::type                   source_t;
    typedef typename source_t::value_type                     type_t;
    typedef typename meta::as_integer<type_t, signed>::type  itype_t;
    typedef typename source_t::index_type                    index_t;
    typedef typename meta::as_real<type_t>::type              base_t;
    typedef T                                                 data_t;
    typedef nt2::table<type_t,nt2::matlab_index_>              tab_t;
    typedef nt2::table<base_t,nt2::matlab_index_>             btab_t;
    typedef nt2::table<itype_t,nt2::matlab_index_>            itab_t;
    typedef nt2::details::workspace<type_t>              workspace_t;
    typedef nt2::table<nt2_la_int,nt2::matlab_index_>         ibuf_t;
    typedef nt2::table<type_t,index_t>                   result_type;

    template<class Input>
    lu_result ( Input& xpr
                , typename boost::
                disable_if_c<boost::is_same<lu_result,Input>::value>::type* = 0
      )
      : a_(xpr)
      , lu_(xpr)
      , m_( nt2::height(xpr) )
      , n_( nt2::width(xpr)  )
      , ldlu_( lu_.leading_size() )
      , ipiv_(nt2::of_size(nt2::min(n_, m_), 1))
      , info_(0)
      , p_(of_size(0, 1))
      , ip_(of_size(0, 1))
      , pl_(of_size(0, 1))
      , invt_(of_size(0, 1))
    {
      nt2::details::getrf(&m_, &n_, lu_.raw(), &ldlu_, ipiv_.raw(), &info_, w_);
    }

    lu_result(lu_result const& src)
      : a_(src.a_) , lu_(src.lu_), m_( src.m_ ), n_( src.n_ )
      , ldlu_( src.ldlu_ ) , ipiv_(src.ipiv_)
      , info_(src.info_) , w_(src.w_)
      , p_(src.p_), ip_(src.ip_), pl_(src.pl_), invt_(src.invt_)
    {}

    lu_result& operator=(lu_result const& src)
    {
      a_      = src.a_;
      lu_     = src.lu_;
      m_      = src.m_;
      n_      = src.n_;
      ldlu_   = src.ldlu_;
      ipiv_   = src.ipiv_;
      info_   = src.info_;
      w_      = src.w_;
      p_      = src.p_;
      ip_     = src.ip_;
      pl_     = src.pl_;
      invt_   = src.invt_;
      return *this;
    }

    //==========================================================================
    // Return raw values
    //==========================================================================
    const tab_t& values() const { return lu_; }

    //==========================================================================
    // Return raw values
    //==========================================================================
    const tab_t& original() const { return a_; }

    //==========================================================================
    // Return u part of the decomposition
    //==========================================================================
    typedef typename meta::call < tag::colon_(ptrdiff_t, ptrdiff_t)>::type                          u_T2;
    typedef typename meta::call < tag::function_(tab_t const&, u_T2, nt2::container::colon_)>::type u_T0;
    typedef typename meta::call < tag::triu_(u_T0)>::type                                           u_result;
    u_result u() const
    {
      ptrdiff_t mm =  std::min(n_, m_);
      return nt2::triu(lu_(_(ptrdiff_t(1), mm),_));
    }
    //==========================================================================
    // Return l part of the decomposition
    //==========================================================================
    typedef typename meta::call < tag::colon_(ptrdiff_t, ptrdiff_t)>::type                          l_T2;
    typedef typename meta::call < tag::function_(tab_t const&, nt2::container::colon_, l_T2)>::type l_T0;
    typedef typename meta::call < tag::tri1l_(l_T0)>::type                                          l_result;
    l_result l() const
    {
      ptrdiff_t mm =  std::min(n_, m_);
      return nt2::tri1l(lu_(_,_(ptrdiff_t(1), mm)));
    }

    //==========================================================================
    // Return p part of the decomposition as a matrix such that p*a = l*u
    //==========================================================================
    const tab_t& p()
    {
      if (isempty(p_))
      {
        std::size_t mm = nt2::numel(ipiv_);
        p_ = nt2::eye(mm, mm, meta::as_<type_t>());
        for(size_t i=1; i <= mm; ++i)
          // p_({i, ipiv_(i)}, _) =  p_({ipiv_(i),i}, _)
        {
          tab_t c = p_(i, _);
          p_(i,_) = p_(ipiv_(i),_);
          p_(ipiv_(i),_) = c;
        }
      }
      return p_;
    }
    //==========================================================================
    // Return p part of the decomposition as a vector
    //==========================================================================
    const itab_t& ip()
    {
      if (isempty(ip_))
      {
        //      itab_t ip = itab_t(ipiv_.raw(), ipiv_.raw()+numel(ipiv_));
        ip_.resize(of_size(1, numel(ipiv_)));
        for(size_t i=1; i <= numel(ipiv_); ++i) ip_(i) = ipiv_(i);
      }
      return ip_;
    }

    //==========================================================================
    // Return tpl part of the decomposition  a =  tpl*u (pl = tp *l)
    //==========================================================================
    const tab_t& pl()
    {
      if (isempty(pl_))
      {
        //    return trans(p())*l;
        std::size_t mm = nt2::numel(ipiv_); //incorrect
        pl_ = l();
        for(size_t i=1; i <= mm; ++i)
          // pp({i, ipiv_(i)}, _) =  pp({ipiv_(i),i}, _)
        {
          tab_t c = pl_(ipiv_(i), _);
          pl_(ipiv_(i),_) = pl_(i,_);
          pl_(i,_) = c;
        }
      }
      return pl_;
    }

    //==========================================================================
    // Return post-computation status
    //==========================================================================
    nt2_la_int  status() const { return info_; }

    //==========================================================================
    // Reverse conditioning evaluation
    //==========================================================================
    base_t rcond(char c = '1')
    {
      /* this method which is presumably faster provides results that depend on
      // the lapack used version
      // it seems that gecon is buggy (J.T.L. 28/2/2013)
      //
      //   base_t rc = 0;
      //   tab_t aa = a_;
      //   char norm = (c == 1) ? '1' : ((c == 0) ? 'o' : c);
      //   base_t anorm = nt2::details::lange(&norm,  &n_,  &n_, aa.raw(), &ldlu_);
      //   nt2::details::gecon(&norm, &n_,  lu_.raw(), &ldlu_, &anorm, &rc, &info_);
      //   return rc;
      //
        So we switch to a direct computation
      */
      return nt2::rec(nt2::norm(a_, c)*nt2::norm(this->inv(false), c));
    }

    //==========================================================================
    // system rank
    //==========================================================================
    size_t rank(base_t epsi = nt2::Eps<base_t>()) //provisouare
    {
      //int32_t r = 0;
      base_t thresh = nt2::max(n_, m_)*epsi*nt2::max(nt2::abs(nt2::diag_of(lu_)(_)));
      return  size_t(sum(if_one_else_zero(gt(nt2::diag_of(lu_), thresh))(_)));

      //       for(int i=1; i <= nt2::min(n_, m_); ++i)
      //         {
      //           if(nt2::abs(lu_(i, i)) > thresh) ++r;
      //         }
      //       return r;
      //      nt2::inbtrue(nt2::abs(diag_of(lu_)) > nt2::max(n_, m_)*epsi*nt2::max(abs(diag_of(lu_()))) );
    }

    base_t absdet()
    {
      BOOST_ASSERT_MSG(m_ == n_, "non square matrix in determinant computation");
      return nt2::prod(nt2::abs(nt2::diag_of(lu_)(_)));
    }

    type_t signdet(bool check = true){
      BOOST_ASSERT_MSG(m_ == n_, "non square matrix in determinant computation");
      boost::dispatch::ignore_unused(check);
      //if (check)     BOOST_ASSERT_MSG(is_real<type_t>::value, "determinant sign is not avalaible for complex matrices");
      //count modulo 2 the number of ipiv_ elements such that ipiv_(i) !=  i
      //return nt2::sum(nt2::sb2b(ipiv_ != cif(numel(ipiv_), 1, meta::as_<itype_t>())))&1 ? Mone<type_t>() : One<type_t>();
      type_t s = One<type_t>();
      const nt2_la_int num = numel(ipiv_);
      for(nt2_la_int i=1; i < num ; ++i)
      {
        if (ipiv_(i) !=  i) s = -s;
      }
      return s;
    }

    type_t det(){
      BOOST_ASSERT_MSG(m_ == n_, "non square matrix in determinant computation");
      //     BOOST_ASSERT_MSG(is_real<type_t>::value, "determinant sign is not avalaible for complex matrices");
      //count modulo 2 the number of ipiv_ elements such that ipiv_(i) !=  i
      return  nt2::prod(nt2::diag_of(lu_)(_))*signdet(false);
    }

    type_t absdet(itype_t & exponent)
    {
      BOOST_ASSERT_MSG(m_ ==  n_, "non square matrix in determinant computation");
      // compute e and return m for matrix determinant such that |det| = ldexp(m, e)
      // if no overflow or underflow can occur,  with 0.5 < abs(m) < 1
      // the exponent result is enough to know the order of magnitude of the determinant
      // (between 0.5*2^e and 2^e if the mantissa is non zero)
      // This routine is inspired from linpack http://www.netlib.org/linpack/dgedi.f
      // that use ten power factor instead
      type_t   m1 = One<type_t>();
      exponent = Zero<itype_t>();
      for(size_t i = 1;  i <= size_t(n_); ++i)
      {
        itype_t e;
        m1 *=  nt2::abs(nt2::frexp(nt2::abs(lu_(i, i)), e));
        exponent+= e;
      }
      if (is_eqz(m1)){
        exponent = Zero<itype_t>();
      }
      return m1;
    }

    type_t det(itype_t & exponent)
    {
      BOOST_ASSERT_MSG(m_ ==  n_, "non square matrix in determinant computation");
      // compute e and m for matrix determinant such that det = ldexp(m, e)
      // if no overflow or underflow can occur,  with 0.5 < abs(m) < 1
      // the exponent result is enough to know the order of magnitude of the determinant
      // (between 0.5*2^e and 2^e if the mantissa is non zero)
      // This routine is inspired from linpack http://www.netlib.org/linpack/dgedi.f
      // that use ten power factor instead
      type_t   m1 = One<type_t>();
      exponent = Zero<itype_t>();
      for(size_t i = 1;  i <= size_t(n_); ++i)
      {
        itype_t e;
        m1 *=  nt2::frexp(nt2::abs(lu_(i, i)), e);
        exponent+= e;
      }
      if (is_eqz(m1)){
        exponent = Zero<itype_t>();
      }
      return m1*signdet(false);
    }

    //==========================================================================
    // Solver interface
    //==========================================================================
    template<class XPR> result_type solve(const XPR& b )
    {
      result_type bb = b;
      inplace_solve(bb);
      return bb;
    }

    //==========================================================================
    // inverse matrix: DO NOT USE THAT TO SOLVE A SYSTEM
    //==========================================================================
    const tab_t& inv(bool warn = true)
    {
      if(isempty(invt_))
      {
        if (warn)
        {
          base_t rc = rcond();
          NT2_WARNING ( (rc >= nt2::Eps<base_t>())
                        , "Matrix is close to singular or badly scaled."
                        " Results may be inaccurate."
            );
          return invt_;  /* it has been calculated by rcond */
        }
        invt_ = lu_;
        nt2::details::getri(&n_, invt_.raw(), &ldlu_, ipiv_.raw(), &info_, w_);
      }
      return invt_;
    }

    template<class Xpr> void inplace_solve(Xpr& b )
    {
      BOOST_ASSERT_MSG(issquare(a_), "matrix must be square to use the lu solver");
      nt2_la_int nrhs = nt2::size(b, 2);
      nt2_la_int ldb  = b.leading_size();
      tab_t x(b);
      nt2_la_int ldx  = x.leading_size();
      btab_t ferr(of_size(nrhs, 1)), berr(of_size(nrhs, 1));
      btab_t r_(of_size(n_, 1)), c_(of_size(n_, 1));
      char equed = 'N';
      base_t rc = rcond();
      nt2::details::gesvx(nt2::details::lapack_option('F'),
                          nt2::details::lapack_option('N'),
                          &n_, &nrhs,
                          a_.raw(), &ldlu_,
                          lu_.raw(), &ldlu_,
                          ipiv_.raw(),
                          &equed,
                          r_.raw(),
                          c_.raw(),
                          b.raw(), &ldb,
                          x.raw(), &ldx,
                          &rc,
                          ferr.raw(),
                          berr.raw(),
                          &info_,
                          w_);
      b = x;
    }

  private:
    data_t                            a_;
    tab_t                            lu_;
    nt2_la_int                     m_,n_;
    nt2_la_int                     ldlu_;
    ibuf_t                         ipiv_;
    nt2_la_int                     info_;
    workspace_t                       w_;
    tab_t                             p_;
    itab_t                           ip_;
    tab_t                            pl_;
    tab_t                          invt_;
  };
} }

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_SSE_SSE2_AVERAGE_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_SSE_SSE2_AVERAGE_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/arithmetic/functions/average.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/bitwise_xor.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/shrai.hpp>
#include <boost/simd/include/constants/half.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::average_, boost::simd::tag::sse2_,
                          (A0),
                          ((simd_<arithmetic_<A0>,boost::simd::tag::sse_>))
                          ((simd_<arithmetic_<A0>,boost::simd::tag::sse_>))
                         )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return b_and(a0, a1)+shrai(b_xor(a0, a1),1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::average_, boost::simd::tag::sse2_,
                          (A0),
                          ((simd_<uint16_<A0>,boost::simd::tag::sse_>))
                          ((simd_<uint16_<A0>,boost::simd::tag::sse_>))
                         )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return b_and(a0, a1)+shrai(b_xor(a0, a1),1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::average_, boost::simd::tag::sse2_,
                          (A0),
                          ((simd_<uint8_<A0>,boost::simd::tag::sse_>))
                          ((simd_<uint8_<A0>,boost::simd::tag::sse_>))
                         )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return b_and(a0, a1)+shrai(b_xor(a0, a1),1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::average_, boost::simd::tag::sse2_,
                          (A0),
                          ((simd_<floating_<A0>,boost::simd::tag::sse_>))
                          ((simd_<floating_<A0>,boost::simd::tag::sse_>))
                         )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
       return (a0+a1)*boost::simd::Half<A0>();
    }
  };
} } }
#endif
#endif


#include "energies.h"

#include "bond.h"

#include <Eigen/Dense>
#include <memory>
#include <vector>
#include <algorithm>
#include <numeric>

double calculate_kinetic_energy(const Eigen::MatrixXd& velocities, const Eigen::MatrixXd& masses) {
    return (masses.transpose()  * velocities.rowwise().squaredNorm()).sum() / 2.0;
}

double calculate_potential_energy(const std::vector<std::unique_ptr<Bond>>& bonds, const Eigen::MatrixXd& positions) {
    std::vector<double> potential_energies;
    potential_energies.reserve(bonds.size());
    std::transform(bonds.begin(), bonds.end(), std::back_inserter(potential_energies),
    [positions](const auto& bond){ return bond->energy(positions);});

    return std::accumulate(potential_energies.begin(),
                                                  potential_energies.end(),
                                                    0.0
                                                   );
}


#ifndef RADIUMENGINE_VECTOR_HPP
#define RADIUMENGINE_VECTOR_HPP

/// This file contains definitions of aliases for basic vector classes and functions

#include <Core/RaCore.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Sparse>
#include <functional>
#include <unsupported/Eigen/AlignedVector3>

#include <Core/Math/Math.hpp>

// General config
// Use this to force vec3 to be aligned for vectorization (FIXME not working yet)
// #define CORE_USE_ALIGNED_VEC3

namespace Ra {
namespace Core {
namespace Math {
//
// Common vector types
//
using VectorN = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
using VectorNf = Eigen::VectorXf;
using VectorNd = Eigen::VectorXd;

using Vector4 = Eigen::Matrix<Scalar, 4, 1>;
using Vector4f = Eigen::Vector4f;
using Vector4d = Eigen::Vector4d;

#ifndef CORE_USE_ALIGNED_VEC3
using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
using Vector3f = Eigen::Vector3f;
using Vector3d = Eigen::Vector3d;
#else
using Vector3 = Eigen::AlignedVector3<Scalar>;
using Vector3f = Eigen::AlignedVector3<float>;
using Vector3d = Eigen::AlignedVector3<double>;
#endif

using Vector2 = Eigen::Matrix<Scalar, 2, 1>;
using Vector2f = Eigen::Vector2f;
using Vector2d = Eigen::Vector2d;

using VectorNi = Eigen::VectorXi;
using Vector2i = Eigen::Vector2i;
using Vector3i = Eigen::Vector3i;
using Vector4i = Eigen::Vector4i;

using VectorNui = Eigen::Matrix<uint, Eigen::Dynamic, 1>;
using Vector2ui = Eigen::Matrix<uint, 2, 1>;
using Vector3ui = Eigen::Matrix<uint, 3, 1>;
using Vector4ui = Eigen::Matrix<uint, 4, 1>;

//
// Common matrix types
//

using MatrixN = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
using Matrix3 = Eigen::Matrix<Scalar, 3, 3>;
using Matrix2 = Eigen::Matrix<Scalar, 2, 2>;

using MatrixNf = Eigen::MatrixXf;
using Matrix4f = Eigen::Matrix4f;
using Matrix3f = Eigen::Matrix3f;
using Matrix2f = Eigen::Matrix2f;

using MatrixNd = Eigen::MatrixXd;
using Matrix4d = Eigen::Matrix4d;
using Matrix3d = Eigen::Matrix3d;
using Matrix2d = Eigen::Matrix2d;

using MatrixNui = Eigen::Matrix<uint, Eigen::Dynamic, Eigen::Dynamic>;

// using Diagonal = Eigen::DiagonalMatrix< Scalar, Eigen::Dynamic >;
using Diagonal =
    Eigen::SparseMatrix<Scalar>; // Not optimized for Diagonal matrices, but the operations between
                                 // Sparse and Diagonal are not defined
using Sparse = Eigen::SparseMatrix<Scalar>;

//
// Transforms and rotations
//

using Quaternion = Eigen::Quaternion<Scalar>;
using Quaternionf = Eigen::Quaternionf;
using Quaterniond = Eigen::Quaterniond;

using Transform = Eigen::Transform<Scalar, 3, Eigen::Affine>;
using Transformf = Eigen::Affine3f;
using Transformd = Eigen::Affine3d;

using Aabb = Eigen::AlignedBox<Scalar, 3>;
using Aabbf = Eigen::AlignedBox3f;
using Aabbd = Eigen::AlignedBox3d;

using AngleAxis = Eigen::AngleAxis<Scalar>;
using AngleAxisf = Eigen::AngleAxisf;
using AngleAxisd = Eigen::AngleAxisd;

using Translation = Eigen::Translation<Scalar, 3>;
using Translationf = Eigen::Translation3f;
using Translationd = Eigen::Translation3d;

inline void print( const MatrixN& matrix );

//
// Geometry types
//

using Line2 = Eigen::ParametrizedLine<Scalar, 2>;
using Line3 = Eigen::ParametrizedLine<Scalar, 3>;
using Plane3 = Eigen::Hyperplane<Scalar, 3>;

// Todo : storage transform using quaternions ?

//
// Misc types
//
using Color = Vector4;
using Colorf = Vector4f;
using Colord = Vector4d;

//
// Vector Functions
//
namespace Vector {

/// Component-wise floor() function on a floating-point vector.
template <typename Vector>
inline Vector floor( const Vector& v );

/// Component-wise ceil() function on a floating-point vector.
template <typename Vector>
inline Vector ceil( const Vector& v );

/// Component-wise trunc() function on a floating-point vector
template <typename Vector>
inline Vector trunc( const Vector& v );

/// Component-wise clamp() function on a floating-point vector.
template <typename Vector>
inline Vector clamp( const Vector& v, const Vector& min, const Vector& max );

/// Component-wise clamp() function on a floating-point vector.
template <typename Vector>
inline Vector clamp( const Vector& v, const Scalar& min, const Scalar& max );

/// Vector range check, works for any numeric vector.
template <typename Vector_>
inline bool checkRange( const Vector_& v, const Scalar& min, const Scalar& max );

/// Call std::isnormal on vector entries.
template <typename Vector_>
inline bool checkInvalidNumbers( Eigen::Ref<const Vector_> v, const bool FAIL_ON_ASSERT = false );

/// Get two vectors orthogonal to a given vector.
inline void getOrthogonalVectors( const Vector3& fx, Eigen::Ref<Vector3> fy,
                                  Eigen::Ref<Vector3> fz );

/// Get the angle between two vectors. Works for types where the cross product is
/// defined (i.e. 2D and 3D vectors).
template <typename Vector_>
inline Scalar angle( const Vector_& v1, const Vector_& v2 );

/// Get the spherical linear interpolation between two unit non-colinear vectors.
/// works for types where the cross-product is defined (i.e. 2D and 3D vectors).
template <typename Vector_>
inline Vector_ slerp( const Vector_& v1, const Vector_& v2, Scalar t );

/// @return the projection of point on the plane define by plane and planeNormal
inline Vector3 projectOnPlane( const Vector3& planePos, const Vector3& planeNormal,
                               const Vector3& point );

/// Get the cotangent of the angle between two vectors. Works for vector types where
/// dot and cross product is defined (2D or 3D vectors).
template <typename Vector_>
inline Scalar cotan( const Vector_& v1, const Vector_& v2 );

/// Get the cosine of the angle between two vectors.
template <typename Vector_>
inline Scalar cos( const Vector_& v1, const Vector_& v2 );

/// Normalize a vector and returns its norm before normalization.
/// If the vector's norm is 0, the vector's components will be overwritten by NaNs
template <typename Vector_>
inline Scalar getNormAndNormalize( Vector_& v );

/// Normalize a vector and returns its norm before normalization.
/// If the vector's norm is 0, the vector remains null
template <typename Vector_>
inline Scalar getNormAndNormalizeSafe( Vector_& v );

} // namespace Vector

namespace MatrixUtils {
inline Matrix4 lookAt( const Vector3& position, const Vector3& target, const Vector3& up );
inline Matrix4 perspective( Scalar fovy, Scalar aspect, Scalar near, Scalar zfar );
inline Matrix4 orthographic( Scalar left, Scalar right, Scalar bottom, Scalar top, Scalar near,
                             Scalar zfar );

/// Call std::isnormal on matrix entry.
/// Dense version
template <typename Matrix_>
inline bool checkInvalidNumbers( Eigen::Ref<const Matrix_> matrix,
                                 const bool FAIL_ON_ASSERT = false );

} // namespace MatrixUtils

//
// Quaternion functions
//
namespace QuaternionUtils {
// Define functions for multiplying a quaternion by a scalar
// and adding two quaternions. While Quaternion is supposed to
// represent a unit quaternion (thus a valid rotation), these functions
// are useful for linear interpolation of quaternions.

/// Returns the quaternion q multipled by a scalar factor of k.
inline Quaternion scale( const Quaternion& q, const Scalar k );

/// Returns the sum of two quaternions.
inline Quaternion add( const Quaternion& q1, const Quaternion& q2 );

/// Returns the sum of two quaternions, resolving antipodality by flipping
/// the sign of q2 if q1.q2 is negative. This operation is usually
/// denoted as a circled + sign, forming the basis of the QLERP algorithm.
/// See "Spherical Blend Skinning" (Kavan & Zara 2005) for more details.
inline Quaternion addQlerp( const Quaternion& q1, const Quaternion& q2 );

// Note : the .inl file also define operator+ for quaternions
// and operator * and / between quaternions and scalar.

/// Decompose a given rotation Qin into a swing rotation and a twist rotation.
/// Qswing is a rotation whose axis lies in the XY plane and Qtwist is a rotation about axis Z.
/// such as Qin = Qswing * Qtwist
/// If the rotation is already around axis z, Qswing will be set to identity
/// and Qtwist equal to Qin
inline void getSwingTwist( const Quaternion& in, Quaternion& swingOut, Quaternion& twistOut );

/// Call std::isnormal on quaternion entries.
template <typename Quaternion_>
inline bool checkInvalidNumbers( Eigen::Ref<const Quaternion_> q,
                                 const bool FAIL_ON_ASSERT = false ) {
    return MatrixUtils::checkInvalidNumbers( q.coeffs(), FAIL_ON_ASSERT );
}
} // namespace QuaternionUtils

// Use this macro in the public: section of a class
// when declaring objects containing Vector or Matrices.
// http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html
#define RA_CORE_ALIGNED_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Use this parameter for aligning structures with Eigen members in it
#define RA_DEFAULT_ALIGN EIGEN_MAX_ALIGN_BYTES

} // namespace Math
} // namespace Core
} // namespace Ra

#include <Core/Math/LinearAlgebra.inl>

#endif // RADIUMENGINE_VECTOR_HPP