/*
 * DIPlib 3.0
 * This file contains definitions for functions that do matrix computations using Eigen
 *
 * (c)2016-2017, Cris Luengo.
 * Based on original DIPlib code: (c)1995-2014, Delft University of Technology.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <array>
#include "diplib/library/numeric.h"

#if defined(__GNUG__) || defined(__clang__)
// For this file, turn off -Wsign-conversion, Eigen is really bad at this!
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-conversion"
#if __GNUC__ == 11
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif
#endif

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

namespace dip {

namespace {

template< class T >
struct GreaterMagnitude {
   bool operator()( T const& a, T const& b ) const {
      return std::abs( a ) > std::abs( b );
   }
};

} // namespace

void SymmetricEigenDecomposition(
      dip::uint n,
      ConstSampleIterator< dfloat > input,
      SampleIterator< dfloat > lambdas,
      SampleIterator< dfloat > vectors
) {
   DIP_ASSERT( input.Stride() >= 0 ); // TODO: (here and other asserts in this file) Eigen doesn't support negative strides, but there's a ticket for that: https://gitlab.com/libeigen/eigen/-/issues/747
   Eigen::Map< Eigen::MatrixXd const, 0, Eigen::InnerStride<> > matrix( input.Pointer(), n, n, Eigen::InnerStride<>( input.Stride() ));
   if( vectors ) {
      Eigen::SelfAdjointEigenSolver< Eigen::MatrixXd > eigensolver( matrix );
      //if( eigensolver.info() != Eigen::Success ) { abort(); }
      Eigen::VectorXd const& eigenvalues = eigensolver.eigenvalues();
      Eigen::MatrixXd const& eigenvectors = eigensolver.eigenvectors();
      std::vector< dip::uint > indices( n );
      std::iota( indices.begin(), indices.end(), 0 );
      std::sort( indices.begin(), indices.end(), [ & ]( dip::uint a, dip::uint b ) { return std::abs( eigenvalues[ b ] ) < std::abs( eigenvalues[ a ] ); } );
      for( dip::uint ii = 0; ii < n; ++ii ) {
         dip::uint kk = indices[ ii ];
         lambdas[ ii ] = eigenvalues[ kk ];
         dip::uint offset = ii * n;
         for( dip::uint jj = 0; jj < n; ++jj ) {
            vectors[ jj + offset ] = eigenvectors( jj, kk );
         }
      }
   } else {
      DIP_ASSERT( lambdas.Stride() >= 0 );
      Eigen::Map< Eigen::VectorXd, 0, Eigen::InnerStride<> > eigenvalues( lambdas.Pointer(), n, Eigen::InnerStride<>( lambdas.Stride() ));
      eigenvalues = matrix.selfadjointView< Eigen::Lower >().eigenvalues();
      std::sort( lambdas, lambdas + n, GreaterMagnitude< dfloat >() );
   }
}

void SymmetricEigenDecomposition2(
      ConstSampleIterator< dfloat > input,
      SampleIterator< dfloat > lambdas,
      SampleIterator< dfloat > vectors
) {
   DIP_ASSERT( input.Stride() >= 0 );
   Eigen::Map< Eigen::Matrix2d const, 0, Eigen::InnerStride<> > matrix( input.Pointer(), Eigen::InnerStride<>( input.Stride() ));
   if( vectors ) {
      Eigen::SelfAdjointEigenSolver< Eigen::Matrix2d > eigensolver( matrix );
      //if( eigensolver.info() != Eigen::Success ) { abort(); }
      Eigen::Vector2d const& eigenvalues = eigensolver.eigenvalues();
      Eigen::Matrix2d const& eigenvectors = eigensolver.eigenvectors();
      dip::uint indices0 = 0;
      dip::uint indices1 = 1;
      if( std::abs( eigenvalues[ 0 ] ) < std::abs( eigenvalues[ 1 ] )) {
         indices0 = 1;
         indices1 = 0;
      }
      lambdas[ 0 ] = eigenvalues[ indices0 ];
      lambdas[ 1 ] = eigenvalues[ indices1 ];
      vectors[ 0 ] = eigenvectors( 0, indices0 );
      vectors[ 1 ] = eigenvectors( 1, indices0 );
      vectors[ 2 ] = eigenvectors( 0, indices1 );
      vectors[ 3 ] = eigenvectors( 1, indices1 );
   } else {
      DIP_ASSERT( lambdas.Stride() >= 0 );
      Eigen::Map< Eigen::Vector2d, 0, Eigen::InnerStride<> > eigenvalues( lambdas.Pointer(), Eigen::InnerStride<>( lambdas.Stride() ));
      eigenvalues = matrix.selfadjointView< Eigen::Lower >().eigenvalues();
      if( std::abs( lambdas[ 0 ] ) < std::abs( lambdas[ 1 ] )) {
         std::swap( lambdas[ 0 ], lambdas[ 1 ] );
      }
   }
}

void SymmetricEigenDecomposition3(
      ConstSampleIterator< dfloat > input,
      SampleIterator< dfloat > lambdas,
      SampleIterator< dfloat > vectors
) {
   DIP_ASSERT( input.Stride() >= 0 );
   Eigen::Map< Eigen::Matrix3d const, 0, Eigen::InnerStride<> > matrix( input.Pointer(), Eigen::InnerStride<>( input.Stride() ));
   if( vectors ) {
      Eigen::SelfAdjointEigenSolver< Eigen::Matrix3d > eigensolver( matrix );
      //if( eigensolver.info() != Eigen::Success ) { abort(); }
      Eigen::Vector3d const& eigenvalues = eigensolver.eigenvalues();
      Eigen::Matrix3d const& eigenvectors = eigensolver.eigenvectors();
      std::array< dip::uint, 3 > indices{{ 0, 1, 2 }};
      std::sort( indices.begin(), indices.end(), [ & ]( dip::uint a, dip::uint b ) { return std::abs( eigenvalues[ b ] ) < std::abs( eigenvalues[ a ] ); } );
      for( dip::uint ii = 0; ii < 3; ++ii ) {
         dip::uint kk = indices[ ii ];
         lambdas[ ii ] = eigenvalues[ kk ];
         dip::uint offset = ii * 3;
         for( dip::uint jj = 0; jj < 3; ++jj ) {
            vectors[ jj + offset ] = eigenvectors( jj, kk );
         }
      }
   } else {
      DIP_ASSERT( lambdas.Stride() >= 0 );
      Eigen::Map< Eigen::Vector3d, 0, Eigen::InnerStride<> > eigenvalues( lambdas.Pointer(), Eigen::InnerStride<>( lambdas.Stride() ));
      eigenvalues = matrix.selfadjointView< Eigen::Lower >().eigenvalues();
      std::sort( lambdas, lambdas + 3, GreaterMagnitude< dfloat >() );
   }
}

void LargestEigenvector(
      dip::uint n,
      ConstSampleIterator< dfloat > input,
      SampleIterator< dfloat > vector
) {
   DIP_ASSERT( input.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXd const, 0, Eigen::InnerStride<> > matrix( input.Pointer(), n, n, Eigen::InnerStride<>( input.Stride() ));
   Eigen::SelfAdjointEigenSolver< Eigen::MatrixXd > eigensolver( matrix );
   //if( eigensolver.info() != Eigen::Success ) { abort(); }
   Eigen::VectorXd const& eigenvalues = eigensolver.eigenvalues();
   Eigen::MatrixXd const& eigenvectors = eigensolver.eigenvectors();
   std::vector< dip::uint > indices( n );
   std::iota( indices.begin(), indices.end(), 0 );
   std::sort( indices.begin(), indices.end(), [ & ]( dip::uint a, dip::uint b ) { return std::abs( eigenvalues[ b ] ) < std::abs( eigenvalues[ a ] ); } );
   dip::uint kk = indices[ 0 ];
   for( dip::uint jj = 0; jj < n; ++jj ) {
      vector[ jj ] = eigenvectors( jj, kk );
   }
}

void SmallestEigenvector(
      dip::uint n,
      ConstSampleIterator< dfloat > input,
      SampleIterator< dfloat > vector
) {
   DIP_ASSERT( input.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXd const, 0, Eigen::InnerStride<> > matrix( input.Pointer(), n, n, Eigen::InnerStride<>( input.Stride() ));
   Eigen::SelfAdjointEigenSolver< Eigen::MatrixXd > eigensolver( matrix );
   //if( eigensolver.info() != Eigen::Success ) { abort(); }
   Eigen::VectorXd const& eigenvalues = eigensolver.eigenvalues();
   Eigen::MatrixXd const& eigenvectors = eigensolver.eigenvectors();
   std::vector< dip::uint > indices( n );
   std::iota( indices.begin(), indices.end(), 0 );
   std::sort( indices.begin(), indices.end(), [ & ]( dip::uint a, dip::uint b ) { return std::abs( eigenvalues[ b ] ) < std::abs( eigenvalues[ a ] ); } );
   dip::uint kk = indices.back();
   for( dip::uint jj = 0; jj < n; ++jj ) {
      vector[ jj ] = eigenvectors( jj, kk );
   }
}

void EigenDecomposition(
      dip::uint n,
      ConstSampleIterator< dfloat > input,
      SampleIterator< dcomplex > lambdas,
      SampleIterator< dcomplex > vectors
) {
   DIP_ASSERT( input.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXd const, 0, Eigen::InnerStride<> > matrix( input.Pointer(), n, n, Eigen::InnerStride<>( input.Stride() ));
   if( vectors ) {
      Eigen::EigenSolver< Eigen::MatrixXd > eigensolver( matrix );
      Eigen::VectorXcd const& eigenvalues = eigensolver.eigenvalues();
      Eigen::MatrixXcd const& eigenvectors = eigensolver.eigenvectors();
      std::vector< dip::uint > indices( n );
      std::iota( indices.begin(), indices.end(), 0 );
      std::sort( indices.begin(), indices.end(), [ & ]( dip::uint a, dip::uint b ) { return std::abs( eigenvalues[ b ] ) < std::abs( eigenvalues[ a ] ); } );
      for( dip::uint ii = 0; ii < n; ++ii ) {
         dip::uint kk = indices[ ii ];
         lambdas[ ii ] = eigenvalues[ kk ];
         dip::uint offset = ii * n;
         for( dip::uint jj = 0; jj < n; ++jj ) {
            vectors[ jj + offset ] = eigenvectors( jj, kk );
         }
      }
   } else {
      DIP_ASSERT( lambdas.Stride() >= 0 );
      Eigen::Map< Eigen::VectorXcd, 0, Eigen::InnerStride<> > eigenvalues( lambdas.Pointer(), n, Eigen::InnerStride<>( lambdas.Stride() ));
      eigenvalues = matrix.eigenvalues();
      std::sort( lambdas, lambdas + n, GreaterMagnitude< dcomplex >() );
   }
}

void EigenDecomposition(
      dip::uint n,
      ConstSampleIterator< dcomplex > input,
      SampleIterator< dcomplex > lambdas,
      SampleIterator< dcomplex > vectors
) {
   DIP_ASSERT( input.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXcd const, 0, Eigen::InnerStride<> > matrix( input.Pointer(), n, n, Eigen::InnerStride<>( input.Stride() ));
   if( vectors ) {
      Eigen::ComplexEigenSolver< Eigen::MatrixXcd > eigensolver( matrix );
      Eigen::VectorXcd const& eigenvalues = eigensolver.eigenvalues();
      Eigen::MatrixXcd const& eigenvectors = eigensolver.eigenvectors();
      std::vector< dip::uint > indices( n );
      std::iota( indices.begin(), indices.end(), 0 );
      std::sort( indices.begin(), indices.end(), [ & ]( dip::uint a, dip::uint b ) { return std::abs( eigenvalues[ b ] ) < std::abs( eigenvalues[ a ] ); } );
      for( dip::uint ii = 0; ii < n; ++ii ) {
         dip::uint kk = indices[ ii ];
         lambdas[ ii ] = eigenvalues[ kk ];
         dip::uint offset = ii * n;
         for( dip::uint jj = 0; jj < n; ++jj ) {
            vectors[ jj + offset ] = eigenvectors( jj, kk );
         }
      }
   } else {
      DIP_ASSERT( lambdas.Stride() >= 0 );
      Eigen::Map< Eigen::VectorXcd, 0, Eigen::InnerStride<> > eigenvalues( lambdas.Pointer(), n, Eigen::InnerStride<>( lambdas.Stride() ));
      eigenvalues = matrix.eigenvalues();
      std::sort( lambdas, lambdas + n, GreaterMagnitude< dcomplex >() );
   }
}

dfloat Determinant( dip::uint n, ConstSampleIterator< dfloat > input ) {
   DIP_ASSERT( input.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXd const, 0, Eigen::InnerStride<> > matrix( input.Pointer(), n, n, Eigen::InnerStride<>( input.Stride() ));
   return matrix.determinant();
}

dcomplex Determinant( dip::uint n, ConstSampleIterator< dcomplex > input ) {
   DIP_ASSERT( input.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXcd const, 0, Eigen::InnerStride<> > matrix( input.Pointer(), n, n, Eigen::InnerStride<>( input.Stride() ));
   return matrix.determinant();
}

void SingularValueDecomposition(
      dip::uint m,
      dip::uint n,
      ConstSampleIterator< dfloat > input,
      SampleIterator< dfloat > Sout,
      SampleIterator< dfloat > Uout,
      SampleIterator< dfloat > Vout
) {
   dip::uint p = std::min( m, n );
   DIP_ASSERT( input.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXd const, 0, Eigen::InnerStride<> > M( input.Pointer(), m, n, Eigen::InnerStride<>( input.Stride() ));
   Eigen::JacobiSVD< Eigen::MatrixXd > svd( M, Eigen::ComputeThinU | Eigen::ComputeThinV );
   DIP_ASSERT( Sout.Stride() >= 0 );
   Eigen::Map< Eigen::VectorXd, 0, Eigen::InnerStride<> > S( Sout.Pointer(), p, Eigen::InnerStride<>( Sout.Stride() ));
   S = svd.singularValues();
   if( Uout && Vout ) {
      DIP_ASSERT( Uout.Stride() >= 0 );
      Eigen::Map< Eigen::MatrixXd, 0, Eigen::InnerStride<> > U( Uout.Pointer(), m, p, Eigen::InnerStride<>( Uout.Stride() ));
      DIP_ASSERT( Vout.Stride() >= 0 );
      Eigen::Map< Eigen::MatrixXd, 0, Eigen::InnerStride<> > V( Vout.Pointer(), n, p, Eigen::InnerStride<>( Vout.Stride() ));
      U = svd.matrixU();
      V = svd.matrixV();
   }
}

void SingularValueDecomposition(
      dip::uint m,
      dip::uint n,
      ConstSampleIterator< dcomplex > input,
      SampleIterator< dcomplex > Sout,
      SampleIterator< dcomplex > Uout,
      SampleIterator< dcomplex > Vout
) {
   dip::uint p = std::min( m, n );
   DIP_ASSERT( input.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXcd const, 0, Eigen::InnerStride<> > M( input.Pointer(), m, n, Eigen::InnerStride<>( input.Stride() ));
   Eigen::JacobiSVD< Eigen::MatrixXcd > svd( M, Eigen::ComputeThinU | Eigen::ComputeThinV );
   DIP_ASSERT( Sout.Stride() >= 0 );
   Eigen::Map< Eigen::VectorXcd, 0, Eigen::InnerStride<> > S( Sout.Pointer(), p, Eigen::InnerStride<>( Sout.Stride() ));
   S = svd.singularValues();
   if( Uout && Vout ) {
      DIP_ASSERT( Uout.Stride() >= 0 );
      Eigen::Map< Eigen::MatrixXcd, 0, Eigen::InnerStride<> > U( Uout.Pointer(), m, p, Eigen::InnerStride<>( Uout.Stride() ));
      DIP_ASSERT( Vout.Stride() >= 0 );
      Eigen::Map< Eigen::MatrixXcd, 0, Eigen::InnerStride<> > V( Vout.Pointer(), n, p, Eigen::InnerStride<>( Vout.Stride() ));
      U = svd.matrixU();
      V = svd.matrixV();
   }
}

void Inverse( dip::uint n, ConstSampleIterator< dfloat > input, SampleIterator< dfloat > output ) {
   DIP_ASSERT( input.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXd const, 0, Eigen::InnerStride<> > matrix( input.Pointer(), n, n, Eigen::InnerStride<>( input.Stride() ));
   DIP_ASSERT( output.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXd, 0, Eigen::InnerStride<> > result( output.Pointer(), n, n, Eigen::InnerStride<>( output.Stride() ));
   result = matrix.inverse();
}

void Inverse( dip::uint n, ConstSampleIterator< dcomplex > input, SampleIterator< dcomplex > output ) {
   DIP_ASSERT( input.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXcd const, 0, Eigen::InnerStride<> > matrix( input.Pointer(), n, n, Eigen::InnerStride<>( input.Stride() ));
   DIP_ASSERT( output.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXcd, 0, Eigen::InnerStride<> > result( output.Pointer(), n, n, Eigen::InnerStride<>( output.Stride() ));
   result = matrix.inverse();
}

void PseudoInverse(
      dip::uint m,
      dip::uint n,
      ConstSampleIterator< dfloat > input,
      SampleIterator< dfloat > output,
      dfloat tolerance
) {
   DIP_ASSERT( input.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXd const, 0, Eigen::InnerStride<> > matrix( input.Pointer(), m, n, Eigen::InnerStride<>( input.Stride() ));
   Eigen::JacobiSVD< Eigen::MatrixXd > svd( matrix, Eigen::ComputeThinU | Eigen::ComputeThinV );
   tolerance = tolerance * static_cast< dfloat >( std::max( m, n )) * svd.singularValues().array().abs()( 0 );
   DIP_ASSERT( output.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXd, 0, Eigen::InnerStride<> > result( output.Pointer(), n, m, Eigen::InnerStride<>( output.Stride() ));
   result = svd.matrixV() *
            ( svd.singularValues().array().abs() > tolerance ).select( svd.singularValues().array().inverse(), 0 )
                                                              .matrix().asDiagonal()
            * svd.matrixU().adjoint();
}

void PseudoInverse(
      dip::uint m,
      dip::uint n,
      ConstSampleIterator< dcomplex > input,
      SampleIterator< dcomplex > output,
      dfloat tolerance
) {
   DIP_ASSERT( input.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXcd const, 0, Eigen::InnerStride<> > matrix( input.Pointer(), m, n, Eigen::InnerStride<>( input.Stride() ));
   Eigen::JacobiSVD< Eigen::MatrixXcd > svd( matrix, Eigen::ComputeThinU | Eigen::ComputeThinV );
   tolerance = tolerance * static_cast< dfloat >( std::max( m, n )) * svd.singularValues().array().abs()( 0 );
   DIP_ASSERT( output.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXcd, 0, Eigen::InnerStride<> > result( output.Pointer(), n, m, Eigen::InnerStride<>( output.Stride() ));
   result = svd.matrixV() *
            ( svd.singularValues().array().abs() > tolerance ).select( svd.singularValues().array().inverse(), 0 )
                                                              .matrix().asDiagonal()
            * svd.matrixU().adjoint();
}

dip::uint Rank( dip::uint m, dip::uint n, ConstSampleIterator< dfloat > input ) {
   DIP_ASSERT( input.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXd const, 0, Eigen::InnerStride<> > matrix( input.Pointer(), m, n, Eigen::InnerStride<>( input.Stride() ));
   Eigen::CompleteOrthogonalDecomposition< Eigen::MatrixXd > decomposition( matrix );
   return static_cast< dip::uint >( decomposition.rank() );
}

dip::uint Rank( dip::uint m, dip::uint n, ConstSampleIterator< dcomplex > input ) {
   DIP_ASSERT( input.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXcd const, 0, Eigen::InnerStride<> > matrix( input.Pointer(), m, n, Eigen::InnerStride<>( input.Stride() ));
   Eigen::CompleteOrthogonalDecomposition< Eigen::MatrixXcd > decomposition( matrix );
   return static_cast< dip::uint >( decomposition.rank() );
}

void Solve(
      dip::uint m,
      dip::uint n,
      ConstSampleIterator< dfloat > A,
      ConstSampleIterator< dfloat > b,
      SampleIterator< dfloat > output
) {
   DIP_ASSERT( A.Stride() >= 0 );
   DIP_ASSERT( b.Stride() >= 0 );
   Eigen::Map< Eigen::MatrixXd const, 0, Eigen::InnerStride<> > matrix( A.Pointer(), m, n, Eigen::InnerStride<>( A.Stride() ));
   Eigen::JacobiSVD< Eigen::MatrixXd > svd( matrix, Eigen::ComputeThinU | Eigen::ComputeThinV );
   Eigen::Map< Eigen::VectorXd const, 0, Eigen::InnerStride<> > vector( b.Pointer(), m, Eigen::InnerStride<>( b.Stride() ));
   Eigen::Map< Eigen::VectorXd, 0, Eigen::InnerStride<> > result( output.Pointer(), n, Eigen::InnerStride<>( output.Stride() ));
   result = svd.solve( vector );
}

} // namespace dip

#if defined(__GNUG__) || defined(__clang__)
#pragma GCC diagnostic pop
#endif

#ifdef DIP_CONFIG_ENABLE_DOCTEST
#include "doctest.h"

DOCTEST_TEST_CASE("[DIPlib] testing the EigenDecomposition functions") {
   // Test generic symmetric code with 2x2 matrix
   dip::dfloat matrix2[] = { 4, 8, 0 };
   dip::dfloat lambdas[ 3 ];
   dip::dfloat vectors[ 9 ];
   dip::SymmetricEigenDecompositionPacked( 2, matrix2, lambdas );
   DOCTEST_CHECK( lambdas[ 0 ] == 8 );
   DOCTEST_CHECK( lambdas[ 1 ] == 4 );
   dip::SymmetricEigenDecompositionPacked( 2, matrix2, lambdas, vectors );
   DOCTEST_CHECK( lambdas[ 0 ] == 8 );
   DOCTEST_CHECK( lambdas[ 1 ] == 4 );
   DOCTEST_CHECK( vectors[ 0 ] == 0 );
   DOCTEST_CHECK( vectors[ 1 ] == 1 );
   DOCTEST_CHECK( vectors[ 2 ] == 1 );
   DOCTEST_CHECK( vectors[ 3 ] == 0 );
   matrix2[ 0 ] = 8;
   matrix2[ 1 ] = 4;
   dip::SymmetricEigenDecompositionPacked( 2, matrix2, lambdas );
   DOCTEST_CHECK( lambdas[ 0 ] == 8 );
   DOCTEST_CHECK( lambdas[ 1 ] == 4 );
   dip::SymmetricEigenDecompositionPacked( 2, matrix2, lambdas, vectors );
   DOCTEST_CHECK( lambdas[ 0 ] == 8 );
   DOCTEST_CHECK( lambdas[ 1 ] == 4 );
   DOCTEST_CHECK( vectors[ 0 ] == 1 );
   DOCTEST_CHECK( vectors[ 1 ] == 0 );
   DOCTEST_CHECK( vectors[ 2 ] == 0 );
   DOCTEST_CHECK( vectors[ 3 ] == 1 );
   matrix2[ 0 ] = 3;
   matrix2[ 1 ] = 3;
   matrix2[ 2 ] = -1;
   dip::SymmetricEigenDecompositionPacked( 2, matrix2, lambdas );
   DOCTEST_CHECK( lambdas[ 0 ] == 4 );
   DOCTEST_CHECK( lambdas[ 1 ] == 2 );
   dip::SymmetricEigenDecompositionPacked( 2, matrix2, lambdas, vectors );
   DOCTEST_CHECK( lambdas[ 0 ] == 4 );
   DOCTEST_CHECK( lambdas[ 1 ] == 2 );
   DOCTEST_CHECK( vectors[ 0 ] == doctest::Approx(  std::cos( dip::pi/4 ))); // signs might be different here...
   DOCTEST_CHECK( vectors[ 1 ] == doctest::Approx( -std::sin( dip::pi/4 )));
   DOCTEST_CHECK( vectors[ 2 ] == doctest::Approx(  std::sin( dip::pi/4 )));
   DOCTEST_CHECK( vectors[ 3 ] == doctest::Approx(  std::cos( dip::pi/4 )));

   // Test 2x2-specific symmetric code
   dip::dfloat matrix2f[] = { 4, 0, 0, 8 };
   dip::SymmetricEigenDecomposition2( matrix2f, lambdas );
   DOCTEST_CHECK( lambdas[ 0 ] == 8 );
   DOCTEST_CHECK( lambdas[ 1 ] == 4 );
   dip::SymmetricEigenDecomposition2( matrix2f, lambdas, vectors );
   DOCTEST_CHECK( lambdas[ 0 ] == 8 );
   DOCTEST_CHECK( lambdas[ 1 ] == 4 );
   DOCTEST_CHECK( vectors[ 0 ] == 0 );
   DOCTEST_CHECK( vectors[ 1 ] == 1 );
   DOCTEST_CHECK( vectors[ 2 ] == 1 );
   DOCTEST_CHECK( vectors[ 3 ] == 0 );
   matrix2f[ 0 ] = 8;
   matrix2f[ 3 ] = 4;
   dip::SymmetricEigenDecomposition2( matrix2f, lambdas );
   DOCTEST_CHECK( lambdas[ 0 ] == 8 );
   DOCTEST_CHECK( lambdas[ 1 ] == 4 );
   dip::SymmetricEigenDecomposition2( matrix2f, lambdas, vectors );
   DOCTEST_CHECK( lambdas[ 0 ] == 8 );
   DOCTEST_CHECK( lambdas[ 1 ] == 4 );
   DOCTEST_CHECK( vectors[ 0 ] == 1 );
   DOCTEST_CHECK( vectors[ 1 ] == 0 );
   DOCTEST_CHECK( vectors[ 2 ] == 0 );
   DOCTEST_CHECK( vectors[ 3 ] == 1 );
   matrix2f[ 0 ] = 3;
   matrix2f[ 1 ] = -1;
   matrix2f[ 2 ] = -1;
   matrix2f[ 3 ] = 3;
   dip::SymmetricEigenDecomposition2( matrix2f, lambdas );
   DOCTEST_CHECK( lambdas[ 0 ] == 4 );
   DOCTEST_CHECK( lambdas[ 1 ] == 2 );
   dip::SymmetricEigenDecomposition2( matrix2f, lambdas, vectors );
   DOCTEST_CHECK( lambdas[ 0 ] == 4 );
   DOCTEST_CHECK( lambdas[ 1 ] == 2 );
   DOCTEST_CHECK( vectors[ 0 ] == doctest::Approx(  std::cos( dip::pi/4 ))); // signs might be different here...
   DOCTEST_CHECK( vectors[ 1 ] == doctest::Approx( -std::sin( dip::pi/4 )));
   DOCTEST_CHECK( vectors[ 2 ] == doctest::Approx(  std::sin( dip::pi/4 )));
   DOCTEST_CHECK( vectors[ 3 ] == doctest::Approx(  std::cos( dip::pi/4 )));

   // Test generic symmetric code with 3x3 matrix
   dip::dfloat matrix3[] = { 3, 1.5, 1.5, 0.0, 0.0, -0.5 };
   dip::SymmetricEigenDecompositionPacked( 3, matrix3, lambdas );
   DOCTEST_CHECK( lambdas[ 0 ] == 3 );
   DOCTEST_CHECK( lambdas[ 1 ] == 2 );
   DOCTEST_CHECK( lambdas[ 2 ] == 1 );
   dip::SymmetricEigenDecompositionPacked( 3, matrix3, lambdas, vectors );
   DOCTEST_CHECK( lambdas[ 0 ] == 3 );
   DOCTEST_CHECK( lambdas[ 1 ] == 2 );
   DOCTEST_CHECK( lambdas[ 2 ] == 1 );
   DOCTEST_CHECK( vectors[ 0 ] == doctest::Approx(  1.0 ));
   DOCTEST_CHECK( vectors[ 1 ] == doctest::Approx(  0.0 ));
   DOCTEST_CHECK( vectors[ 2 ] == doctest::Approx(  0.0 ));
   DOCTEST_CHECK( vectors[ 3 ] == doctest::Approx(  0.0 ));
   DOCTEST_CHECK( vectors[ 4 ] == doctest::Approx(  1.0 / std::sqrt( 2.0 )));
   DOCTEST_CHECK( vectors[ 5 ] == doctest::Approx( -1.0 / std::sqrt( 2.0 )));
   DOCTEST_CHECK( vectors[ 6 ] == doctest::Approx(  0.0 ));
   DOCTEST_CHECK( vectors[ 7 ] == doctest::Approx(  1.0 / std::sqrt( 2.0 )));
   DOCTEST_CHECK( vectors[ 8 ] == doctest::Approx(  1.0 / std::sqrt( 2.0 )));

   // Test 3x3-specific symmetric code
   dip::dfloat matrix3f[] = { 3, 0.0, 0.0, 0.0, 1.5, -0.5, 0.0, -0.5, 1.5 };
   dip::SymmetricEigenDecomposition3( matrix3f, lambdas );
   DOCTEST_CHECK( lambdas[ 0 ] == 3 );
   DOCTEST_CHECK( lambdas[ 1 ] == 2 );
   DOCTEST_CHECK( lambdas[ 2 ] == 1 );
   dip::SymmetricEigenDecomposition3( matrix3f, lambdas, vectors );
   DOCTEST_CHECK( lambdas[ 0 ] == 3 );
   DOCTEST_CHECK( lambdas[ 1 ] == 2 );
   DOCTEST_CHECK( lambdas[ 2 ] == 1 );
   DOCTEST_CHECK( vectors[ 0 ] == doctest::Approx(  1.0 ));
   DOCTEST_CHECK( vectors[ 1 ] == doctest::Approx(  0.0 ));
   DOCTEST_CHECK( vectors[ 2 ] == doctest::Approx(  0.0 ));
   DOCTEST_CHECK( vectors[ 3 ] == doctest::Approx(  0.0 ));
   DOCTEST_CHECK( vectors[ 4 ] == doctest::Approx(  1.0 / std::sqrt( 2.0 )));
   DOCTEST_CHECK( vectors[ 5 ] == doctest::Approx( -1.0 / std::sqrt( 2.0 )));
   DOCTEST_CHECK( vectors[ 6 ] == doctest::Approx(  0.0 ));
   DOCTEST_CHECK( vectors[ 7 ] == doctest::Approx(  1.0 / std::sqrt( 2.0 )));
   DOCTEST_CHECK( vectors[ 8 ] == doctest::Approx(  1.0 / std::sqrt( 2.0 )));

   // Test generic non-symmetric code with 2x2 matrix
   dip::dfloat matrix22[] = { 3, -1, -1, 3 };
   dip::dcomplex c_lambdas[ 2 ];
   dip::dcomplex c_vectors[ 4 ];
   dip::EigenDecomposition( 2, matrix22, c_lambdas, c_vectors );
   DOCTEST_CHECK( c_lambdas[ 0 ].real() == doctest::Approx( 4.0 ));
   DOCTEST_CHECK( c_lambdas[ 1 ].real() == doctest::Approx( 2.0 ));
   DOCTEST_CHECK( c_vectors[ 0 ].real() == doctest::Approx(  cos( dip::pi/4 ))); // signs might be different here...
   DOCTEST_CHECK( c_vectors[ 1 ].real() == doctest::Approx( -sin( dip::pi/4 )));
   DOCTEST_CHECK( c_vectors[ 2 ].real() == doctest::Approx(  sin( dip::pi/4 )));
   DOCTEST_CHECK( c_vectors[ 3 ].real() == doctest::Approx(  cos( dip::pi/4 )));
   DOCTEST_CHECK( c_lambdas[ 0 ].imag() == 0 );
   DOCTEST_CHECK( c_lambdas[ 1 ].imag() == 0 );
   DOCTEST_CHECK( c_vectors[ 0 ].imag() == 0 );
   DOCTEST_CHECK( c_vectors[ 1 ].imag() == 0 );
   DOCTEST_CHECK( c_vectors[ 2 ].imag() == 0 );
   DOCTEST_CHECK( c_vectors[ 3 ].imag() == 0 );

}

DOCTEST_TEST_CASE("[DIPlib] testing the SingularValueDecomposition and related functions") {
   dip::dfloat matrix22[] = { 4, 0, 0, 8 };
   dip::dfloat S[ 2 ];
   dip::dfloat U[ 4 ];
   dip::dfloat V[ 6 ];
   dip::SingularValueDecomposition( 2, 2, matrix22, S, U, V );
   DOCTEST_CHECK( S[ 0 ] == 8 );
   DOCTEST_CHECK( S[ 1 ] == 4 );
   DOCTEST_CHECK( U[ 0 ] == 0 );
   DOCTEST_CHECK( U[ 1 ] == 1 );
   DOCTEST_CHECK( U[ 2 ] == 1 );
   DOCTEST_CHECK( U[ 3 ] == 0 );
   DOCTEST_CHECK( V[ 0 ] == 0 );
   DOCTEST_CHECK( V[ 1 ] == 1 );
   DOCTEST_CHECK( V[ 2 ] == 1 );
   DOCTEST_CHECK( V[ 3 ] == 0 );

   dip::dfloat matrix23[] = { 3, 2, 2, 3, 2, -2 };
   dip::SingularValueDecomposition( 2, 3, matrix23, S, U, V );
   DOCTEST_CHECK( S[ 0 ] == doctest::Approx( 5.0 ));
   DOCTEST_CHECK( S[ 1 ] == doctest::Approx( 3.0 ));
   DOCTEST_CHECK( U[ 0 ] == doctest::Approx( -1.0 / std::sqrt( 2 ))); // signs might be different here...
   DOCTEST_CHECK( U[ 1 ] == doctest::Approx( -1.0 / std::sqrt( 2 )));
   DOCTEST_CHECK( U[ 2 ] == doctest::Approx( 1.0 / std::sqrt( 2 )));
   DOCTEST_CHECK( U[ 3 ] == doctest::Approx( -1.0 / std::sqrt( 2 )));
   DOCTEST_CHECK( V[ 0 ] == doctest::Approx( -1.0 / std::sqrt( 2 )));
   DOCTEST_CHECK( V[ 1 ] == doctest::Approx( -1.0 / std::sqrt( 2 )));
   DOCTEST_CHECK( V[ 2 ] == doctest::Approx( 0.0 ));
   DOCTEST_CHECK( V[ 3 ] == doctest::Approx( 1.0 / std::sqrt( 18 )));
   DOCTEST_CHECK( V[ 4 ] == doctest::Approx( -1.0 / std::sqrt( 18 )));
   DOCTEST_CHECK( V[ 5 ] == doctest::Approx( 4.0 / std::sqrt( 18 )));

   DOCTEST_CHECK( dip::Rank( 2, 3, matrix23 ) == 2 );

   dip::dfloat matrix32[ 6 ];
   dip::PseudoInverse( 2, 3, matrix23, matrix32 );
   DOCTEST_CHECK( matrix32[ 0 ] == doctest::Approx( 28.0 / 180.0 ));
   DOCTEST_CHECK( matrix32[ 1 ] == doctest::Approx(  8.0 / 180.0 ));
   DOCTEST_CHECK( matrix32[ 2 ] == doctest::Approx( 40.0 / 180.0 ));
   DOCTEST_CHECK( matrix32[ 3 ] == doctest::Approx(  8.0 / 180.0 ));
   DOCTEST_CHECK( matrix32[ 4 ] == doctest::Approx( 28.0 / 180.0 ));
   DOCTEST_CHECK( matrix32[ 5 ] == doctest::Approx(-40.0 / 180.0 ));

   dip::dfloat b[ 3 ] = { 44.0 / 180.0, 64.0 / 180.0, -40.0 / 180.0 };
   dip::dfloat x[ 2 ];
   dip::Solve( 3, 2, matrix32, b, x );
   DOCTEST_CHECK( x[ 0 ] == doctest::Approx( 1.0 ));
   DOCTEST_CHECK( x[ 1 ] == doctest::Approx( 2.0 ));
}

#endif // DIP_CONFIG_ENABLE_DOCTEST


#include <cmath>
#include <memory>
#include <vector>

#include <Eigen/Core>
#include <gtest/gtest.h>

#include "test/utils.hpp"

#include "ldaplusplus/Document.hpp"
#include "ldaplusplus/em/FastSupervisedEStep.hpp"
#include "ldaplusplus/Parameters.hpp"
#include "ldaplusplus/em/SupervisedEStep.hpp"
#include "ldaplusplus/e_step_utils.hpp"

using namespace Eigen;
using namespace ldaplusplus;

// T will be available as TypeParam in TYPED_TEST functions
template <typename T>
class TestExpectationStep : public ParameterizedTest<T> {};

TYPED_TEST_CASE(TestExpectationStep, ForFloatAndDouble);

TYPED_TEST(TestExpectationStep, ComputeH) {
    VectorXi X(10);
    X << 10, 9, 8, 7, 6, 5, 4, 3, 2, 1;
    VectorX<TypeParam> X_ratio = X.cast<TypeParam>() / X.sum();

    MatrixX<TypeParam> eta = MatrixX<TypeParam>::Random(5, 3);
    MatrixX<TypeParam> phi = MatrixX<TypeParam>::Random(5, 10);
    phi.array() -= phi.minCoeff();
    phi = phi.array().rowwise() / phi.colwise().sum().array();
    VectorX<TypeParam> h(5);

    e_step_utils::compute_h<TypeParam>(X, X_ratio, eta, phi, h);

    VectorX<TypeParam> hphi = h.transpose() * phi;

    TypeParam hphi_actual = 0;
    for (int y=0; y<3; y++) {
        TypeParam p = 1;
        for (int n=0; n<10; n++) {
            TypeParam t = 0;
            for (int k=0; k<5; k++) {
                t += phi(k, n) * exp( X.cast<TypeParam>()(n)/X.sum() * eta(k, y) );
            }
            p *= t;
        }
        hphi_actual += p;
    }

    EXPECT_NEAR(hphi_actual, hphi(9), 1e-2);
}

TYPED_TEST(TestExpectationStep, ComputeLikelihood) {
    for (int i=0; i<100; i++) {
        VectorX<TypeParam> Xtmp = VectorX<TypeParam>::Random(10).array().abs() * 5;
        VectorXi X = Xtmp.template cast<int>();
        VectorX<TypeParam> X_ratio = X.cast<TypeParam>() / X.sum();
        int y = 0;
        VectorX<TypeParam> alpha = VectorX<TypeParam>::Constant(5, 0.1);
        MatrixX<TypeParam> beta = MatrixX<TypeParam>::Random(5, 10);
        MatrixX<TypeParam> eta = MatrixX<TypeParam>::Random(5, 3);
        MatrixX<TypeParam> phi = MatrixX<TypeParam>::Constant(5, 10, 0.1);
        VectorX<TypeParam> gamma = VectorX<TypeParam>::Constant(5, X.sum() / 5.0);

        // normalize beta, phi
        phi.array() -= phi.minCoeff() - 0.001;
        phi.array().rowwise() /= phi.colwise().sum().array();
        beta.array() -= beta.minCoeff() - 0.001;
        beta.array().rowwise() /= beta.colwise().sum().array();

        TypeParam likelihood = e_step_utils::compute_supervised_likelihood(
            X,
            y,
            alpha,
            beta,
            eta,
            phi,
            gamma
        );

        ASSERT_FALSE(std::isnan(likelihood)) << phi.array().log();
        EXPECT_GT(0, likelihood);
    }
}

TYPED_TEST(TestExpectationStep, DocEStep) {
    return;

    VectorX<TypeParam> Xtmp = VectorX<TypeParam>::Random(10).array().abs() * 5;
    VectorXi X = Xtmp.template cast<int>();
    int y = 0;

    auto doc = std::make_shared<corpus::ClassificationDecorator>(
        std::make_shared<corpus::EigenDocument>(X),
        y
    );

    VectorX<TypeParam> alpha = VectorX<TypeParam>::Constant(5, 0.2);
    MatrixX<TypeParam> beta = MatrixX<TypeParam>::Random(5, 10);
    MatrixX<TypeParam> eta = MatrixX<TypeParam>::Zero(5, 3);

    // normalize beta
    beta.array() -= beta.minCoeff() - 0.001;
    beta.array().rowwise() /= beta.colwise().sum().array();

    auto model = std::make_shared<parameters::SupervisedModelParameters<TypeParam> >(
        alpha,
        beta,
        eta
    );

    size_t fixed_point_iterations = 5;
    TypeParam convergence_tolerance = -10;
    std::vector<TypeParam> likelihoods(10);
    for (int i=0; i<10; i++) {
        em::SupervisedEStep<TypeParam> e_step(
            i,
            convergence_tolerance,
            fixed_point_iterations
        );
        auto vp = std::static_pointer_cast<parameters::VariationalParameters<TypeParam> >(
            e_step.doc_e_step(
                doc,
                model
            )
        );

        likelihoods[i] = e_step_utils::compute_supervised_likelihood<TypeParam>(
            doc->get_words(),
            doc->get_class(),
            model->alpha,
            model->beta,
            model->eta,
            vp->phi,
            vp->gamma
        );
    }
    for (int i=1; i<10; i++) {
        EXPECT_GT(likelihoods[i], likelihoods[i-1]);
    }
}

TYPED_TEST(TestExpectationStep, FastDocEStep) {
    return;

    VectorX<TypeParam> Xtmp = VectorX<TypeParam>::Random(10).array().abs() * 5;
    VectorXi X = Xtmp.template cast<int>();
    int y = 0;

    auto doc = std::make_shared<corpus::ClassificationDecorator>(
        std::make_shared<corpus::EigenDocument>(X),
        y
    );

    VectorX<TypeParam> alpha = VectorX<TypeParam>::Constant(5, 0.1);
    MatrixX<TypeParam> beta = MatrixX<TypeParam>::Random(5, 10);
    MatrixX<TypeParam> eta = MatrixX<TypeParam>::Random(5, 3);

    // normalize beta
    beta.array() -= beta.minCoeff() - 0.001;
    beta.array().rowwise() /= beta.colwise().sum().array();

    auto model = std::make_shared<parameters::SupervisedModelParameters<TypeParam> >(
        alpha,
        beta,
        eta
    );

    VectorX<TypeParam> h(beta.rows());

    int fixed_point_iterations = 10;
    TypeParam convergence_tolerance = 0;
    std::vector<TypeParam> likelihoods(10);
    for (int i=0; i<10; i++) {
        em::FastSupervisedEStep<TypeParam> e_step(
            i,
            convergence_tolerance,
            fixed_point_iterations
        );
        auto vp = std::static_pointer_cast<parameters::VariationalParameters<TypeParam> >(
            e_step.doc_e_step(
                doc,
                model
            )
        );
        likelihoods[i] = e_step_utils::compute_supervised_likelihood<TypeParam>(
            doc->get_words(),
            doc->get_class(),
            model->alpha,
            model->beta,
            model->eta,
            vp->phi,
            vp->gamma,
            h
        );
    }
    for (int i=1; i<10; i++) {
        EXPECT_GT(likelihoods[i], likelihoods[i-1]);
    }
}


/**
 * @file Sonar.cpp
 * @brief Projection functions for imaging sonar, e.g. DIDSON
 * @author: Michael Kaess
 * @date: Aug 2014
 */

