/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include "Tudat/Basics/utilities.h"
#include "Tudat/Basics/testMacros.h"
#include "Tudat/Basics/basicTypedefs.h"
#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/InputOutput/matrixTextFileReader.h"

#include "Tudat/Mathematics/Statistics/basicStatistics.h"
#include "Tudat/Mathematics/Filters/unscentedKalmanFilter.h"
#include "Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h"

#include "Tudat/Mathematics/Filters/UnitTests/controlClass.h"

namespace tudat
{

namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_unscented_kalman_filter )

// Functions for unscented Kalman filter
Eigen::Vector2d stateFunction1( const double time, const Eigen::Vector2d& state, const Eigen::Vector2d& control )
{
    TUDAT_UNUSED_PARAMETER( time );
    TUDAT_UNUSED_PARAMETER( control );
    Eigen::Vector2d stateDerivative;
    stateDerivative[ 0 ] = state[ 1 ] * std::pow( std::cos( state[ 0 ] ), 3 );
    stateDerivative[ 1 ] = std::sin( state[ 0 ] );
    return stateDerivative;
}
Eigen::Vector1d measurementFunction1( const double time, const Eigen::Vector2d& state )
{
    TUDAT_UNUSED_PARAMETER( time );
    Eigen::Vector1d measurement;
    measurement[ 0 ] = std::pow( state[ 0 ], 3 );
    return measurement;
}

// Test implementation of unscented Kalman filter class.
BOOST_AUTO_TEST_CASE( testUnscentedKalmanFilterFirstCase )
{
    using namespace tudat::filters;

    // Set initial conditions
    const double initialTime = 0.0;
    const double timeStep = 0.01;
    const unsigned int numberOfTimeSteps = 1000;

    Eigen::Vector2d initialStateVector;
    initialStateVector[ 0 ] = 3.0;
    initialStateVector[ 1 ] = -0.3;

    Eigen::Vector2d initialEstimatedStateVector;
    initialEstimatedStateVector[ 0 ] = 10.0;
    initialEstimatedStateVector[ 1 ] = -3.0;

    Eigen::Matrix2d initialEstimatedStateCovarianceMatrix = Eigen::Matrix2d::Zero( );
    initialEstimatedStateCovarianceMatrix( 0, 0 ) = 100.0;
    initialEstimatedStateCovarianceMatrix( 1, 1 ) = 100.0;

    // Set system and measurement uncertainty
    Eigen::Matrix2d systemUncertainty = Eigen::Matrix2d::Zero( );
    Eigen::Vector1d measurementUncertainty = Eigen::Vector1d::Zero( );
    systemUncertainty( 0, 0 ) = 100.0;
    systemUncertainty( 1, 1 ) = 100.0;
    measurementUncertainty[ 0 ] = 100.0;

    // Set integrator settings
    std::shared_ptr< numerical_integrators::IntegratorSettings< > > integratorSettings =
            std::make_shared< numerical_integrators::IntegratorSettings< > > (
                numerical_integrators::euler, initialTime, timeStep );

    // Create control class
    std::shared_ptr< ControlWrapper< double, double, 2 > > control =
            std::make_shared< ControlWrapper< double, double, 2 > >(
                [ & ]( const double, const Eigen::Vector2d& ){ return Eigen::Vector2d::Zero( ); } );

    // Create unscented Kalman filter object
    UnscentedKalmanFilterDoublePointer unscentedFilter = std::make_shared< UnscentedKalmanFilterDouble >(
                std::bind( &stateFunction1, std::placeholders::_1, std::placeholders::_2,
                           std::bind( &ControlWrapper< double, double, 2 >::getCurrentControlVector, control ) ),
                std::bind( &measurementFunction1, std::placeholders::_1, std::placeholders::_2 ),
                systemUncertainty, measurementUncertainty, timeStep,
                initialTime, initialEstimatedStateVector, initialEstimatedStateCovarianceMatrix,
                integratorSettings );

    // Load noise from file
    Eigen::MatrixXd systemNoise = input_output::readMatrixFromFile( tudat::input_output::getTudatRootPath( ) +
                                                                    "/Mathematics/Filters/UnitTests/noiseData/ukfSystemNoise1.dat" );
    Eigen::MatrixXd measurementNoise = input_output::readMatrixFromFile( tudat::input_output::getTudatRootPath( ) +
                                                                         "/Mathematics/Filters/UnitTests/noiseData/ukfMeasurementNoise1.dat" );

    // Loop over each time step
    const bool showProgress = false;
    double currentTime = initialTime;
    Eigen::Vector2d currentActualStateVector = initialStateVector;
    Eigen::Vector2d currentControlVector = Eigen::Vector2d::Zero( );
    Eigen::Vector1d currentMeasurementVector;
    std::map< double, Eigen::Vector2d > actualStateVectorHistory;
    std::map< double, Eigen::Vector1d > measurementVectorHistory;
    actualStateVectorHistory[ initialTime ] = initialStateVector;
    for ( unsigned int i = 0; i < numberOfTimeSteps; i++ )
    {
        // Compute actual values and perturb them
        currentActualStateVector += ( stateFunction1( currentTime, currentActualStateVector, currentControlVector ) +
                                      systemNoise.col( i ) ) * timeStep;
        currentMeasurementVector = measurementFunction1( currentTime, currentActualStateVector ) + measurementNoise.col( i );

        // Update control class
        control->setCurrentControlVector( currentTime, unscentedFilter->getCurrentStateEstimate( ) );

        // Update filter
        unscentedFilter->updateFilter( currentMeasurementVector );
        currentTime = unscentedFilter->getCurrentTime( );

        // Store values
        actualStateVectorHistory[ currentTime ] = currentActualStateVector;
        measurementVectorHistory[ currentTime ] = currentMeasurementVector;

        // Print progress
        if ( showProgress )
        {
            std::cout << "Time: " << currentTime << std::endl
                      << "Measurement: " << currentMeasurementVector.transpose( ) << std::endl
                      << "Estimated State: " << unscentedFilter->getCurrentStateEstimate( ).transpose( ) << std::endl << std::endl;
        }
    }

    // Check that final state is as expected
    Eigen::Vector2d expectedFinalState = Eigen::Vector2d::Zero( );
    expectedFinalState << 4.9650721756454104, -12.853194315262826;
    for ( int i = 0; i < expectedFinalState.rows( ); i++ )
    {
        BOOST_CHECK_SMALL( unscentedFilter->getCurrentStateEstimate( )[ i ] - expectedFinalState[ i ], 1.0e-8 );
    }
}

//! Typedefs.
typedef Eigen::Matrix< long double, 1, 1 > Vector1ld;
typedef Eigen::Matrix< long double, 3, 1 > Vector3ld;
typedef Eigen::Matrix< long double, 3, 3 > Matrix3ld;

// Functions for unscented Kalman filter
Vector3ld stateFunction2( const long double time, const Vector3ld& state, const Vector3ld& control )
{
    TUDAT_UNUSED_PARAMETER( time );
    TUDAT_UNUSED_PARAMETER( control );
    Vector3ld stateFunction;
    stateFunction[ 0 ] = state[ 1 ];
    stateFunction[ 1 ] = state[ 2 ];
    stateFunction[ 2 ] = 0.05L * state[ 0 ] * ( state[ 1 ] + state[ 2 ] );
    return stateFunction;
}
Vector1ld measurementFunction2( const long double time, const Vector3ld& state )
{
    TUDAT_UNUSED_PARAMETER( time );
    Vector1ld measurement;
    measurement[ 0 ] = state[ 0 ];
    return measurement;
}

// Test implementation of unscented Kalman filter class. Tested by comparison with code by
// https://de.mathworks.com/matlabcentral/fileexchange/18217-learning-the-unscented-kalman-filter
BOOST_AUTO_TEST_CASE( testUnscentedKalmanFilterSecondCase )
{
    using namespace tudat::filters;

    // Set initial conditions
    const long double initialTime = 0.0L;
    const long double timeStep = 1.0L;
    const unsigned int numberOfTimeSteps = 100;

    Vector3ld initialStateVector = Vector3ld::Zero( );
    initialStateVector[ 2 ] = 1.0L;

    Vector3ld initialEstimatedStateVector;
    initialEstimatedStateVector[ 0 ] = 0.05376671395461L;
    initialEstimatedStateVector[ 1 ] = 0.183388501459509L;
    initialEstimatedStateVector[ 2 ] = 0.774115313899635L;

    Matrix3ld initialEstimatedStateCovarianceMatrix = Matrix3ld::Identity( );

    // Set system and measurement uncertainty
    Matrix3ld systemUncertainty = 0.01 * Matrix3ld::Identity( );
    Vector1ld measurementUncertainty = 0.01 * Vector1ld::Identity( );

    // Set null integrator settings
    std::shared_ptr< numerical_integrators::IntegratorSettings< long double > > integratorSettings = nullptr;

    // Create control class
    std::shared_ptr< ControlWrapper< long double, long double, 3 > > control =
            std::make_shared< ControlWrapper< long double, long double, 3 > >(
                [ & ]( const double, const Vector3ld& ){ return Vector3ld::Zero( ); } );

    // Create unscented Kalman filter object
    std::shared_ptr< KalmanFilterBase< long double, long double > > unscentedFilter =
            std::make_shared< UnscentedKalmanFilter< long double, long double > >(
                std::bind( &stateFunction2, std::placeholders::_1, std::placeholders::_2,
                           std::bind( &ControlWrapper< long double, long double, 3 >::getCurrentControlVector, control ) ),
                std::bind( &measurementFunction2, std::placeholders::_1, std::placeholders::_2 ),
                systemUncertainty, measurementUncertainty, timeStep,
                initialTime, initialEstimatedStateVector, initialEstimatedStateCovarianceMatrix,
                integratorSettings, custom_parameters, std::make_pair( 0.001, 0.0 ) );

    // Load noise from file
    Eigen::MatrixXld systemNoise = input_output::readMatrixFromFile< long double >(
                tudat::input_output::getTudatRootPath( ) + "/Mathematics/Filters/UnitTests/noiseData/ukfSystemNoise2.dat" );
    Eigen::MatrixXld measurementNoise = input_output::readMatrixFromFile< long double >(
                tudat::input_output::getTudatRootPath( ) + "/Mathematics/Filters/UnitTests/noiseData/ukfMeasurementNoise2.dat" );

    // Loop over each time step
    const bool showProgress = false;
    long double currentTime = initialTime;
    Vector3ld currentActualStateVector = initialStateVector;
    Vector3ld currentControlVector = Vector3ld::Zero( );
    Vector1ld currentMeasurementVector;
    std::map< long double, Vector3ld > actualStateVectorHistory;
    std::map< long double, Vector1ld > measurementVectorHistory;
    actualStateVectorHistory[ initialTime ] = initialStateVector;
    for ( unsigned int i = 0; i < numberOfTimeSteps; i++ )
    {
        // Compute actual values and perturb them
        currentActualStateVector = stateFunction2( currentTime, currentActualStateVector, currentControlVector ) +
                systemNoise.col( i );
        currentMeasurementVector = measurementFunction2( currentTime, currentActualStateVector ) + measurementNoise.col( i );

        // Update control class
        control->setCurrentControlVector( currentTime, unscentedFilter->getCurrentStateEstimate( ) );

        // Update filter
        unscentedFilter->updateFilter( currentMeasurementVector );
        currentTime = unscentedFilter->getCurrentTime( );

        // Store values
        actualStateVectorHistory[ currentTime ] = currentActualStateVector;
        measurementVectorHistory[ currentTime ] = currentMeasurementVector;

        // Print progress
        if ( showProgress )
        {
            std::cout << "Time: " << currentTime << std::endl
                      << "Measurement: " << currentMeasurementVector.transpose( ) << std::endl
                      << "Estimated State: " << unscentedFilter->getCurrentStateEstimate( ).transpose( ) << std::endl << std::endl;
        }
    }

    // Check that final state is as expected
    for ( int i = 0; i < initialStateVector.rows( ); i++ )
    {
        BOOST_CHECK_SMALL( unscentedFilter->getCurrentStateEstimate( )[ i ], 1e-16L );
    }
}

// Constant parameters for example
const double gravitationalParameter = 32.2;

// Functions for unscented Kalman filter.
Eigen::Vector3d stateFunction3( const double time, const Eigen::Vector3d& state, const Eigen::Vector3d& control )
{
    TUDAT_UNUSED_PARAMETER( time );
    TUDAT_UNUSED_PARAMETER( control );
    Eigen::Vector3d stateDerivative = Eigen::Vector3d::Zero( );
    stateDerivative[ 0 ] = state[ 1 ];
    stateDerivative[ 1 ] = 0.0034 * gravitationalParameter * std::exp( - state[ 0 ] / 22000.0 ) *
            std::pow( state[ 1 ], 2 ) / ( 2.0 * state[ 2 ] ) - gravitationalParameter;
    return stateDerivative;
}
Eigen::Vector1d measurementFunction3( const double time, const Eigen::Vector3d& state )
{
    TUDAT_UNUSED_PARAMETER( time );
    Eigen::Vector1d measurement;
    measurement[ 0 ] = state[ 0 ];
    return measurement;
}

