//==============================================================================
//         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 NT2_TRIGONOMETRIC_FUNCTIONS_GENERIC_ACOTD_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_FUNCTIONS_GENERIC_ACOTD_HPP_INCLUDED

#include <nt2/trigonometric/functions/acotd.hpp>
#include <nt2/include/functions/simd/atand.hpp>
#include <nt2/include/functions/simd/if_else_zero.hpp>
#include <nt2/include/functions/simd/if_zero_else.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/is_nez.hpp>
#include <nt2/include/functions/simd/bitofsign.hpp>
#include <nt2/include/functions/simd/bitwise_or.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/constants/_90.hpp>
#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/functions/simd/is_inf.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( acotd_,tag::cpu_
                            , (A0)
                            , ((generic_<floating_<A0> >))
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      A0 z = nt2::_90<A0>()-nt2::if_else_zero(nt2::is_nez(a0),nt2::atand(nt2::abs(a0)));
      #ifndef BOOST_SIMD_NO_INFINITIES
      z = nt2::if_zero_else(nt2::is_inf(a0),z);
      #endif
      return nt2::b_or(z, nt2::bitofsign(a0));
    }
  };
} }

#endif


/*
 * benchmark_hogwild_regression.cpp
 * author: Abhijit Chowdhary (achowdh2@ncsu.edu)
 *
 * Benchmark HOGWILD! as applied to regression on to a simple random normal 50
 * x 50 matrix A and random normal vector b:
 *
 *  minimize (1/2)||Ax-b||_2^2
 *
 * Outputs to results.txt and stdout time taken to reach desired tolerance for
 * each core count possible in system.
 *  ---------------------------------------------------------------------------
 * 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/.
 */
#include <omp.h>
#include <stdio.h>

#include <algorithm>
#include <array>
#include <atomic>
#include <iostream>
#include <random>

#include <Eigen/Dense>

#include "readCSV.h"

#define ETA 0.00025
#define NUM_EPOCHS 5

int
main(int argc, char **argv)
{
  FILE *fresults, *fX;
  fresults = fopen("results.txt", "w+"); fX = fopen("X.txt", "w+");
  Eigen::initParallel();
  omp_set_dynamic(0);
  auto rng = std::default_random_engine {}; rng.seed(0);

  unsigned P = omp_get_max_threads();
  double timings[P];
  for (unsigned k = 0; k < P; k++) { timings[k] = 0; }

  // Read MSD dataset into memory and format matrices.
  Eigen::MatrixXd Data;
  readCSV<double>("winequality-white.csv", Data);
  unsigned const num_data = 4898; unsigned const num_features = 11;

  Eigen::MatrixXd A = Data.topLeftCorner(num_data, num_features);
  Eigen::VectorXd b = Data.col(num_features);

  std::array<unsigned, num_data> ordering;
  std::iota(ordering.begin(), ordering.end(), 0);

  std::atomic<double> *x = new std::atomic<double>[num_features];
  Eigen::MatrixXd X(num_features,1);
  for (unsigned k = 0; k < num_features; k++) { x[k] = 1; X(k) = 1; }
  fprintf(fresults, "%f\n", (0.5/num_data)*(A*X-b).squaredNorm());

  for (unsigned epoch = 0; epoch < NUM_EPOCHS; epoch++)
  {
    std::shuffle(ordering.begin(), ordering.end(), rng);
    #pragma omp parallel for
    for (unsigned k = 0; k < num_data; k++)
    {
      unsigned id = ordering[k];
      double dg = 0;
      for (unsigned i = 0; i < num_features; i++) { dg += A(id, i)*x[i].load(); }
      dg -= b(id);
      for (unsigned i = 0; i < num_features; i++)
      {
        double dgi = x[i].load() - ETA*(1.0/num_data)*( A(id,i)*dg + num_data*x[i].load() );
        x[i].exchange( dgi );
      }
    }
    for (unsigned k = 0; k < num_features; k++) { X(k) = x[k].load(); }
    double epsilon = (0.5/num_data)*(A*X-b).squaredNorm();
    fprintf(fresults, "%f\n", epsilon);
    if (epsilon < 1e-5)
    {
      break;
    }
  }

  for (unsigned k = 0; k < num_features; k++)
  {
    fprintf(fX, "%f\n", X(k));
  }

  fclose(fresults); fclose(fX);
  return 0;
}


/* =========================================================================
   Copyright (c) 2010-2012, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/*
*
*   Tutorial: BLAS level 2 functionality (blas2.cpp and blas2.cu are identical, the latter being required for compilation using CUDA nvcc)
*
*/

//
// include necessary system headers
//
#include <iostream>

//
// ublas includes
//
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

// Must be set if you want to use ViennaCL algorithms on ublas objects
#define VIENNACL_WITH_UBLAS 1

//
// ViennaCL includes
//
#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/linalg/direct_solve.hpp"
#include "viennacl/linalg/prod.hpp"       //generic matrix-vector product
#include "viennacl/linalg/norm_2.hpp"     //generic l2-norm for vectors
#include "viennacl/linalg/lu.hpp"         //LU substitution routines

// Some helper functions for this tutorial:
#include "Random.hpp"
#include "vector-io.hpp"

using namespace boost::numeric;

int main()
{
  typedef float       ScalarType;

  //
  // Set up some ublas objects
  //
  ublas::vector<ScalarType> rhs(12);
  for (unsigned int i = 0; i < rhs.size(); ++i)
    rhs(i) = random<ScalarType>();
  ublas::vector<ScalarType> rhs2 = rhs;
  ublas::vector<ScalarType> result = ublas::zero_vector<ScalarType>(10);
  ublas::vector<ScalarType> result2 = result;
  ublas::vector<ScalarType> rhs_trans = rhs;
  rhs_trans.resize(result.size(), true);
  ublas::vector<ScalarType> result_trans = ublas::zero_vector<ScalarType>(rhs.size());

  ublas::matrix<ScalarType> matrix(result.size(),rhs.size());

  //
  // Fill the matrix
  //
  for (unsigned int i = 0; i < matrix.size1(); ++i)
    for (unsigned int j = 0; j < matrix.size2(); ++j)
      matrix(i,j) = random<ScalarType>();

  //
  // Use some plain STL types:
  //
  std::vector< ScalarType > stl_result(result.size());
  std::vector< ScalarType > stl_rhs(rhs.size());
  std::vector< std::vector<ScalarType> > stl_matrix(result.size());
  for (unsigned int i=0; i < result.size(); ++i)
  {
    stl_matrix[i].resize(rhs.size());
    for (unsigned int j = 0; j < matrix.size2(); ++j)
    {
      stl_rhs[j] = rhs[j];
      stl_matrix[i][j] = matrix(i,j);
    }
  }

  //
  // Set up some ViennaCL objects
  //
  viennacl::vector<ScalarType> vcl_rhs(rhs.size());
  viennacl::vector<ScalarType> vcl_result(result.size());
  viennacl::matrix<ScalarType> vcl_matrix(result.size(), rhs.size());
  viennacl::matrix<ScalarType> vcl_matrix2(result.size(), rhs.size());

  viennacl::copy(rhs.begin(), rhs.end(), vcl_rhs.begin());
  viennacl::copy(matrix, vcl_matrix);     //copy from ublas dense matrix type to ViennaCL type

  //
  // Some basic matrix operations
  //
  vcl_matrix2 = vcl_matrix;
  vcl_matrix2 += vcl_matrix;
  vcl_matrix2 -= vcl_matrix;
  vcl_matrix2 = vcl_matrix2 + vcl_matrix;
  vcl_matrix2 = vcl_matrix2 - vcl_matrix;

  viennacl::scalar<ScalarType> vcl_3(3.0);
  vcl_matrix2 *= ScalarType(2.0);
  vcl_matrix2 /= ScalarType(2.0);
  vcl_matrix2 *= vcl_3;
  vcl_matrix2 /= vcl_3;

  //
  // A matrix can be cleared directly:
  //
  vcl_matrix.clear();

  viennacl::copy(stl_matrix, vcl_matrix); //alternative: copy from STL vector< vector<> > type to ViennaCL type

  //for demonstration purposes (no effect):
  viennacl::copy(vcl_matrix, matrix); //copy back from ViennaCL to ublas type.
  viennacl::copy(vcl_matrix, stl_matrix); //copy back from ViennaCL to STL type.

  /////////////////////////////////////////////////
  //////////// Matrix vector products /////////////
  /////////////////////////////////////////////////

  //
  // Compute matrix-vector products
  //
  std::cout << "----- Matrix-Vector product -----" << std::endl;
  result = ublas::prod(matrix, rhs);                            //the ublas way
  stl_result = viennacl::linalg::prod(stl_matrix, stl_rhs);     //using STL
  vcl_result = viennacl::linalg::prod(vcl_matrix, vcl_rhs);     //the ViennaCL way

  //
  // Compute transposed matrix-vector products
  //
  std::cout << "----- Transposed Matrix-Vector product -----" << std::endl;
  result_trans = prod(trans(matrix), rhs_trans);

  viennacl::vector<ScalarType> vcl_rhs_trans(rhs_trans.size());
  viennacl::vector<ScalarType> vcl_result_trans(result_trans.size());
  viennacl::copy(rhs_trans.begin(), rhs_trans.end(), vcl_rhs_trans.begin());
  vcl_result_trans = viennacl::linalg::prod(trans(vcl_matrix), vcl_rhs_trans);

  /////////////////////////////////////////////////
  //////////////// Direct solver  /////////////////
  /////////////////////////////////////////////////

  //
  // Setup suitable matrices
  //
  ublas::matrix<ScalarType> tri_matrix(10,10);
  for (size_t i=0; i<tri_matrix.size1(); ++i)
  {
    for (size_t j=0; j<i; ++j)
      tri_matrix(i,j) = 0.0;

    for (size_t j=i; j<tri_matrix.size2(); ++j)
      tri_matrix(i,j) = matrix(i,j);
  }

  viennacl::matrix<ScalarType> vcl_tri_matrix(tri_matrix.size1(), tri_matrix.size2());
  viennacl::copy(tri_matrix, vcl_tri_matrix);

  rhs.resize(tri_matrix.size1(), true);
  rhs2.resize(tri_matrix.size1(), true);
  vcl_rhs.resize(tri_matrix.size1(), true);

  viennacl::copy(rhs.begin(), rhs.end(), vcl_rhs.begin());
  vcl_result.resize(10);

  //
  // Triangular solver
  //
  std::cout << "----- Upper Triangular solve -----" << std::endl;
  result = ublas::solve(tri_matrix, rhs, ublas::upper_tag());                                    //ublas
  vcl_result = viennacl::linalg::solve(vcl_tri_matrix, vcl_rhs, viennacl::linalg::upper_tag());  //ViennaCL

  //
  // Inplace variants of the above
  //
  ublas::inplace_solve(tri_matrix, rhs, ublas::upper_tag());                                //ublas
  viennacl::linalg::inplace_solve(vcl_tri_matrix, vcl_rhs, viennacl::linalg::upper_tag());  //ViennaCL

  //
  // Set up a full system for LU solver:
  //
  std::cout << "----- LU factorization -----" << std::endl;
  size_t lu_dim = 300;
  ublas::matrix<ScalarType> square_matrix(lu_dim, lu_dim);
  ublas::vector<ScalarType> lu_rhs(lu_dim);
  viennacl::matrix<ScalarType> vcl_square_matrix(lu_dim, lu_dim);
  viennacl::vector<ScalarType> vcl_lu_rhs(lu_dim);

  for (size_t i=0; i<lu_dim; ++i)
    for (size_t j=0; j<lu_dim; ++j)
      square_matrix(i,j) = random<ScalarType>();

  //put some more weight on diagonal elements:
  for (size_t j=0; j<lu_dim; ++j)
  {
    square_matrix(j,j) += 10.0;
    lu_rhs(j) = random<ScalarType>();
  }

  viennacl::copy(square_matrix, vcl_square_matrix);
  viennacl::copy(lu_rhs, vcl_lu_rhs);
  viennacl::linalg::lu_factorize(vcl_square_matrix);
  viennacl::linalg::lu_substitute(vcl_square_matrix, vcl_lu_rhs);
  viennacl::copy(square_matrix, vcl_square_matrix);
  viennacl::copy(lu_rhs, vcl_lu_rhs);

  //
  // ublas:
  //
  ublas::lu_factorize(square_matrix);
  ublas::inplace_solve (square_matrix, lu_rhs, ublas::unit_lower_tag ());
  ublas::inplace_solve (square_matrix, lu_rhs, ublas::upper_tag ());

  //
  // ViennaCL:
  //
  viennacl::linalg::lu_factorize(vcl_square_matrix);
  viennacl::linalg::lu_substitute(vcl_square_matrix, vcl_lu_rhs);

  //
  //  That's it.
  //
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return 0;
}


#pragma once

#include <mtao/types.h>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <set>

#include "mtao/algebra/sort_with_permutation_sign.hpp"
#include "mtao/eigen/stl2eigen.hpp"