#include <iostream>
#include <vector>
#include <utility>

#include <Eigen/Dense>

#include "Sonar.h"

using namespace std;
using namespace isam;
using namespace Eigen;

namespace sonar {

Sonar::Sonar(double rMin, double rMax, double bearingFov, double elevationFov,
    int numBearings, int numRanges) :
    _rMin(rMin), _rMax(rMax), _bearingFov(bearingFov), _elevationFov(
        elevationFov), _numBearings(numBearings), _numRanges(numRanges) {
  if (rMax <= rMin) {
    cout << "ERROR: rMax must be larger than rMin" << endl;
    exit(1);
  }
  if (bearingFov <= 0 || elevationFov <= 0) {
    cout << "ERROR: bearingFov and elevationFov must be positive" << endl;
    exit(1);
  }
  if (numBearings <= 0 || numRanges <= 0) {
    cout << "ERROR: numBearing and numRanges must be positive" << endl;
    exit(1);
  }
}

vector<Point3d> Sonar::getFrustum(const isam::Pose3d& pose) const {
  vector<Point3d> frustum;
  vector<double> range(2);
  range[0] = _rMin; range[1] = _rMax;
  vector<pair<double, double> > angles;
  double yaw = 0.5 * _bearingFov;
  double pitch = 0.5 * _elevationFov;
  angles.push_back(make_pair(-yaw, -pitch));
  angles.push_back(make_pair(yaw, -pitch));
  angles.push_back(make_pair(yaw, pitch));
  angles.push_back(make_pair(-yaw, pitch));

  for (int r = 0; r < 2; r++) {
    for (int a = 0; a < 4; a++) {
      yaw = angles[a].first;
      pitch = angles[a].second;
      Point3d p_local = Point3d(range[r], 0, 0);
      Point3d p_sonar = Pose3d(0, 0, 0, yaw, pitch, 0).transform_from(p_local);
      frustum.push_back(pose.transform_from(p_sonar));
    }
  }

  return frustum;
}

bool Sonar::project(const isam::Pose3d& pose, const isam::Point3d& point,
    isam::Point2d& projection) const {
  Point3d point_sonar = pose.transform_to(point);
  double range = point_sonar.vector().norm();
  double x = point_sonar.x();
  double y = point_sonar.y();
  double z = point_sonar.z();
  Vector2d direction(point_sonar.x(), point_sonar.y());
  direction.normalize();
  Vector2d mapping = direction * range;

  double u = mapping(0);
  double v = mapping(1);
  double b = (atan2(v, u) / _bearingFov + 0.5) * _numBearings;
  double r = (range - _rMin) / (_rMax - _rMin) * _numRanges;
  double elevation = atan2(z, sqrt(x * x + y * y)) / _elevationFov;

  projection = Point2d(b, r);

  // projection within field of view of sensor?
  if (b >= 0 && b < _numBearings && r >= 0 && r < _numRanges
      && elevation >= -0.5 && elevation <= 0.5) {
    return true;
  }

  return false;
}

} /* namespace sonar */