// Test implementation of unscented Kalman filter class.
BOOST_AUTO_TEST_CASE( testUnscentedKalmanFilterThirdCase )
{
    using namespace tudat::filters;

    // Set initial conditions
    const double initialTime = 0.0;
    const double timeStep = 0.1;
    const unsigned int numberOfTimeSteps = 300;

    Eigen::Vector3d initialStateVector;
    initialStateVector[ 0 ] = 200000.0;
    initialStateVector[ 1 ] = -6000.0;
    initialStateVector[ 2 ] = 500.0;

    Eigen::Vector3d initialEstimatedStateVector;
    initialEstimatedStateVector[ 0 ] = 200025.0;
    initialEstimatedStateVector[ 1 ] = -6150.0;
    initialEstimatedStateVector[ 2 ] = 800.0;

    Eigen::Matrix3d initialEstimatedStateCovarianceMatrix = Eigen::Matrix3d::Zero( );
    initialEstimatedStateCovarianceMatrix( 0, 0 ) = std::pow( 1000.0, 2 );
    initialEstimatedStateCovarianceMatrix( 1, 1 ) = 20000.0;
    initialEstimatedStateCovarianceMatrix( 2, 2 ) = std::pow( 300.0, 2 );

    // Set system and measurement uncertainty
    Eigen::Matrix3d systemUncertainty = Eigen::Matrix3d::Zero( );
    Eigen::Vector1d measurementUncertainty = Eigen::Vector1d::Zero( );
    systemUncertainty( 0, 0 ) = std::pow( 100.0, 2 );
    systemUncertainty( 1, 1 ) = std::pow( 10.0, 2 );
    systemUncertainty( 2, 2 ) = std::pow( 1.0, 2 );
    measurementUncertainty[ 0 ] = std::pow( 25.0, 2 );

    // Set integrator settings
    std::shared_ptr< numerical_integrators::IntegratorSettings< > > integratorSettings =
            std::make_shared< numerical_integrators::IntegratorSettings< > > (
                numerical_integrators::euler, initialTime, timeStep );

    // Create control class
    std::shared_ptr< ControlWrapper< double, double, 3 > > control =
            std::make_shared< ControlWrapper< double, double, 3 > >(
                [ & ]( const double, const Eigen::Vector3d& ){ return Eigen::Vector3d::Zero( ); } );

    // Create unscented Kalman filter object
    UnscentedKalmanFilterDoublePointer unscentedFilter = std::make_shared< UnscentedKalmanFilterDouble >(
                std::bind( &stateFunction3, std::placeholders::_1, std::placeholders::_2,
                           std::bind( &ControlWrapper< double, double, 3 >::getCurrentControlVector, control ) ),
                std::bind( &measurementFunction3, std::placeholders::_1, std::placeholders::_2 ),
                systemUncertainty, measurementUncertainty, timeStep,
                initialTime, initialEstimatedStateVector, initialEstimatedStateCovarianceMatrix,
                integratorSettings );

    // Load noise from file
    Eigen::MatrixXd systemNoise = input_output::readMatrixFromFile( tudat::input_output::getTudatRootPath( ) +
                                                                    "/Mathematics/Filters/UnitTests/noiseData/ukfSystemNoise3.dat" );
    Eigen::MatrixXd measurementNoise = input_output::readMatrixFromFile(
                tudat::input_output::getTudatRootPath( ) + "/Mathematics/Filters/UnitTests/noiseData/ukfMeasurementNoise3.dat" );

    // Loop over each time step
    const bool showProgress = false;
    double currentTime = initialTime;
    Eigen::Vector3d currentActualStateVector = initialStateVector;
    Eigen::Vector3d currentControlVector = Eigen::Vector3d::Zero( );
    Eigen::Vector1d currentMeasurementVector;
    std::map< double, Eigen::Vector3d > actualStateVectorHistory;
    std::map< double, Eigen::Vector1d > measurementVectorHistory;
    actualStateVectorHistory[ initialTime ] = initialStateVector;
    for ( unsigned int i = 0; i < numberOfTimeSteps; i++ )
    {
        // Compute actual values and perturb them
        currentActualStateVector += ( stateFunction3( currentTime, currentActualStateVector, currentControlVector ) +
                                      systemNoise.col( i ) ) * timeStep;
        currentMeasurementVector = measurementFunction3( currentTime, currentActualStateVector ) + measurementNoise.col( i );

        // Update control class
        control->setCurrentControlVector( currentTime, unscentedFilter->getCurrentStateEstimate( ) );

        // Update filter
        unscentedFilter->updateFilter( currentMeasurementVector );
        currentTime = unscentedFilter->getCurrentTime( );

        // Store values
        actualStateVectorHistory[ currentTime ] = currentActualStateVector;
        measurementVectorHistory[ currentTime ] = currentMeasurementVector;

        // Print progress
        if ( showProgress )
        {
            std::cout << "Time: " << currentTime << std::endl
                      << "Measurement: " << currentMeasurementVector.transpose( ) << std::endl
                      << "Estimated State: " << unscentedFilter->getCurrentStateEstimate( ).transpose( ) << std::endl << std::endl;
        }
    }

    // Check that final state is as expected
    Eigen::Vector3d expectedFinalState = Eigen::Vector3d::Zero( );
    expectedFinalState << 25200.383066001908, -3334.8971957588378, 502.3444368942321;
    for ( int i = 0; i < expectedFinalState.rows( ); i++ )
    {
        BOOST_CHECK_SMALL( unscentedFilter->getCurrentStateEstimate( )[ i ] - expectedFinalState[ i ], 1.0e-10 );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


/*
 *            Copyright 2009-2020 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      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.
 *
 */

// Third party includes
#include <boost/format.hpp>

// VOTCA includes
#include <votca/tools/elements.h>

// Local VOTCA includes
#include "votca/xtp/gyration.h"
#include "votca/xtp/vxc_grid.h"

using namespace votca::tools;

namespace votca {
namespace xtp {

void Density2Gyration::Initialize(tools::Property& options) {
  std::string key = Identify();

  std::string statestring = options.get(key + ".state").as<std::string>();
  _state.FromString(statestring);
  _dostateonly = options.ifExistsReturnElseReturnDefault<bool>(
      key + ".difference_to_groundstate", false);
  _gridsize = options.ifExistsReturnElseReturnDefault<std::string>(
      key + ".gridsize", "medium");
}

void Density2Gyration::AnalyzeDensity(const Orbitals& orbitals) {
  XTP_LOG(Log::error, _log) << "===== Running on " << OPENMP::getMaxThreads()
                            << " threads ===== " << std::flush;

  const QMMolecule& Atomlist = orbitals.QMAtoms();
  BasisSet bs;
  bs.Load(orbitals.getDFTbasisName());
  AOBasis basis;
  basis.Fill(bs, Atomlist);
  AnalyzeGeometry(Atomlist);

  // setup numerical integration grid
  Vxc_Grid grid;
  grid.GridSetup(_gridsize, Atomlist, basis);
  DensityIntegration<Vxc_Grid> numway(grid);

  if (!_dostateonly) {
    Eigen::MatrixXd DMAT_tot = orbitals.DensityMatrixFull(_state);
    Gyrationtensor gyro = numway.IntegrateGyrationTensor(DMAT_tot);
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es;
    es.computeDirect(gyro.gyration);
    XTP_LOG(Log::error, _log)
        << TimeStamp() << " Converting to Eigenframe " << std::flush;
    XTP_LOG(Log::error, _log) << TimeStamp() << " Reporting " << std::flush;
    ReportAnalysis(_state.ToLongString(), gyro, es);

  } else {
    // hole density first
    std::array<Eigen::MatrixXd, 2> DMAT =
        orbitals.DensityMatrixExcitedState(_state);
    Gyrationtensor gyro_hole = numway.IntegrateGyrationTensor(DMAT[0]);
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es_h;
    es_h.computeDirect(gyro_hole.gyration);
    XTP_LOG(Log::error, _log)
        << TimeStamp() << " Converting to Eigenframe " << std::flush;
    XTP_LOG(Log::error, _log) << TimeStamp() << " Reporting " << std::flush;
    ReportAnalysis("hole", gyro_hole, es_h);

    // electron density
    Gyrationtensor gyro_electron = numway.IntegrateGyrationTensor(DMAT[1]);
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es_e;
    es_e.computeDirect(gyro_electron.gyration);
    XTP_LOG(Log::error, _log)
        << TimeStamp() << " Converting to Eigenframe " << std::flush;
    XTP_LOG(Log::error, _log) << TimeStamp() << " Reporting " << std::flush;
    ReportAnalysis("electron", gyro_electron, es_e);
  }
  return;
}

void Density2Gyration::AnalyzeGeometry(const QMMolecule& atoms) {

  tools::Elements elements;
  double mass = 0.0;
  Eigen::Vector3d centroid = Eigen::Vector3d::Zero();
  Eigen::Matrix3d gyration = Eigen::Matrix3d::Zero();
  for (const QMAtom& atom : atoms) {
    double m = elements.getMass(atom.getElement());
    const Eigen::Vector3d& pos = atom.getPos();
    mass += m;
    centroid += m * pos;
    gyration += m * pos * pos.transpose();
  }
  centroid /= mass;
  gyration /= mass;
  gyration -= centroid * centroid.transpose();
  Gyrationtensor gyro;
  gyro.mass = mass;
  gyro.centroid = centroid;
  gyro.gyration = gyration;
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es;
  es.computeDirect(gyro.gyration);
  ReportAnalysis("geometry", gyro, es);
}

void Density2Gyration::ReportAnalysis(
    std::string label, const Gyrationtensor& gyro,
    const Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>& es) {

  XTP_LOG(Log::error, _log)
      << "---------------- " << label << " ----------------" << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Norm               = %1$9.4f ") % (gyro.mass))
      << std::flush;

  XTP_LOG(Log::error, _log)
      << (boost::format("  Centroid x         = %1$9.4f Ang") %
          (gyro.centroid.x() * tools::conv::bohr2ang))
      << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Centroid y         = %1$9.4f Ang") %
          (gyro.centroid.y() * tools::conv::bohr2ang))
      << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Centroid y         = %1$9.4f Ang") %
          (gyro.centroid.z() * tools::conv::bohr2ang))
      << std::flush;

  double RA2 = tools::conv::bohr2ang * tools::conv::bohr2ang;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Gyration Tensor xx = %1$9.4f Ang^2") %
          (gyro.gyration(0, 0) * RA2))
      << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Gyration Tensor xy = %1$9.4f Ang^2") %
          (gyro.gyration(0, 1) * RA2))
      << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Gyration Tensor xz = %1$9.4f Ang^2") %
          (gyro.gyration(0, 2) * RA2))
      << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Gyration Tensor yy = %1$9.4f Ang^2") %
          (gyro.gyration(1, 1) * RA2))
      << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Gyration Tensor yz = %1$9.4f Ang^2") %
          (gyro.gyration(1, 2) * RA2))
      << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Gyration Tensor zz = %1$9.4f Ang^2") %
          (gyro.gyration(2, 2) * RA2))
      << std::flush;

  XTP_LOG(Log::error, _log)
      << (boost::format("  Gyration Tensor D1 = %1$9.4f Ang^2") %
          (es.eigenvalues()[0] * RA2))
      << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Gyration Tensor D2 = %1$9.4f Ang^2") %
          (es.eigenvalues()[1] * RA2))
      << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Gyration Tensor D3 = %1$9.4f Ang^2") %
          (es.eigenvalues()[2] * RA2))
      << std::flush;

  XTP_LOG(Log::error, _log)
      << (boost::format("  Radius of Gyration = %1$9.4f Ang") %
          (std::sqrt(es.eigenvalues().sum()) * tools::conv::bohr2ang))
      << std::flush;

  XTP_LOG(Log::error, _log)
      << (boost::format("  Tensor EF Axis 1 1 = %1$9.4f ") %
          es.eigenvectors().col(0).x())
      << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Tensor EF Axis 1 2 = %1$9.4f ") %
          es.eigenvectors().col(0).y())
      << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Tensor EF Axis 1 3 = %1$9.4f ") %
          es.eigenvectors().col(0).z())
      << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Tensor EF Axis 2 1 = %1$9.4f ") %
          es.eigenvectors().col(1).x())
      << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Tensor EF Axis 2 2 = %1$9.4f ") %
          es.eigenvectors().col(1).y())
      << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Tensor EF Axis 2 3 = %1$9.4f ") %
          es.eigenvectors().col(1).z())
      << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Tensor EF Axis 3 1 = %1$9.4f ") %
          es.eigenvectors().col(2).x())
      << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Tensor EF Axis 3 2 = %1$9.4f ") %
          es.eigenvectors().col(2).y())
      << std::flush;
  XTP_LOG(Log::error, _log)
      << (boost::format("  Tensor EF Axis 3 3 = %1$9.4f ") %
          es.eigenvectors().col(2).z())
      << std::flush;
}

}  // namespace xtp
}  // namespace votca