namespace mtao::geometry::mesh {

// Only for simplices
template <typename CellType>
auto unique_simplices(const Eigen::MatrixBase<CellType>& C) ->
    typename CellType::PlainMatrix {
    // each simplex can be uniquely canonically represented by
    // itself sorted and whether it took an even or odd permutation to obtain it
    // Because we're assuming simplices of dimension <= 4 bubble sort is not
    // only the most efficient, its easy to impl and check the number of swaps
    // :)
    constexpr int Rows = CellType::RowsAtCompileTime;
    constexpr bool DynamicSize = Rows == Eigen::Dynamic;
    using StlType = std::conditional_t<DynamicSize, std::vector<int>,
                                       std::array<int, Rows>>;

    std::map<StlType, bool> uniq_map;

    for (int i = 0; i < C.cols(); ++i) {
        if constexpr (DynamicSize) {
            StlType s(C.rows());
            mtao::eigen::stl2eigen(s) = C.col(i);
            bool even = algebra::sort_with_permutation_sign_in_place<
                algebra::SortType::Bubble>(s);

            uniq_map.try_emplace(std::move(s), even);
        } else {
            StlType s;
            mtao::eigen::stl2eigen(s) = C.col(i);
            bool even = algebra::sort_with_permutation_sign_in_place<
                algebra::SortType::Bubble>(s);

            uniq_map.try_emplace(std::move(s), even);
        }
    }

    // if  the input simplices were unique lets avoid messing with the input
    if (int(uniq_map.size()) != C.cols()) {
        typename CellType::PlainMatrix CC(C.rows(), uniq_map.size());
        for (auto&& [idx, pr] : mtao::iterator::enumerate(uniq_map)) {
            auto&& [c, even] = pr;
            auto v = CC.col(idx) = mtao::eigen::stl2eigen(c);
            if (C.rows() > 1 && !even) {
                std::swap(v(0), v(1));
            }
        }
        return CC;
    }
    return C;
}

}  // namespace mtao::geometry::mesh


//
//  Copyright (c) 2018, Cem Bassoy, cem.bassoy@gmail.com
//  Copyright (c) 2019, Amit Singh, amitsingh19975@gmail.com
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//

#include <boost/numeric/ublas/tensor.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include "utility.hpp"

BOOST_AUTO_TEST_SUITE(test_tensor_static_rank_comparison)

using double_extended = boost::multiprecision::cpp_bin_float_double_extended;

using test_types = zip<int,float,double_extended>::with_t<boost::numeric::ublas::layout::first_order, boost::numeric::ublas::layout::last_order>;

struct fixture {
  template<size_t N>
  using extents_t = boost::numeric::ublas::extents<N>;

  std::tuple<
    extents_t<2>, // 1
    extents_t<2>, // 2
    extents_t<3>, // 3
    extents_t<3>, // 4
    extents_t<4>  // 5
    > extents = {
      extents_t<2>{1,1},
      extents_t<2>{2,3},
      extents_t<3>{4,1,3},
      extents_t<3>{4,2,3},
      extents_t<4>{4,2,3,5}
  };
};

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_comparison, value,  test_types, fixture)
{
  namespace ublas = boost::numeric::ublas;
  using value_t  = typename value::first_type;
  using layout_t = typename value::second_type;

  auto check = [](auto const& /*unused*/, auto& e)
  {
    using extents_t = std::decay_t<decltype (e)>;
    using tensor_t = ublas::tensor_static_rank<value_t, std::tuple_size_v<extents_t>, layout_t>;
    auto t  = tensor_t (e);
    auto t2 = tensor_t (e);
    auto v  = value_t  {};

    std::iota(t.begin(), t.end(), v);
    std::iota(t2.begin(), t2.end(), v+2);

    BOOST_CHECK( t == t  );
    BOOST_CHECK( t != t2 );

    if(t.empty())
      return;

    BOOST_CHECK(!(t < t));
    BOOST_CHECK(!(t > t));
    BOOST_CHECK( t < t2 );
    BOOST_CHECK( t2 > t );
    BOOST_CHECK( t <= t );
    BOOST_CHECK( t >= t );
    BOOST_CHECK( t <= t2 );
    BOOST_CHECK( t2 >= t );
    BOOST_CHECK( t2 >= t2 );
    BOOST_CHECK( t2 >= t );
  };

  for_each_in_tuple(extents,check);

}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_comparison_with_tensor_expressions, value,  test_types, fixture)
{
  namespace ublas = boost::numeric::ublas;
  using value_t   = typename value::first_type;
  using layout_t  = typename value::second_type;

  for_each_in_tuple(extents,[](auto const& /*unused*/, auto& e) {
    using extents_t = std::decay_t<decltype (e)>;
    using tensor_t  = ublas::tensor_static_rank<value_t, std::tuple_size_v<extents_t>, layout_t>;

    auto t  = tensor_t (e);
    auto t2 = tensor_t (e);
    auto v  = value_t  {};

    std::iota(t.begin(), t.end(), v);
    std::iota(t2.begin(), t2.end(), v+2);

    BOOST_CHECK( t == t  );
    BOOST_CHECK( t != t2 );

    if(t.empty())
      return;

    BOOST_CHECK( !(t < t) );
    BOOST_CHECK( !(t > t) );
    BOOST_CHECK( t < (t2+t) );
    BOOST_CHECK( (t2+t) > t );
    BOOST_CHECK( t <= (t+t) );
    BOOST_CHECK( (t+t2) >= t );
    BOOST_CHECK( (t2+t2+2) >= t);
    BOOST_CHECK( 2*t2 > t );
    BOOST_CHECK( t < 2*t2 );
    BOOST_CHECK( 2*t2 > t);
    BOOST_CHECK( 2*t2 >= t2 );
    BOOST_CHECK( t2 <= 2*t2);
    BOOST_CHECK( 3*t2 >= t );
  });

}

//BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_comparison_with_scalar, value,  test_types, fixture)
//{
//  namespace ublas = boost::numeric::ublas;
//  using value_t  = typename value::first_type;
//  using layout_t = typename value::second_type;

//  for_each_in_tuple(extents, [](auto const& /*unused*/, auto& e) {
//    using extents_t = std::decay_t<decltype (e)>;
//    using tensor_t  = ublas::tensor_static_rank<value_t, std::tuple_size_v<extents_t>, layout_t>;

//    BOOST_CHECK( tensor_t(e,value_t{2}) == tensor_t(e,value_t{2})  );
//    BOOST_CHECK( tensor_t(e,value_t{2}) != tensor_t(e,value_t{1})  );

//    if(ublas::empty(e))
//      return;

//    BOOST_CHECK( !(tensor_t(e,2) <  2) );
//    BOOST_CHECK( !(tensor_t(e,2) >  2) );
//    BOOST_CHECK(  (tensor_t(e,2) >= 2) );
//    BOOST_CHECK(  (tensor_t(e,2) <= 2) );
//    BOOST_CHECK(  (tensor_t(e,2) == 2) );
//    BOOST_CHECK(  (tensor_t(e,2) != 3) );

//    BOOST_CHECK( !(2 >  tensor_t(e,2)) );
//    BOOST_CHECK( !(2 <  tensor_t(e,2)) );
//    BOOST_CHECK(  (2 <= tensor_t(e,2)) );
//    BOOST_CHECK(  (2 >= tensor_t(e,2)) );
//    BOOST_CHECK(  (2 == tensor_t(e,2)) );
//    BOOST_CHECK(  (3 != tensor_t(e,2)) );

//    BOOST_CHECK( !( tensor_t(e,2)+3 <  5) );
//    BOOST_CHECK( !( tensor_t(e,2)+3 >  5) );
//    BOOST_CHECK(  ( tensor_t(e,2)+3 >= 5) );
//    BOOST_CHECK(  ( tensor_t(e,2)+3 <= 5) );
//    BOOST_CHECK(  ( tensor_t(e,2)+3 == 5) );
//    BOOST_CHECK(  ( tensor_t(e,2)+3 != 6) );

//    BOOST_CHECK( !( 5 >  tensor_t(e,2)+3) );
//    BOOST_CHECK( !( 5 <  tensor_t(e,2)+3) );
//    BOOST_CHECK(  ( 5 >= tensor_t(e,2)+3) );
//    BOOST_CHECK(  ( 5 <= tensor_t(e,2)+3) );
//    BOOST_CHECK(  ( 5 == tensor_t(e,2)+3) );
//    BOOST_CHECK(  ( 6 != tensor_t(e,2)+3) );

//    BOOST_CHECK( !( tensor_t(e,2)+tensor_t(e,3) <  5) );
//    BOOST_CHECK( !( tensor_t(e,2)+tensor_t(e,3) >  5) );
//    BOOST_CHECK(  ( tensor_t(e,2)+tensor_t(e,3) >= 5) );
//    BOOST_CHECK(  ( tensor_t(e,2)+tensor_t(e,3) <= 5) );
//    BOOST_CHECK(  ( tensor_t(e,2)+tensor_t(e,3) == 5) );
//    BOOST_CHECK(  ( tensor_t(e,2)+tensor_t(e,3) != 6) );

//    BOOST_CHECK( !( 5 >  tensor_t(e,2)+tensor_t(e,3)) );
//    BOOST_CHECK( !( 5 <  tensor_t(e,2)+tensor_t(e,3)) );
//    BOOST_CHECK(  ( 5 >= tensor_t(e,2)+tensor_t(e,3)) );
//    BOOST_CHECK(  ( 5 <= tensor_t(e,2)+tensor_t(e,3)) );
//    BOOST_CHECK(  ( 5 == tensor_t(e,2)+tensor_t(e,3)) );
//    BOOST_CHECK(  ( 6 != tensor_t(e,2)+tensor_t(e,3)) );

//  });

//}

BOOST_AUTO_TEST_SUITE_END()


/**
 * @file lqr_controller.cpp
 * @brief Infinite-horizon Linear Quadratic Regulator.
 *
 */

#include <lqr_controller.hpp>
#include <Eigen/Dense>

using namespace controller;

LQR::LQR(
    const Eigen::MatrixXd& Q,
    const Eigen::MatrixXd& R
    ):
    Q(Q),
    R(R)
{
    P.setZero(Q.rows(), Q.cols());
    cmd_vel.setZero(1, R.rows()); // [translation rate, rotation rate].
}

Eigen::MatrixXd LQR::computeHamiltonian(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B, const Eigen::MatrixXd& E)
{
    // Set Hamilton matrix.
    Ham = Eigen::MatrixXd::Zero(2 * A.rows(), 2 * A.rows());
    Ham << A, -B * R.inverse() * B.transpose(), -Q, -A.transpose();

    // Get eigenvalues and eigenvectors from Hamilton matrix.
    Eigen::EigenSolver<Eigen::MatrixXd> eigen(Ham);

    // Form a 2nxn matrix whos columns from a basis of the corresponding subspace.
    Eigen::MatrixXcd eigenVec = Eigen::MatrixXcd::Zero(2 * A.rows(), A.rows());

    // Iterate over the Hamilton matrix and extract a stable Eigenvector.
    int j = 0;
    for(unsigned int i = 0; i < 2 * A.rows(); ++i)
    {
        // Get the negative real part: a stable value in the Laplace domain.
        if(eigen.eigenvalues()[i].real() < 0)
        {
            eigenVec.col(j) = eigen.eigenvectors().block(0, i, 2 * A.rows(), 1);
            ++j;
        }
    }

    // Compute the solution to the Riccati equation.
    Eigen::MatrixXcd U11, U21;

    U11 = eigenVec.block(0, 0, A.rows(), A.rows());
    U21 = eigenVec.block(A.rows(), 0, A.rows(), A.rows());
    P = (U21 * U11.inverse()).real();

    // Update LQR gain.
    K = R.inverse() * B.transpose() * P;
    controlUpdate = K * E;

    // Compute magnitude of velocity vector.
    cmd_vel <<
        sqrt(pow(controlUpdate(0, 0), 2.0) + pow(controlUpdate(0, 1), 2.0) + pow(controlUpdate(0, 2), 2.0)),
        sqrt(pow(controlUpdate(1, 0), 2.0) + pow(controlUpdate(1, 1), 2.0) + pow(controlUpdate(1, 2), 2.0));

    return cmd_vel;
}


/*
Provided to you by Emlid Ltd (c) 2014.
twitter.com/emlidtech || www.emlid.com || info@emlid.com

Example: Get pressure from MS5611 barometer onboard of Navio shield for Raspberry Pi

To run this example navigate to the directory containing it and run following commands:
make
./Barometer
*/

#include <Common/MS5611.h>
#include <Common/Util.h>
#include <unistd.h>
#include <stdio.h>
#include <boost/circular_buffer.hpp>
#define C_TO_KELVIN 273.15f

float get_altitude_difference(float base_pressure, float pressure, float temperature)
{
    float ret;
    float temp    = temperature + C_TO_KELVIN;
    float scaling = pressure / base_pressure;

    // This is an exact calculation that is within +-2.5m of the standard
    // atmosphere tables in the troposphere (up to 11,000 m amsl).
    ret = 153.8462f * temp * (1.0f - expf(0.190259f * logf(scaling)));

    return ret;
}