// Copyright 2004 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine

#include <boost/graph/use_mpi.hpp>
#include <boost/throw_exception.hpp>
#include <boost/graph/distributed/page_rank.hpp>
#include <boost/test/minimal.hpp>
#include <boost/graph/distributed/adjacency_list.hpp>
#include <boost/graph/distributed/mpi_process_group.hpp>
#include <boost/test/minimal.hpp>
#include <vector>
#include <iostream>
#include <stdlib.h>

#ifdef BOOST_NO_EXCEPTIONS
void
boost::throw_exception(std::exception const& ex)
{
    std::cout << ex.what() << std::endl;
    abort();
}
#endif

using namespace boost;
using boost::graph::distributed::mpi_process_group;

bool close_to(double x, double y)
{
  double diff = x - y;
  if (diff < 0) diff = -diff;
  double base = (y == 0? x : y);
  if (base != 0) return diff / base < 0.01;
  else return true;
}

// Make convenient labels for the vertices
enum vertex_id_t { A, B, C, D, N };

void test_distributed_page_rank(int iterations)
{
  using namespace boost::graph;

  // create a typedef for the Graph type
  typedef adjacency_list<vecS,
                         distributedS<mpi_process_group, vecS>,
                         bidirectionalS
                         > Graph;
  typedef graph_traits<Graph>::vertex_descriptor vertex_descriptor;

  // writing out the edges in the graph
  typedef std::pair<int, int> Edge;
  Edge edge_array[] =
    { Edge(A,B), Edge(A,C), Edge(B,C), Edge(C,A), Edge(D,C) };
  const int num_edges = sizeof(edge_array)/sizeof(edge_array[0]);

  // declare a graph object
  Graph g(edge_array, edge_array + num_edges, N);

  std::vector<double> ranks(num_vertices(g));

  page_rank(g,
            make_iterator_property_map(ranks.begin(),
                                       get(boost::vertex_index, g)),
            n_iterations(iterations), 0.85, N);

  double local_sum = 0.0;
  for(unsigned int i = 0; i < num_vertices(g); ++i) {
    std::cout << (char)('A' + g.distribution().global(i)) << " = "
              << ranks[i] << std::endl;
    local_sum += ranks[i];
  }
  double sum=0.;
  boost::mpi::reduce(communicator(g.process_group()),
                     local_sum, sum, std::plus<double>(), 0);
  if (process_id(g.process_group()) == 0) {
    std::cout << "Sum = " << sum << "\n\n";
    BOOST_CHECK(close_to(sum, 4)); // 1 when alpha=0
  }

  //   double expected_ranks0[N] = {0.400009, 0.199993, 0.399998, 0.0};
  double expected_ranks[N] = {1.49011, 0.783296, 1.5766, 0.15};
  for (int i = 0; i < N; ++i) {
    vertex_descriptor v = vertex(i, g);
    if (v != Graph::null_vertex()
        && owner(v) == process_id(g.process_group())) {
      BOOST_CHECK(close_to(ranks[local(v)], expected_ranks[i]));
    }
  }
}