//
// Created by qfeng on 2019-01-10.
//

#include "gtest/gtest.h"
#include <iostream>
#include "model/linear_model.h"
#include <fstream>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

using namespace std;
using namespace algocomp;

namespace algocomp_test {

    class TestLinearModel : public ::testing::Test {
    protected:
        unique_ptr<LinearModel> model;

        virtual void SetUp() {
            model.reset(new LinearModel(pow(2, 18)));
        }

        virtual void TearDown() {

        }
    };

    TEST_F(TestLinearModel, linear_model) {
        model->update_weight(0, 0.032);
        model->update_weight(1, 0.125);
        model->update_weight(3, -0.0265);
        model->update_weight(6, 0);
        Eigen::SparseVector<float> sample(pow(2, 18));
        sample.coeffRef(0) = 0.683;
        sample.coeffRef(1) = -0.0234;
        sample.coeffRef(2) = 0.435;
        sample.coeffRef(3) = 0.112;
        sample.coeffRef(4) = -0.087;
        sample.coeffRef(5) = 0.123;
        sample.coeffRef(6) = -0.1345;
        sample.coeffRef(8) = 0.1;
        float score = model->score(sample);
        EXPECT_NEAR(score, 0.015963003, 1E-7);
    }

    TEST_F(TestLinearModel, skl_model) {
        auto model = LinearModel::load_skl_model("test/test-data/1-models/1-skl-model.txt");

        ifstream sample_file;
        sample_file.open("test/test-data/1-models/1-skl-sample.txt");
        if (sample_file.is_open()) {
            Eigen::SparseVector<float> kv(int(pow(2,30)));
            string line;
            while (!sample_file.eof()) {
                getline(sample_file, line);
                if (line.length() > 0) {
                    vector<string> splits;
                    boost::algorithm::split(splits, line, boost::algorithm::is_any_of(" "));
                    uint64_t index = boost::lexical_cast<uint64_t>(splits[0]);
                    float value = boost::lexical_cast<float>(splits[1]);
                    kv.insert(index) = value;
                }
            }

            sample_file.close();
            auto score = model->score(kv);
            auto pred = 1.0 / (1.0 + exp(-score));
            EXPECT_NEAR(pred, 0.450796238486696, 1E-3);
        }
    }

}


#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <math.h>
#include <omp.h>
#include <iostream>

#include "mvnormal.h"
#include "linop.h"
#include "bpmfutils.h"
#include "macauoneprior.h"

using namespace std;
using namespace Eigen;

template<class FType>
void MacauOnePrior<FType>::init(const int nlatent, std::unique_ptr<FType> &Fmat) {
  num_latent = nlatent;

  // parameters of Normal-Gamma distributions
  mu     = VectorXd::Constant(num_latent, 0.0);
  lambda = VectorXd::Constant(num_latent, 10.0);
  // their hyperparameter (lambda_0)
  l0 = 2.0;
  lambda_a0 = 1.0;
  lambda_b0 = 1.0;

  // side information
  F       = std::move(Fmat);
  F_colsq = col_square_sum(*F);

  Uhat = MatrixXd::Constant(num_latent, F->rows(), 0.0);
  beta = MatrixXd::Constant(num_latent, F->cols(), 0.0);

  // initial value (should be determined automatically)
  // Hyper-prior for lambda_beta (mean 1.0):
  lambda_beta     = VectorXd::Constant(num_latent, 5.0);
  lambda_beta_a0 = 0.1;
  lambda_beta_b0 = 0.1;
}

template<class FType>
void MacauOnePrior<FType>::sample_latents(
    Eigen::MatrixXd &U,
    const Eigen::SparseMatrix<double> &Ymat,
    double mean_value,
    const Eigen::MatrixXd &V,
    double alpha,
    const int num_latent)
{
  const int N = U.cols();
  const int D = U.rows();

#pragma omp parallel for schedule(dynamic, 4)
  for (int i = 0; i < N; i++) {

    const int nnz = Ymat.outerIndexPtr()[i + 1] - Ymat.outerIndexPtr()[i];
    VectorXd Yhat(nnz);

    // precalculating Yhat and Qi
    int idx = 0;
    VectorXd Qi = lambda;
    for (SparseMatrix<double>::InnerIterator it(Ymat, i); it; ++it, idx++) {
      Qi.noalias() += alpha * V.col(it.row()).cwiseAbs2();
      Yhat(idx)     = mean_value + U.col(i).dot( V.col(it.row()) );
    }
    VectorXd rnorms(num_latent);
    bmrandn_single(rnorms);

    for (int d = 0; d < D; d++) {
      // computing Lid
      const double uid = U(d, i);
      double Lid = lambda(d) * (mu(d) + Uhat(d, i));

      idx = 0;
      for ( SparseMatrix<double>::InnerIterator it(Ymat, i); it; ++it, idx++) {
        const double vjd = V(d, it.row());
        // L_id += alpha * (Y_ij - k_ijd) * v_jd
        Lid += alpha * (it.value() - (Yhat(idx) - uid*vjd)) * vjd;
      }
      // Now use Lid and Qid to update uid
      double uid_old = U(d, i);
      double uid_var = 1.0 / Qi(d);

      // sampling new u_id ~ Norm(Lid / Qid, 1/Qid)
      U(d, i) = Lid * uid_var + sqrt(uid_var) * rnorms(d);

      // updating Yhat
      double uid_delta = U(d, i) - uid_old;
      idx = 0;
      for (SparseMatrix<double>::InnerIterator it(Ymat, i); it; ++it, idx++) {
        Yhat(idx) += uid_delta * V(d, it.row());
      }
    }
  }
}

template<class FType>
void MacauOnePrior<FType>::sample_latents(
        ProbitNoise& noiseModel,
        TensorData & data,
        std::vector< std::unique_ptr<Eigen::MatrixXd> > & samples,
        int mode,
        const int num_latent)
{
  throw std::runtime_error("Unimplemented: sample_latents");
}

template<class FType>
void MacauOnePrior<FType>::sample_latents(
        double noisePrecision,
        TensorData & data,
        std::vector< std::unique_ptr<Eigen::MatrixXd> > & samples,
        int mode,
        const int num_latent)
{
  auto& sparseMode = (*data.Y)[mode];
  auto& U = samples[mode];
  const int N = U->cols();
  const int D = num_latent;
  VectorView<Eigen::MatrixXd> view(samples, mode);
  const int nmodes1 = view.size();
  const double mean_value = data.mean_value;

  if (U->rows() != num_latent) {
    throw std::runtime_error("U->rows() must be equal to num_latent.");
  }

  Eigen::VectorXi & row_ptr = sparseMode->row_ptr;
  Eigen::MatrixXi & indices = sparseMode->indices;
  Eigen::VectorXd & values  = sparseMode->values;

#pragma omp parallel for schedule(dynamic, 8)
  for (int i = 0; i < N; i++) {
    // looping over all non-zeros for row i of the mode
    // precalculating Yhat and Qi
    const int nnz = row_ptr(i + 1) - row_ptr(i);
    VectorXd Yhat(nnz);
    VectorXd tmpd(nnz);
    VectorXd Qi = lambda;

    for (int idx = 0; idx < nnz; idx++) {
      int j = idx + row_ptr(i);
      VectorXd prod = VectorXd::Ones(D);
      for (int m = 0; m < nmodes1; m++) {
        auto v = view.get(m)->col(indices(j, m));
        prod.noalias() = prod.cwiseProduct(v);
      }
      Qi.noalias() += noisePrecision * prod.cwiseAbs2();
      Yhat(idx) = mean_value + U->col(i).dot(prod);
    }

    // generating random numbers
    VectorXd rnorms(num_latent);
    bmrandn_single(rnorms);

    for (int d = 0; d < D; d++) {
      // computing Lid
      const double uid = (*U)(d, i);
      double Lid = lambda(d) * (mu(d) + Uhat(d, i));

      for (int idx = 0; idx < nnz; idx++) {
        int j = idx + row_ptr(i);

        // computing t = vjd * wkd * ..
        double t = 1.0;
        for (int m = 0; m < nmodes1; m++) {
          t *= (*view.get(m))(d, indices(j, m));
        }
        tmpd(idx) = t;
        // L_id += alpha * (Y_ijk - k_ijkd) * v_jd * wkd
        Lid += noisePrecision * (values(j) - (Yhat(idx) - uid * t)) * t;
      }
      // Now use Lid and Qid to update uid
      double uid_old = uid;
      double uid_var = 1.0 / Qi(d);

      // sampling new u_id ~ Norm(Lid / Qid, 1/Qid)
      (*U)(d, i) = Lid * uid_var + sqrt(uid_var) * rnorms(d);

      // updating Yhat
      double uid_delta = (*U)(d, i) - uid_old;
      for (int idx = 0; idx < nnz; idx++) {
        Yhat(idx) += uid_delta * tmpd(idx);
      }
    }
  }
}

template<class FType>
void MacauOnePrior<FType>::update_prior(const Eigen::MatrixXd &U) {
  sample_mu_lambda(U);
  sample_beta(U);
  compute_uhat(Uhat, *F, beta);
  sample_lambda_beta();
}

template<class FType>
void MacauOnePrior<FType>::sample_mu_lambda(const Eigen::MatrixXd &U) {
  MatrixXd Lambda(num_latent, num_latent);
  MatrixXd WI(num_latent, num_latent);
  WI.setIdentity();
  int N = U.cols();

  MatrixXd Udelta(num_latent, N);
#pragma omp parallel for schedule(static)
  for (int i = 0; i < N; i++) {
    for (int d = 0; d < num_latent; d++) {
      Udelta(d, i) = U(d, i) - Uhat(d, i);
    }
  }
  tie(mu, Lambda) = CondNormalWishart(Udelta, VectorXd::Constant(num_latent, 0.0), 2.0, WI, num_latent);
  lambda = Lambda.diagonal();
}

template<class FType>
void MacauOnePrior<FType>::sample_beta(const Eigen::MatrixXd &U) {
  // updating beta and beta_var
  const int nfeat = beta.cols();
  const int N = U.cols();
  const int blocksize = 4;

  MatrixXd Z;

#pragma omp parallel for private(Z) schedule(static, 1)
  for (int dstart = 0; dstart < num_latent; dstart += blocksize) {
    const int dcount = std::min(blocksize, num_latent - dstart);
    Z.resize(dcount, U.cols());

    for (int i = 0; i < N; i++) {
      for (int d = 0; d < dcount; d++) {
        int dx = d + dstart;
        Z(d, i) = U(dx, i) - mu(dx) - Uhat(dx, i);
      }
    }

    for (int f = 0; f < nfeat; f++) {
      VectorXd zx(dcount), delta_beta(dcount), randvals(dcount);
      // zx = Z[dstart : dstart + dcount, :] * F[:, f]
      At_mul_Bt(zx, *F, f, Z);
      // TODO: check if sampling randvals for whole [nfeat x dcount] matrix works faster
      bmrandn_single( randvals );

      for (int d = 0; d < dcount; d++) {
        int dx = d + dstart;
        double A_df     = lambda_beta(dx) + lambda(dx) * F_colsq(f);
        double B_df     = lambda(dx) * (zx(d) + beta(dx,f) * F_colsq(f));
        double A_inv    = 1.0 / A_df;
        double beta_new = B_df * A_inv + sqrt(A_inv) * randvals(d);
        delta_beta(d)   = beta(dx,f) - beta_new;

        beta(dx, f)     = beta_new;
      }
      // Z[dstart : dstart + dcount, :] += F[:, f] * delta_beta'
      add_Acol_mul_bt(Z, *F, f, delta_beta);
    }
  }
}

template<class FType>
void MacauOnePrior<FType>::sample_latents(ProbitNoise & noise, Eigen::MatrixXd &U, const Eigen::SparseMatrix<double> &mat,
                                          double mean_value, const Eigen::MatrixXd &samples, const int num_latent) {
 //TODO
 throw std::runtime_error("Not implemented!");
}

template<class FType>
void MacauOnePrior<FType>::sample_lambda_beta() {
  double lambda_beta_a = lambda_beta_a0 + beta.cols() / 2.0;
  VectorXd lambda_beta_b = VectorXd::Constant(beta.rows(), lambda_beta_b0);
  const int D = beta.rows();
  const int F = beta.cols();
#pragma omp parallel
  {
    VectorXd tmp(D);
    tmp.setZero();
#pragma omp for schedule(static)
    for (int f = 0; f < F; f++) {
      for (int d = 0; d < D; d++) {
        tmp(d) += square(beta(d, f));
      }
    }
#pragma omp critical
    {
      lambda_beta_b += tmp / 2;
    }
  }
  for (int d = 0; d < D; d++) {
    lambda_beta(d) = rgamma(lambda_beta_a, 1.0 / lambda_beta_b(d));
  }
}

template<class FType>
void MacauOnePrior<FType>::saveModel(std::string prefix) {
  writeToCSVfile(prefix + "-latentmean.csv", mu);
  writeToCSVfile(prefix + "-link.csv", beta);
}

template class MacauOnePrior<SparseFeat>;
template class MacauOnePrior<SparseDoubleFeat>;