class AverageFilter
{
public:
    AverageFilter(int size = 20) : buffer(size){

    }
    float update(float value)
    {
        buffer.push_back(value);
        float sum = 0.0f;
        for(auto v: buffer)
        {
            sum += v;
        }
        return sum/buffer.size();
    }
private:
    boost::circular_buffer<float> buffer;
};

int main()
{
    MS5611 barometer;

    if (check_apm()) {
        return 1;
    }

    barometer.initialize();

    AverageFilter filter, base_pressure_filter;
    float base_pressure = -1;
    long  count = 0;
    while (true) {
        barometer.refreshPressure();
        usleep(10000); // Waiting for pressure data ready
        barometer.readPressure();

        barometer.refreshTemperature();
        usleep(10000); // Waiting for temperature data ready
        barometer.readTemperature();

        barometer.calculatePressureAndTemperature();
        if(count < 100)
        {
            base_pressure = barometer.getPressure();
            base_pressure = base_pressure_filter.update(base_pressure);
        }
        float relative_altitude = get_altitude_difference(base_pressure, barometer.getPressure(), barometer.getTemperature());
        float filtered_relative_altitude = filter.update(relative_altitude);
        printf("#%d: Temperature(C): %f Pressure(millibar): %f, relative height(m): %f, filtered: %f\n", count,
                barometer.getTemperature(), barometer.getPressure(), relative_altitude, filtered_relative_altitude);

        //usleep(300000);
        count ++;
    }

    return 0;
}


//=======================================================================
// Copyright 2014-2015 David Simmons-Duffin.
// Distributed under the MIT License.
// (See accompanying file LICENSE or copy at
//  http://opensource.org/licenses/MIT)
//=======================================================================

#pragma once

#include "Block_Info.hxx"
#include "Block_Matrix.hxx"
#include "Block_Vector.hxx"
#include "Index_Tuple.hxx"

#include <boost/filesystem.hpp>

// The class SDP encodes a semidefinite program of the following form
//
// Dual: maximize f + b.y over y,Y such that
//                Tr(A_p Y) + (B y)_p = c_p  (0 <= p < P)
//                Y >= 0
// Primal: minimize f + c.x over x,X such that
//                X = \sum_p A_p x_p - C
//                B^T x = b
//                X >= 0
//
// where the data of the SDP has the following structure
//
// - b,y are vectors of length N
//
// - c,x are vectors of length P
//
// - f is a constant that we add to the objective functions for
//   convenience. It has no effect on the running of our algorithm.
//
// - B is a P x N matrix, called the free variable matrix
//
// - X and Y are block diagonal:
//
//   X = BlockDiagonal(X^(0), ..., X^(bMax-1))
//   Y = BlockDiagonal(Y^(0), ..., Y^(bMax-1))
//
//   Let us define Block_b(M) as BlockDiagonal(0,...,0,M,0,...,0),
//   where M is in the b-th block.  Then X and Y can be written
//
//   X = \sum_{0<=b<bMax} Block_b(X^(b))
//   Y = \sum_{0<=b<bMax} Block_b(Y^(b))
//
// - The constraints labeled by 0 <= p < P are in 1-to-1
//   correspondence with tuples
//
//   p <-> (j,r,s,k) where 0 <= j < J,
//                         0 <= s < m_j,
//                         0 <= r <= s,
//                         0 <= k <= d_j,
//
//   We often interchange the index p with the tuple (j,r,s,k).  The
//   constraint matrices A_p are given by
//
//   A_(j,r,s,k) = \sum_{b \in blocks[j]}
//                     Block_b(v_{b,k} v_{b,k}^T \otimes E^{rs}),
//
//   where
//   - E^{rs} is the symmetrization of the m_j x m_j matrix with a
//     1 in entry (r,s) and zeros elsewhere.
//   - v_{b,k} is a vector of length (delta_b+1)
//   - \otimes denotes a tensor product
//   - each block above thus has dimension (delta_b+1)*m_j.
//   - blocks[j_1] and blocks[j_2] are disjoint if j_1 != j_2.  Thus,
//     the block index b determines a unique j, but not vice-versa.
//

struct SDP
{
  // bilinear_bases is a vector of Matrices encoding the v_{b,k}
  // that enter the constraint matrices. Specifically, v_{b,k} are
  // the columns of bilinear_bases[b],
  //
  // bilinear_bases[b].elt(m,k) = (v_{b,k})_m  (0 <= b < bMax,
  //                                           0 <= k <= d_j,
  //                                           0 <= m <= delta_b)
  //
  std::vector<El::Matrix<El::BigFloat>> bilinear_bases_local;
  std::vector<El::DistMatrix<El::BigFloat>> bilinear_bases_dist;

  // free_var_matrix = B, a PxN matrix
  Block_Matrix free_var_matrix;

  // c, a vector of length P used with primal_objective
  Block_Vector primal_objective_c;

  // b, a vector of length N used with dual_objective
  // It is duplicated amongst all the blocks
  El::DistMatrix<El::BigFloat> dual_objective_b;
  // Transformation from internal yp to original y
  El::DistMatrix<El::BigFloat> yp_to_y;

  // objectiveConst = f
  El::BigFloat objective_const;

  SDP(const boost::filesystem::path &sdp_directory,
      const Block_Info &block_info, const El::Grid &grid);
  SDP(const El::BigFloat &objective_const,
      const std::vector<El::BigFloat> &dual_objective_b_input,
      const std::vector<std::vector<El::BigFloat>> &primal_objective_c_input,
      const std::vector<El::Matrix<El::BigFloat>> &free_var_input,
      const Block_Info &block_info, const El::Grid &grid);
};


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

#include "libmv/multiview/five_point.h"

#include <Eigen/QR>

#include "libmv/multiview/fundamental.h"
#include "libmv/multiview/fundamental_kernel.h"
#include "libmv/multiview/five_point_internal.h"

namespace libmv {

Mat FivePointsNullspaceBasis(const Mat2X &x1, const Mat2X &x2) {
  Matrix<double, 9, 9> A;
  A.setZero();  // Make A square until Eigen supports rectangular SVD.
  fundamental::kernel::EncodeEpipolarEquation(x1, x2, &A);
  Eigen::JacobiSVD<Matrix<double, 9, 9> > svd;
  return svd.compute(A, Eigen::ComputeFullV).matrixV().topRightCorner<9, 4>();
}

Vec o1(const Vec &a, const Vec &b) {
  Vec res = Vec::Zero(20);

  res(coef_xx) = a(coef_x) * b(coef_x);
  res(coef_xy) = a(coef_x) * b(coef_y)
               + a(coef_y) * b(coef_x);
  res(coef_xz) = a(coef_x) * b(coef_z)
               + a(coef_z) * b(coef_x);
  res(coef_yy) = a(coef_y) * b(coef_y);
  res(coef_yz) = a(coef_y) * b(coef_z)
               + a(coef_z) * b(coef_y);
  res(coef_zz) = a(coef_z) * b(coef_z);
  res(coef_x)  = a(coef_x) * b(coef_1)
               + a(coef_1) * b(coef_x);
  res(coef_y)  = a(coef_y) * b(coef_1)
               + a(coef_1) * b(coef_y);
  res(coef_z)  = a(coef_z) * b(coef_1)
               + a(coef_1) * b(coef_z);
  res(coef_1)  = a(coef_1) * b(coef_1);

  return res;
}

Vec o2(const Vec &a, const Vec &b) {
  Vec res(20);

  res(coef_xxx) = a(coef_xx) * b(coef_x);
  res(coef_xxy) = a(coef_xx) * b(coef_y)
                + a(coef_xy) * b(coef_x);
  res(coef_xxz) = a(coef_xx) * b(coef_z)
                + a(coef_xz) * b(coef_x);
  res(coef_xyy) = a(coef_xy) * b(coef_y)
                + a(coef_yy) * b(coef_x);
  res(coef_xyz) = a(coef_xy) * b(coef_z)
                + a(coef_yz) * b(coef_x)
                + a(coef_xz) * b(coef_y);
  res(coef_xzz) = a(coef_xz) * b(coef_z)
                + a(coef_zz) * b(coef_x);
  res(coef_yyy) = a(coef_yy) * b(coef_y);
  res(coef_yyz) = a(coef_yy) * b(coef_z)
                + a(coef_yz) * b(coef_y);
  res(coef_yzz) = a(coef_yz) * b(coef_z)
                + a(coef_zz) * b(coef_y);
  res(coef_zzz) = a(coef_zz) * b(coef_z);
  res(coef_xx)  = a(coef_xx) * b(coef_1)
                + a(coef_x)  * b(coef_x);
  res(coef_xy)  = a(coef_xy) * b(coef_1)
                + a(coef_x)  * b(coef_y)
                + a(coef_y)  * b(coef_x);
  res(coef_xz)  = a(coef_xz) * b(coef_1)
                + a(coef_x)  * b(coef_z)
                + a(coef_z)  * b(coef_x);
  res(coef_yy)  = a(coef_yy) * b(coef_1)
                + a(coef_y)  * b(coef_y);
  res(coef_yz)  = a(coef_yz) * b(coef_1)
                + a(coef_y)  * b(coef_z)
                + a(coef_z)  * b(coef_y);
  res(coef_zz)  = a(coef_zz) * b(coef_1)
                + a(coef_z)  * b(coef_z);
  res(coef_x)   = a(coef_x)  * b(coef_1)
                + a(coef_1)  * b(coef_x);
  res(coef_y)   = a(coef_y)  * b(coef_1)
                + a(coef_1)  * b(coef_y);
  res(coef_z)   = a(coef_z)  * b(coef_1)
                + a(coef_1)  * b(coef_z);
  res(coef_1)   = a(coef_1)  * b(coef_1);

  return res;
}

// Builds the polynomial constraint matrix M.
Mat FivePointsPolynomialConstraints(const Mat &E_basis) {
  // Build the polynomial form of E (equation (8) in Stewenius et al. [1])
  Vec E[3][3];
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      E[i][j] = Vec::Zero(20);
      E[i][j](coef_x) = E_basis(3 * i + j, 0);
      E[i][j](coef_y) = E_basis(3 * i + j, 1);
      E[i][j](coef_z) = E_basis(3 * i + j, 2);
      E[i][j](coef_1) = E_basis(3 * i + j, 3);
    }
  }

  // The constraint matrix.
  Mat M(10, 20);
  int mrow = 0;

  // Determinant constraint det(E) = 0; equation (19) of Nister [2].
  M.row(mrow++) = o2(o1(E[0][1], E[1][2]) - o1(E[0][2], E[1][1]), E[2][0]) +
                  o2(o1(E[0][2], E[1][0]) - o1(E[0][0], E[1][2]), E[2][1]) +
                  o2(o1(E[0][0], E[1][1]) - o1(E[0][1], E[1][0]), E[2][2]);

  // Cubic singular values constraint.
  // Equation (20).
  Vec EET[3][3];
  for (int i = 0; i < 3; ++i) {    // Since EET is symmetric, we only compute
    for (int j = 0; j < 3; ++j) {  // its upper triangular part.
      if (i <= j) {
        EET[i][j] = o1(E[i][0], E[j][0])
                  + o1(E[i][1], E[j][1])
                  + o1(E[i][2], E[j][2]);
      } else {
        EET[i][j] = EET[j][i];
      }
    }
  }

  // Equation (21).
  Vec (&L)[3][3] = EET;
  Vec trace  = 0.5 * (EET[0][0] + EET[1][1] + EET[2][2]);
  for (int i = 0; i < 3; ++i) {
    L[i][i] -= trace;
  }

  // Equation (23).
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      Vec LEij = o2(L[i][0], E[0][j])
               + o2(L[i][1], E[1][j])
               + o2(L[i][2], E[2][j]);
      M.row(mrow++) = LEij;
    }
  }

  return M;
}

// Gauss--Jordan elimination for the constraint matrix.
void FivePointsGaussJordan(Mat *Mp) {
  Mat &M = *Mp;

  // Gauss Elimination.
  for (int i = 0; i < 10; ++i) {
    M.row(i) /= M(i, i);
    for (int j = i + 1; j < 10; ++j) {
      M.row(j) = M.row(j) / M(j, i) - M.row(i);
    }
  }

  // Backsubstitution.
  for (int i = 9; i >= 0; --i) {
    for (int j = 0; j < i; ++j) {
      M.row(j) = M.row(j) - M(j, i) * M.row(i);
    }
  }
}