int test_main(int argc, char* argv[])
{
  mpi::environment env(argc, argv);

  int iterations = 50;
  if (argc > 1) {
    iterations = atoi(argv[1]);
  }

  test_distributed_page_rank(iterations);

  return 0;
}


//==================================================================================================
/*!

  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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/asinh.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/eps.hpp>
#include <cmath>

STF_CASE_TPL (" asinh",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::asinh;
  using r_t = decltype(asinh(T()));

  // return type conformity test
  STF_TYPE_IS(r_t,T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(asinh(bs::Inf<T>()), bs::Inf<r_t>(), 0);
  STF_ULP_EQUAL(asinh(bs::Minf<T>()), bs::Minf<r_t>(), 0);
  STF_ULP_EQUAL(asinh(bs::Nan<T>()), bs::Nan<r_t>(), 0);
#endif
  STF_ULP_EQUAL(asinh(bs::Zero<T>()), bs::Zero<r_t>(), 0);
  STF_ULP_EQUAL(asinh(bs::Valmax<T>()), std::asinh(bs::Valmax<T>()), 0.5);
  STF_ULP_EQUAL(asinh(bs::rec(bs::Sqrteps<T>())*2),  std::asinh(bs::rec(bs::Sqrteps<T>())*2), 0.5);
  STF_ULP_EQUAL(asinh(bs::Eps<T>()), bs::Eps<T>(), 0.5);
 for(T i=T(0.1); i <= T(1.1); i+= T(0.5))
 {
   T ri =  bs::rec(i);
   STF_ULP_EQUAL(asinh(i), std::asinh(i), 0.5);
   STF_ULP_EQUAL(asinh(ri), std::asinh(ri), 0.5);
 }
}


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

#ifndef MATH_DIAGMATRIX3_HPP_INCLUDED
#define MATH_DIAGMATRIX3_HPP_INCLUDED

// Math::DiagMatrix3<T> is a 3x3 matrix where all non-diagonal entries are zero.
//
// The difference to Math::Vector3<T> is mostly the effect of operations
// (Vector3<T> * Vector3<T> is a dot product while DiagMatrix3<T> * Vector3<T>
// is a matrix-vector-multiplication / an elementwise multiplication)

#include <Core/Assert.hpp>
#include <Core/Util.hpp>

#include <Math/Forward.hpp>
#include <Math/DiagMatrix3.hpp>

#include <complex>

#include <boost/type_traits/is_convertible.hpp>
#include <boost/utility/enable_if.hpp>

namespace Math {
  template <typename T> class DiagMatrix3 {
    Vector3<T> diag_;

    class PrivateType {
      friend class DiagMatrix3;
      PrivateType () {}
    };

  public:
    DiagMatrix3 () {}
    explicit DiagMatrix3 (T v) : diag_ (v, v, v) {}
    DiagMatrix3 (T x, T y, T z) : diag_ (x, y, z) {}
    template <typename U> explicit DiagMatrix3 (const Vector3<U>& diag) : diag_ (diag) {}
#if (defined (__clang__) || GCC_VERSION_IS_ATLEAST(4, 6)) && !defined (__CUDACC__)
#pragma GCC diagnostic push
#endif
#pragma GCC diagnostic ignored "-Wconversion"
    template <typename U> DiagMatrix3 (DiagMatrix3<U> v, UNUSED typename boost::enable_if<boost::is_convertible<U, T>, PrivateType>::type dummy = PrivateType ()) : diag_ (v.diag ()) {}
    template <typename U> explicit DiagMatrix3 (DiagMatrix3<U> v, UNUSED typename boost::disable_if<boost::is_convertible<U, T>, PrivateType>::type dummy = PrivateType ()) : diag_ (v.diag ()) {}
#if (defined (__clang__) || GCC_VERSION_IS_ATLEAST(4, 6)) && !defined (__CUDACC__)
#pragma GCC diagnostic pop
#endif

    const Vector3<T>& diag () const {
      return diag_;
    }
    Vector3<T>& diag () {
      return diag_;
    }

    T m11 () const {
      return diag ().x ();
    }
    T m22 () const {
      return diag ().y ();
    }
    T m33 () const {
      return diag ().z ();
    }

    T& m11 () {
      return diag ().x ();
    }
    T& m22 () {
      return diag ().y ();
    }
    T& m33 () {
      return diag ().z ();
    }

    // off-diagonal entries (read-only, = 0)
    T m12 () const { return T (); }
    T m13 () const { return T (); }
    T m21 () const { return T (); }
    T m23 () const { return T (); }
    T m31 () const { return T (); }
    T m32 () const { return T (); }

    // Return the inverse matrix
    DiagMatrix3<T> inverse () const {
      return DiagMatrix3<T> (T (1) / m11 (), T (1) / m22 (), T (1) / m33 ());
    }

    // Access values on the diagonal
    const T& operator[] (size_t i) const {
      return diag ()[i];
    }
    T& operator[] (size_t i) {
      return diag ()[i];
    }
  };

  // Operations on DiagMatrix3

#define RTS(op) DECLTYPE ((*(T*)NULL) op (*(U*)NULL))
#define RT(op) DiagMatrix3<RTS(op)>
  template <typename T, typename U> inline RT(+) operator+ (DiagMatrix3<T> v1, DiagMatrix3<U> v2) {
    return RT(+) (v1.m11 () + v2.m11 (), v1.m22 () + v2.m22 (), v1.m33 () + v2.m33 ());
  }

  template <typename T, typename U> inline DiagMatrix3<T>& operator+= (DiagMatrix3<T>& v1, DiagMatrix3<U> v2) {
    v1.m11 () += v2.m11 (); v1.m22 () += v2.m22 (); v1.m33 () += v2.m33 ();
    return v1;
  }

  template <typename T, typename U> inline RT(-) operator- (DiagMatrix3<T> v1, DiagMatrix3<U> v2) {
    return RT(-) (v1.m11 () - v2.m11 (), v1.m22 () - v2.m22 (), v1.m33 () - v2.m33 ());
  }

  template <typename T, typename U> inline DiagMatrix3<T>& operator-= (DiagMatrix3<T>& v1, DiagMatrix3<U> v2) {
    v1.m11 () -= v2.m11 (); v1.m22 () -= v2.m22 (); v1.m33 () -= v2.m33 ();
    return v1;
  }

  template <typename T, typename U> inline RT(*) operator* (DiagMatrix3<T> v, U scalar) {
    return RT(*) (v.m11 () * scalar, v.m22 () * scalar, v.m33 () * scalar);
  }

  template <typename T, typename U> inline RT(*) operator* (T scalar, DiagMatrix3<U> v) {
    return RT(*) (scalar * v.m11 (), scalar * v.m22 (), scalar * v.m33 ());
  }

  template <typename T, typename U> inline DiagMatrix3<T>& operator*= (DiagMatrix3<T>& v, U scalar) {
    v.m11 () *= scalar; v.m22 () *= scalar; v.m33 () *= scalar;
    return v;
  }

  template <typename T, typename U> inline RT(/) operator/ (DiagMatrix3<T> v, U scalar) {
    return RT(/) (v.m11 () / scalar, v.m22 () / scalar, v.m33 () / scalar);
  }

  template <typename T, typename U> inline DiagMatrix3<T>& operator/= (DiagMatrix3<T>& v, U scalar) {
    v.m11 () /= scalar; v.m22 () /= scalar; v.m33 () /= scalar;
    return v;
  }

  template <typename T> inline bool operator== (DiagMatrix3<T> v1, DiagMatrix3<T> v2) {
    return v1.m11 () == v2.m11 () && v1.m22 () == v2.m22 () && v1.m33 () == v2.m33 ();
  }
  template <typename T> inline bool operator!= (DiagMatrix3<T> v1, DiagMatrix3<T> v2) {
    return !(v1 == v2);
  }

  /*
  template <typename T> struct Abs2Impl<DiagMatrix3<T> > {
    static DECLTYPE(Math::abs2 (*(T*)0)) apply (DiagMatrix3<T> v) {
      return Math::abs2 (v.m11 ()) + Math::abs2 (v.m22 ()) + Math::abs2 (v.m33 ());
    }
  };
  */

  // Matrix-Matrix multiplication
  template <typename T, typename U> inline RT(*) operator* (DiagMatrix3<T> v1, DiagMatrix3<U> v2) {
    return RT(*) (v1.m11 () * v2.m11 (), v1.m22 () * v2.m22 (), v1.m33 () * v2.m33 ());
  }

  // Matrix-Vector multiplication
  template <typename T, typename U> inline Vector3<RTS(*)> operator* (DiagMatrix3<T> v1, Vector3<U> v2) {
    return Vector3<RTS(*)> (v1.m11 () * v2.x (), v1.m22 () * v2.y (), v1.m33 () * v2.z ());
  }