// Author: Tucker Haydon

#include <cstdlib>

#include <Eigen/Dense>

#include "polynomial_solver.h"
#include "polynomial_sampler.h"

using namespace p4;

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

  const size_t num_nodes = 10000;
  std::vector<double> times = {0};

  // Equality bounds paramater order is:
  // 1) Dimension index
  // 2) Node index
  // 3) Derivative index
  // 4) Value
  std::vector<NodeEqualityBound> equality_bounds = {
    NodeEqualityBound(0,0,0,0),
    NodeEqualityBound(0,0,1,0),
    NodeEqualityBound(0,0,2,0),
  };

  for(size_t node_idx = 1; node_idx < num_nodes; ++node_idx) {
    times.push_back(node_idx);

    equality_bounds.push_back(NodeEqualityBound(0,node_idx,0,node_idx));
  }

  PolynomialSolver::Options solver_options;
  solver_options.num_dimensions = 1;
  solver_options.polynomial_order = 7;
  solver_options.continuity_order = 4;
  solver_options.derivative_order = 4;

  solver_options.osqp_settings.polish = true;      // Polish the solution, getting the best answer possible
  solver_options.osqp_settings.verbose = true;     // Suppress the printout

  PolynomialSolver solver(solver_options);
  const PolynomialSolver::Solution solution
    = solver.Run(times, equality_bounds,{},{});

  return EXIT_SUCCESS;
}


#include <antgame/Ant.h>
#include <antgame/Food.h>
#include <antgame/World.h>

#include <easyloggingpp/easylogging++.h>

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

#include <math.h>
#include <stdlib.h>

namespace {
	float RandFloat(float lo, float hi) {
		return lo + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(hi - lo)));
	}

	inline float DegreesToRadians(float degrees) {
		return degrees * boost::math::constants::pi<double>() / 180;
	}

	static constexpr auto kStartingEnergy = 100;
}

Ant::Ant(const Point& initialPosition, const std::string& name)
: Agent(initialPosition, name)
, m_direction(RandFloat(-180, 180))
, m_energy(kStartingEnergy)
{ }

void Ant::Turn(float Angle) { //updates direction of Ant
	m_direction += Angle;

	// if (m_direction < -180){
	// 	m_direction += 360;
	// }
	// else if (m_direction > 180) {
	// 	m_direction -= 360;
	// }
}

void Ant::Move(float turningAngle, float distanceForward) {
	Ant::Turn(turningAngle);
	this->SetPosX(this->GetPosX() + (distanceForward * cos(DegreesToRadians(m_direction)))); //move forward in new direction
	this->SetPosY(this->GetPosY() + (distanceForward * sin(DegreesToRadians(m_direction))));
}

void Ant::Eat(Food& food) {
	AddEnergy(food.GetNutritionalValue());
	food.BecomeEaten();
}

void Ant::Update(const WorldTree& world) {
	static constexpr auto kDriftDelta = 0.05;
	static constexpr auto kEatingDistance = 0.2;
	static constexpr auto kDivideEnergy = 200;
	static constexpr auto kEnergyDrain = 0.1;
	static constexpr auto kMaxTurnAngle = 10;
	static constexpr auto kSpeed = 0.01; //ant speed
	static constexpr auto kVisionDistance = 200 * kSpeed; //ant speed

	AddEnergy(-kEnergyDrain);

	if (m_energy < 0) {
		LOG(INFO) << "died from lack of food";
		RemoveSelfFromWorld();
		return;
	}

	if (m_energy > kDivideEnergy) {
        auto child1 = std::make_shared<Ant>(m_position, GetName() + "_1");
        m_world->QueueAddAgent(std::move(child1));
        auto child2 = std::make_shared<Ant>(m_position, GetName() + "_2");
        m_world->QueueAddAgent(std::move(child2));

		RemoveSelfFromWorld();
		return;
	}

    auto nearestObject =  world.GetNearestNeighbours(m_position, 2).front();

	// This is just demo activity.
	// TODO: ant brain
	if (typeid(Food) == typeid(*nearestObject.get())) {

		auto direction = this->GetPosition(); //gets ants current position
		boost::geometry::subtract_point(direction, nearestObject->GetPosition()); //p1 - p2 = p21 a vector from the food to the ant
		auto distance = boost::geometry::strategy::distance::pythagoras().apply(m_position, nearestObject->GetPosition()); // distance to food

		if(distance < kVisionDistance) { //if you see food but it's too far away you don't see it sike
			boost::geometry::divide_value(direction, -distance); //normalises and  flips the vector to point from ant to food

			float angleToTurn;
			if (direction.get<0>() >= 0) {
				angleToTurn = DegreesToRadians(atan(direction.get<1>()/direction.get<0>())) - m_direction;
			}
			else if (direction.get<1>() >= 0){
				angleToTurn = 180 - DegreesToRadians(atan(direction.get<1>()/direction.get<0>())) - m_direction;
			}
			else {
				angleToTurn = -180 + DegreesToRadians(atan(direction.get<1>()/direction.get<0>())) - m_direction;
			}

			if (angleToTurn > kMaxTurnAngle) {
				angleToTurn = kMaxTurnAngle;
			}
			else if (angleToTurn < -kMaxTurnAngle) {
				angleToTurn = -kMaxTurnAngle;
			}

			if (distance < kEatingDistance) {
				Eat(*dynamic_cast<Food*>(nearestObject.get()));
			}
			else {
				Ant::Move(angleToTurn, kSpeed);
			}
		}
		else {
			Ant::Move(RandFloat(-kMaxTurnAngle, kMaxTurnAngle), kSpeed);
		}
	}
	else {
		Ant::Move(RandFloat(-kMaxTurnAngle, kMaxTurnAngle), RandFloat(kSpeed, kSpeed*2));
	}
}


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


#include <armadillo>

#include "SparseMatrix.h"

namespace lexicographic {

struct SparseMatrix::Impl {
    arma::sp_mat sparse_matrix;

  public:
    Impl() = default;
};

SparseMatrix::SparseMatrix(lexicographic::IndexConverter converter) :
    pImpl {std::make_unique<Impl>()},
    converter_(std::move(converter)) {
    resize(converter_.get_total_space_size());
}
SparseMatrix::~SparseMatrix() = default;
SparseMatrix::SparseMatrix(SparseMatrix&&) noexcept = default;
SparseMatrix& SparseMatrix::operator=(SparseMatrix&&) noexcept = default;

std::ostream& operator<<(std::ostream& os, const SparseMatrix& data) {
    os << data.pImpl->sparse_matrix << std::endl;
    return os;
}

uint32_t SparseMatrix::size() const {
    return pImpl->sparse_matrix.n_cols;
}

void SparseMatrix::add_to_position(double value, uint32_t i, uint32_t j) {
    pImpl->sparse_matrix(i, j) += value;
}

double SparseMatrix::operator()(uint32_t i, uint32_t j) const {
    return pImpl->sparse_matrix(i, j);
}

void SparseMatrix::resize(uint32_t new_size) {
    pImpl->sparse_matrix.resize(new_size, new_size);
}

void SparseMatrix::add_scalar_product(
    uint32_t index_of_vector,
    uint32_t center_a,
    uint32_t center_b,
    double factor) {
    uint32_t projection_of_center_a =
        converter_.convert_lex_index_to_one_sz_projection(index_of_vector, center_a);
    uint32_t projection_of_center_b =
        converter_.convert_lex_index_to_one_sz_projection(index_of_vector, center_b);

    // (Sa, Sb) = Sax*Sbx + Say*Sby + Saz*Sbz = 0.5 * (Sa+Sb- + Sa-Sb+) + Saz*Sbz

    // Saz Sbz
    double diagonal_value = (projection_of_center_a - converter_.get_spins()[center_a])
        * (projection_of_center_b - converter_.get_spins()[center_b]) * factor;
    add_to_position(diagonal_value, index_of_vector, index_of_vector);

    // Sa+ Sb-
    add_scalar_product_nondiagonal_part(
        index_of_vector,
        center_a,
        center_b,
        projection_of_center_a,
        projection_of_center_b,
        factor);

    // Sa- Sb+
    add_scalar_product_nondiagonal_part(
        index_of_vector,
        center_b,
        center_a,
        projection_of_center_b,
        projection_of_center_a,
        factor);
}

void SparseMatrix::add_scalar_product_nondiagonal_part(
    uint32_t index_of_vector,
    uint32_t plus_center,
    uint32_t minus_center,
    uint32_t projection_of_plus_center,
    uint32_t projection_of_minus_center,
    double factor) {
    if (projection_of_plus_center == converter_.get_mults()[plus_center] - 1
        || projection_of_minus_center == 0) {
        return;
    }
    uint32_t index_of_new_vector = converter_.ladder_projection(
        converter_.ladder_projection(index_of_vector, plus_center, +1),
        minus_center,
        -1);

    // projection m = number n - spin S
    // so S(S+1)-m(m+1) = (2S-n)(n+1)
    // so S(S+1)-m(m-1) = n(2S+1-n)
    double factor_a = (2 * converter_.get_spins()[plus_center] - projection_of_plus_center)
        * (projection_of_plus_center + 1);
    double factor_b = projection_of_minus_center
        * (2 * converter_.get_spins()[minus_center] + 1 - projection_of_minus_center);

    // TODO: fix plus-minus
    add_to_position(0.5 * sqrt(factor_a * factor_b) * factor, index_of_vector, index_of_new_vector);
}
}  // namespace lexicographic


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

#include <stdio.h>
#include <math.h>

#include <cmath>
#include <memory>
#include <vector>
#include <limits>
#include <random>
#include <cmath>

#include "statespace.hpp"

using namespace boost::numeric::ublas;

int udu(matrix<double> &M_mat, matrix<double> &U_mat, vector<double> &d_vec, unsigned int state_size) {

  unsigned int n = M_mat.size1();
  double *M = &(M_mat(0,0));
  double *U = &(U_mat(0,0));
  double *d = &(d_vec(0));

  // this method stolen from pykalman.sqrt.bierman by Daniel Duckworth (BSD license)
  /*Construct the UDU' decomposition of a positive, semidefinite matrix M

    Parameters
    ----------
    M : [n, n] array
        Matrix to factorize

    Returns
    -------
    UDU : UDU_decomposition of size n
        UDU' representation of M
  */

  // make M upper triangular
  for (unsigned i=0; i < state_size; ++i) {
    for (unsigned idx=i*n; idx < i*n+i; ++idx) {
      M[idx] = 0;
    }
  }

  // initialize U as the identity
  for (unsigned i=0; i < state_size; ++i) {
    for (unsigned idx=i*n; idx < i*n+state_size; ++idx) {
      U[idx] = 0;
    }
  }
  for (unsigned i=0; i < state_size; ++i) {
    U[i*n+i] = 1;
  }

  // note j cannot be unsigned, to handle state_size=0
  for (int j=state_size-1; j >= 1; --j) {
    d[j] = M[j*n+j];
    double alpha = 0.0;
    double beta = 0.0;
    if (d[j] > 0) {
      alpha = 1.0 / d[j];
    } else {
      if (fabs(d[j] > 1e-5) ) {
	printf("WARNING: nonpositive d[%d] %f in udu decomp\n", j, d[j]);
	return -1;
      }
      d[j] = 0.0;
      alpha = 0.0;
    }
    for (unsigned k=0; k < j; ++k) {
      beta = M[k*n+ j ];
      U[k*n+ j] = alpha * beta;
      for (unsigned kk=0; kk <= k; ++kk) {
	M[kk*n+ k] = M[kk*n+k] - beta * U[kk*n+j];
      }
    }
  }
  d[0] = M[0];

  if (d[0] < 0) {
    printf("ERROR: udu decomposition on non-posdef matrix with M(0,0)=%f\n", M[0]);
    return -1;
  }
  return 0;
}

FilterState::FilterState(int max_dimension, double eps_stationary) {
  this->eps_stationary = eps_stationary;
  this->at_fixed_point=false;
  this->alpha = 0;
  this->wasnan = false;

  this->obs_U = matrix<double>(max_dimension, max_dimension);
  this->pred_U = matrix<double>(max_dimension, max_dimension);
  this->tmp_U1 = matrix<double>(max_dimension, max_dimension);
  this->tmp_U2 = matrix<double>(max_dimension, max_dimension);

  // we only ever write to the upper triangle of this matrix, so
  // zeroing the full matrix now guarantees that it will always be zero.
  this->obs_U.clear();
  this->pred_U.clear();
  this->tmp_U1.clear();
  this->tmp_U2.clear();

  this->P = matrix<double>(max_dimension, max_dimension);

  this->obs_d = vector<double>(max_dimension);
  this->pred_d = vector<double>(max_dimension);
  this->obs_d.clear();
  this->pred_d.clear();

  this->gain = vector<double>(max_dimension);
  this->f = vector<double>(max_dimension);
  this->v = vector<double>(max_dimension);

  this->xk = vector<double>(max_dimension);
}

void print_vec(const vector<double> & v) {
  for(unsigned i=0; i < v.size(); ++i) {
    printf("%.3f ", v(i));
  }
  printf("\n");
}