void FivePointsRelativePose(const Mat2X &x1,
                            const Mat2X &x2,
                            vector<Mat3> *Es) {
  // Step 1: Nullspace exrtraction.
  Mat E_basis = FivePointsNullspaceBasis(x1, x2);

  // Step 2: Constraint expansion.
  Mat M = FivePointsPolynomialConstraints(E_basis);

  // Step 3: Gauss-Jordan elimination.
  FivePointsGaussJordan(&M);

  // For the next steps, follow the matlab code given in Stewenius et al [1].

  // Build the action matrix.
  Mat B = M.topRightCorner<10, 10>();
  Mat At = Mat::Zero(10, 10);
  At.row(0) = -B.row(0);
  At.row(1) = -B.row(1);
  At.row(2) = -B.row(2);
  At.row(3) = -B.row(4);
  At.row(4) = -B.row(5);
  At.row(5) = -B.row(7);
  At(6, 0) = 1;
  At(7, 1) = 1;
  At(8, 3) = 1;
  At(9, 6) = 1;

  // Compute the solutions from action matrix's eigenvectors.
  Eigen::EigenSolver<Mat> es(At);
  typedef Eigen::EigenSolver<Mat>::EigenvectorsType Matc;
  Matc V = es.eigenvectors();
  Matc solutions(4, 10);
  solutions.row(0) = V.row(6).array() / V.row(9).array();
  solutions.row(1) = V.row(7).array() / V.row(9).array();
  solutions.row(2) = V.row(8).array() / V.row(9).array();
  solutions.row(3).setOnes();

  // Get the ten candidate E matrices in vector form.
  Matc Evec = E_basis * solutions;

  // Build the essential matrices for the real solutions.
  Es->reserve(10);
  for (int s = 0; s < 10; ++s) {
    Evec.col(s) /= Evec.col(s).norm();
    bool is_real = true;
    for (int i = 0; i < 9; ++i) {
      if (Evec(i, s).imag() != 0) {
        is_real = false;
        break;
      }
    }
    if (is_real) {
      Mat3 E;
      for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
          E(i, j) = Evec(3 * i + j, s).real();
        }
      }
      Es->push_back(E);
    }
  }
}

}  // namespace libmv


#ifndef sphere_hpp
#define sphere_hpp

#include <three-dim-util/opengl2/primitives/abstract-primitive.hpp>
#include <Eigen/Core>

namespace threedimutil
{
    class Sphere : public AbstractPrimitive
    {
    public:
        static void Initialize(int latitude_resolution = 20, int longitude_resolution = 30)
        {
            Sphere::GetInstance().latitude_resolution()  = latitude_resolution;
            Sphere::GetInstance().longitude_resolution() = longitude_resolution;
            Sphere::GetInstance().InitializeInternal();
        }

        static void Draw()
        {
            Sphere::GetInstance().DrawInternal();
        }

        static Sphere& GetInstance()
        {
            static Sphere sphere;
            return sphere;
        }

        int latitude_resolution() const { return latitude_resolution_; }
        int& latitude_resolution() { return latitude_resolution_; }
        int longitude_resolution() const { return longitude_resolution_; }
        int& longitude_resolution() { return longitude_resolution_; }

    private:
        void CreateVertexData()
        {
            constexpr double pi = M_PI;

            vertices_.resize(3, latitude_resolution_ * longitude_resolution_ * 6);
            for (int i = 0; i < longitude_resolution_; ++ i)
            {
                const double theta_xy_1 = 2.0 * static_cast<double>(i + 0) * pi / static_cast<double>(longitude_resolution_);
                const double theta_xy_2 = 2.0 * static_cast<double>(i + 1) * pi / static_cast<double>(longitude_resolution_);
                const double x_1        = std::cos(theta_xy_1);
                const double x_2        = std::cos(theta_xy_2);
                const double y_1        = std::sin(theta_xy_1);
                const double y_2        = std::sin(theta_xy_2);

                for (int j = 0; j < latitude_resolution_; ++ j)
                {
                    const double theta_z_1 = static_cast<double>(j + 0) * pi / static_cast<double>(latitude_resolution_);
                    const double theta_z_2 = static_cast<double>(j + 1) * pi / static_cast<double>(latitude_resolution_);
                    const double cos_1 = std::cos(theta_z_1);
                    const double cos_2 = std::cos(theta_z_2);
                    const double sin_1 = std::sin(theta_z_1);
                    const double sin_2 = std::sin(theta_z_2);

                    const int offset = i * latitude_resolution_ * 6 + j * 6;

                    vertices_.col(offset + 0) = Eigen::Vector3d(sin_2 * x_1, sin_2 * y_1, cos_2);
                    vertices_.col(offset + 1) = Eigen::Vector3d(sin_2 * x_2, sin_2 * y_2, cos_2);
                    vertices_.col(offset + 2) = Eigen::Vector3d(sin_1 * x_2, sin_1 * y_2, cos_1);
                    vertices_.col(offset + 3) = Eigen::Vector3d(sin_2 * x_1, sin_2 * y_1, cos_2);
                    vertices_.col(offset + 4) = Eigen::Vector3d(sin_1 * x_2, sin_1 * y_2, cos_1);
                    vertices_.col(offset + 5) = Eigen::Vector3d(sin_1 * x_1, sin_1 * y_1, cos_1);
                }
            }
            normals_ = vertices_;
        }

        int latitude_resolution_  = 20;
        int longitude_resolution_ = 30;
    };
}

#endif /* sphere_hpp */


#include <aikido/common/PseudoInverse.hpp>

#include <iostream>
#include <limits>
#include <memory>
#include <Eigen/Dense>