#undef RTS
#undef RT

  // Unary +/-
  template <typename T> inline DiagMatrix3<T> operator+ (DiagMatrix3<T> v) {
    return DiagMatrix3<T> (+v.m11 (), +v.m22 (), +v.m33 ());
  }
  template <typename T> inline DiagMatrix3<T> operator- (DiagMatrix3<T> v) {
    return DiagMatrix3<T> (-v.m11 (), -v.m22 (), -v.m33 ());
  }

  template <typename F> DiagMatrix3<F> real (DiagMatrix3<std::complex<F> > v) {
    return DiagMatrix3<F> (real (v.m11 ()), real (v.m22 ()), real (v.m33 ()));
  }

  template <typename F> DiagMatrix3<F> imag (DiagMatrix3<std::complex<F> > v) {
    return DiagMatrix3<F> (imag (v.m11 ()), imag (v.m22 ()), imag (v.m33 ()));
  }
}

#endif // !MATH_DIAGMATRIX3_HPP_INCLUDED


//
// Created by 叶璨铭 on 2022/3/31.
//
#include "cn/edu/SUSTech/YeCanming/Algs/DivideAndConquer/ClosestPoint.hpp"
#include "gtest/gtest.h"
//#include <boost/log/trivial.hpp>
#include <numeric>
#include <random>
#include <sstream>
namespace cn::edu::SUSTech::YeCanming::Algs::DivideAndConquer{
    class GTestClosestPoint : public ::testing::Test {
    protected:
        ClosestPoint closestPoint;
        std::vector<std::array<double, 2>> vec2d1 = {{1, 2},{3, 4}, {2.5, 10}, {10, 2.5}};

        //random
        std::random_device rd;
    };
    TEST_F(GTestClosestPoint, TestClosestPoint2DRandomlyWithND){
        auto m = int(1e7);
        std::mt19937 mt(rd());
        std::uniform_int_distribution<> dis(-m, m);

    }
#define TEST_TestClosestPoint2DWhenXAreEqual_single(Name, ...) \
  TEST_F(GTestClosestPoint, TestClosestPoint2DWhenXAreEqualSingle##Name) { \
            std::vector<double> vec1d = { __VA_ARGS__ };                  \
           ASSERT_NE(vec1d.size(), 0);                                                        \
            std::vector<std::array<double, 2>> data(vec1d.size());                   \
            for (int j = 0; j < vec1d.size(); ++j) {\
                data[j] = {20.0, vec1d[j]};\
            }\
            std::stringstream ss;\
            std::copy(vec1d.begin(), vec1d.end(), std::ostream_iterator<double>(ss, ", "));\
            ss<<std::endl;\
            auto resultExpected = closestPoint.findClosestPointPairND<double, 2>(data);\
            auto resultActually = closestPoint.findClosestPointPair2D<double>(data);\
            EXPECT_EQ(std::get<1>(resultExpected),std::get<1>(resultActually))<<ss.str();\
    }
#define TEST_TestClosestPoint2DWhenXAreEqual(Name, m, n, times)\
    TEST_F(GTestClosestPoint, TestClosestPoint2DWhenXAreEqual##Name){\
        std::mt19937 mt(rd());\
        std::uniform_int_distribution<> dis(-(m), m);\
        auto N = n;\
        std::vector<double> vec1d(N);\
        for (int i = 0; i < (times); ++i) {\
            std::generate(vec1d.begin(), vec1d.end(), std::bind(dis, std::ref(mt)));\
            std::vector<std::array<double, 2>> data(N);\
            for (int j = 0; j < N; ++j) {\
                data[j] = {20.0, vec1d[j]};\
            }\
            std::stringstream ss;\
            std::copy(vec1d.begin(), vec1d.end(), std::ostream_iterator<double>(ss, ", "));\
            ss<<std::endl;\
            auto resultExpected = closestPoint.findClosestPointPair1D<double>(vec1d);\
            auto resultActually = closestPoint.findClosestPointPair2D<double>(data);\
            EXPECT_EQ(std::get<1>(resultExpected),std::get<1>(resultActually))<<ss.str();\
        }\
    }
    TEST_TestClosestPoint2DWhenXAreEqual(1, 100, 10, 100)
    TEST_TestClosestPoint2DWhenXAreEqual(2, 1000, 20, 100)
    TEST_TestClosestPoint2DWhenXAreEqual(3, 1000, 100, 100)
    TEST_TestClosestPoint2DWhenXAreEqual(4, 1e7, 1e3, 50)
    TEST_TestClosestPoint2DWhenXAreEqual(5, 1e7, 1e4, 5)

    //为什么过不了，因为原理错了。当x相等时候，不可能只看6个。也不能只看15个。 除非一开始排序的时候，进行先x后y的同比排序，关系才成立。
    TEST_TestClosestPoint2DWhenXAreEqual_single(1, 32, 123, 827, 224, 686, 351, 171, 154, 678, 941, 574, 421, 769, 374, 905, 357, 215, 386, 885, 233)

    TEST_F(GTestClosestPoint, TestClosestPoint2D){
#define Index(it) std::distance(vec2d1.cbegin(),it)
        auto [cl, d] = closestPoint.findClosestPointPair2D<double>(vec2d1);
        EXPECT_EQ(d, 2*sqrt(2));
        EXPECT_EQ(Index(cl[0])+Index(cl[1]), 1);
        EXPECT_EQ(std::max(Index(cl[0]),Index(cl[1]))-std::min(Index(cl[0]),Index(cl[1])), 1);
    }
//    TEST_F(GTestClosestPoint, CanLog){
//        BOOST_LOG_TRIVIAL(trace) << "A trace severity message";
//        BOOST_LOG_TRIVIAL(debug) << "A debug severity message";
//        BOOST_LOG_TRIVIAL(info) << "An informational severity message";
//        BOOST_LOG_TRIVIAL(warning) << "A warning severity message";
//        BOOST_LOG_TRIVIAL(error) << "An error severity message";
//        BOOST_LOG_TRIVIAL(fatal) << "A fatal severity message";
//    }
}
int main(int argc, char* argv[])
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


/*
 * Copyright 2011 Nate Koenig & Andrew Howard
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
/* Desc: Random number generator
 * Author: Nate Koenig
 * Date: 27 May 2009
 */

#ifndef RAND_HH
#define RAND_HH

#include <boost/random.hpp>

namespace gazebo
{
  namespace math
  {
    typedef boost::mt19937 GeneratorType;
    typedef boost::uniform_real<double> UniformRealDist;
    typedef boost::normal_distribution<double> NormalRealDist;
    typedef boost::uniform_int<int> UniformIntDist;

    typedef boost::variate_generator<GeneratorType&, UniformRealDist > URealGen;
    typedef boost::variate_generator<GeneratorType&, NormalRealDist > NRealGen;
    typedef boost::variate_generator<GeneratorType&, UniformIntDist > UIntGen;

    /// \addtogroup gazebo_math
    /// \{
    /// \brief Random number generator class
    class Rand
    {
      /// \brief Get a double from a uniform distribution
      /// \param min Minimum bound for the random number
      /// \param max Maximum bound for the random number
      public: static double GetDblUniform(double _min = 0, double _max = 1);

      /// \brief Get a double from a normal distribution
      /// \param mean Mean value for the distribution
      /// \param sigma Sigma value for the distribution
      public: static double GetDblNormal(double _mean = 0, double _sigma = 1);

      /// \brief Get a integer from a uniform distribution
      /// \param min Minimum bound for the random number
      /// \param max Maximum bound for the random number
      public: static int GetIntUniform(int _min, int _max);

      /// \brief Get a double from a normal distribution
      /// \param mean Mean value for the distribution
      /// \param sigma Sigma value for the distribution
      public: static int GetIntNormal(int _mean, int _sigma);

      // The random number generator
      private: static GeneratorType *randGenerator;
    };
    /// \}
  }
}
#endif


#ifdef WIN32
#define NOMINMAX
#endif
#define _USE_MATH_DEFINES
#include <math.h>

#include <app.h>

#include <NewtonFunctionMinimizer.h>
#include <RigidBodySimulation.h>
#include <MechanismOptimizer.h>

#include "mechanisms.h"

#include <iostream>
#include <chrono>
#include <algorithm>

#include <Eigen/Core>

using Eigen::Vector2f;
using Eigen::Vector2d;
using Eigen::VectorXd;

#define PLOT_N 100 // number of data points in plot

class MoveRigidBodyObj : public ObjectiveFunction
{
public:
    MoveRigidBodyObj(Vector2d pTarget, Vector2d pLocal)
        : rb({0, 1.0}), pTarget(pTarget), pLocal(pLocal) {
    }