void print_mat(const matrix<double> & m) {
  for(unsigned i=0; i < m.size1(); ++i) {
    for(unsigned j=0; j < m.size2(); ++j) {
      printf("%.3f ", m(i,j));
    }
    printf("\n");
  }
  printf("\n");
}

void write_vec(const char *fname, const vector<double> & v) {
  FILE *f = fopen(fname, "w");
  if (f == NULL)
    {
      printf("Error opening file!\n");
      exit(1);
    }
  for(unsigned i=0; i < v.size(); ++i) {
    fprintf(f, "%.12f ", v(i));
  }
  fprintf(f, "\n");
  fclose(f);

}

void write_mat_col(const char *fname, const matrix<double> & m) {

  FILE *f = fopen(fname, "w");
  if (f == NULL)
    {
      printf("Error opening file!\n");
      exit(1);
    }

  for(unsigned i=0; i < m.size1(); ++i) {
    for(unsigned j=0; j < m.size2(); ++j) {
      fprintf(f, "%.12f ", m(i,j));
    }
    fprintf(f, "\n");
  }
  fprintf(f, "\n");

  fclose(f);
}

void write_stuff(const char *item, unsigned int k, const vector<double> & v) {
  char fname[100];
  snprintf(fname, 100, "matrices/%s_c_%d.txt", item, k);
  write_vec(fname, v);
}

void write_stuff(const char *item, unsigned int k, const matrix<double> & m) {
  char fname[100];
  snprintf(fname, 100, "matrices/%s_c_%d.txt", item, k);
  write_mat_col(fname, m);
}

void print_mat_col(const matrix<double> & m) {
  for(unsigned i=0; i < m.size1(); ++i) {
    for(unsigned j=0; j < m.size2(); ++j) {
      printf("%.3f ", m(i,j));
    }
    printf("\n");
  }
  printf("\n");
}

#include <cblas.h>
void compute_explicit_cov_atlas(FilterState &cache,
				matrix<double> &U,
				vector<double> &d,
				int prev_state_size) {

  matrix<double> &mtmp = cache.tmp_U2;
  matrix<double> &P = cache.P;
  unsigned state_size = cache.state_size;

  if (prev_state_size == -1) {
    prev_state_size = state_size;
  }

  // U has dimension (ss x pss)
  for (unsigned i=0; i < state_size; ++i) {
    for (unsigned j=0; j < prev_state_size; ++j) {
      mtmp(i,j) = U(i,j) * d(j);
    }
  }

  cblas_dgemm(CblasRowMajor,
	      CblasNoTrans,
	      CblasTrans,
	      state_size,  state_size, prev_state_size, 1.0,
	      &(mtmp(0,0)), mtmp.size2(),
	      &(U(0,0)), U.size2(),
	      0.0, &(P(0,0)), P.size2());

}

/* void compute_explicit_cov(FilterState &cache,
			  matrix<double> &U_tmp,
			  vector<double> &d_tmp,
			  int prev_state_size) {

  // this is the original version of this function in ublas.
  // it's slow and superceded by the atlas version above.

  matrix<double> &mtmp = cache.tmp_U2;
  matrix<double> &P = cache.P;
  unsigned state_size = cache.state_size;

  if (prev_state_size == -1) {
    prev_state_size = state_size;
  }

  // construct the cov matrix
  for (unsigned i=0; i < prev_state_size; ++i) {
    subrange(mtmp, 0, state_size, i, i+1) = subrange(U_tmp, 0, state_size, i, i+1);
    subrange(mtmp, 0, state_size, i, i+1) *= d_tmp(i);
  }
  noalias(subrange(P, 0, state_size, 0, state_size))		\
    = prod(subrange(mtmp, 0, state_size, 0, prev_state_size),
	   trans(subrange(U_tmp, 0, state_size, 0, prev_state_size)));
} */

void FilterState::init_priors(StateSpaceModel &ssm) {
  this->pred_d.clear();
  this->xk.clear();

  this->state_size = ssm.prior_mean(&(this->xk(0)));
  this->state_size = ssm.prior_vars(&(this->pred_d(0)));

  this->pred_U.clear();
  for (unsigned i=0; i < ssm.max_dimension; ++i) {
    this->pred_U(i,i) = 1;
  }

  return;
}

void FilterState::init_incremental_state(StateSpaceModel &ssm, int k) {
  /*
    Initialize the state to a high-variance prior on the state variables
    active at some (nonzero) step k. This allows us to start filtering
    somewhere other than the beginning of the sequence.
   */

  this->pred_d.clear();
  this->xk.clear();
  this->state_size = ssm.state_size_at_timestep(k);

  for (unsigned i=0; i < ssm.max_dimension; ++i) {
    this->pred_d(i) = 1e8;
  }

  this->pred_U.clear();
  for (unsigned i=0; i < ssm.max_dimension; ++i) {
    this->pred_U(i,i) = 1;
  }

  return;
}

double kalman_observe_sqrt(StateSpaceModel &ssm, FilterState &cache, int k, double zk) {

  // if needed, use this for isnan: #include <boost/math/special_functions/fpclassify.hpp>
  if (std::isnan(zk)) {
    // printf("declaring nan at timestep %d, val %f\n", k, zk);
    if (!cache.wasnan && cache.at_fixed_point) {
       cache.at_fixed_point = false;
    }
    cache.wasnan = true;

    // no observation, so the predicted state becomes the "observed" state
    cache.obs_d = cache.pred_d;
    cache.obs_U = cache.pred_U;

    return 0;
  } else {
    if (cache.wasnan && cache.at_fixed_point) {
       cache.at_fixed_point = false;
    }
    cache.wasnan = false;
  }

  double alpha = cache.alpha;
  unsigned int state_size = cache.state_size;
  if (!cache.at_fixed_point || !ssm.stationary(k)) {
    cache.at_fixed_point = false;

    matrix<double> &U_old = cache.pred_U;
    vector<double> &d_old = cache.pred_d;

    matrix<double> &U = cache.obs_U;
    vector<double> &d = cache.obs_d;
    vector<double> &K = cache.gain;
    vector<double> &f = cache.f;
    vector<double> &v = cache.v;
    double r = ssm.observation_noise(k);

    K.clear();

    ssm.apply_observation_matrix(U_old, 0,
				 k, &(f(0)), &(v(0)), state_size);

    for (unsigned i=0; i < state_size; ++i) {
      v(i) = d_old(i)*f(i);
    }

    D(write_stuff("U_obs_old", k, U_old);)
      D(write_stuff("d_obs_old", k, d_old);)
      D(write_stuff("f", k, f);)
      D(write_stuff("v", k, v);)

    alpha = r + v(0)*f(0);
    if (alpha > 1e-20) {
       d(0) = d_old(0) * r/alpha;
    } else {
      //printf("step %d correcting initial alpha from %f to 1e-20\n", k, alpha);
      d(0) = d_old(0);
      alpha = 1e-20;
    }
    // printf("   alpha C: %f\n", alpha);
    K(0)=v(0);

    U(0, 0) = U_old(0,0);
    for (unsigned j=1; j < state_size; ++j) {
      double old_alpha = alpha;
      alpha += v(j)*f(j);
      //printf("   alpha C %d: %f\n", j, alpha);
      if (alpha > 1e-20) {
	d(j) = d_old(j) * (old_alpha/alpha);
	//printf("d = %f * %f = %f\n" , d_old(j), old_alpha/alpha, d(j));
      } else {
	//printf("step %d correcting alpha from %f to 1e-20\n", k, alpha);
	d(j) = d_old(j);
	alpha = 1e-20;
      }

      for (unsigned i=0; i < state_size; ++i) {
	U(i, j) = U_old(i,j) - (K(i)/old_alpha)*f(j);
        K(i) += v(j) * U_old(i,j);
      }

    }
    cache.alpha = alpha;

      D(write_stuff("U_obs", k, U);)
      D(write_stuff("d_obs", k, d);)

  }

  // given the Kalman gain from the covariance update, compute
  // the updated mean vector.
  vector<double> &xk = cache.xk;
  double pred_z = ssm.apply_observation_matrix(&(xk(0)), k) + ssm.observation_bias(k);
  cache.pred_z = pred_z;
  double yk = zk - pred_z;
  for (unsigned i=0; i < state_size; ++i) {
    xk(i) += cache.gain(i) * yk/alpha;
  }

  // also compute log marginal likelihood for this observation
  double step_ell = -.5 * log(2*PI*alpha) - .5 * yk*yk / alpha;

  //printf("step %d (C) pred %.4f alpha %.4f z %.4f y %.4f ell %.4f\n", k, pred_z, alpha, zk, yk, step_ell);

  if (std::isnan(step_ell)) {
    printf("step %d (C) pred %.4f alpha %.4f z %.4f y %.4f ell %.4f\n", k, pred_z, alpha, zk, yk, step_ell);
    //print_vec(cache.obs_d);
    //printf("\n");
    //print_vec(cache.xk);
    //exit(-1);
    step_ell = -INFINITY;
  }

  return step_ell;
}

int kalman_predict_sqrt(StateSpaceModel &ssm, FilterState &cache, int k, bool force_P) {

  unsigned int prev_state_size = cache.state_size;

  vector<double> &tmp = cache.f;

  unsigned int state_size = ssm.apply_transition_matrix( &(cache.xk(0)), k,  &(tmp(0)));
  cache.state_size = state_size;

  vector<double> &xk = cache.xk;
  subrange(xk, 0, state_size) = subrange(tmp, 0, state_size);
  ssm.transition_bias(k, &(xk(0)));

  D(write_stuff("xk_posttransit", k, xk);)

  if (cache.at_fixed_point and ssm.stationary(k)) {
    return 0;
  }

  cache.at_fixed_point = false;

  matrix<double> &U_old = cache.obs_U;
  vector<double> &d_old = cache.obs_d;

  matrix<double> &U_tmp = cache.tmp_U1;
  vector<double> &d_tmp = cache.v;

  // get transition noise into temporary storage
  ssm.transition_noise_diag(k, &(tmp(0)));

  /* pushing the covariance P through the transition model F yields
     FPF'. In a factored representation, this is FUDU'F', so we just need
     to compute FU. */
  unsigned int min_size = std::min(prev_state_size, state_size);

  D(write_stuff("U_pretransit", k, U_old);)

  // COMMENTED OUT: this loop is equivalent to the matrix-valued transition call
  // directly below. I'm leaving it in for debugging and to run speed comparisons.

    /*
  for (int i=0; i < min_size; ++i) {
    // THIS ONLY WORKS IF U_old is in column-major order
    ssm.apply_transition_matrix(&(column(U_old, i)(0) ), k, &(d_tmp(0)) );
    noalias(column(U_tmp, i)) = d_tmp;
    }*/
  subrange(d_tmp, 0, prev_state_size) = subrange(d_old, 0, prev_state_size);
  ssm.apply_transition_matrix(U_old, 0, k, U_tmp, 0, prev_state_size);
  for (unsigned i=prev_state_size; i < state_size; ++i) {
    d_tmp(i) = 0;
    for (unsigned j=0; j < state_size; ++j) {
      U_tmp(j, i) = 0;
    }
  }

    D(write_stuff("d_posttransit", k, d_tmp);)
    D(write_stuff("U_posttransit", k, U_tmp);)

  // if there is transition noise, do the expensive reconstruction/factoring step
  if (force_P || state_size != prev_state_size || norm_2(subrange(tmp, 0, state_size)) > 0) {

    compute_explicit_cov_atlas(cache, U_tmp, d_tmp, prev_state_size);
    // add transition noise
    matrix<double> &P = cache.P;

    D(write_stuff("P_prenoise", k, P);)

    for (unsigned i=0; i < state_size; ++i) {
      P(i,i) += tmp(i);
    }
    //printf("noise at time %d ", k);
    //print_vec(tmp);

      D(write_stuff("P", k, P);)

    // printf("step %d state size %d\n", k, state_size);

    // udu overwrites the cov matrix, so we need to
    // save it if we're going to explicitly use it
    // later on.
    matrix<double> & mtmp = cache.tmp_U2;
    if (force_P) {
      mtmp = P;
    }

    // get the new factored representation
    int err = udu(P, U_tmp, d_tmp, state_size);
    if (err != 0) {
      return err;
    }

      D(write_stuff("d_decomp", k, d_tmp);)
      D(write_stuff("U_decomp", k, U_tmp);)

    if (force_P) {
      P = mtmp;
    }

  }

  // if our factored representation is (almost) the same as the previous invocation,
  // we've reached a stationary state
  matrix<double> &U_cached = cache.pred_U;
  vector<double> &d_cached = cache.pred_d;
  if (ssm.stationary(k)) {
    if (k > 0 && ssm.stationary(k-1)) {
      bool potential_fixed_point = true;
      for (unsigned i=0; i < state_size; ++i) {
	if (std::abs(d_tmp(i) - d_cached(i)) > cache.eps_stationary) {
	  potential_fixed_point=false;
	  break;
	}
	for (unsigned j=0; j < state_size; ++j) {
	  if (std::abs(U_tmp(i,j) - U_cached(i,j)) > cache.eps_stationary) {
	    potential_fixed_point=false;
	    break;
	  }
	}
	if (!potential_fixed_point) {
	  break;
	}
      }
      if (potential_fixed_point) {
	cache.at_fixed_point = true;
      }
    }
  }
  if (!cache.at_fixed_point) {
    subrange(U_cached, 0, state_size, 0, state_size) = subrange(U_tmp, 0, state_size, 0, state_size);
    subrange(d_cached, 0, state_size) = subrange(d_tmp, 0, state_size);
  }
  return 0;
}