namespace aikido {
namespace common {

//==============================================================================
Eigen::MatrixXd pseudoinverse(const Eigen::MatrixXd& mat, double eps)
{
  if (mat.rows() == mat.cols() && mat.determinant() > eps)
    return mat.inverse();

  else
  {
    if (mat.cols() == 1)
    {
      if (mat.isApproxToConstant(0))
      {
        return Eigen::VectorXd::Zero(mat.rows());
      }

      return mat.transpose() / (pow(mat.norm(), 2));
    }

    /// Use SVD decomposition.
    Eigen::JacobiSVD<Eigen::MatrixXd> jacSVD(
        mat, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::MatrixXd U = jacSVD.matrixU();
    Eigen::MatrixXd V = jacSVD.matrixV();
    Eigen::VectorXd S = jacSVD.singularValues();

    Eigen::MatrixXd S_inv(Eigen::MatrixXd::Zero(mat.cols(), mat.rows()));

    for (int i = 0; i < S.rows(); i++)
    {
      if (S(i) > eps)
      {
        S_inv(i, i) = 1.0 / S(i);
      }
      else
      {
        S_inv(i, i) = 0;
      }
    }

    return V * S_inv * U.transpose();
  }
}

} // namespace common
} // namespace aikido


/*=============================================================================
Copyright 2020 Syed Ali Hasan <alihasan9922@gmail.com>

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

#ifndef BOOST_ASTRONOMY_UTILITY_HPP
#define BOOST_ASTRONOMY_UTILITY_HPP

#include <iostream>
#include <utility>
#include <cmath>

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

//Angle
#include <boost/units/quantity.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/dimensionless.hpp>
#include <boost/units/physical_dimensions/plane_angle.hpp>

//Time
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include <boost/astronomy/coordinate/coord_sys/coord_sys.hpp>

constexpr long double PI = 3.141592653589793238462643383279502884L;

using namespace std;
namespace bu = boost::units;
namespace bud = boost::units::degree;
using namespace boost::numeric::ublas;
namespace bnu = boost::numeric::ublas;

using namespace boost::gregorian;

namespace boost { namespace astronomy { namespace coordinate {

// Phi φ is the geographical latitude
// ST is the Local Sidereal Time
// ε is the obliquity of the ecliptic.

template
    <
        typename CoordinateType = double,
        typename Angle = bu::quantity<bu::si::plane_angle, CoordinateType>,
        typename ElementType = double
    >
struct column_vector
{
 public:
  matrix<ElementType> vec = matrix<ElementType>(3, 1);

  column_vector() {}

  column_vector(Angle u, Angle v){
    double _u = static_cast<bu::quantity<bu::si::plane_angle>>(u).value();
    double _v = static_cast<bu::quantity<bu::si::plane_angle>>(v).value();

    vec(0,0) = std::cos(_u) * std::cos(_v);
    vec(1,0) = std::sin(_u) * std::cos(_v);
    vec(2,0) = std::sin(_v);
  }

  matrix<ElementType> get(){
    return vec;
  }

  std::string to_string(){
    return "Column Vector";
  }
};

template
    <
        typename CoordinateType = double,
        typename Angle = bu::quantity<bu::si::plane_angle, CoordinateType>,
        typename ElementType = double
    >
struct ha_dec_horizon
{
 public:
  matrix<ElementType> conv = matrix<ElementType>(3, 3);

  ha_dec_horizon() {}

  ha_dec_horizon(Angle phi){
    double _phi = static_cast<bu::quantity<bu::si::plane_angle>>(phi).value();

    conv(0,0) = -std::sin(_phi);
    conv(0,1) = 0;
    conv(0,2) = std::cos(_phi);
    conv(1,0) = 0;
    conv(1,1) = -1;
    conv(1,2) = 0;
    conv(2,0) = std::cos(_phi);
    conv(2,1) = 0;
    conv(2,2) = std::sin(_phi);
  }

  matrix<ElementType> get(){
    return conv;
  }

  std::string to_string(){
    return "Equatorial Coordinate Hour Angle to and from Horizon";
  }
};

template
    <
        typename CoordinateType = double,
        typename Angle = bu::quantity<bu::si::plane_angle, CoordinateType>,
        typename ElementType = double
    >
struct ha_dec_ra_dec
{
 public:
  matrix<ElementType> conv = matrix<ElementType>(3, 3);

  ha_dec_ra_dec() {}

  ha_dec_ra_dec(Angle ST){
    double _ST = static_cast<bu::quantity<bu::si::plane_angle>>(ST).value();

    conv(0,0) = std::cos(_ST);
    conv(0,1) = std::sin(_ST);
    conv(0,2) = 0;
    conv(1,0) = std::sin(_ST);
    conv(1,1) = -std::cos(_ST);
    conv(1,2) = 0;
    conv(2,0) = 0;
    conv(2,1) = 0;
    conv(2,2) = 1;
  }

  matrix<ElementType> get(){
    return conv;
  }

  std::string to_string(){
    return "Equatorial Coordinate Hour Angle to and from Equatorial Coordinate Right Ascension";
  }
};

template
    <
        typename CoordinateType = double,
        typename Angle = bu::quantity<bu::si::plane_angle, CoordinateType>,
        typename ElementType = double
    >
struct ecliptic_to_ra_dec
{
 public:
  matrix<ElementType> conv = matrix<ElementType>(3, 3);

  ecliptic_to_ra_dec() {}

  ecliptic_to_ra_dec(Angle obliquity){
    double _obliquity = static_cast<bu::quantity<bu::si::plane_angle>>(obliquity).value();

    conv(0,0) = 1;
    conv(0,1) = 0;
    conv(0,2) = 0;
    conv(1,0) = 0;
    conv(1,1) = std::cos(_obliquity);
    conv(1,2) = -std::sin(_obliquity);
    conv(2,0) = 0;
    conv(2,1) = std::sin(_obliquity);
    conv(2,2) = std::cos(_obliquity);
  }

  matrix<ElementType> get(){
    return conv;
  }

  std::string to_string(){
    return "Ecliptic to Equatorial Coordinate Right Ascension";
  }
};

template
    <
        typename CoordinateType = double,
        typename Angle = bu::quantity<bu::si::plane_angle, CoordinateType>,
        typename ElementType = double
    >
struct ra_dec_to_ecliptic
{
 public:
  matrix<ElementType> conv = matrix<ElementType>(3, 3);

  ra_dec_to_ecliptic() {}

  ra_dec_to_ecliptic(Angle obliquity){
    double _obliquity = static_cast<bu::quantity<bu::si::plane_angle>>(obliquity).value();

    conv(0,0) = 1;
    conv(0,1) = 0;
    conv(0,2) = 0;
    conv(1,0) = 0;
    conv(1,1) = std::cos(_obliquity);
    conv(1,2) = std::sin(_obliquity);
    conv(2,0) = 0;
    conv(2,1) = -std::sin(_obliquity);
    conv(2,2) = std::cos(_obliquity);
  }

  matrix<ElementType> get(){
    return conv;
  }

  std::string to_string(){
    return "Equatorial Coordinate Right Ascension to Ecliptic";
  }
};

template
    <typename ElementType = double>
struct galactic_to_ra_dec
{
 public:
  matrix<ElementType> conv = matrix<ElementType>(3, 3);

  galactic_to_ra_dec(){
    conv(0,0) = -0.0669887;
    conv(0,1) = 0.8727558;
    conv(0,2) = -0.4835389;
    conv(1,0) = -0.4927285;
    conv(1,1) = -0.4503470;
    conv(1,2) = -0.7445846;
    conv(2,0) = -0.8676008;
    conv(2,1) = 0.1883746;
    conv(2,2) = 0.4601998;
  }

  matrix<ElementType> get(){
    return conv;
  }

  std::string to_string(){
    return "Galactic to Equatorial Coordinate Right Ascension";
  }
};

template
    <typename ElementType = double>
struct ra_dec_to_galactic
{
 public:
  matrix<ElementType> conv = matrix<ElementType>(3, 3);

  ra_dec_to_galactic()
  {
    conv(0,0) = -0.0669887;
    conv(0,1) = -0.8727558;
    conv(0,2) = -0.4835389;
    conv(1,0) = 0.4927285;
    conv(1,1) = -0.4503470;
    conv(1,2) = 0.7445846;
    conv(2,0) = -0.8676008;
    conv(2,1) = -0.1883746;
    conv(2,2) = 0.4601998;
  }

  matrix<ElementType> get()
  {
    return conv;
  }

  std::string to_string()
  {
    return "Equatorial Coordinate Right Ascension to Galactic";
  }

};

typedef bu::quantity<bu::si::plane_angle, double> angle_radian;

struct extract_coordinates{
 private:
  double theta = 0;
  double phi = 0;

 public:
  extract_coordinates(){}

  explicit extract_coordinates(matrix<double> column_vector)
  {
    double m = column_vector(0,0);
    double n = column_vector(1,0);
    double p = column_vector(2,0);

    theta = atan2(n,m);
    phi = asin(p);
  }

  pair<angle_radian,angle_radian> get_coordinates() const{
    return make_pair(theta * bu::si::radian,phi * bu::si::radian);
  }
};

struct obliquity_of_ecliptic{
 private:
  angle_radian e = 0.0 * bu::si::radian;

 public:
  obliquity_of_ecliptic(date d)
  {
    double julian_date = d.julian_day();

    double modified_julian_date = julian_date - 2451545.0;

    double julian_centuries = modified_julian_date / 36525.0;

    double e_degrees =  23.439292 - (46.815 * julian_centuries + (0.0006 - 0.00181 * julian_centuries) * julian_centuries * julian_centuries) / 3600.0;

    e = (e_degrees * PI / 180.0) * bu::si::radian;
  }

  angle_radian get(){
    return e;
  }
};

}}}

#endif  // BOOST_ASTRONOMY_UTILITY_HPP


//savageTestMain.cpp

#include "savageFunctions.h"
#include <iostream>
#include <cmath>
#include <cstdlib>
#include <string>
#include <Eigen/Dense>

int main()
{

	// Eigen::Matrix2d m;
	// 	m(0,0) = 1;
	// 	m(0,1) = 2;
	// 	m(1,0) = 3;
	// 	m(1,1) = 4;

	// std::cout << m << std::endl;

	// Eigen::Vector3d testVec;
	// 	testVec << 1,2,3;
	// 	// testVec(1) = 2;
	// 	// testVec(2) = 3;

	// std::cout << testVec << std::endl;

	// //instance of the class
	savageFunctions savageObject;
	// std::cout << savageObject.skew(testVec) << std::endl;

	Eigen::Matrix3d dcm;
	dcm << 1, 0, 0, 0, 1, 0, 0, 0, 1;

	Eigen::Vector3d acc;
	acc << 1.31, -2.5, 4.37;

	Eigen::Vector3d omega;
	omega << 0.23,0.42,-0.11;

	double t = .54;

	Eigen::VectorXd state(9);
	state << 0,0,0,0,0,0,0,0,0;

	std::cout << savageObject.stateIntegrate(omega, acc, state, dcm, t) << std::endl;

	return 0;
}


#include <gmock/gmock.h>
#include <Eigen/Dense>
#include <chrono>

class eigen_runtime_test: public testing::Test
{
  public:
};

template<int dim=3>
void test_rt(int const num_iterations)
{
  Eigen::MatrixXd M1 =  Eigen::MatrixXd::Random(dim,dim);
  Eigen::Matrix<double, dim, dim> M_stat = Eigen::Matrix<double, dim, dim>::Random();

  typedef std::chrono::high_resolution_clock clk_t;
  typedef clk_t::time_point tp_t;
  {
    tp_t t1 = clk_t::now();

    for(int i=0; i < num_iterations; i++)
    {
      M1 = M1*M1;
    }
    tp_t t2 = clk_t::now();

    auto duration = std::chrono::duration_cast<std::chrono::microseconds>( t2 - t1 ).count();
    std::cout << "["<<dim << "x"<< dim << "] Dynamic * Dynamic avg: " << duration/(1.0*num_iterations) << " us" << std::endl;
  }

  {
    tp_t t1 = clk_t::now();

    for(int i=0; i < num_iterations; i++)
    {
      M_stat = M_stat*M_stat;
    }
    tp_t t2 = clk_t::now();

    auto duration = std::chrono::duration_cast<std::chrono::microseconds>( t2 - t1 ).count();
    std::cout << "["<<dim << "x"<< dim << "] Static * Static avg: " << duration/(1.0*num_iterations) << " us" << std::endl;
  }

  {
    tp_t t1 = clk_t::now();

    for(int i=0; i < num_iterations; i++)
    {
      M_stat = M1*M_stat;
    }
    tp_t t2 = clk_t::now();

    auto duration = std::chrono::duration_cast<std::chrono::microseconds>( t2 - t1 ).count();
    std::cout << "["<<dim << "x"<< dim << "] Dynamic * Static avg: " << duration/(1.0*num_iterations) << " us" << std::endl;
  }
}

TEST_F(eigen_runtime_test, time_measurement_3)
{
  test_rt<3>(100000);
}

TEST_F(eigen_runtime_test, time_measurement_20)
{
  test_rt<20>(10000);
}

TEST_F(eigen_runtime_test, time_measurement_100)
{
  test_rt<100>(100);
}


#include <cstdlib>
#include <iostream>
#include <type_traits>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/hermitian.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/banded.hpp>
#include <boost/numeric/bindings/ublas/hermitian.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include <boost/numeric/bindings/lapack/driver.hpp>
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;
namespace lapack=boost::numeric::bindings::lapack;

int main(int argc, char *argv[]) {
  typedef ublas::vector<double> vector;
  typedef ublas::matrix<double, ublas::column_major> matrix;
  typedef ublas::banded_matrix<double, ublas::column_major> banded_matrix;
  typedef typename std::make_signed<vector::size_type>::type size_type;

  rand_normal<double>::reset();
  size_type n=128, k=8;
  banded_matrix A(n, n, 0, k);
  for (size_type j=0; j<n; ++j) {
    for (size_type i=std::max(j-k, size_type(0)); i<=j; ++i)
      A(i, j)=rand_normal<double>::get();
  }
  {
    vector lambda(n);
    banded_matrix A_bak(A);
    matrix vr(n ,n);
    ublas::hermitian_adaptor<banded_matrix, ublas::upper> B(A);
    int info=lapack::sbev('V', B, lambda, vr);
    ublas::hermitian_adaptor<banded_matrix, ublas::upper> B_bak(A_bak);
    if (info==0) {
      for (int i=0; i<n; ++i) {
  	// res <- A*vr(i) - lambda(i)*vr(i)
  	ublas::matrix_column<matrix> v(vr, i);
  	vector res(ublas::prod(B_bak, v)-lambda(i)*v);
  	std::cout << "norm of residual (right eigen vector " << i
  		  << " ): " << blas::nrm2(res) << '\n';
      }
    } else
      if (info>0)
  	std::cout << "unable to compute all eigen values\n";
      else
  	std::cout << "illegal arguments\n";
  }
  return EXIT_SUCCESS;
}


#include <ceres/ceres.h>
#include <Eigen/Core>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/quaternion-math.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

#include "ceres-error-terms/parameterization/quaternion-param-jpl.h"
#include "ceres-error-terms/test/parameterization-numerical-diff.h"

TEST(JplQuaternionParameterization, JacobianCorrect) {
  typedef Eigen::Matrix<double, 4, 3, Eigen::RowMajor> JacobianType;
  Eigen::Quaterniond q_AB(1.0, 1.0, 0.0, 1.0);
  q_AB.normalize();

  ceres_error_terms::JplQuaternionParameterization parameterization;
  JacobianType dq_dtheta;
  ASSERT_TRUE(
      parameterization.ComputeJacobian(q_AB.coeffs().data(), dq_dtheta.data()));

  JacobianType dq_dtheta_numeric;
  const bool success =
      ceres_error_terms::EvaluateNumericalJacobianOfParameterization<
          ceres_error_terms::JplQuaternionParameterization, 4, 3>(
          parameterization, q_AB.coeffs(), &dq_dtheta_numeric);
  ASSERT_TRUE(success);

  EXPECT_NEAR_EIGEN(dq_dtheta, dq_dtheta_numeric, 1e-10);
}

TEST(JplYawQuaternionParameterization, Plus) {
  Eigen::Quaterniond q_AB(1.0, 1.0, 0.0, 1.0);
  q_AB.normalize();
  const Eigen::Vector3d rpy_init =
      common::getRollPitchYawFromQuaternionJpl(q_AB);

  double delta_total_rad = 0.0;
  ceres_error_terms::JplYawQuaternionParameterization yaw_param;
  for (double delta : std::vector<double>{-0.5, 0.0, 0.5, 1.0}) {
    Eigen::Quaterniond q_AB_plus_delta;
    ASSERT_TRUE(yaw_param.Plus(q_AB.coeffs().data(), &delta,
                               q_AB_plus_delta.coeffs().data()));
    delta_total_rad += delta;
    q_AB = q_AB_plus_delta;

    const Eigen::Vector3d rpy =
        common::getRollPitchYawFromQuaternionJpl(q_AB_plus_delta);
    EXPECT_NEAR(rpy_init[0], rpy[0], 1e-6);
    EXPECT_NEAR(rpy_init[1], rpy[1], 1e-6);

    if (delta_total_rad == 0.0) {
      // We are at the same yaw as initially.
      EXPECT_NEAR(rpy_init[2], rpy[2], 1e-6);
    } else {
      EXPECT_NE(rpy_init[2], rpy[2]);
    }
  }
}

TEST(JplYawQuaternionParameterization, JacobianCorrect) {
  typedef Eigen::Matrix<double, 4, 1> JacobianType;
  Eigen::Quaterniond q_AB(1.0, 1.0, 0.0, 1.0);
  q_AB.normalize();

  ceres_error_terms::JplYawQuaternionParameterization parameterization;
  JacobianType dq_dtheta;
  ASSERT_TRUE(
      parameterization.ComputeJacobian(q_AB.coeffs().data(), dq_dtheta.data()));

  JacobianType dq_dtheta_numeric;
  const bool success =
      ceres_error_terms::EvaluateNumericalJacobianOfParameterization<
          ceres_error_terms::JplYawQuaternionParameterization, 4, 1>(
          parameterization, q_AB.coeffs(), &dq_dtheta_numeric);
  ASSERT_TRUE(success);

  EXPECT_NEAR_EIGEN(dq_dtheta, dq_dtheta_numeric, 1e-10);
}

TEST(JplRollPitchQuaternionParameterization, JacobianCorrect) {
  typedef Eigen::Matrix<double, 4, 2, Eigen::RowMajor> JacobianType;
  Eigen::Quaterniond q_IM(1.0, 1.0, 5.0, 0.0);
  q_IM.normalize();

  Eigen::Quaterniond q_GM(1.0, 5.0, 0.0, 2.0);
  q_GM.normalize();

  ceres_error_terms::JplRollPitchQuaternionParameterization parameterization(
      q_GM.coeffs());
  JacobianType dq_dtheta;
  ASSERT_TRUE(
      parameterization.ComputeJacobian(q_IM.coeffs().data(), dq_dtheta.data()));

  JacobianType dq_dtheta_numeric;
  const bool success =
      ceres_error_terms::EvaluateNumericalJacobianOfParameterization<
          ceres_error_terms::JplRollPitchQuaternionParameterization, 4, 2>(
          parameterization, q_IM.coeffs(), &dq_dtheta_numeric);
  ASSERT_TRUE(success);

  EXPECT_NEAR_EIGEN(dq_dtheta, dq_dtheta_numeric, 1e-10);
}

MAPLAB_UNITTEST_ENTRYPOINT


#include "deadreckoning.h"

#include "math.h"
#include "geometry.h"
#include "nonmoveable.h"
#include "occupancy_grid.h"
#include "occupancy_grid.inl"
#include "robot_configuration.h"

#include <vector>
#include <chrono>
#include <boost/algorithm/cxx11/all_of.hpp>

CDeadReckoningMapping::CDeadReckoningMapping()
{}

void CDeadReckoningMapping::receivedSensorData(SOdometryData const& odom) {
    auto const posePrev = m_vecpose.empty()
        ? rbt::pose<double>(rbt::point<double>::zero(), 0)
        : m_vecpose.back();

    auto const poseNew = UpdatePose(posePrev, odom);

    m_vecpose.emplace_back(poseNew);
}

// void CDeadReckoningMapping::receivedSensorData(SScanLine const& scanline) {
//     m_occgrid.update(m_vecpose.back(), rbt::rad(data.m_nAngle), data.m_nDistance);
// }

cv::Mat const& CDeadReckoningMapping::getMap() {
    return m_occgrid.ObstacleMap();
}


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <vector>

#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>

using namespace boost;

//a class to hold the coordinates of the straight line embedding
struct coord_t
{
  std::size_t x;
  std::size_t y;
};

int main(int argc, char** argv)
{
  typedef adjacency_list
    < vecS,
      vecS,
      undirectedS,
      property<vertex_index_t, int>
    > graph;

  //Define the storage type for the planar embedding
  typedef std::vector< std::vector< graph_traits<graph>::edge_descriptor > >
    embedding_storage_t;
  typedef boost::iterator_property_map
    < embedding_storage_t::iterator,
      property_map<graph, vertex_index_t>::type
    >
    embedding_t;

  // Create the graph - a maximal planar graph on 7 vertices. The functions
  // planar_canonical_ordering and chrobak_payne_straight_line_drawing both
  // require a maximal planar graph. If you start with a graph that isn't
  // maximal planar (or you're not sure), you can use the functions
  // make_connected, make_biconnected_planar, and make_maximal planar in
  // sequence to add a set of edges to any undirected planar graph to make
  // it maximal planar.

  graph g(7);
  add_edge(0,1,g);
  add_edge(1,2,g);
  add_edge(2,3,g);
  add_edge(3,0,g);
  add_edge(3,4,g);
  add_edge(4,5,g);
  add_edge(5,6,g);
  add_edge(6,3,g);
  add_edge(0,4,g);
  add_edge(1,3,g);
  add_edge(3,5,g);
  add_edge(2,6,g);
  add_edge(1,4,g);
  add_edge(1,5,g);
  add_edge(1,6,g);

  // Create the planar embedding
  embedding_storage_t embedding_storage(num_vertices(g));
  embedding_t embedding(embedding_storage.begin(), get(vertex_index,g));

  boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                               boyer_myrvold_params::embedding = embedding
                               );

  // Find a canonical ordering
  std::vector<graph_traits<graph>::vertex_descriptor> ordering;
  planar_canonical_ordering(g, embedding, std::back_inserter(ordering));

  //Set up a property map to hold the mapping from vertices to coord_t's
  typedef std::vector< coord_t > straight_line_drawing_storage_t;
  typedef boost::iterator_property_map
    < straight_line_drawing_storage_t::iterator,
      property_map<graph, vertex_index_t>::type
    >
    straight_line_drawing_t;

  straight_line_drawing_storage_t straight_line_drawing_storage
    (num_vertices(g));
  straight_line_drawing_t straight_line_drawing
    (straight_line_drawing_storage.begin(),
     get(vertex_index,g)
     );

  // Compute the straight line drawing
  chrobak_payne_straight_line_drawing(g,
                                      embedding,
                                      ordering.begin(),
                                      ordering.end(),
                                      straight_line_drawing
                                      );

  std::cout << "The straight line drawing is: " << std::endl;
  graph_traits<graph>::vertex_iterator vi, vi_end;
  for(boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
    {
      coord_t coord(get(straight_line_drawing,*vi));
      std::cout << *vi << " -> (" << coord.x << ", " << coord.y << ")"
                << std::endl;
    }

  // Verify that the drawing is actually a plane drawing
  if (is_straight_line_drawing(g, straight_line_drawing))
    std::cout << "Is a plane drawing." << std::endl;
  else
    std::cout << "Is not a plane drawing." << std::endl;

  return 0;
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-ieee
    This function object returns  \f$x\f$ multiplied by
    by \f$2^{n}\f$

    @par Header <boost/simd/function/ldexp.hpp>

    @par Note

     If @c n is not of integral type it is truncated:
     `ldexp(x,n)` is similar to: `x*pow(2, trunc(n))`

    @pre
     The @ref cardinal_of and the size of elements value of the types
     of @c x and @c n must be identical

    @par Decorators

     - pedantic_ By default @c ldexp does not take care of denormal or limiting values.
       Use the @c pedantic_ decorator if these are to be properly computed.

     - std_ give access to std::ldexp

    @par Example:

      @snippet ldexp.cpp ldexp

    @par Possible output:

      @snippet ldexp.txt ldexp

  **/
  Value0 ldexp(Value0 const& x, Value1 const& n);
} }
#endif