    double evaluate(const VectorXd& x) const override {
        return (pTarget - rb.pWorld(x, pLocal)).squaredNorm();
    }

public:
    RigidBody rb;
    Vector2d pTarget, pLocal;
};

class RigidBodyApp : public App
{
public:
	RigidBodyApp(int width, int height, const char * title, float pixelRatio = 0.f)
		: App(width, height, title, pixelRatio), base(width) {

		clear_color = ImVec4(0.8f, 0.8f, 0.8f, 1.00f);
		lastFrame = std::chrono::high_resolution_clock::now();

		font = nvgCreateFont(vg, "sans", DATA_FOLDER"/Roboto-Regular.ttf");
		if (font == -1) {
			printf("Could not add font.\n");
		}

		for (float & d : dataEnergy)
			d = 0;

		sim = make4barSim();
		mechOpt = MechanismOptimizer(sim);
	}

    void process() override{
		// move image if right mouse button is pressed
		if(mouseDown[GLFW_MOUSE_BUTTON_RIGHT]){
			auto dw = (int)(cursorPos[0] - cursorPosDown[0]);
			auto dh = (int)(cursorPos[1] - cursorPosDown[1]);
			translation[0] += dw/(double)base;
			translation[1] -= dh/(double)base;
			cursorPosDown[0] = cursorPos[0];
			cursorPosDown[1] = cursorPos[1];
		}

		// run at 60fps, or in slow mo
		std::chrono::high_resolution_clock::time_point now = std::chrono::high_resolution_clock::now();
		if(std::chrono::duration_cast<std::chrono::milliseconds>(now-lastFrame).count() > ((slowMo) ? 320 : 16)){

			if(selectedRb != -1){
				double scale = 1.0;
				if(keyDown[GLFW_KEY_UP])
					scale = 1.01;
				else if(keyDown[GLFW_KEY_DOWN])
					scale = 0.99;
				if(scale != 1.0){
					sim.scaleRigidBody(selectedRb, scale);

					if(sim.motorIdx != -1)
                        trackedTrajectory = sim.recordTrajectory();
						cout << "trackedTrajectory: " << trackedTrajectory << endl;
				}
			}

			if(sim.motorIdx != -1){
				if(keyDown[GLFW_KEY_LEFT])
					sim.fixedAngle()[sim.motorIdx].angle += 4 * M_PI / 180;
				if(keyDown[GLFW_KEY_RIGHT])
					sim.fixedAngle()[sim.motorIdx].angle -= 4 * M_PI / 180;

				if(sim.fixedAngle()[sim.motorIdx].angle > 2*M_PI)
					sim.fixedAngle()[sim.motorIdx].angle -= 2*M_PI;
				if(sim.fixedAngle()[sim.motorIdx].angle < 0)
					sim.fixedAngle()[sim.motorIdx].angle += 2*M_PI;
			}

			if(mouseDown[GLFW_MOUSE_BUTTON_LEFT] && selectedRb != -1){

                Vector2d pos = fromScreen(cursorPos[0], cursorPos[1]);
                const auto &rb = sim.rigidbodies()[selectedRb];
                MoveRigidBodyObj obj({pos, selectedRbLocal});
                GradientDescentVariableStep gd;
                VectorXd xRb = sim.x.segment<3>(rb.dofIdx);
                gd.minimize(&obj, xRb, false);
                sim.x.segment<3>(rb.dofIdx) = xRb;
			}

			if(runSim){
				sim.run();
			}

			energy = sim.energy.evaluate(sim.x);
			dataCounter %= PLOT_N;
			dataEnergy.at(dataCounter++) = (float)energy;

			lastFrame = now;
		}
	}

    void drawScene() override {

		// this is the GUI
		{
			ImGui::Begin("Assignement 1");

			ImGui::TextColored(ImVec4(1.f, 1.f, 1.f, 0.5f), "left mouse:  Select rigid bodies, apply forces");
			ImGui::TextColored(ImVec4(1.f, 1.f, 1.f, 0.5f), "right mouse: move around");
			ImGui::TextColored(ImVec4(1.f, 1.f, 1.f, 0.5f), "mouse wheel: zoom");
			ImGui::TextColored(ImVec4(1.f, 1.f, 1.f, 0.5f), "space bar:   play/pause");

			//Define the mechanisms to include in main app.
			const RigidBodySimulation sims[] = {
				makeJansenSim(),
				make4barSim(),
				makePrismaticSim1(),
				makePrismaticSim2(),
				makePrismaticSim3(),
				makePrismaticSim4_XY(),
				makePrismaticSim5_XY(),
				makePrismaticSim6_XYLoop(),
				makeTangentSim1(),
				makeTangentSim2(),
			};
			const int numberOfSims = 10;

			//Construct the dropdown list based on the included function
			char* items[numberOfSims];
			for (int i = 0; i < numberOfSims; i++) {
				items[i] = sims[i].name;
			}

			if(ImGui::Combo("Mechanism", &loadMech, items, numberOfSims)){
				sim = sims[loadMech];
				targetTrajectory.resize(0, 2);
				//if (loadMech == 0) {
				//	targetTrajectory = makeJansenTargetPath();
				//	sim.energy.fixedAngleEnabled = true;
				//	trackedTrajectory = sim.recordTrajectory();
				//}
				mechOpt = MechanismOptimizer(sim);
			}

            if(ImGui::CollapsingHeader("Simulation")){
                ImGui::Checkbox("run simulation", &runSim);
                ImGui::PlotLines("Energy", dataEnergy.data(), PLOT_N, dataCounter, "energy", 0, 1.0, ImVec2(0, 100));
                ImGui::Separator();

                if(sim.motorIdx != -1){
                    ImGui::Checkbox("Fixed Angles enabled", &sim.energy.fixedAngleEnabled);
                    auto &fixedAngle = sim.fixedAngle()[sim.motorIdx];
                    float angle = (sim.energy.fixedAngleEnabled) ?
                                fixedAngle.angle :
                                sim.rigidbodies()[fixedAngle.rbIdx].theta(sim.x);
                    if(ImGui::SliderAngle("angle", &angle, 0, 360)){
                        fixedAngle.angle = angle;
                        sim.rigidbodies()[fixedAngle.rbIdx].theta(sim.x) = angle;
                    }
                    ImGui::Separator();
                }
            }

   //         if(sim.motorIdx != -1 && ImGui::CollapsingHeader("Design Optimization")){

			//	ImGui::Checkbox("Run optimization", &isMechOpt);
			//	if(isMechOpt){
   //                 mechOpt.targetPath = targetTrajectory;
   //                 mechOpt.optimizeTrajectory();
			//		VectorXd p = sim.getDesignParameters();
			//		if((p-mechOpt.p).norm() > 1e-10){
			//			sim.setDesignParameters(mechOpt.p);
   //                     trackedTrajectory = sim.recordTrajectory();
			//		}
			//	}

   //             if(ImGui::Button("print link lengths"))
   //                 std::cout << "Link lengths:" << std::endl <<
   //                              sim.getDesignParameters() << std::endl;
			//}

			ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate);
			ImGui::End();
		}

		for (const auto &f : sim.fixedAngle()) {
			nvgResetTransform(vg);
			nvgBeginPath(vg);
			const auto &rb = sim.rigidbodies()[f.rbIdx];
			Vector2d p = rb.pos(sim.x);

			nvgCircle(vg, toScreen(p.x(), 0), toScreen(p.y(), 1), toScreen(0.5));
			nvgFillColor(vg, (sim.motorIdx == -1) ? nvgRGBAf(0, 0, 1, 0.5) : nvgRGBAf(0, 1, 1, 0.5));
			nvgFill(vg);
		}

		int i = 0;
		for (const auto &rb : sim.rigidbodies()) {
			nvgResetTransform(vg);
			Vector2d p = rb.pos(sim.x);
			nvgTranslate(vg, toScreen(p.x(), 0), toScreen(p.y(), 1));
			nvgRotate(vg, -rb.theta(sim.x));
			nvgBeginPath(vg);
			double r = rb.width/2;
			nvgRoundedRect(vg, toScreen(-rb.length*0.5 - r), toScreen(-rb.width*0.5), toScreen(rb.length + 2*r), toScreen(rb.width), toScreen(r));
			if(i == selectedRb)
				nvgFillColor(vg, nvgRGBAf(1.0, 0.9, 0.7, 0.5));
			else
				nvgFillColor(vg, nvgRGBAf(0.5, 0.5, 0.5, 0.5));
			nvgFill(vg);
			nvgStrokeColor(vg, nvgRGBAf(0, 0, 0, 1));
			nvgStrokeWidth(vg, 2*pixelRatio);
			nvgStroke(vg);

			nvgFontSize(vg, toScreen(0.3));
			nvgFontFace(vg, "sans");

			nvgFillColor(vg, nvgRGBAf(0,0,0,1));
			nvgText(vg, 0, 100/zoom, rb.name.c_str(), nullptr);
			i++;
		}

		for (const auto &joint : sim.hingeJoints()) {
			nvgResetTransform(vg);
			nvgBeginPath(vg);
			const auto &rb0 = sim.rigidbodies()[joint.rbIdx[0]];
			const auto &rb1 = sim.rigidbodies()[joint.rbIdx[1]];
			Vector2d p0 = rb0.pWorld(sim.x, joint.local[0]);
			Vector2d p1 = rb1.pWorld(sim.x, joint.local[1]);
			nvgMoveTo(vg, toScreen(p0.x(), 0), toScreen(p0.y(), 1));
			nvgLineTo(vg, toScreen(p1.x(), 0), toScreen(p1.y(), 1));
			nvgStrokeColor(vg, nvgRGBAf(1, 0, 0, 0.5));
			nvgStrokeWidth(vg, 2.0*pixelRatio);
			nvgStroke(vg);

			nvgBeginPath(vg);
			nvgCircle(vg, toScreen(p0.x(), 0), toScreen(p0.y(), 1), toScreen(rb0.width/3));
			nvgCircle(vg, toScreen(p1.x(), 0), toScreen(p1.y(), 1), toScreen(rb1.width/3));
			nvgStrokeColor(vg, nvgRGBAf(1, 0, 0, 0.5));
			nvgStrokeWidth(vg, 2.0*pixelRatio);
			nvgStroke(vg);
		}

		//Added Draw Point On Line Joint Graphics
		for (const auto &joint : sim.pointOnLineJoints()) {
			nvgResetTransform(vg);

			nvgBeginPath(vg);
			const auto &rb0 = sim.rigidbodies()[joint.rbIdx[0]];

			Vector2d p0_bgn = rb0.pWorld(sim.x, joint.local0Point);
			Vector2d p0_end = rb0.pWorld(sim.x, joint.local0Point + joint.local0Vector);
			nvgMoveTo(vg, toScreen(p0_bgn.x(), 0), toScreen(p0_bgn.y(), 1));
			nvgLineTo(vg, toScreen(p0_end.x(), 0), toScreen(p0_end.y(), 1));
			nvgStrokeColor(vg, nvgRGBAf(0, 0, 1, 0.5));
			nvgStrokeWidth(vg, 2.0*pixelRatio);
			nvgStroke(vg);

			//Draw circle on rb1 point
			const auto &rb1 = sim.rigidbodies()[joint.rbIdx[1]];
			Vector2d p1 = rb1.pWorld(sim.x, joint.local1Point);

			nvgBeginPath(vg);
			nvgCircle(vg, toScreen(p1.x(), 0), toScreen(p1.y(), 1), toScreen(rb1.width / 3));
			nvgStrokeColor(vg, nvgRGBAf(0, 0, 1, 0.5));
			nvgStrokeWidth(vg, 2.0*pixelRatio);
			nvgStroke(vg);
		}

		for (const auto &f : sim.fixed()) {
			nvgResetTransform(vg);
			nvgBeginPath(vg);
			const auto &rb = sim.rigidbodies()[f.rbIdx];
			Vector2d p0 = rb.pWorld(sim.x, f.localPos);
			Vector2d p1 = f.pos;
			nvgMoveTo(vg, toScreen(p0.x(), 0), toScreen(p0.y(), 1));
			nvgLineTo(vg, toScreen(p1.x(), 0), toScreen(p1.y(), 1));
			nvgStrokeColor(vg, nvgRGBAf(0.2, 0.2, 0, 0.5));
			nvgStrokeWidth(vg, 2.0*pixelRatio);
			nvgStroke(vg);

			nvgBeginPath(vg);
			nvgCircle(vg, toScreen(p0.x(), 0), toScreen(p0.y(), 1), toScreen(rb.width/4));
			nvgCircle(vg, toScreen(p1.x(), 0), toScreen(p1.y(), 1), toScreen(rb.width/4));
			nvgStrokeColor(vg, nvgRGBAf(0.2, 0.2, 0, 0.5));
			nvgStrokeWidth(vg, 2.0*pixelRatio);
			nvgStroke(vg);
		}

		// draw tracked point
		if(sim.trackRBPoint.rbIdx >= 0){
			nvgResetTransform(vg);
			nvgBeginPath(vg);
			const auto &rb = sim.rigidbodies()[sim.trackRBPoint.rbIdx];
			Vector2d p = rb.pWorld(sim.x, sim.trackRBPoint.local);
			nvgCircle(vg, toScreen(p.x(), 0), toScreen(p.y(), 1), 2.0);
			nvgFillColor(vg, nvgRGBAf(0.2, 0.8, 0.2, 0.5));
			nvgFill(vg);
		}