double filter_likelihood(StateSpaceModel &ssm, const vector<double> &z) {
  FilterState cache(ssm.max_dimension, 1e-10);
  cache.init_priors(ssm);
  unsigned int N = z.size();
  double ell = 0;

  D(write_stuff("U_prior", 0, cache.pred_U);)
  D(write_stuff("d_prior", 0, cache.pred_d);)
  D(write_stuff("xk_prior", 0, cache.xk);)

    if (N == 0) {
      return ell;
    }

  double step_ell = kalman_observe_sqrt(ssm, cache, 0, z(0));
  if (std::isinf(step_ell)) {
    return step_ell;
  }
  ell += step_ell;

  D(write_stuff("U_post_obs", 0, cache.obs_U);)
    D(write_stuff("d_post_obs", 0, cache.obs_d);)
    D(write_stuff("xk_post_obs", 0, cache.xk);)

  for (unsigned k=1; k < N; ++k) {
    int err = kalman_predict_sqrt(ssm, cache, k, false);

    if (err != 0) {
      return -INFINITY;
    }

    D(write_stuff("U_post_predict", k, cache.pred_U);)
    D(write_stuff("d_post_predict", k, cache.pred_d);)
    D(write_stuff("xk_post_predict", k, cache.xk);)

    double step_ell = kalman_observe_sqrt(ssm, cache, k, z(k));
    if (std::isinf(step_ell)) {
      return step_ell;
    }
    ell += step_ell;

      D(write_stuff("U_post_obs", k, cache.obs_U);)
      D(write_stuff("d_post_obs", k, cache.obs_d);)
      D(write_stuff("xk_post_obs", k, cache.xk);)

  }
  return ell;
}

double filter_incremental(StateSpaceModel &ssm,
			  const vector<double> &z,
			  double * ells,
			  int filter_start_idx,
			  int incr_start_idx,
			  int incr_end_idx,
			  int update_ells,
			  double step_ell_tol,
			  int * steps_processed,
			  int * errcode) {

  unsigned int N = z.size();
  if ((filter_start_idx < 0) || (incr_start_idx < filter_start_idx) || (incr_start_idx >= N)) {
    printf("fatal: filter_incremental received bad indices (%d, %d, %d) for signal of size %d\n",
	   filter_start_idx, incr_start_idx, incr_end_idx, N);
  }

  FilterState cache(ssm.max_dimension, 1e-10);
  if (filter_start_idx == 0) {
    cache.init_priors(ssm);
  } else {
    cache.init_incremental_state(ssm, filter_start_idx);
  }

  double total_discrepancy = 0;
  double step_ell_discrepancy;
  double step_ell;

  //printf("update ells %d\n", update_ells);

  // do some initial filtering steps to "warm up" the hidden state
  for (unsigned k=filter_start_idx; k < incr_start_idx; ++k) {
    step_ell = kalman_observe_sqrt(ssm, cache, k, z(k));
    if (std::isinf(step_ell)) {
      *errcode = ERR_INCR_NUMERIC;
      return -INFINITY;
    }

    int err = kalman_predict_sqrt(ssm, cache, k+1, false);
    if (err != 0) {
      *errcode = ERR_INCR_NUMERIC;
      return -INFINITY;
    }

    step_ell_discrepancy= step_ell - ells[k];

    if (k == incr_start_idx-1) {
      // check that our likelihood calculations are now in sync with the
      // previous calculation.
      if ( fabs(step_ell_discrepancy) > step_ell_tol ) {
	// printf("warmup %d discrepancy %.10f\n", k, step_ell_discrepancy);
         *errcode = ERR_INCR_INIT;
         return -INFINITY;
      }
    }
  }

  // now start the "normal" filtering process with an observation step
  step_ell = kalman_observe_sqrt(ssm, cache, incr_start_idx, z(incr_start_idx));
  if (std::isinf(step_ell)) {
    *errcode = ERR_INCR_NUMERIC;
    return step_ell;
  }
  step_ell_discrepancy = step_ell - ells[incr_start_idx];
  if (update_ells) {
    ells[incr_start_idx] = step_ell;
  }
  total_discrepancy += step_ell_discrepancy;

  //printf("first obs %d discrepancy %.2f\n", incr_start_idx, step_ell_discrepancy);

  *steps_processed = N - filter_start_idx;
  int match_counter = 0;
  for (unsigned k=incr_start_idx+1; k < N; ++k) {
    int err = kalman_predict_sqrt(ssm, cache, k, false);

    if (err != 0) {
      *errcode = ERR_INCR_NUMERIC;
      return -INFINITY;
    }

    step_ell = kalman_observe_sqrt(ssm, cache, k, z(k));
    if (std::isinf(step_ell)) {
      *errcode = ERR_INCR_NUMERIC;
      return -INFINITY;
    }
    step_ell_discrepancy = step_ell - ells[k];
    if (update_ells) {
      ells[k] = step_ell;
    }
    total_discrepancy += step_ell_discrepancy;
    //printf("step %d discrepancy %.2f\n", k, step_ell_discrepancy);

    // quit filtering if we're past the end idx and the likelihoods seem to
    // have reverted to their previous values
    if ((k > incr_end_idx) && (fabs(step_ell_discrepancy) < step_ell_tol)) {
      *steps_processed = k - filter_start_idx;
      match_counter += 1;
      if (match_counter >= 5) {
	break;
      }
    }
  }
  return total_discrepancy;
}

void step_obs_likelihoods(StateSpaceModel &ssm, const vector<double> &z,
			  vector<double> & ells,
			  vector<double> & preds,
			  vector<double> & alphas) {
  FilterState cache(ssm.max_dimension, 1e-10);
  cache.init_priors(ssm);
  unsigned int N = z.size();

  if (N == 0) {
    return;
  }

  ells(0) = kalman_observe_sqrt(ssm, cache, 0, z(0));
  preds(0) = cache.pred_z;
  alphas(0) = cache.alpha;
  for (unsigned k=1; k < N; ++k) {
    kalman_predict_sqrt(ssm, cache, k, false);
    ells(k) = kalman_observe_sqrt(ssm, cache, k, z(k));
    preds(k) = cache.pred_z;
    alphas(k) = cache.alpha;
    //printf("got ell %f at step %d\n", ells(k), k);
  }
}

void mean_obs(StateSpaceModel &ssm, vector<double> & result) {
  vector<double> x(ssm.max_dimension);
  vector<double> x2(ssm.max_dimension);
  x.clear();
  x2.clear();

  ssm.prior_mean(&(x(0)));

  for (unsigned k = 0; k < result.size(); ++k) {
    result[k] = ssm.apply_observation_matrix(&(x(0)), k);
    result[k] += ssm.observation_bias(k);

    if (k+1 < result.size()) {
      ssm.apply_transition_matrix(&(x(0)), k+1, &(x2(0)));
      ssm.transition_bias(k+1, &(x2(0)));
    }
    x = x2; // this copy is unnecessary, we could swap
            // pointers instead, but it doesn't
            // matter cause this method is never the
            // performance bottleneck.
  }
}

void obs_var(StateSpaceModel &ssm, vector<double> & result) {
  FilterState cache(ssm.max_dimension, 1e-10);
  cache.init_priors(ssm);
  compute_explicit_cov_atlas(cache, cache.pred_U, cache.pred_d, -1);

  matrix<double> P = cache.P;

  for (unsigned k = 0; k < result.size(); ++k) {
    ssm.apply_observation_matrix(P, 0,
    				 k, &(cache.f(0)), &(cache.v(0)), cache.f.size());
    result(k) = ssm.apply_observation_matrix(&(cache.f(0)), k);
    result(k) += ssm.observation_noise(k);
    if (k+1 < result.size()) {

      ssm.apply_transition_matrix(P, 0, k+1, cache.obs_U, 0, ssm.max_dimension);
      cache.obs_U = trans(cache.obs_U);
      ssm.apply_transition_matrix(cache.obs_U, 0, k+1, P, 0, ssm.max_dimension);

      vector<double> &tmp = cache.f;
      ssm.transition_noise_diag(k+1, &(tmp(0)));
      for (unsigned i=0; i < ssm.max_dimension; ++i) {
	P(i,i) += tmp(i);
      }
    }
  }
}

void prior_sample(StateSpaceModel &ssm, vector<double> & result, unsigned long seed) {
  FilterState cache(ssm.max_dimension, 1e-10);
  cache.init_priors(ssm);

  std::mt19937 gen(seed);
  std::normal_distribution<double> randn(0,1);
  /* auto randn = std::bind(std::normal_distribution<double>(0,1),
			     ); */

  printf("seed with %lu, first randn %f\n", seed, randn(gen));

  // sample initial state from the prior
  vector<double> &d = cache.pred_d;
  for (unsigned i=0; i < ssm.max_dimension; ++i) {
    cache.xk(i) += randn(gen) * sqrt(d(i));
  }

  unsigned k = 0;
  result(k) = ssm.apply_observation_matrix(&(cache.xk(0)), k);
  result(k) += ssm.observation_bias(k);

  for (k=1; k < result.size(); ++k) {
    cache.obs_d = cache.pred_d;
    cache.obs_U = cache.pred_U;

    vector<double> &tmp = cache.f;
    unsigned int state_size = ssm.apply_transition_matrix( &(cache.xk(0)), k,  &(tmp(0)));
    vector<double> &xk = cache.xk;
    subrange(xk, 0, state_size) = subrange(tmp, 0, state_size);
    ssm.transition_bias(k, &(xk(0)));

    ssm.transition_noise_diag(k, &(tmp(0)));
    for (unsigned i=0; i < state_size; ++i) {
      cache.xk(i) += randn(gen) * sqrt(tmp(i));
    }

    result(k) = ssm.apply_observation_matrix(&(cache.xk(0)), k);
    result(k) += ssm.observation_bias(k);
    result(k) += randn(gen) * sqrt(ssm.observation_noise(k));

  }
}

double all_filtered_cssm_coef_marginals(TransientCombinedSSM &ssm,
				      const vector<double> &z,
				      vector<double> & step_ells,
				      std::vector<vector<double> > & cmeans,
				      std::vector<vector<double> > & cvars) {
  FilterState cache(ssm.max_dimension, 1e-10);
  cache.init_priors(ssm);
  ssm.init_coef_priors(cmeans, cvars);

  unsigned int N = z.size();
  double ell = 0;

  if (N == 0) {
    return ell;
  }

  step_ells[0] = kalman_observe_sqrt(ssm, cache, 0, z(0));
  ell += step_ells[0];
  compute_explicit_cov_atlas(cache, cache.obs_U, cache.obs_d, -1);
  ssm.extract_all_coefs(cache, 0, cmeans, cvars);
  for (unsigned k=1; k < N; ++k) {
    kalman_predict_sqrt(ssm, cache, k, false);
    step_ells[k] = kalman_observe_sqrt(ssm, cache, k, z(k));
    ell += step_ells[k];

    compute_explicit_cov_atlas(cache, cache.obs_U, cache.obs_d, -1);
    ssm.extract_all_coefs(cache, k, cmeans, cvars);
  }
  return ell;
}

double tssm_component_means(TransientCombinedSSM &ssm,
			    const vector<double> &z,
			    std::vector<vector<double> > & means) {

  if (means.size() != ssm.n_ssms) {
    printf("component_means() needs exactly one vector for each component SSM\n");
    exit(-1);
  }

  FilterState cache(ssm.max_dimension, 1e-10);
  cache.init_priors(ssm);

  unsigned int N = z.size();
  double ell = 0;

  if (N == 0) {
    return ell;
  }

  ell += kalman_observe_sqrt(ssm, cache, 0, z(0));
  ssm.extract_component_means(&(cache.xk(0)), 0, means);
  for (unsigned k=1; k < N; ++k) {
    kalman_predict_sqrt(ssm, cache, k, false);
    ell += kalman_observe_sqrt(ssm, cache, k, z(k));
    ssm.extract_component_means(&(cache.xk(0)), k, means);
  }
  return ell;
}