#include <boost/simd/function/scalar/ldexp.hpp>
#include <boost/simd/function/simd/ldexp.hpp>

#endif


//////////////////////////////////////////////////////////////////////////////
// random::poisson::devroye::detail::crtp::crtp.hpp                        	//
//                                                                          //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are 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)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_POISSON_EXT_DEVROYE_CRTP_CRTP_HPP_ER_2010
#define BOOST_RANDOM_POISSON_EXT_DEVROYE_CRTP_CRTP_HPP_ER_2010
#include <boost/range.hpp>
#include <boost/math/special_functions/modf.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/exponential_distribution.hpp>

#include <boost/random/poisson_ext/devroye/crtp/parameters.hpp>

namespace boost{
namespace random{
namespace poisson{
namespace devroye{

    // Samples from the poisson distribution with an integer mean
    //
    // Source : The computer generation of poisson random variables. L. Devroye,
    // Computing 26, Springer-Verlag, 1981
    //
    // Parameter     Description
    // Step4         A class that models the concept by the same name
    // Int           An integer type
    // T             A float type
    // P             An error handling policy
    // IntT          A converter from Int to T
    // TInt          A converter from T to Int
	//
    // Requiremens
    // Expression                       Description
    // crtp<Step4,Int,T,P,IntT,TInt>    Public base of Step4
	//
    // Concept Step4:
    // crtp<Step4,Int,T,P> is a public base, and
    // Expression    Result type   Side effect
    //  accept()     bool          if true, y is set to the value to be returned
    //
    // The numbers on the right are those of the lines of the Fortran listing
    // given in the reference above.
    template<typename Step4,typename Int,typename T,
        typename P = boost::math::policies::policy<>,
        typename IntT = boost::numeric::converter<T,Int>,
        typename TInt = boost::numeric::converter<Int,T>
    >
    class crtp : public devroye::parameters<Int,T,P,IntT>
    {
        typedef devroye::parameters<Int,T,P,IntT> parameters_;
        typedef IntT converter_;
        typedef std::string str_;

		public:

        typedef T input_type;
        typedef Int result_type;

        T mean() const { return converter_::convert(this->i_mean_); }
        void reset() { }

		crtp(){}
        explicit crtp(const Int& i_mean)
        	:parameters_(i_mean),max_step1_cnt_(10)
            {
            	BOOST_ASSERT(this->i_mean()>=0);
            }

        explicit crtp(const Int& i_mean,const Int& max_step1_cnt)
        	:parameters_(i_mean),max_step1_cnt_(max_step1_cnt)
            {
            	BOOST_ASSERT(this->i_mean()>=0);
            }

  		template<class U>
  		Int operator()(U& urng)const
  		{
            this->step1_cnt_ = 0;
			Int result =  this->step1(urng);
            BOOST_ASSERT(result >= 0);
            return result;
        }

        const Int& step1_cnt()const{ return this->step1_cnt_; }

        // --- I/O --- //

        template<class CharT, class Traits>
        friend std::basic_ostream<CharT,Traits>&
        operator<<(
            std::basic_ostream<CharT,Traits>& os,
            const crtp& pd
        )
        {
            os 	<< "devroye("
                << pd.i_mean()
                << ','
                << pd.u()
                << ','
                << pd.x()
                << ','
                << pd.i_y()
                << ','
                << pd.v()
                << ','
                << pd.step1_cnt()
                << ')';
            return os;
        }

        template<class CharT, class Traits>
        friend std::basic_istream<CharT,Traits>&
        operator>>(std::basic_istream<CharT,Traits>& is, crtp& pd)
        {
            Int new_mean;
            is	>> std::ws
                >> new_mean
                >> std::ws
                >> std::ws 	// u
                >> std::ws
                >> std::ws	// x
                >> std::ws
                >> std::ws	// y
                >> std::ws
                >> std::ws	// v
                >> std::ws
                >> std::ws	// step1_cnt
                >> std::ws;
            static_cast<Step4&>(pd) = Step4(new_mean);
            return is;
        }

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

        protected:

		template<typename U>
		Int step1(U& urng)const
        {
            static const str_ name = "devroy::crtp<%1%>::step1()";

            using namespace boost::math::policies;
            if( ( this->step1_cnt_++ ) > this->max_step1_cnt() ){
                 raise_evaluation_error( //<boost::uintmax_t>
                    name.c_str(),
                    "step1_cnt_ exceeded limit = %1%",
                    this->max_step1_cnt(), P());
            }
            // line 10 : case mean = 0;

            this->u_ = urng();												//13
            if( this->u() > this->p1() ){
            	// Reconciliation with Fortran listing :
                // ptail = p2 + p3 = 1-p1.
                // u' ~ 1-u. (u'<ptail) <=> (1-u<p2+p3) <=> (u > p1)
            	return this->step3( urng );
            }else{
                return this->step2( urng );
            }
		}

		template<typename U>
		Int step2(U& urng)const
        {
        	T z = this->random_z( urng ); 							        //15
        	this->x_ = ( this->sd1() * z ) + parameters_::loc1();
            if(
                ( this->x() > this->delta() ) || ( this->x() < ( -this->m1() ) )
            )
            {
            	return this->step1( urng );
            }else{
                using namespace std;
            	// y is rounded away from zero
                if( this->x() <= -parameters_::eps /*strictly negative*/)
                {
                    this->i_y_ = floor( this->x() );
                }else{
                    this->i_y_ = ceil( this->x() );
                }
                T e  = this->random_exp1(urng);
                this->v_ = -( e + pow( z, 2 ) / parameters_::two ) + this->c1();
                return this->step4( urng );
            }
        }

		template<typename U>
		Int step3(U& urng)const
        {

            // Reconciliation with Fortran listing :
            // pbody = p3 = 1-(p1+p2)
            // u' ~ 1-u. (u'<pbody) <=> (1-u<p3) <=> (u > p1+p2)
        	if( this->u() > this->p1() + this->p2() ){
            	this->i_y_ = this->i_mean();
                return this->i_y();
            }else{
                using namespace std;
            	T e1 = this->random_exp1( urng );
            	T e2 = this->random_exp1( urng );
            	this->x_ = this->delta() + e1 / this->shape2();				//51
            	this->i_y_ = TInt::convert( ceil( this->x() ) );
            	this->v_ = - (
                	e2 + this->shape2() * ( parameters_::one + this->x() )
                );
            	return this->step4( urng );
            }
        }

		template<typename U>
		Int step4(U& urng)const
        {
            const Step4& derived = static_cast<const Step4&>(*this);
            if( derived.accept() ){
            	return this->i_y();
            }else{
				return this->step1( urng );
            }
		}

		template<typename U>
		T random_z(U& urng)const
        {
        	typedef boost::normal_distribution<T> d_;
            static d_ d = d_( parameters_::zero, parameters_::one );
            return d( urng );
        }

		template<typename U>
		T random_exp1(U& urng)const
        {
        	typedef boost::exponential_distribution<T> d_;
            static d_ d = d_( parameters_::one );
            return d( urng );
        }

		// Parameter
		Int i_mean_;

        // These are quantities that vary throughout sampling
		mutable T	u_;
        mutable T 	x_;
        mutable Int i_y_;
        mutable T 	v_;

        // Implementation detail
        mutable Int step1_cnt_;
        mutable Int max_step1_cnt_;

        const Int& 	i_mean()const{ return this->i_mean_; }
        const T& 	u()const{ return this->u_; }
        const T& 	x()const{ return this->x_; }
        const Int& 	i_y()const{ return this->i_y_; }
        const T& 	v()const{ return this->v_; }
        const Int& 	max_step1_cnt()const{ return this->max_step1_cnt_; }

	};

}// devroye
}// poisson
}// random
}// boost

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-ieee
    Computes the distance between its argument and the next representable value of its argument type.

    @par Semantic:

    For every parameter of type @c T

    @code
    T r = eps(x);
    @endcode

    is similar to:

    @code
    T  r = pow(2,exponent(x))*Eps<T>();
    @endcode

    if @c T is a floating-point type and to :

    @code
    T r = 1;
    @endcode

    if @c T is an integral type.

    @param  x Base value for epsilon computation.
    @return The distance between @c x and its next representable value.

    @see ulp, ulpdist, Eps
**/
  Value eps(Value const & x);

} }
#endif

#include <boost/simd/function/scalar/eps.hpp>
#include <boost/simd/function/simd/eps.hpp>

#endif


#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <fmt/format.h>
#include <string>
#include <iomanip>
#include <boost/variant.hpp>
#include <unordered_map>

struct Not
{
};
struct And
{
};
struct Or
{
};
struct Lshift
{
};
struct Rshift
{
};

template<typename OpTag>
struct binary_op;
//template<typename OpTag>
//struct unary_op;
typedef boost::variant<unsigned short,
  boost::recursive_wrapper<binary_op<And>>,
  boost::recursive_wrapper<binary_op<Or>>,
  boost::recursive_wrapper<binary_op<Lshift>>,
  boost::recursive_wrapper<binary_op<Rshift>>,
  boost::recursive_wrapper<binary_op<Not>>>
  Expr;
typedef std::unordered_map<std::string, Expr> ExprMap;
template<typename OpTag>
struct binary_op
{
  Expr left;
  Expr right;

  binary_op(const Expr &lhs, const Expr &rhs)
    : left(lhs), right(rhs)
  {
  }
  binary_op() {}
};
template<class IntType>
class Calculator : public boost::static_visitor<IntType>
{
public:
  using value_type = IntType;
  IntType operator()(IntType value) const
  {
    //std::cout << "calculator value:" << value << std::endl;
    return value;
  }