		auto draw_path = [=](const Matrix<double, -1, 2> &path, NVGcolor color){
			if(path.rows() > 0){
				nvgBeginPath(vg);

				nvgMoveTo(vg, toScreen(path(0, 0), 0), toScreen(path(0, 1), 1));
				for (int i = 0; i < path.rows(); i++) {
					nvgLineTo(vg, toScreen(path(i, 0), 0), toScreen(path(i, 1), 1));
				}
				nvgStrokeColor(vg, color);
				nvgStrokeWidth(vg, 2);
				nvgStroke(vg);

				nvgBeginPath(vg);
				for (int i = 0; i < path.rows(); i++)
					nvgCircle(vg, toScreen(path(i, 0), 0), toScreen(path(i, 1), 1), toScreen(0.07));
				nvgFillColor(vg, color);
				nvgFill(vg);
			}
		};

        draw_path(trackedTrajectory, nvgRGBAf(0.2, 0.8, 0.2, 0.5));
        draw_path(targetTrajectory, nvgRGBAf(0.8, 0.5, 0.2, 0.5));

	}

protected:
    void keyPressed(int key, int  /*mods*/) override {
		// play / pause with space bar
		if(key == GLFW_KEY_SPACE)
			runSim = !runSim;
	}

    void mousePressed(int button) override {
		cursorPosDown[0] = cursorPos[0];
		cursorPosDown[1] = cursorPos[1];

		if(button == GLFW_MOUSE_BUTTON_LEFT){
			Vector2d cursor = fromScreen(cursorPos[0], cursorPos[1]);

            int i = 0;
            selectedRb = -1;
            for (const auto &rb : sim.rigidbodies()) {
                auto rot = rotationMatrix(-rb.theta(sim.x));
                Vector2d d = rot * (cursor - rb.pos(sim.x));
                if(std::abs(d.x()) <= rb.length/2 && std::abs(d.y()) <= rb.width/2){
                    selectedRb = i;
                    selectedRbLocal = d;
                    break;
                }
                i++;
            }
		}
	}

    void mouseReleased(int  /*button*/) override {
	}

    void scrollWheel(double  /*xoffset*/, double yoffset) override {
		double zoomOld = zoom;
		zoom *= std::pow(1.10, yoffset);
		for (int dim = 0; dim < 2; ++dim) {
			double c = cursorPos[dim]/(double) ((dim == 0) ? base : -base);
			translation[dim] = c - zoomOld/zoom * (c-translation[dim]);
		}
	}

    void windowResized(int /*w*/, int /*h*/) override {
	}

private:

	VectorXd fromScreen(int i, int j, int w, int h) const {
		VectorXd x(2);
		x[0] = ((double)i/(double)w - translation[0])*zoom/pixelRatio;
		x[1] = (-(double)j/(double)h - translation[1])*zoom/pixelRatio;
		return x;
	}

	template<class S>
	VectorXd fromScreen(S i, S j) const {
		return fromScreen((double)i, (double)j, base, base);
	}

	double toScreen(double s, int dim) const {
		return (s/zoom*pixelRatio + translation[dim]) * (double)((dim == 0) ? base : -base);
	}

	double toScreen(double s) const {
		return s/zoom*pixelRatio * base;
	}

private:
	int font = -1;

	int loadMech = 0;
	bool runSim = false;
	std::chrono::high_resolution_clock::time_point lastFrame;
	bool slowMo = false;

	double cursorPosDown[2]{};
	double translation[2] = {0.75*pixelRatio, -0.25*pixelRatio};
	double zoom = 24;
	int base;

	int selectedRb = -1;
    Vector2d selectedRbLocal;

	bool isMechOpt = false;

public:
	// optimization
	double energy = 0;
	int dataCounter = 0;
	std::array<float, PLOT_N> dataEnergy{};

	RigidBodySimulation sim;

    Matrix<double, -1, 2> trackedTrajectory, targetTrajectory;
	MechanismOptimizer mechOpt;
};

int main(int, char**)
{
	// If you have high DPI screen settings, you can change the pixel ratio
	// accordingly. E.g. for 200% scaling use `pixelRatio = 2.f`
	RigidBodyApp app(1080, 720, "Assignement 1");

	app.run();
	return 0;
}


// Copyright (C) 2013  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/matrix.h>
#include <sstream>
#include <string>
#include <ctime>
#include <vector>
#include <dlib/statistics.h>

#include "tester.h"
#include <dlib/svm.h>

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.svr_linear_trainer");

    typedef matrix<double, 0, 1> sample_type;
    typedef std::vector<std::pair<unsigned int, double> > sparse_sample_type;

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

    double sinc(double x)
    {
        if (x == 0)
            return 1;
        return sin(x)/x;
    }

    template <typename scalar_type>
    void test1()
    {
        typedef matrix<scalar_type,0,1> sample_type;

        typedef radial_basis_kernel<sample_type> kernel_type;

        print_spinner();

        std::vector<sample_type> samples;
        std::vector<scalar_type> targets;

        // The first thing we do is pick a few training points from the sinc() function.
        sample_type m(1);
        for (scalar_type x = -10; x <= 4; x += 1)
        {
            m(0) = x;

            samples.push_back(m);
            targets.push_back(sinc(x)+1.1);
        }

        randomize_samples(samples, targets);

        empirical_kernel_map<kernel_type> ekm;
        ekm.load(kernel_type(0.1), samples);

        for (unsigned long i = 0; i < samples.size(); ++i)
            samples[i] = ekm.project(samples[i]);

        svr_linear_trainer<linear_kernel<sample_type> > linear_trainer;
        linear_trainer.set_epsilon(0.0001);
        linear_trainer.set_c(30);
        linear_trainer.set_epsilon_insensitivity(0.001);

        matrix<double> res = cross_validate_regression_trainer(linear_trainer, samples, targets, 5);
        dlog << LINFO << "MSE and R-Squared: "<< res;
        DLIB_TEST(res(0) < 1e-4);
        DLIB_TEST(res(1) > 0.99);

        dlib::rand rnd;

        samples.clear();
        targets.clear();
        std::vector<scalar_type> noisefree_targets;
        for (scalar_type x = 0; x <= 5; x += 0.1)
        {
            m(0) = x;
            samples.push_back(matrix_cast<scalar_type>(linpiece(m, linspace(0,5,20))));
            targets.push_back(x*x + rnd.get_random_gaussian());
            noisefree_targets.push_back(x*x);
        }
        linear_trainer.set_learns_nonnegative_weights(true);
        linear_trainer.set_epsilon_insensitivity(1.0);
        decision_function<linear_kernel<sample_type> > df2 = linear_trainer.train(samples, targets);

        print_spinner();
        res = test_regression_function(df2, samples, noisefree_targets);
        dlog << LINFO << "MSE and R-Squared: "<< res;
        DLIB_TEST(res(0) < 0.15);
        DLIB_TEST(res(1) > 0.98);
        DLIB_TEST(df2.basis_vectors.size()==1);
        DLIB_TEST(max(df2.basis_vectors(0)) >= 0);

        linear_trainer.force_last_weight_to_1(true);
        df2 = linear_trainer.train(samples, targets);
        DLIB_TEST(std::abs(df2.basis_vectors(0)(samples[0].size()-1) - 1.0) < 1e-14);

        res = test_regression_function(df2, samples, noisefree_targets);
        dlog << LINFO << "MSE and R-Squared: "<< res;
        DLIB_TEST(res(0) < 0.20);
        DLIB_TEST(res(1) > 0.98);

        // convert into sparse vectors and try it out
        typedef std::vector<std::pair<unsigned long, scalar_type> > sparse_samp;
        std::vector<sparse_samp> ssamples;
        for (unsigned long i = 0; i < samples.size(); ++i)
        {
            sparse_samp s;
            for (long j = 0; j < samples[i].size(); ++j)
                s.push_back(make_pair(j,samples[i](j)));
            ssamples.push_back(s);
        }

        svr_linear_trainer<sparse_linear_kernel<sparse_samp> > strainer;
        strainer.set_learns_nonnegative_weights(true);
        strainer.set_epsilon_insensitivity(1.0);
        strainer.set_c(30);
        decision_function<sparse_linear_kernel<sparse_samp> > df;
        df = strainer.train(ssamples, targets);
        res = test_regression_function(df, ssamples, noisefree_targets);
        dlog << LINFO << "MSE and R-Squared: "<< res;
        DLIB_TEST(res(0) < 0.15);
        DLIB_TEST(res(1) > 0.98);
        DLIB_TEST(df2.basis_vectors.size()==1);
        DLIB_TEST(max(sparse_to_dense(df2.basis_vectors(0))) >= 0);
    }

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

    class tester_svr_linear_trainer : public tester
    {
    public:
        tester_svr_linear_trainer (
        ) :
            tester ("test_svr_linear_trainer",
                    "Runs tests on the svr_linear_trainer.")
        {}

        void perform_test (
        )
        {
            dlog << LINFO << "TEST double";
            test1<double>();
            dlog << LINFO << "TEST float";
            test1<float>();
        }
    } a;

}


/*
 * concatenate Eigen matrices
 * by R. Falque
 * 03/07/2019
 */

#ifndef EIGEN_CAST_HPP
#define EIGEN_CAST_HPP

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

//https://stackoverflow.com/a/21068014/2562693
template <typename T>
inline void cast_to_matrix(std::vector< std::vector <T> > input, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& output)
{
    const int numRows = input.size();
    const int numCols = input[0].size();

    //output = Eigen::Map<Eigen::Matrix<T, input.size(), input[0].size()>, Eigen::Unaligned>(input.data(), input.size());
    output.resize(numRows, numCols);

    for (int i=0; i<numRows; i++)
        for (int j=0; j<numCols; j++)
            output(i,j) = input[i][j];
}

#endif


#include "utils/math_utils.h"
#include "utils/algorithms.hpp"
#include <boost/program_options.hpp>
#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <iterator>
#include <fstream>
#include <algorithm>
#include <map>
#include <string>

template<typename It, typename T>
bool verify(It from, It to, T min, T max)
{
    return std::all_of(from, to, [min, max](auto val) {
                    return val >= min && val <= max;
                }
            );
}

struct program_args
{
    enum norm_type {
        MIN_MAX, MAX, Z_NORM, NONE
    };

    norm_type type = NONE;
    double min = 0;
    double max = 1;
    std::string in_file;

    program_args() = default;
    program_args(norm_type t, double mn, double mx, const std::string& p) :
            type(t), min(mn), max(mx), in_file(p)
    {
    }

    static norm_type type_convert(const std::string& from) {
            static const std::map<std::string, norm_type> convertion{
                {"minmax", MIN_MAX}, {"max", MAX}, {"znorm", Z_NORM}
            };
            auto i = convertion.find(from);
            return i == std::end(convertion) ? NONE : i->second;
    }
};

namespace po = boost::program_options;
program_args process_args(int argc, char** argv)
{
    program_args args;
    po::options_description desc("Allowed options");
    std::string algo;
    desc.add_options()
        ("help,h", "test usage:")
        ("min,m", po::value<double>(&args.min)->default_value(args.min),
         "algorithm min value (for min/max norm only)")
        ("max,x", po::value<double>(&args.max)->default_value(args.max),
         "max value (for min/max  norm only")
        ("file,f", po::value<std::string>(&args.in_file),
         "input file to read data from")
        ("algo,a", po::value<std::string>(&algo),
         "normalization algoritm, select from 'minmax' 'max' or 'znorm'");
    po::variables_map cli;
    if (argc == 1 || cli.count("help")) {
        std::cerr<<desc<<std::endl;
        throw std::runtime_error{"no args"};;
    }
    try {
        po::store(po::command_line_parser(argc, argv).options(desc).run(), cli);
        po::notify(cli);
    } catch (const std::exception& e) {
        std::cerr<<"error - invalid command line: "<<e.what()<<"\n"<<desc<<std::endl;
        throw e;
    }
    if (algo.empty()) {
        std::cerr<<"algorithm type not set\n"<<desc<<std::endl;
        throw std::runtime_error{"missing args"};
    } else {
        auto a = program_args::type_convert(algo);
        if (a == program_args::NONE) {
            std::cerr<<"invalid algorithm: "<<algo<<"\n"<<desc<<std::endl;
            throw std::runtime_error{"invalid algortihm"};
        }
        args.type = a;
        return args;
    }
}

int process_min_max(const std::vector<double>& data, double min, double max)
{
    auto out = utils::min_max_normalization(data, min, max);
    std::cout<<"after normalization:\n";
    out_range(std::cout, out);
    std::cout<<std::endl;
    if (verify(std::begin(out), std::end(out), min, max)) {
        std::cout<<"all values after norm are valid"<<std::endl;
        return 0;
    } else {
        std::cout<<"verification failed - we have values that are out of range ["<<min<<", "<<max<<"]\n";
        return -1;
    }
}

int process_max(const std::vector<double>& data, double)
{
    auto out = utils::max_normalization(data);
    std::cout<<"after normalization:\n";
    out_range(std::cout, out);
    std::cout<<std::endl;
    return 0;
}

int process_znorm(const std::vector<double>& data)
{
    auto out = utils::z_normalization(data);
    std::cout<<"after normalization:\n";
    out_range(std::cout, out);
    std::cout<<std::endl;
    return 0;
}