double tssm_component_vars(TransientCombinedSSM &ssm,
			  const vector<double> &z,
			  std::vector<vector<double> > & vars) {

  if (vars.size() != ssm.n_ssms) {
    printf("component_vars() needs exactly one vector for each component SSM\n");
    exit(-1);
  }

  FilterState cache(ssm.max_dimension, 1e-10);
  cache.init_priors(ssm);
  compute_explicit_cov_atlas(cache, cache.pred_U, cache.pred_d, -1);

  unsigned int N = z.size();
  double ell = 0;

  if (N == 0) {
    return ell;
  }

  ell += kalman_observe_sqrt(ssm, cache, 0, z(0));
  compute_explicit_cov_atlas(cache, cache.obs_U, cache.obs_d, -1);
  ssm.extract_component_vars(cache.P, cache.tmp_U2, 0, vars);
  for (unsigned k=1; k < N; ++k) {
    kalman_predict_sqrt(ssm, cache, k, false);
    ell += kalman_observe_sqrt(ssm, cache, k, z(k));
    compute_explicit_cov_atlas(cache, cache.obs_U, cache.obs_d, -1);
    ssm.extract_component_vars(cache.P, cache.tmp_U2, k, vars);
  }
  return ell;
}


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

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

// 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)

#ifndef BOOST_GEOMETRY_ARITHMETIC_DETERMINANT_HPP
#define BOOST_GEOMETRY_ARITHMETIC_DETERMINANT_HPP

#include <cstddef>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/geometries/concepts/point_concept.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename ReturnType, typename U, typename V>
class calculate_determinant
{
    template <typename T>
    static inline ReturnType rt(T const& v)
    {
        return boost::numeric_cast<ReturnType>(v);
    }

public :

    static inline ReturnType apply(U const& ux, U const& uy
                                 , V const& vx, V const& vy)
    {
        return rt(ux) * rt(vy) - rt(uy) * rt(vx);
    }
};

template <typename ReturnType, typename U, typename V>
inline ReturnType determinant(U const& ux, U const& uy
                            , V const& vx, V const& vy)
{
    return calculate_determinant
        <
            ReturnType, U, V
        >::apply(ux, uy, vx, vy);
}

template <typename ReturnType, typename U, typename V>
inline ReturnType determinant(U const& u, V const& v)
{
    BOOST_CONCEPT_ASSERT( (concept::ConstPoint<U>) );
    BOOST_CONCEPT_ASSERT( (concept::ConstPoint<V>) );

    return calculate_determinant
        <
            ReturnType,
            typename geometry::coordinate_type<U>::type,
            typename geometry::coordinate_type<V>::type
        >::apply(get<0>(u), get<1>(u), get<0>(v), get<1>(v));
}

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ARITHMETIC_DETERMINANT_HPP


/*
* TS Elements
* Copyright 2015-2018 M. Newhouse
* Released under the MIT license.
*/

#pragma once

#include "entity_type.hpp"

#include "resources/collision_mask.hpp"

#include "utility/vector2.hpp"
#include "utility/rotation.hpp"

#include <boost/container/small_vector.hpp>

#include <memory>

namespace ts
{
  namespace resources
  {
    struct CollisionShape;
  }

  namespace world
  {
    using EntityId = std::uint16_t;
    class PhysicsSpace;

    struct PhysicsBodyDeleter
    {
      void operator()(void*) const;
    };

    // The Entity class is the base class of all dynamic world objects.
    // All of these entities share a few common properties, such as position, velocity and rotation,
    // and this class provides an interface to examine and modify said properties.
    class Entity
    {
    public:
      explicit Entity(EntityId entity_id, EntityType entity_type,
                      const resources::CollisionShape& collision_shape, double mass, double moment);

      virtual ~Entity() = default;

      void set_position(Vector2d position);
      Vector2d position() const;

      void set_velocity(Vector2d velocity);
      Vector2d velocity() const;

      double z_speed() const;
      void set_z_speed(double z_speed);

      void set_rotation(Rotation<double> rotation);
      Rotation<double> rotation() const;

      void set_angular_velocity(double rotation_speed);
      double angular_velocity() const;

      void set_z_position(double z_position);
      double z_position() const;
      std::uint32_t z_level() const;

      EntityType type() const;
      EntityId entity_id() const;

      double mass() const;
      void set_mass(double mass);

      Vector2d center_of_mass() const;
      void set_center_of_mass(Vector2d center);

      void set_moment_of_inertia(double m);
      double moment_of_inertia() const;

      bool is_flying() const;
      void set_hover_distance(double hover_distance);
      double hover_distance() const;

      void update_z_speed(double frame_duration);

      struct RawState
      {
        Vector2<std::uint32_t> position = {};
        Vector2<std::uint32_t> velocity = {};

        std::uint32_t rotating_speed = 0;
        std::uint32_t rotation = 0;

        std::uint16_t z_speed = 0;
        std::uint16_t z_position = 0;
      };

      RawState raw_state() const;
      void load_raw_state(RawState state);

      void apply_force(Vector2d force, Vector2d point);
      void apply_torque(double t);

      Vector2d applied_force() const;
      double applied_torque() const;

    protected:
      void* physics_body();
      const void* physics_body() const;

    private:
      RawState raw_state_;

      Vector2<double> position_;
      Vector2<double> velocity_;
      Rotation<double> rotation_;
      double rotating_speed_ = 0.0;
      double z_speed_ = 0.0;
      double z_position_ = 0.0;
      double hover_distance_ = 0.0;

      EntityId entity_id_;
      EntityType entity_type_;

      double bounciness_ = 0.0;
      double mass_ = 100.0;

      friend PhysicsSpace;

      std::unique_ptr<void, PhysicsBodyDeleter> physics_body_;
    };
  }
}


//
// Expansion Hunter
// Copyright 2016-2019 Illumina, Inc.
// All rights reserved.
//
// Author: Xiao Chen <xchen2@illumina.com>
//         Egor Dolzhenko <edolzhenko@illumina.com>
//
// 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 "genotyping/CopyNumberGenotyper.hh"
#include <boost/math/distributions/normal.hpp>
#include <math.h>
#include <numeric>

using boost::optional;
using boost::math::normal_distribution;
using std::vector;