  IntType operator()(const binary_op<And> &binary) const
  {
    return boost::apply_visitor(Calculator(), binary.left)
           & boost::apply_visitor(Calculator(), binary.right);
  }

  IntType operator()(const binary_op<Or> &binary) const
  {
    return boost::apply_visitor(Calculator(), binary.left)
           | boost::apply_visitor(Calculator(), binary.right);
  }
  IntType operator()(const binary_op<Lshift> &binary) const
  {
    return boost::apply_visitor(Calculator(), binary.left)
           << boost::apply_visitor(Calculator(), binary.right);
  }
  IntType operator()(const binary_op<Rshift> &binary) const
  {
    return boost::apply_visitor(Calculator(), binary.left)
           >> boost::apply_visitor(Calculator(), binary.right);
  }
  IntType operator()(const binary_op<Not> &binary) const
  {
    return ~(boost::apply_visitor(Calculator(), binary.left));
  }
};
using AocCal = Calculator<unsigned short>;
struct IS
{
  AocCal c_;
  ExprMap exprmap_;
  AocCal::value_type operator()(const std::string &s) const
  {
    if (auto i = exprmap_.find(s); i != exprmap_.end()) {
      AocCal::value_type r = boost::apply_visitor(c_, i->second);
      return r;
    } else {
      return 0;
    }
  }
};

int main(int argc, char **argv)
{
  Expr e;
  e = 32;

  if (argc > 1) {
    std::ifstream ifs(argv[1]);
    std::string line;
    IS asms;
    while (std::getline(ifs, line)) {
      std::istringstream iss(line);
      std::vector<std::string> v;
      std::istream_iterator<std::string> b(iss);
      std::istream_iterator<std::string> e;
      std::copy(b, e, std::back_inserter(v));
      if (v.size() == 3) {
        //std::cout << v[0] << "\n";
        if (std::isdigit(v[0][0])) {
          auto n = static_cast<unsigned short>(std::stoi(v[0]));
          asms.exprmap_[v[2]] = n;
        } else {
          asms.exprmap_[v[2]] = asms.exprmap_[v[0]];
        }
      } else if (v.size() == 4) {
        asms.exprmap_[v[3]] = binary_op<Not>(asms.exprmap_[v[1]], asms.exprmap_[v[1]]);
      } else if (v.size() == 5) {
        if (v[1] == "OR") {
          asms.exprmap_[v[4]] = binary_op<Or>(asms.exprmap_[v[0]], asms.exprmap_[v[2]]);
        } else if (v[1] == "AND") {
          asms.exprmap_[v[4]] = binary_op<And>(asms.exprmap_[v[0]], asms.exprmap_[v[2]]);
        } else if (v[1] == "LSHIFT") {
          //std::cout << v[2] << "\n";
          asms.exprmap_[v[4]] = binary_op<Lshift>(asms.exprmap_[v[0]], std::stoi(v[2]));
        } else if (v[1] == "RSHIFT") {
          //std::cout << v[2] << "\n";
          asms.exprmap_[v[4]] = binary_op<Rshift>(asms.exprmap_[v[0]], std::stoi(v[2]));
        }
      }
    }
    std::cout << asms("d") << std::endl;
    std::cout << asms("e") << std::endl;
    std::cout << asms("f") << std::endl;
    std::cout << asms("g") << std::endl;
    std::cout << asms("h") << std::endl;
    std::cout << asms("i") << std::endl;
    std::cout << asms("x") << std::endl;
    std::cout << asms("y") << std::endl;
  }
}


/**
 * @file Newton.t.h Test driver for newton interpolation algorithm
 *
 * Copyright (C) 2010  Arthur D. Cherba
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "interp/Newton.h"
#include "operation/field/setup.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(newton)

BOOST_AUTO_TEST_CASE_TEMPLATE(newton_alg, F, math::FieldTypes) {
  math::FieldFixture<F> f;
  BOOST_CHECK(f);  // avoid unreferenced local variable warning

  BOOST_TEST_MESSAGE("  Testing " << F::getName());

  int const MAX_DEG = 10;
  F x[MAX_DEG + 1];
  F y[MAX_DEG + 1];
  // Assume polynomial f = sum (i+1) x^i
  for (int i = 0; i <= MAX_DEG; ++i) {
    F sum;
    sum.template setTo<0>();
    x[i].setTo(i+1);
    for (int j = 0; j <= MAX_DEG; ++j) {
      sum += x[i].getPow(j) * F(j+1);
    }
    y[i] = sum;
  }
  boost::scoped_array<F> coeffs(new F[MAX_DEG+1]);
  interp::newton(MAX_DEG, x, y, coeffs.get());

  // verify result
  for (int i = 0; i <= MAX_DEG; ++i) {
    BOOST_CHECK_MESSAGE(coeffs[i] == F(i+1),
        " f[i]==F(i+1) with i=" << i
        << " f[i]=" << coeffs[i] << " F(i+1)" << F(i+1));
  }
}

BOOST_AUTO_TEST_SUITE_END()

bool init_unit_test() {
  return true;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file testNoiseModel.cpp
 * @date Jan 13, 2010
 * @author Richard Roberts
 * @author Frank Dellaert
 */

#include <iostream>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/linear/NoiseModel.h>

using namespace std;
using namespace gtsam;
using namespace noiseModel;

static double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var;
static Matrix R = Matrix_(3, 3,
		s_1, 0.0, 0.0,
		0.0, s_1, 0.0,
		0.0, 0.0, s_1);
static Matrix Sigma = Matrix_(3, 3,
		var, 0.0, 0.0,
		0.0, var, 0.0,
		0.0, 0.0, var);

//static double inf = std::numeric_limits<double>::infinity();

/* ************************************************************************* */
TEST(NoiseModel, constructors)
{
	Vector whitened = Vector_(3,5.0,10.0,15.0);
	Vector unwhitened = Vector_(3,10.0,20.0,30.0);

	// Construct noise models
	vector<Gaussian::shared_ptr> m;
	m.push_back(Gaussian::SqrtInformation(R));
	m.push_back(Gaussian::Covariance(Sigma));
	//m.push_back(Gaussian::Information(Q));
	m.push_back(Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma)));
	m.push_back(Diagonal::Variances(Vector_(3, var, var, var)));
	m.push_back(Diagonal::Precisions(Vector_(3, prc, prc, prc)));
	m.push_back(Isotropic::Sigma(3, sigma));
	m.push_back(Isotropic::Variance(3, var));
	m.push_back(Isotropic::Precision(3, prc));

	// test whiten
	BOOST_FOREACH(Gaussian::shared_ptr mi, m)
		EXPECT(assert_equal(whitened,mi->whiten(unwhitened)));

	// test unwhiten
	BOOST_FOREACH(Gaussian::shared_ptr mi, m)
		EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened)));

	// test Mahalanobis distance
	double distance = 5*5+10*10+15*15;
	BOOST_FOREACH(Gaussian::shared_ptr mi, m)
		DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9);

	// test R matrix
	Matrix expectedR(Matrix_(3, 3,
			s_1, 0.0, 0.0,
			0.0, s_1, 0.0,
			0.0, 0.0, s_1));

	BOOST_FOREACH(Gaussian::shared_ptr mi, m)
		EXPECT(assert_equal(expectedR,mi->R()));

	// test Whiten operator
	Matrix H(Matrix_(3, 4,
			0.0, 0.0, 1.0, 1.0,
			0.0, 1.0, 0.0, 1.0,
			1.0, 0.0, 0.0, 1.0));

	Matrix expected(Matrix_(3, 4,
			0.0, 0.0, s_1, s_1,
			0.0, s_1, 0.0, s_1,
			s_1, 0.0, 0.0, s_1));

	BOOST_FOREACH(Gaussian::shared_ptr mi, m)
		EXPECT(assert_equal(expected,mi->Whiten(H)));

	// can only test inplace version once :-)
	m[0]->WhitenInPlace(H);
	EXPECT(assert_equal(expected,H));
}

/* ************************************************************************* */
TEST(NoiseModel, Unit)
{
	Vector v = Vector_(3,5.0,10.0,15.0);
	Gaussian::shared_ptr u(Unit::Create(3));
	EXPECT(assert_equal(v,u->whiten(v)));
}

/* ************************************************************************* */
TEST(NoiseModel, equals)
{
	Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R),
											 g2 = Gaussian::SqrtInformation(eye(3,3));
	Diagonal::shared_ptr d1 = Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma)),
											 d2 = Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
	Isotropic::shared_ptr i1 = Isotropic::Sigma(3, sigma),
												i2 = Isotropic::Sigma(3, 0.7);

	EXPECT(assert_equal(*g1,*g1));
	EXPECT(assert_inequal(*g1, *g2));

	EXPECT(assert_equal(*d1,*d1));
	EXPECT(assert_inequal(*d1,*d2));

	EXPECT(assert_equal(*i1,*i1));
	EXPECT(assert_inequal(*i1,*i2));
}

// TODO enable test once a mechanism for smart constraints exists
///* ************************************************************************* */
//TEST(NoiseModel, ConstrainedSmart )
//{
//	Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma), true);
//	Diagonal::shared_ptr n1 = boost::shared_dynamic_cast<Diagonal>(nonconstrained);
//	Constrained::shared_ptr n2 = boost::shared_dynamic_cast<Constrained>(nonconstrained);
//	EXPECT(n1);
//	EXPECT(!n2);
//
//	Gaussian::shared_ptr constrained = Constrained::MixedSigmas(zero(3), true);
//	Diagonal::shared_ptr c1 = boost::shared_dynamic_cast<Diagonal>(constrained);
//	Constrained::shared_ptr c2 = boost::shared_dynamic_cast<Constrained>(constrained);
//	EXPECT(c1);
//	EXPECT(c2);
//}

/* ************************************************************************* */
TEST(NoiseModel, ConstrainedConstructors )
{
	Constrained::shared_ptr actual;
	size_t d = 3;
	double m = 100.0;
	Vector sigmas = Vector_(3, sigma, 0.0, 0.0);
	Vector mu = Vector_(3, 200.0, 300.0, 400.0);
	actual = Constrained::All(d);
	EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu()));

	actual = Constrained::All(d, m);
	EXPECT(assert_equal(gtsam::repeat(d, m), actual->mu()));

	actual = Constrained::All(d, mu);
	EXPECT(assert_equal(mu, actual->mu()));

	actual = Constrained::MixedSigmas(mu, sigmas);
	EXPECT(assert_equal(mu, actual->mu()));

	actual = Constrained::MixedSigmas(m, sigmas);
	EXPECT(assert_equal( gtsam::repeat(d, m), actual->mu()));
}

/* ************************************************************************* */
TEST(NoiseModel, ConstrainedMixed )
{
	Vector feasible = Vector_(3, 1.0, 0.0, 1.0),
			infeasible = Vector_(3, 1.0, 1.0, 1.0);
	Diagonal::shared_ptr d = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma));
	// NOTE: we catch constrained variables elsewhere, so whitening does nothing
	EXPECT(assert_equal(Vector_(3, 0.5, 1.0, 0.5),d->whiten(infeasible)));
	EXPECT(assert_equal(Vector_(3, 0.5, 0.0, 0.5),d->whiten(feasible)));

	DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9);
	DOUBLES_EQUAL(0.5,d->distance(feasible),1e-9);
}

/* ************************************************************************* */
TEST(NoiseModel, ConstrainedAll )
{
	Vector feasible = Vector_(3, 0.0, 0.0, 0.0),
			 infeasible = Vector_(3, 1.0, 1.0, 1.0);

	Constrained::shared_ptr i = Constrained::All(3);
	// NOTE: we catch constrained variables elsewhere, so whitening does nothing
	EXPECT(assert_equal(Vector_(3, 1.0, 1.0, 1.0),i->whiten(infeasible)));
	EXPECT(assert_equal(Vector_(3, 0.0, 0.0, 0.0),i->whiten(feasible)));

	DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9);
	DOUBLES_EQUAL(0.0,i->distance(feasible),1e-9);
}

/* ************************************************************************* */
namespace exampleQR {
  // create a matrix to eliminate
  Matrix Ab = Matrix_(4, 6+1,
      -1.,  0.,  1.,  0.,  0.,  0., -0.2,
      0., -1.,  0.,  1.,  0.,  0.,  0.3,
      1.,  0.,  0.,  0., -1.,  0.,  0.2,
      0.,  1.,  0.,  0.,  0., -1., -0.1);
  Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1);

  // the matrix AB yields the following factorized version:
	Matrix Rd = Matrix_(4, 6+1,
			11.1803,   0.0,   -2.23607, 0.0,    -8.94427, 0.0,     2.23607,
			0.0,   11.1803,    0.0,    -2.23607, 0.0,    -8.94427,-1.56525,
			0.0,       0.0,    4.47214, 0.0,    -4.47214, 0.0,     0.0,
			0.0,       0.0,   0.0,     4.47214, 0.0,    -4.47214, 0.894427);

	SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas);
}