int process(const program_args& args)
{
    std::ifstream input(args.in_file.c_str());
    if (!input) {
        std::cerr<<"failed to open "<<args.in_file<<" for reading\n";
        return -1;
    }
    std::cout<<"reading input from "<<args.in_file<<std::endl;
    const std::vector<double> data{std::istream_iterator<double>(input),
                                   std::istream_iterator<double>()
    };
    if (data.empty()) {
        std::cerr<<"failed  to read from "<<args.in_file<<std::endl;
        return -1;
    }
    std::cout<<"before data:\n";
    out_range(std::cout, data);
    std::cout<<std::endl;
    switch (args.type) {
        case program_args::MAX:
            return process_max(data, args.max);
            utils::max_normalization(data);
            break;
            case program_args::MIN_MAX:
                return process_min_max(data, args.min, args.max);
            case program_args::Z_NORM:
                return process_znorm(data);
            default:
                throw std::runtime_error{"invalid algorithm type"};
    }
}

int main(int argc, char** argv)
{
    try {
        auto args = process_args(argc, argv);
        return process(args);
    } catch (const std::exception&) {
        return -1;
    }
#if 0
    if (argc != 4) {
        std::cerr<<"usage: "<<argv[0]<<" <input file> <min> <max>\n";
        return -1;
    }
#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)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/nthroot.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/half.hpp>

template <typename T, std::size_t N, typename Env>
void test(Env& runtime)
{

  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using iT =  bd::as_integer_t<T>;
  using p_t = bs::pack<T, N>;
  using pi_t= bs::pack<iT, N>;

  T a1[N], b[N];
  iT a2[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : bs::rec(T(i));
    a2[i] = i+2;
    b[i] = bs::nthroot(a1[i], a2[i]) ;
  }
  p_t aa1(&a1[0], &a1[0]+N);
  pi_t aa2(&a2[0], &a2[0]+N);
  p_t bb (&b[0], &b[0]+N);

  STF_ULP_EQUAL(bs::nthroot(aa1, aa2), bb, 1);
  STF_ULP_EQUAL(bs::raw_(bs::nthroot)(aa1, aa2), bb, 1);
}

STF_CASE_TPL("Check nthroot on pack" , STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>(runtime);
  test<T, N/2>(runtime);
  test<T, N*2>(runtime);
}

STF_CASE_TPL (" nthroot",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::nthroot;
  using p_t = bs::pack<T>;
  using pi_t = bd::as_integer_t<p_t>;
  using r_t = decltype(nthroot(p_t(), pi_t()));

  // return type conformity test
 STF_TYPE_IS(r_t, p_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(nthroot(bs::Inf<p_t>(),pi_t(3)), bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Inf<p_t>(),pi_t(4)), bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Minf<p_t>(),pi_t(3)), bs::Minf<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Minf<p_t>(),pi_t(4)), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Nan<p_t>(),pi_t(3)), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Nan<p_t>(),pi_t(4)), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Mone<p_t>(),pi_t(4)), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(nthroot(bs::Mone<p_t>(),pi_t(0)), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::One <p_t>(),pi_t(0)), bs::One<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Half<p_t>(),pi_t(0)), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Two <p_t>(),pi_t(0)), bs::Inf <r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Zero<p_t>(),pi_t(0)), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Two <p_t>(),pi_t(0)), bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Half<p_t>(),pi_t(0)), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Mone<p_t>(),pi_t(3)), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::One<p_t>(),pi_t(3)), bs::One<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::One<p_t>(),pi_t(4)), bs::One<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Zero<p_t>(),pi_t(3)), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(bs::Zero<p_t>(),pi_t(4)), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(nthroot(p_t(-8),pi_t(3)), r_t(-2), 0.5);
  STF_ULP_EQUAL(nthroot(p_t(256),pi_t(4)), r_t(4), 0.5);
  STF_ULP_EQUAL(nthroot(p_t(8),pi_t(3)), r_t(2), 0.5);
  STF_ULP_EQUAL(nthroot(p_t(0.5), pi_t(4)), r_t(0.84089641525371454303112547623321), 0.5);
}


#include <unistd.h>	// sleep
#include <armadillo>
#include <vector>
#include <deque>
#include <algorithm>    // std::min

#include "Functions.h"

using namespace arma;
using namespace std;

vec eta(const vec &v, const double tau){
	vec out(size(v),fill::zeros);

	uvec ind = find(abs(v) > tau);
	out(ind) = sign(v(ind))%(abs(v(ind)) - tau );
	return out;
}

vec eta_deriv(const vec &v, const double tau){
	vec out(size(v),fill::zeros);

	uvec ind = find(abs(v) > tau);
	out(ind).ones();
	return out;
}

vec eta(const vec &v, const vec tau){
	vec out(size(v),fill::zeros);

	uvec ind = find(abs(v) > tau);
	out(ind) = sign(v(ind))%(abs(v(ind)) - tau(ind) );
	return out;
}

vec eta_deriv(const vec &v, const vec tau){
	vec out(size(v),fill::zeros);

	uvec ind = find(abs(v) > tau);
	out(ind).ones();
	return out;
}

vec AMP(const mat &A, const vec &y, const int sparsity, const unsigned int max_iter,
	const double tol, unsigned int &num_iters, const simulation_parameters simulation_params){

	const unsigned int N = A.n_cols;   	// signal dimension
	const unsigned int M = y.n_elem;	// number of measurements
	//const double delta = double(num_measurements)/sig_dim;
	unsigned int i = 0;
	vec x_t(N,fill::zeros);
	vec z_t = y;
	double tau = .1;
	bool done = false;
	vec pseudo_data (N,fill::zeros);

	while(!done){
		i++;
		z_t = y - A*x_t + z_t * sum (eta_deriv( pseudo_data,tau) ) / M;
		pseudo_data = A.t() * z_t + x_t;
		tau = tau * sum(eta_deriv(pseudo_data,tau)) / M;
		x_t = eta(pseudo_data,tau) ;

		if (norm (y - A*x_t) < tol || i >= max_iter){
			done = true;
		}
	}

	num_iters = i;
	return x_t;
}

// J. Zhu, R. Pilgrim and D. Baron, "An overview of multi-processor approximate message passing,"
// http://ieeexplore.ieee.org/document/7926166/
vec R_MP_AMP(const mat &A, const vec &y, const int sparsity, const unsigned int max_iter,
	const double tol, unsigned int &num_iters, const simulation_parameters simulation_params){
	uvec slow_cores;
	set_slow_cores(slow_cores, simulation_params);

	const unsigned int N = A.n_cols;
	const unsigned int M = y.n_elem;
	const unsigned int P = simulation_params.num_cores;
	unsigned int i = 0;
	bool done = false;
	vec x_t(N,fill::zeros);
	double g_t = M;
	double tau = .1;

	vector <vec> pseudo_data (P,vec(N,fill::zeros));
	// parallel section of the code starts here
	#pragma omp parallel num_threads(simulation_params.num_cores)
	{
	// initializing variables in local memory
	const int p = omp_get_thread_num();
	mat A_p = A.rows(M*p/P , M*(p+1)/P -1 );
	vec y_p = y.subvec( M*p/P  , M*(p+1)/P -1 );

	vec z_t_p = y_p;
	// R_MP_AMP itearations
	while(!done){
		//AT processor p:
		z_t_p = y_p - A_p*x_t + z_t_p * g_t / M;
		pseudo_data[p] = A_p.t() * z_t_p + x_t/P;

		//slow cores sleep for  simulation_params.sleep_slow_cores microseconds
		if (any( slow_cores == omp_get_thread_num()) ){
			usleep(simulation_params.sleep_slow_cores);
		}

		#pragma omp barrier
		//AT fusion center:
		#pragma omp single
		{
			i++;
			vec pseudo_data_total(N,fill::zeros);
			for (unsigned int j = 0; j < P; j++){
				pseudo_data_total = pseudo_data_total + pseudo_data[j];
			}

			tau = tau * sum(eta_deriv(pseudo_data_total,tau)) / M;
			g_t = sum(eta_deriv(pseudo_data_total,tau));
			x_t = eta(pseudo_data_total,tau);
			if (norm (y - A*x_t) < tol || i >= max_iter){
				done = true;
			}
		}
		#pragma omp barrier
	}
	// parallel section of the code ends here

	}

	num_iters = i;
	return x_t;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_BITWISE_FUNCTIONS_SCALAR_ILOG2_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_SCALAR_ILOG2_HPP_INCLUDED

#include <boost/simd/bitwise/functions/ilog2.hpp>
#include <boost/simd/include/functions/scalar/clz.hpp>
#include <boost/simd/include/functions/scalar/exponent.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/mpl/less_equal.hpp>
#include <boost/mpl/sizeof.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/assert.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( ilog2_, tag::cpu_, (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
      {
        return exponent(a0);
      }
  };

  BOOST_DISPATCH_IMPLEMENT         ( ilog2_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      BOOST_ASSERT_MSG( a0 > 0, "Logarithm is not defined for zero or negative values." );
      return result_type(sizeof(A0)*8-boost::simd::clz(a0)-1);
    }
  };

#if defined(BOOST_MSVC)
  BOOST_DISPATCH_IMPLEMENT_IF         ( ilog2_, tag::cpu_
                            , (A0)
                            , (mpl::less_equal< mpl::sizeof_<A0>, mpl::size_t<4> >)
                            , (scalar_< integer_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      BOOST_ASSERT_MSG( a0 > 0, "Logarithm is not defined for zero or negative values." );
      __assume( a0 > 0 );
      unsigned long index;
      BOOST_VERIFY(::_BitScanReverse(&index, a0));
      return result_type(index);
    }
  };
#endif

#if defined(BOOST_MSVC) && defined(_WIN64)
  BOOST_DISPATCH_IMPLEMENT         ( ilog2_, tag::cpu_
                            , (A0)
                            , (scalar_< ints64_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      BOOST_ASSERT_MSG( a0 > 0, "Logarithm is not defined for zero or negative values." );
      __assume( a0 > 0 );
      unsigned long index;
      BOOST_VERIFY(::_BitScanReverse64(&index, a0));
      return index;
    }
  };
#endif

} } }

#endif


/// ---------------------------------------------------------------------------
/// @section LICENSE
///
/// Copyright (c) 2016 Georgia Tech Research Institute (GTRI)
///               All Rights Reserved
///
/// 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.
/// ---------------------------------------------------------------------------
/// @file filename.ext
/// @author Kevin DeMarco <kevin.demarco@gtri.gatech.edu>
/// @author Eric Squires <eric.squires@gtri.gatech.edu>
/// @version 1.0
/// ---------------------------------------------------------------------------
/// @brief A brief description.
///
/// @section DESCRIPTION
/// A long description.
/// ---------------------------------------------------------------------------

#include <gtest/gtest.h>
#include <gmock/gmock.h>
#include <scrimmage/math/Quaternion.h>
#include <scrimmage/math/Angles.h>
#include <Eigen/Dense>
#define _USE_MATH_DEFINES
#include <cmath>

using Eigen::Vector3d;
namespace sc = scrimmage;

TEST(test_quaternion, rotation) {
    Vector3d vector = Vector3d(1, 0, 0);
    sc::Quaternion quaternion(Vector3d(1, 1, 1), 2 * M_PI / 3);
    Vector3d rotated_vector = quaternion.rotate(vector);
    EXPECT_NEAR(rotated_vector(0), 0, 1e-10);
    EXPECT_NEAR(rotated_vector(1), 1, 1e-10);
    EXPECT_NEAR(rotated_vector(2), 0, 1e-10);
}

TEST(test_quaternion, reverse_rotation) {
    Vector3d vector = Vector3d(1, 0, 0);
    sc::Quaternion quaternion(Vector3d(1, 1, 1), -2 * M_PI / 3);
    Vector3d rotated_vector = quaternion.rotate_reverse(vector);
    EXPECT_NEAR(rotated_vector(0), 0, 1e-10);
    EXPECT_NEAR(rotated_vector(1), 1, 1e-10);
    EXPECT_NEAR(rotated_vector(2), 0, 1e-10);
}

TEST(test_quaternion, euler_convert) {
    double roll = 0.3;
    double pitch = 0.2;
    double yaw = 0.1;
    sc::Quaternion quaternion(roll, pitch, yaw);
    EXPECT_NEAR(roll, quaternion.roll(), 1e-10);
    EXPECT_NEAR(pitch, quaternion.pitch(), 1e-10);
    EXPECT_NEAR(yaw, quaternion.yaw(), 1e-10);
}

TEST(test_quaternion, frames) {
    Vector3d vec1(0, 0, 0);
    Vector3d vec2(1, 0, 1);
    Vector3d vec_diff = vec2 - vec1;

    // pointing upward with negative pitch
    sc::Quaternion q1(0, -sc::Angles::deg2rad(45), 0);

    Vector3d vec_diff_local1 = q1.rotate_reverse(vec_diff);
    EXPECT_NEAR(sqrt(2), vec_diff_local1(0), 1e-10);
    EXPECT_NEAR(0, vec_diff_local1(1), 1e-10);
    EXPECT_NEAR(0, vec_diff_local1(2), 1e-10);

    Vector3d vec_diff_global = q1.rotate(vec_diff_local1);
    EXPECT_NEAR(vec_diff(0), vec_diff_global(0), 1e-10);
    EXPECT_NEAR(vec_diff(1), vec_diff_global(1), 1e-10);
    EXPECT_NEAR(vec_diff(2), vec_diff_global(2), 1e-10);
}