namespace ehunter
{

CopyNumberGenotyper::CopyNumberGenotyper(
    int maxCopyNumber, double depthScaleFactor, double standardDeviationOfCN2,
    const std::vector<double>& meanDepthValues, const std::vector<double>& priorCopyNumberFreq)
    : maxCopyNumber_(maxCopyNumber)
    , depthScaleFactor_(depthScaleFactor)
    , standardDeviationOfCN2_(standardDeviationOfCN2)
    , meanDepthValues_(meanDepthValues)
    , priorCopyNumberFreq_(priorCopyNumberFreq)
{
    if (maxCopyNumber_ + 1 != static_cast<int>(meanDepthValues_.size()))
    {
        throw std::runtime_error("Number of mean values is inconsistent with total copy number states.");
    }

    if (maxCopyNumber_ + 1 != static_cast<int>(priorCopyNumberFreq_.size()))
    {
        throw std::runtime_error("Number of prior frequencies is inconsistent with total copy number states.");
    }
}

boost::optional<int> CopyNumberGenotyper::genotype(double normalizedDepth) const
{
    const double adjustedDepth = normalizedDepth / depthScaleFactor_;
    std::vector<double> likelihoodOfAllCN;
    std::vector<double> pvalueOfAllCN;

    for (int currentGenotype = 0; currentGenotype != maxCopyNumber_ + 1; currentGenotype++)
    {
        std::pair<double, double> likelihoodAndPvalue = genotypeLikelihoodAndPvalue(currentGenotype, adjustedDepth);
        double currentLikelihood = likelihoodAndPvalue.first;
        double currentPvalue = likelihoodAndPvalue.second;
        likelihoodOfAllCN.emplace_back(currentLikelihood);
        pvalueOfAllCN.emplace_back(currentPvalue);
    }

    const std::pair<int, double> bestGenotypeAndPosterior = getBestGenotypeAndPosterior(likelihoodOfAllCN);
    const int bestGenotype = bestGenotypeAndPosterior.first;
    const double posteriorProbabilityOfBestGenotype = bestGenotypeAndPosterior.second;
    const bool posteriorCheck = posteriorProbabilityOfBestGenotype > posteriorProbabilityThreshold_;
    const bool pvalueCheck = pvalueOfAllCN[bestGenotype] > pvalueThreshold_;
    const optional<int> genotype = (posteriorCheck && pvalueCheck) ? bestGenotype : optional<int>();
    return genotype;
}

std::pair<int, double>
CopyNumberGenotyper::getBestGenotypeAndPosterior(const std::vector<double>& likelihoodOfAllCN) const
{
    double sumOfLikelihood = 0;
    for (double likelihoodValue : likelihoodOfAllCN)
    {
        sumOfLikelihood += likelihoodValue;
    }

    assert(!likelihoodOfAllCN.empty());
    auto maxElement = max_element(likelihoodOfAllCN.begin(), likelihoodOfAllCN.end());
    const double maxLikelihood = *maxElement;
    std::pair<int, double> bestGenotypeAndPosterior;
    bestGenotypeAndPosterior.first = distance(likelihoodOfAllCN.begin(), maxElement);
    bestGenotypeAndPosterior.second = maxLikelihood / sumOfLikelihood;

    return bestGenotypeAndPosterior;
}

std::pair<double, double>
CopyNumberGenotyper::genotypeLikelihoodAndPvalue(int currentGenotype, double adjustedDepth) const
{
    assert(currentGenotype < (int)meanDepthValues_.size());
    assert(currentGenotype < (int)priorCopyNumberFreq_.size());

    const double meanValue = meanDepthValues_[currentGenotype];
    // standard deviation for each CN state can be computed from CN2 except for CN0 it is pre-defined
    const double standardDeviation
        = currentGenotype == 0 ? standardDeviationOfCN0_ : standardDeviationOfCN2_ * sqrt(double(currentGenotype) / 2);
    const double priorFreq = priorCopyNumberFreq_[currentGenotype];

    const normal_distribution<> cnDistribution(meanValue, standardDeviation);
    const double genotypeLikelihood = priorFreq * pdf(cnDistribution, adjustedDepth);
    const double cumulativeFrequency = cdf(cnDistribution, adjustedDepth);
    const double pvalue = std::min(cumulativeFrequency, 1 - cumulativeFrequency);

    std::pair<double, double> likelihoodAndPvalue;
    likelihoodAndPvalue.first = genotypeLikelihood;
    likelihoodAndPvalue.second = pvalue;

    return likelihoodAndPvalue;
}
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_

#ifndef BOOST_MP_INT_FUNC_HPP
#define BOOST_MP_INT_FUNC_HPP

#include <boost/multiprecision/number.hpp>

namespace boost{ namespace multiprecision{

namespace default_ops
{

template <class Backend>
inline void eval_qr(const Backend& x, const Backend& y, Backend& q, Backend& r)
{
   eval_divide(q, x, y);
   eval_modulus(r, x, y);
}

template <class Backend, class Integer>
inline Integer eval_integer_modulus(const Backend& x, Integer val)
{
   BOOST_MP_USING_ABS
   using default_ops::eval_modulus;
   using default_ops::eval_convert_to;
   typedef typename boost::multiprecision::detail::canonical<Integer, Backend>::type int_type;
   Backend t;
   eval_modulus(t, x, static_cast<int_type>(val));
   Integer result;
   eval_convert_to(&result, t);
   return abs(result);
}

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4127)
#endif

template <class B>
inline void eval_gcd(B& result, const B& a, const B& b)
{
   using default_ops::eval_lsb;
   using default_ops::eval_is_zero;
   using default_ops::eval_get_sign;

   int shift;

   B u(a), v(b);

   int s = eval_get_sign(u);

   /* GCD(0,x) := x */
   if(s < 0)
   {
      u.negate();
   }
   else if(s == 0)
   {
      result = v;
      return;
   }
   s = eval_get_sign(v);
   if(s < 0)
   {
      v.negate();
   }
   else if(s == 0)
   {
      result = u;
      return;
   }

   /* Let shift := lg K, where K is the greatest power of 2
   dividing both u and v. */

   unsigned us = eval_lsb(u);
   unsigned vs = eval_lsb(v);
   shift = (std::min)(us, vs);
   eval_right_shift(u, us);
   eval_right_shift(v, vs);

   do
   {
      /* Now u and v are both odd, so diff(u, v) is even.
      Let u = min(u, v), v = diff(u, v)/2. */
      s = u.compare(v);
      if(s > 0)
         u.swap(v);
      if(s == 0)
         break;
      eval_subtract(v, u);
      vs = eval_lsb(v);
      eval_right_shift(v, vs);
   }
   while(true);

   result = u;
   eval_left_shift(result, shift);
}

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

template <class B>
inline void eval_lcm(B& result, const B& a, const B& b)
{
   typedef typename mpl::front<typename B::unsigned_types>::type ui_type;
   B t;
   eval_gcd(t, a, b);

   if(eval_is_zero(t))
   {
      result = static_cast<ui_type>(0);
   }
   else
   {
      eval_divide(result, a, t);
      eval_multiply(result, b);
   }
   if(eval_get_sign(result) < 0)
      result.negate();
}

}

template <class Backend, expression_template_option ExpressionTemplates>
inline typename enable_if_c<number_category<Backend>::value == number_kind_integer>::type
   divide_qr(const number<Backend, ExpressionTemplates>& x, const number<Backend, ExpressionTemplates>& y,
   number<Backend, ExpressionTemplates>& q, number<Backend, ExpressionTemplates>& r)
{
   using default_ops::eval_qr;
   eval_qr(x.backend(), y.backend(), q.backend(), r.backend());
}

template <class Backend, expression_template_option ExpressionTemplates, class tag, class A1, class A2, class A3, class A4>
inline typename enable_if_c<number_category<Backend>::value == number_kind_integer>::type
   divide_qr(const number<Backend, ExpressionTemplates>& x, const multiprecision::detail::expression<tag, A1, A2, A3, A4>& y,
   number<Backend, ExpressionTemplates>& q, number<Backend, ExpressionTemplates>& r)
{
   divide_qr(x, number<Backend, ExpressionTemplates>(y), q, r);
}

template <class tag, class A1, class A2, class A3, class A4, class Backend, expression_template_option ExpressionTemplates>
inline typename enable_if_c<number_category<Backend>::value == number_kind_integer>::type
   divide_qr(const multiprecision::detail::expression<tag, A1, A2, A3, A4>& x, const number<Backend, ExpressionTemplates>& y,
   number<Backend, ExpressionTemplates>& q, number<Backend, ExpressionTemplates>& r)
{
   divide_qr(number<Backend, ExpressionTemplates>(x), y, q, r);
}

template <class tag, class A1, class A2, class A3, class A4, class tagb, class A1b, class A2b, class A3b, class A4b, class Backend, expression_template_option ExpressionTemplates>
inline typename enable_if_c<number_category<Backend>::value == number_kind_integer>::type
   divide_qr(const multiprecision::detail::expression<tag, A1, A2, A3, A4>& x, const multiprecision::detail::expression<tagb, A1b, A2b, A3b, A4b>& y,
   number<Backend, ExpressionTemplates>& q, number<Backend, ExpressionTemplates>& r)
{
   divide_qr(number<Backend, ExpressionTemplates>(x), number<Backend, ExpressionTemplates>(y), q, r);
}

template <class Backend, expression_template_option ExpressionTemplates, class Integer>
inline typename enable_if<mpl::and_<is_integral<Integer>, mpl::bool_<number_category<Backend>::value == number_kind_integer> >, Integer>::type
   integer_modulus(const number<Backend, ExpressionTemplates>& x, Integer val)
{
   using default_ops::eval_integer_modulus;
   return eval_integer_modulus(x.backend(), val);
}

template <class tag, class A1, class A2, class A3, class A4, class Integer>
inline typename enable_if<mpl::and_<is_integral<Integer>, mpl::bool_<number_category<typename multiprecision::detail::expression<tag, A1, A2, A3, A4>::result_type>::value == number_kind_integer> >, Integer>::type
   integer_modulus(const multiprecision::detail::expression<tag, A1, A2, A3, A4>& x, Integer val)
{
   typedef typename multiprecision::detail::expression<tag, A1, A2, A3, A4>::result_type result_type;
   return integer_modulus(result_type(x), val);
}

template <class Backend, expression_template_option ExpressionTemplates>
inline typename enable_if_c<number_category<Backend>::value == number_kind_integer, unsigned>::type
   lsb(const number<Backend, ExpressionTemplates>& x)
{
   using default_ops::eval_lsb;
   return eval_lsb(x.backend());
}

template <class tag, class A1, class A2, class A3, class A4>
inline typename enable_if_c<number_category<typename multiprecision::detail::expression<tag, A1, A2, A3, A4>::result_type>::value == number_kind_integer, unsigned>::type
   lsb(const multiprecision::detail::expression<tag, A1, A2, A3, A4>& x)
{
   typedef typename multiprecision::detail::expression<tag, A1, A2, A3, A4>::result_type number_type;
   number_type n(x);
   using default_ops::eval_lsb;
   return eval_lsb(n.backend());
}

template <class Backend, expression_template_option ExpressionTemplates>
inline typename enable_if_c<number_category<Backend>::value == number_kind_integer, bool>::type
   bit_test(const number<Backend, ExpressionTemplates>& x, unsigned index)
{
   using default_ops::eval_bit_test;
   return eval_bit_test(x.backend(), index);
}

template <class tag, class A1, class A2, class A3, class A4>
inline typename enable_if_c<number_category<typename multiprecision::detail::expression<tag, A1, A2, A3, A4>::result_type>::value == number_kind_integer, bool>::type
   bit_test(const multiprecision::detail::expression<tag, A1, A2, A3, A4>& x, unsigned index)
{
   typedef typename multiprecision::detail::expression<tag, A1, A2, A3, A4>::result_type number_type;
   number_type n(x);
   using default_ops::eval_bit_test;
   return eval_bit_test(n.backend(), index);
}

template <class Backend, expression_template_option ExpressionTemplates>
inline typename enable_if_c<number_category<Backend>::value == number_kind_integer, number<Backend, ExpressionTemplates>&>::type
   bit_set(number<Backend, ExpressionTemplates>& x, unsigned index)
{
   using default_ops::eval_bit_set;
   eval_bit_set(x.backend(), index);
   return x;
}

template <class Backend, expression_template_option ExpressionTemplates>
inline typename enable_if_c<number_category<Backend>::value == number_kind_integer, number<Backend, ExpressionTemplates>&>::type
   bit_unset(number<Backend, ExpressionTemplates>& x, unsigned index)
{
   using default_ops::eval_bit_unset;
   eval_bit_unset(x.backend(), index);
   return x;
}

template <class Backend, expression_template_option ExpressionTemplates>
inline typename enable_if_c<number_category<Backend>::value == number_kind_integer, number<Backend, ExpressionTemplates>&>::type
   bit_flip(number<Backend, ExpressionTemplates>& x, unsigned index)
{
   using default_ops::eval_bit_flip;
   eval_bit_flip(x.backend(), index);
   return x;
}

namespace detail{

//
// Within powm, we need a type with twice as many digits as the argument type, define
// a traits class to obtain that type:
//
template <class Backend>
struct double_precision_type
{
   typedef Backend type;
};

//
// Calculate (a^p)%c:
//
template <class Backend>
void eval_powm(Backend& result, const Backend& a, const Backend& p, const Backend& c)
{
   using default_ops::eval_bit_test;
   using default_ops::eval_get_sign;
   using default_ops::eval_multiply;
   using default_ops::eval_modulus;
   using default_ops::eval_right_shift;

   typedef typename double_precision_type<Backend>::type double_type;
   typedef typename canonical<unsigned char, double_type>::type ui_type;

   double_type x, y(a), b(p), t;
   x = ui_type(1u);

   while(eval_get_sign(b) > 0)
   {
      if(eval_bit_test(b, 0))
      {
         eval_multiply(t, x, y);
         eval_modulus(x, t, c);
      }
      eval_multiply(t, y, y);
      eval_modulus(y, t, c);
      eval_right_shift(b, ui_type(1));
   }
   Backend x2(x);
   eval_modulus(result, x2, c);
}

template <class Backend, class Integer>
void eval_powm(Backend& result, const Backend& a, const Backend& p, Integer c)
{
   typedef typename double_precision_type<Backend>::type double_type;
   typedef typename canonical<unsigned char, double_type>::type ui_type;
   typedef typename canonical<Integer, double_type>::type i1_type;
   typedef typename canonical<Integer, Backend>::type i2_type;

   using default_ops::eval_bit_test;
   using default_ops::eval_get_sign;
   using default_ops::eval_multiply;
   using default_ops::eval_modulus;
   using default_ops::eval_right_shift;

   if(eval_get_sign(p) < 0)
   {
      BOOST_THROW_EXCEPTION(std::runtime_error("powm requires a positive exponent."));
   }

   double_type x, y(a), b(p), t;
   x = ui_type(1u);

   while(eval_get_sign(b) > 0)
   {
      if(eval_bit_test(b, 0))
      {
         eval_multiply(t, x, y);
         eval_modulus(x, t, static_cast<i1_type>(c));
      }
      eval_multiply(t, y, y);
      eval_modulus(y, t, static_cast<i1_type>(c));
      eval_right_shift(b, ui_type(1));
   }
   Backend x2(x);
   eval_modulus(result, x2, static_cast<i2_type>(c));
}

template <class Backend, class Integer>
typename enable_if<is_unsigned<Integer> >::type eval_powm(Backend& result, const Backend& a, Integer b, const Backend& c)
{
   typedef typename double_precision_type<Backend>::type double_type;
   typedef typename canonical<unsigned char, double_type>::type ui_type;

   using default_ops::eval_bit_test;
   using default_ops::eval_get_sign;
   using default_ops::eval_multiply;
   using default_ops::eval_modulus;
   using default_ops::eval_right_shift;

   double_type x, y(a), t;
   x = ui_type(1u);

   while(b > 0)
   {
      if(b & 1)
      {
         eval_multiply(t, x, y);
         eval_modulus(x, t, c);
      }
      eval_multiply(t, y, y);
      eval_modulus(y, t, c);
      b >>= 1;
   }
   Backend x2(x);
   eval_modulus(result, x2, c);
}

template <class Backend, class Integer>
typename enable_if<is_signed<Integer> >::type eval_powm(Backend& result, const Backend& a, Integer b, const Backend& c)
{
   if(b < 0)
   {
      BOOST_THROW_EXCEPTION(std::runtime_error("powm requires a positive exponent."));
   }
   eval_powm(result, a, static_cast<typename make_unsigned<Integer>::type>(b), c);
}

template <class Backend, class Integer1, class Integer2>
typename enable_if<is_unsigned<Integer1> >::type eval_powm(Backend& result, const Backend& a, Integer1 b, Integer2 c)
{
   typedef typename double_precision_type<Backend>::type double_type;
   typedef typename canonical<unsigned char, double_type>::type ui_type;
   typedef typename canonical<Integer1, double_type>::type i1_type;
   typedef typename canonical<Integer2, Backend>::type i2_type;

   using default_ops::eval_bit_test;
   using default_ops::eval_get_sign;
   using default_ops::eval_multiply;
   using default_ops::eval_modulus;
   using default_ops::eval_right_shift;

   double_type x, y(a), t;
   x = ui_type(1u);

   while(b > 0)
   {
      if(b & 1)
      {
         eval_multiply(t, x, y);
         eval_modulus(x, t, static_cast<i1_type>(c));
      }
      eval_multiply(t, y, y);
      eval_modulus(y, t, static_cast<i1_type>(c));
      b >>= 1;
   }
   Backend x2(x);
   eval_modulus(result, x2, static_cast<i2_type>(c));
}

template <class Backend, class Integer1, class Integer2>
typename enable_if<is_signed<Integer1> >::type eval_powm(Backend& result, const Backend& a, Integer1 b, Integer2 c)
{
   if(b < 0)
   {
      BOOST_THROW_EXCEPTION(std::runtime_error("powm requires a positive exponent."));
   }
   eval_powm(result, a, static_cast<typename make_unsigned<Integer1>::type>(b), c);
}

struct powm_func
{
   template <class T, class U, class V>
   void operator()(T& result, const T& b, const U& p, const V& m)const
   {
      eval_powm(result, b, p, m);
   }
};

}

template <class T, class U, class V>
inline typename enable_if<
   mpl::and_<
      mpl::bool_<number_category<T>::value == number_kind_integer>,
      mpl::or_<
         is_number<T>,
         is_number_expression<T>
      >,
      mpl::or_<
         is_number<U>,
         is_number_expression<U>,
         is_integral<U>
      >,
      mpl::or_<
         is_number<V>,
         is_number_expression<V>,
         is_integral<V>
      >
   >,
   detail::expression<detail::function, detail::powm_func, T, U, V> >::type
   powm(const T& b, const U& p, const V& mod)
{
   return detail::expression<detail::function, detail::powm_func, T, U, V>(
      detail::powm_func(), b, p, mod);
}

}} //namespaces

#endif


#include <cmath>

#include <boost/make_shared.hpp>

#include <gtest/gtest.h>

#include <sm/kinematics/EulerAnglesYawPitchRoll.hpp>

#include <aslam/backend/test/ErrorTermTestHarness.hpp>
#include <aslam/backend/EuclideanPoint.hpp>
#include <aslam/backend/RotationQuaternion.hpp>

#include "aslam/calibration/error-terms/ErrorTermAccelerometer.h"

using namespace aslam::backend;
using namespace aslam::calibration;
using namespace sm::kinematics;

TEST(AslamCalibrationTestSuite, testErrorTermAccelerometer) {
  EuclideanPoint a_m_mr(Eigen::Vector3d::Random());
  RotationQuaternion q_i_m(sm::kinematics::quatRandom());
  EuclideanPoint bias(Eigen::Vector3d::Random());
  EuclideanPoint gravity(Eigen::Vector3d::Random());

  ErrorTermAccelerometer aerr(a_m_mr.toExpression(),
                              q_i_m.toExpression(),
                              gravity.toExpression(),
                              bias.toExpression(),
                              Eigen::Vector3d::Random(), Eigen::Matrix3d::Identity());

  // test the error term
  try {
    ErrorTermTestHarness<3> harness(&aerr);
    harness.testAll(1e-5);
  }
  catch (const std::exception& e) {
    FAIL() << e.what();
  }
}