TEST( NoiseModel, QR )
{
  Matrix Ab1 = exampleQR::Ab;
	Matrix Ab2 = exampleQR::Ab; // otherwise overwritten !

	// Expected result
	Vector expectedSigmas = Vector_(4, 0.0894427, 0.0894427, 0.223607, 0.223607);
	SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas);

	// Call Gaussian version
	SharedDiagonal actual1 = exampleQR::diagonal->QR(Ab1);
	SharedDiagonal expected = noiseModel::Unit::Create(4);
	EXPECT(assert_equal(*expected,*actual1));
	EXPECT(linear_dependent(exampleQR::Rd,Ab1,1e-4)); // Ab was modified in place !!!

	// Call Constrained version
	SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(exampleQR::sigmas);
	SharedDiagonal actual2 = constrained->QR(Ab2);
	SharedDiagonal expectedModel2 = noiseModel::Diagonal::Sigmas(expectedSigmas);
	EXPECT(assert_equal(*expectedModel2,*actual2,1e-6));
	Matrix expectedRd2 = Matrix_(4, 6+1,
			1.,  0., -0.2,  0., -0.8, 0.,  0.2,
			0.,  1.,  0.,-0.2,   0., -0.8,-0.14,
			0.,  0.,  1.,   0., -1.,  0.,  0.0,
			0.,  0.,  0.,   1.,  0., -1.,  0.2);
	EXPECT(linear_dependent(expectedRd2,Ab2,1e-6)); // Ab was modified in place !!!
}

/* ************************************************************************* */
TEST(NoiseModel, QRNan )
{
	SharedDiagonal constrained = noiseModel::Constrained::All(2);
	Matrix Ab = Matrix_(2, 5, 1., 2., 1., 2., 3., 2., 1., 2., 4., 4.);

	SharedDiagonal expected = noiseModel::Constrained::All(2);
	Matrix expectedAb = Matrix_(2, 5, 1., 2., 1., 2., 3., 0., 1., 0., 0., 2.0/3);

	SharedDiagonal actual = constrained->QR(Ab);
	EXPECT(assert_equal(*expected,*actual));
	EXPECT(assert_equal(expectedAb,Ab));
}

/* ************************************************************************* */
TEST(NoiseModel, SmartCovariance )
{
	bool smart = true;
	gtsam::SharedGaussian expected = Unit::Create(3);
	gtsam::SharedGaussian actual = Gaussian::Covariance(eye(3), smart);
	EXPECT(assert_equal(*expected,*actual));
}

/* ************************************************************************* */
TEST(NoiseModel, ScalarOrVector )
{
	bool smart = true;
	SharedGaussian expected = Unit::Create(3);
	SharedGaussian actual = Gaussian::Covariance(eye(3), smart);
	EXPECT(assert_equal(*expected,*actual));
}

/* ************************************************************************* */
TEST(NoiseModel, WhitenInPlace)
{
	Vector sigmas = Vector_(3, 0.1, 0.1, 0.1);
	SharedDiagonal model = Diagonal::Sigmas(sigmas);
	Matrix A = eye(3);
	model->WhitenInPlace(A);
	Matrix expected = eye(3) * 10;
	EXPECT(assert_equal(expected, A));
}

/* ************************************************************************* */
TEST(NoiseModel, robustFunction)
{
  const double k = 5.0, error1 = 1.0, error2 = 10.0;
  const MEstimator::Huber::shared_ptr huber = MEstimator::Huber::Create(k);
  const double weight1 = huber->weight(error1),
               weight2 = huber->weight(error2);
  DOUBLES_EQUAL(1.0, weight1, 1e-8);
  DOUBLES_EQUAL(0.5, weight2, 1e-8);
}

/* ************************************************************************* */
TEST(NoiseModel, robustNoise)
{
  const double k = 10.0, error1 = 1.0, error2 = 100.0;
  Matrix A = Matrix_(2, 2, 1.0, 10.0, 100.0, 1000.0);
  Vector b = Vector_(2, error1, error2);
  const Robust::shared_ptr robust = Robust::Create(
    MEstimator::Huber::Create(k, MEstimator::Huber::Scalar),
    Unit::Create(2));

  robust->WhitenSystem(A,b);

  DOUBLES_EQUAL(error1, b(0), 1e-8);
  DOUBLES_EQUAL(sqrt(k*error2), b(1), 1e-8);

  DOUBLES_EQUAL(1.0, A(0,0), 1e-8);
  DOUBLES_EQUAL(10.0, A(0,1), 1e-8);
  DOUBLES_EQUAL(sqrt(k*100.0), A(1,0), 1e-8);
  DOUBLES_EQUAL(sqrt(k/100.0)*1000.0, A(1,1), 1e-8);
}

/* ************************************************************************* */
int main() {	TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE MaxCliqueTest

#include <boost/test/unit_test.hpp>

#include <popart/maxclique.hpp>

using namespace popart;
using namespace popart::graphclique;

BOOST_AUTO_TEST_CASE(MaxCliqueTest_0) {

  graphclique::AGraph ag(7);

  for (int i = 0; i < 5; ++i) {
    for (int j = 0; j < 5; ++j) {
      ag.addEdge(i, j);
    }
  }

  for (int i = 4; i < 7; ++i) {
    for (int j = 4; j < 7; ++j) {
      ag.addEdge(i, j);
    }
  }

  graphclique::MaxClique mq(ag);

  auto mcliques = mq.getMaximumCliques(1, ag.numVertices());
  BOOST_ASSERT(mcliques.size() == 2);
  for (int i = 0; i < 5; ++i) {
    BOOST_ASSERT(std::find(mcliques[0].begin(), mcliques[0].end(), i) !=
                 mcliques[0].end());
  }
  for (int i = 5; i < 7; ++i) {
    BOOST_ASSERT(std::find(mcliques[1].begin(), mcliques[1].end(), i) !=
                 mcliques[1].end());
  }
}


#include <algorithm>
#include <charconv>
#include <iostream>
#include <limits>
#include <unordered_map>
#include <utility>
#include <vector>

#include <boost/range/irange.hpp>

#include "input.hpp"

using intmax = std::intmax_t;
using uintmax = std::uintmax_t;

constexpr auto SPACE = ' ';
constexpr auto NEWLINE = '\n';

/*
 * compile-time parsing to allow for reserving the size of vectors
 * when taking another pass through the puzzle input
 */
constexpr auto parse_stats(std::string_view input) {
  auto num_words = uintmax{},
       num_lines = uintmax{};

  for(auto c : input) {
    num_words += (c == SPACE);
    num_lines += (c == NEWLINE);
  }

  ++num_lines;
  num_words = (num_words / num_lines) + 1;

  return std::pair{num_words, num_lines};
}

constexpr auto PARSE_STATS = parse_stats(puzzle_input);
constexpr auto NUM_WORDS = PARSE_STATS.first;
constexpr auto NUM_LINES = PARSE_STATS.second;

auto triangle(uintmax n) {
  return (n * (n - 1)) / 2;
}

template<typename T>
class AdjacencyMatrix {
  private:
    std::vector<T> data;

  public:
    AdjacencyMatrix(uintmax size) : data(triangle(size), {}) {}

    auto operator[](uintmax i) {
      return &data[triangle(i)];
    }

    auto operator[](uintmax i) const {
      return &data[triangle(i)];
    }
};

using ID = uintmax;
using Pair = std::pair<ID, ID>;

auto operator==(const Pair& lhs, const Pair& rhs) {
  return lhs.first == rhs.first
         and lhs.second == rhs.second;
}

namespace std {
  template<>
  struct hash<Pair> {
    auto operator()(const Pair& pair) const {
      const auto shift = std::numeric_limits<decltype(pair.second)>::digits / 2;
      return pair.first + (pair.second << shift);
    }
  };
}

auto minmax(const Pair& pair) {
  return std::minmax(pair.first, pair.second);
}

class Happymeter {
  private:
    AdjacencyMatrix<intmax> matrix;

  public:
    uintmax num_people;

    Happymeter(uintmax size) : matrix(size), num_people(size) {};

    auto& operator[](const Pair& pair) {
      const auto [min, max] = minmax(pair);
      return matrix[max][min];
    }

    auto operator[](const Pair& pair) const {
      const auto [min, max] = minmax(pair);
      return matrix[max][min];
    }
};

auto parse(std::string_view input) {

  using Person = std::string_view;

  auto num_people = ID{};

  using People = std::unordered_map<Person, ID>;

  auto lookup = [&, people = People{}] (auto person) mutable {
    if(people.find(person) == people.end()) {
      people[person] = num_people++;
    }
    return people.at(person);
  };

  const auto split = [] (auto input, auto delimiter, auto size) {
    auto tokens = std::vector<decltype(input)>{};
    tokens.reserve(size);
    auto pos = decltype(input.size()){};
    while(pos != input.npos) {
      pos = input.find(delimiter);
      tokens.push_back(input.substr({}, pos));
      input.remove_prefix(pos + 1);
    }
    return tokens;
  };

  const auto to_int = [] (auto input) {
    intmax result;
    std::from_chars(input.begin(), input.end(), result);
    return result;
  };

  using Score = intmax;
  auto scores = std::unordered_map<Pair, Score>{};

  for(const auto line : split(input, NEWLINE, NUM_LINES)) {
    auto words = split(line, SPACE, NUM_WORDS);
    words.back().remove_suffix(1);

    const auto person1 = lookup(words.front());
    const auto person2 = lookup(words.back());

    const auto score = to_int(words[3]);
    const auto modifier = ((words[2] == "lose") ? -1 : 1);

    scores[{person1, person2}] = (score * modifier);
  }

  auto happymeter = Happymeter{num_people};

  for(const auto [pair, score] : scores) {
    happymeter[pair] += score;
  }

  return happymeter;
}

using Seating = std::vector<ID>;

auto get_happiness(const Seating& seating, const Happymeter& happymeter) {

  /*
   * We take out the last person so we can insert them
   * where the happiness is the lowest later on
   */
  const auto begin = seating.begin();
  const auto end  = seating.end() - 1;

  const auto first = seating.front();
  const auto last  = seating.back();
  const auto next_to_last = *(end - 1);

  /*
   * a) Because the seating is circular, the people at both ends of
   *    the seating need to be included as well, but it's not easily
   *    done when iterating over the seating. So we include that pair
   *    before the start of iterations.
   *
   * b) The last person is not yet included when collecting the scores
   *    for happiness (see comment at the start of this function), so
   *    we start off with the score for the next-to-last pair
   */
  const auto penultimate_score = happymeter[{first, next_to_last}];

  auto total_score = penultimate_score;
  auto min = penultimate_score;

  std::adjacent_find(begin, end, [&] (const auto a, const auto b) {
    const auto score = happymeter[{a, b}];
    total_score += score;
    min = std::min(min, score);
    return false;
  });

  /*
   * Now that we have all the necessary scores, we can insert the last
   * person in order to get the highest potential score.
   *
   * "Inserting" includes removing the lowest happiness score, then adding
   * two scores, one for the last two people and one for the people at both
   * ends of the seating.
   */
  total_score -= min;
  total_score += happymeter[{next_to_last, last}];
  total_score += happymeter[{first, last}];

  return total_score;
}

auto solution(std::string_view input) {

  auto max = intmax{};

  const auto happymeter = parse(input);
  const auto num_people = happymeter.num_people;

  auto seating = Seating(num_people, 0);

  /*
   * - Fixing one neighbor of the first person removes rotated seatings that are equivalent
   *   (A-B-C-D == D-A-B-C)
   * - Fixing the second neighbor removes mirrored seatings, which are also equivalent
   *   (A-B-C-D == D-C-B-A)
   * - Fixing the position of the first person to the second position of the seating lets
   *   us fill up the remaining seating from the fourth position to the end through
   *   std::generate.
   * - Fixing the last person allows us to only look at permutations of the first seven seats,
   *   at which point we can optimize the happiness function to insert the last person where
   *   the happiness score is the lowest - this removes another chunk of permutations to look at
   *
   * - The effect of fixing a total of four positions of the seating before performing permutations
   *   is a huge reduction of the search space: from 8! (40'320) to [(choose 2 out of 6) * 4!] (360).
   *   That's a reduction of 99%.
   */
  const auto num_permutable_seats = num_people - 1;
  seating[1] = 0;
  seating.back() = num_permutable_seats;

  for(const auto i : boost::irange({1}, (num_permutable_seats - 1))) {
    // fixt the first neighbor of the first person
    seating[0] = i;

    for(const auto j : boost::irange((i + 1), num_permutable_seats)) {
      // fix the second neighbor of the first person
      seating[2] = j;

      const auto begin = seating.begin() + 3;
      const auto end   = seating.end() - 1;

      auto current = ID{};

      // fill the remaining seats
      std::generate(begin, end, [&] {
        while(++current == i or current == j);
        return current;
      });

      do {
        max = std::max(max, get_happiness(seating, happymeter));
      } while(std::next_permutation(begin, end));
    }
  }

  return max;
}

int main() {
  std::cout << solution(puzzle_input) << std::endl;
}