/*    Copyright (c) 2010-2019, 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.
 *
 *    References
 *      Musegaas, P. (2012). Optimization of Space Trajectories Including Multiple Gravity Assists
 *          and Deep Space Maneuvers. MSc Thesis, Delft University of Technology, Delft,
 *          The Netherlands.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include <tudat/astro/basic_astro/physicalConstants.h>
#include <tudat/basics/testMacros.h>

#include "tudat/astro/ephemerides/constantEphemeris.h"
#include "tudat/astro/mission_segments/transferLeg.h"
#include "tudat/astro/mission_segments/transferNode.h"

namespace tudat
{
namespace unit_tests
{

//! Test implementation of the swing-by leg within MGA trajectory model
BOOST_AUTO_TEST_SUITE( test_swingby_leg_mga )

//! Test delta-V computation
BOOST_AUTO_TEST_CASE( testVelocities )
{
    // Set tolerance. Due to iterative nature in the process, (much) higher accuracy cannot be
    // achieved.
    const double tolerance = 1.0e-6;

    // Expected test result based on the second leg of the ideal Cassini 1 trajectory as modelled
    // by GTOP software distributed and downloadable from the ESA website, or within the PaGMO
    // Astrotoolbox.
    const double expectedDeltaV = 1090.64622926316;
    const Eigen::Vector3d expectedArrivalVelocity (
                37952.8844553685, -14096.9656774702, -5753.51245833761 );

    // Specify the required parameters.
    // Set the planetary positions and velocities.

    // Specify the required parameters.
    // Set the planetary positions and velocities.
    const Eigen::Vector6d planet1State =
            ( Eigen::Vector6d( ) <<
              -35554348960.8278, -102574987127.178, 648696819.780156,
              32851.224953746, -11618.7310059974, -2055.04615890989 ).finished( );
    std::shared_ptr< ephemerides::Ephemeris > constantEphemeris1 =
            std::make_shared< ephemerides::ConstantEphemeris >( planet1State );

    const Eigen::Vector6d planet2State =
            ( Eigen::Vector6d( ) <<
              -35568329915.7073, -102569794949.529, 650816245.825226,
              TUDAT_NAN, TUDAT_NAN, TUDAT_NAN ).finished( );
    std::shared_ptr< ephemerides::Ephemeris > constantEphemeris2 =
            std::make_shared< ephemerides::ConstantEphemeris >( planet2State );

    // Set the time of flight, which has to be converted from JD (in GTOP) to seconds (in Tudat).
    const double timeOfFlight = 449.385873819743 * physical_constants::JULIAN_DAY;

    // Set the gravitational parameters
    const double sunGravitationalParameter = 1.32712428e20;

    using namespace mission_segments;
    tudat::mission_segments::UnpoweredUnperturbedTransferLeg transferLeg(
                constantEphemeris1, constantEphemeris2,
                sunGravitationalParameter );
    transferLeg.updateLegParameters( ( Eigen::VectorXd( 2 )<<0.0, timeOfFlight ).finished( ) );

    // Set velocity before and after swingby body
    Eigen::Vector3d velocityBeforePlanet1 (
                34216.4827530912, -15170.1440677825, 395.792122152361 );
    Eigen::Vector3d velocityAfterPlanet1 = transferLeg.getDepartureVelocity( );

    // Set the planet gravitational parameters
    const double planet1GravitationalParameter = 3.24860e14;

    //set the minimum pericenter radius
    const double minimumRadiusPlanet1 = 6351800;

    SwingbyWithFixedOutgoingVelocity transferNode(
                constantEphemeris1,
                planet1GravitationalParameter, minimumRadiusPlanet1,
                [=]( ){ return velocityBeforePlanet1; },
                [=]( ){ return velocityAfterPlanet1; } );
    transferNode.updateNodeParameters( ( Eigen::VectorXd( 1 ) << 0.0 ).finished( ) );

    BOOST_CHECK_CLOSE_FRACTION( transferLeg.getLegDeltaV( ), 0.0, tolerance );
    BOOST_CHECK_CLOSE_FRACTION( transferNode.getNodeDeltaV( ), expectedDeltaV, tolerance );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( transferLeg.getArrivalVelocity( ), expectedArrivalVelocity, tolerance );

    // Get data on 10 equispace points on trajectory
    std::map< double, Eigen::Vector6d > statesAlongTrajectory;
    transferLeg.getStatesAlongTrajectory( statesAlongTrajectory, 10 );

    // Check initial and final time on output list
    BOOST_CHECK_SMALL( statesAlongTrajectory.begin( )->first, 1.0E-14 );
    BOOST_CHECK_CLOSE_FRACTION( statesAlongTrajectory.rbegin( )->first, timeOfFlight, 1.0E-14 );

    // Check if Keplerian state (slow elements) is the same for each output point
    Eigen::Vector6d previousKeplerianState = Eigen::Vector6d::Constant( TUDAT_NAN );
    for( auto it : statesAlongTrajectory )
    {
        Eigen::Vector6d currentCartesianState = it.second;
        Eigen::Vector6d currentKeplerianState = tudat::orbital_element_conversions::convertCartesianToKeplerianElements(
                    currentCartesianState, sunGravitationalParameter );
        if( previousKeplerianState == previousKeplerianState )
        {
            TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                        ( previousKeplerianState.segment( 0, 5 ) ),
                        ( currentKeplerianState.segment( 0, 5 ) ),
                        1.0E-14 );

        }
        previousKeplerianState = currentKeplerianState;
    }

    // Check if output meets boundary conditions
    for( int i = 0; i < 3; i++ )
    {
        //TODO: Find out why tolerance needs to be so big for one of the legs
        BOOST_CHECK_SMALL( std::fabs( statesAlongTrajectory.begin( )->second( i ) - planet1State( i ) ), 20.0E3 );
        BOOST_CHECK_SMALL( std::fabs( statesAlongTrajectory.rbegin( )->second( i ) - planet2State( i ) ), 20.0E3 );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


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

  @copyright 2016 NumScale SAS

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

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

 /*!
    @ingroup group-operator
    Function object implementing multiplies capabilities

    Perform the product of two parameters of the same type.

    Infix notation can be used with operator '*',

    @par Semantic

    For any value @c a and @c b of type @c T,

    @code
    auto x = multiplies(a,b);
    @endcode

    or

    @code
    auto r = a*b;
    @endcode

    returns the product of @c a and @c b

    @see fma, fms, fnma, fnms

  **/
  Value multiplies(Value const & x, Value const& y);
} }
#endif

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

#endif


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <fstream>              // for file I/O
#include <boost/graph/graphviz.hpp>     // for read/write_graphviz()
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/lexical_cast.hpp>

namespace boost {
  enum graph_color_t { graph_color = 5556 };
  BOOST_INSTALL_PROPERTY(graph, color);
}

int
main()
{
  using namespace boost;
  typedef
    adjacency_list<vecS, vecS, directedS,
                   property<vertex_name_t, std::string>,
                   property<edge_color_t, std::string,
                            property<edge_weight_t, int> >,
                   property<graph_color_t, std::string> >
    g_dot_type;
  g_dot_type g_dot;

  dynamic_properties dp(ignore_other_properties);
  dp.property("node_id", get(vertex_name, g_dot));
  dp.property("label", get(edge_weight, g_dot));
  dp.property("color", get(edge_color, g_dot));
  dp.property("color", ref_property_map<g_dot_type*, std::string>(get_property(g_dot, graph_color)));
  {
    std::ifstream infile("figs/ospf-graph.dot");
    read_graphviz(infile, g_dot, dp);
  }

  typedef adjacency_list < vecS, vecS, directedS, no_property,
    property < edge_weight_t, int > > Graph;
  typedef graph_traits < Graph >::vertex_descriptor vertex_descriptor;
  Graph g(num_vertices(g_dot));
  graph_traits < g_dot_type >::edge_iterator ei, ei_end;
  for (boost::tie(ei, ei_end) = edges(g_dot); ei != ei_end; ++ei) {
    int weight = get(edge_weight, g_dot, *ei);
    property < edge_weight_t, int >edge_property(weight);
    add_edge(source(*ei, g_dot), target(*ei, g_dot), edge_property, g);
  }

  vertex_descriptor router_six;
  graph_traits < g_dot_type >::vertex_iterator vi, vi_end;
  for (boost::tie(vi, vi_end) = vertices(g_dot); vi != vi_end; ++vi)
    if ("RT6" == get(vertex_name, g_dot, *vi)) {
      router_six = *vi;
      break;
    }

  std::vector < vertex_descriptor > parent(num_vertices(g));
  // All vertices start out as there own parent
  typedef graph_traits < Graph >::vertices_size_type size_type;
  for (size_type p = 0; p < num_vertices(g); ++p)
    parent[p] = p;

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  std::vector<int> distance(num_vertices(g));
  property_map<Graph, edge_weight_t>::type weightmap = get(edge_weight, g);
  property_map<Graph, vertex_index_t>::type indexmap = get(vertex_index, g);
  dijkstra_shortest_paths
    (g, router_six, &parent[0], &distance[0], weightmap,
     indexmap, std::less<int>(), closed_plus<int>(),
     (std::numeric_limits<int>::max)(), 0, default_dijkstra_visitor());
#else
  dijkstra_shortest_paths(g, router_six, predecessor_map(&parent[0]));
#endif

  graph_traits < g_dot_type >::edge_descriptor e;
  for (size_type i = 0; i < num_vertices(g); ++i)
    if (parent[i] != i) {
      e = edge(parent[i], i, g_dot).first;
      put(edge_color, g_dot, e, "black");
    }

  get_property(g_dot, graph_color) = "grey";
  {
    std::ofstream outfile("figs/ospf-sptree.dot");
    write_graphviz_dp(outfile, g_dot, dp);
  }

  std::ofstream rtable("routing-table.dat");
  rtable << "Dest    Next Hop    Total Cost" << std::endl;
  for (boost::tie(vi, vi_end) = vertices(g_dot); vi != vi_end; ++vi)
    if (parent[*vi] != *vi) {
      rtable << get(vertex_name, g_dot, *vi) << "    ";
      vertex_descriptor v = *vi, child;
      int path_cost = 0;
      property_map < Graph, edge_weight_t >::type
        weight_map = get(edge_weight, g);
      do {
        path_cost += get(weight_map, edge(parent[v], v, g).first);
        child = v;
        v = parent[v];
      } while (v != parent[v]);
      rtable << get(vertex_name, g_dot, child) << "     ";
      rtable << path_cost << std::endl;

    }

  return EXIT_SUCCESS;
}


#define BOOST_TEST_MODULE spherical_representation_test

#include <boost/test/unit_test.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/prefixes.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/astronomy/coordinate/arithmetic.hpp>
#include <boost/astronomy/coordinate/representation.hpp>

using namespace std;
using namespace boost::astronomy::coordinate;
using namespace boost::units::si;
using namespace boost::geometry;
using namespace boost::units;
namespace bud = boost::units::degree;

BOOST_AUTO_TEST_SUITE(spherical_representation_constructors)

BOOST_AUTO_TEST_CASE(spherical_representation_default_constructor)
{
    //using set functions
    spherical_representation<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>,
    quantity<si::length>> point1;
    point1.set_lat_lon_dist(45.0 * bud::degrees, 18.0 * bud::degrees, 3.5 * meters);
    BOOST_CHECK_CLOSE(point1.get_lat().value(), 45.0, 0.001);
    BOOST_CHECK_CLOSE(point1.get_lon().value(), 18.0, 0.001);
    BOOST_CHECK_CLOSE(point1.get_dist().value(), 3.5, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point1.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_representation_quantities_constructor)
{
    //checking construction from value
    auto point1 = make_spherical_representation
    (15.0 * bud::degrees, 39.0 * bud::degrees, 3.0*si::centi*meter);
    BOOST_CHECK_CLOSE(point1.get_lat().value(), 15.0, 0.001);
    BOOST_CHECK_CLOSE(point1.get_lon().value(), 39.0, 0.001);
    BOOST_CHECK_CLOSE(point1.get_dist().value(), 3.0, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point1.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_dist()), quantity<decltype(si::centi*meter)>>::value));

    spherical_representation<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>,
    quantity<si::length>> point2(1.5 * bud::degrees, 9.0 * bud::degrees, 3.0 * meter);
    BOOST_CHECK_CLOSE(point2.get_lat().value(), 1.5, 0.001);
    BOOST_CHECK_CLOSE(point2.get_lon().value(), 9.0, 0.001);
    BOOST_CHECK_CLOSE(point2.get_dist().value(), 3, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point2.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_representation_copy_constructor)
{
    //checking construction from value
    auto point1 = make_spherical_representation
    (15.0 * bud::degrees, 30.0 * bud::degrees, 3.0*si::centi*meter);
    BOOST_CHECK_CLOSE(point1.get_lat().value(), 15.0, 0.001);
    BOOST_CHECK_CLOSE(point1.get_lon().value(), 30.0, 0.001);
    BOOST_CHECK_CLOSE(point1.get_dist().value(), 3, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point1.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_dist()), quantity<decltype(si::centi*meter)>>::value));

    //copy constructor
    auto point2 = make_spherical_representation(point1);
    BOOST_CHECK_CLOSE(point1.get_lat().value(), point2.get_lat().value(), 0.001);
    BOOST_CHECK_CLOSE(point1.get_lon().value(), point2.get_lon().value(), 0.001);
    BOOST_CHECK_CLOSE(point1.get_dist().value(), point2.get_dist().value(), 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point2.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_dist()), quantity<decltype(si::centi*meter)>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_representation_copy_constructor_with_different_units)
{
    //checking construction from value
    auto point1 = make_spherical_representation
    (15.0 * bud::degrees, 10.0 * bud::degrees, 3.0*si::centi*meter);
    BOOST_CHECK_CLOSE(point1.get_lat().value(), 15.0, 0.001);
    BOOST_CHECK_CLOSE(point1.get_lon().value(), 10.0, 0.001);
    BOOST_CHECK_CLOSE(point1.get_dist().value(), 3, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point1.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_dist()), quantity<decltype(si::centi*meter)>>::value));

    //Conversion from one unit type to other
    auto point2 = make_spherical_representation
    <double, quantity<bud::plane_angle>, quantity<bud::plane_angle>, quantity<si::length>>(point1);
    BOOST_CHECK_CLOSE(point2.get_lat().value(), 15.0, 0.001);
    BOOST_CHECK_CLOSE(point2.get_lon().value(), 10.0, 0.001);
    BOOST_CHECK_CLOSE(point2.get_dist().value(), 0.03, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point2.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_representation_geometry_point_constructor)
{
    //constructing from boost::geometry::model::point
    model::point<double, 3, cs::cartesian> model_point(30, 60, 10);
    auto point1 = make_spherical_representation
    <double,quantity<bud::plane_angle>,quantity<bud::plane_angle>,quantity<si::length>>(model_point);
    BOOST_CHECK_CLOSE(point1.get_lat().value(), 63.434948822922, 0.001);
    BOOST_CHECK_CLOSE(point1.get_lon().value(), 81.521286852914, 0.001);
    BOOST_CHECK_CLOSE(point1.get_dist().value(), 67.823299831253, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point1.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_dist()), quantity<si::length>>::value));

    spherical_representation<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>,
    quantity<si::length>> point2(model_point);
    BOOST_CHECK_CLOSE(point2.get_lat().value(), 63.434948822922, 0.001);
    BOOST_CHECK_CLOSE(point2.get_lon().value(), 81.521286852914, 0.001);
    BOOST_CHECK_CLOSE(point2.get_dist().value(), 67.823299831253, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point2.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_representation_conversion_from_cartesian_representation)
{
    //constructing from spherical representation
    auto cartesian_point = make_cartesian_representation(20.0 * meters, 60.0 * meters, 1.0 * meter);
    auto point1 = make_spherical_representation(cartesian_point);
    BOOST_CHECK_CLOSE(point1.get_lat().value(), 1.2490457723983, 0.001);
    BOOST_CHECK_CLOSE(point1.get_lon().value(), 1.5549862559121, 0.001);
    BOOST_CHECK_CLOSE(point1.get_dist().value(), 63.253458403474, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point1.get_lat()), quantity<si::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_lon()), quantity<si::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_representation_conversion_from_spherical_equatorial_representation)
{
    //constructing from spherical_equitorial representation
    auto spherical_equatorial_point = make_spherical_equatorial_representation
    (0.523599 * si::radian, 60.0 * bud::degrees, 1.0 * meter);
    auto point2 = make_spherical_representation(spherical_equatorial_point);
    BOOST_CHECK_CLOSE(point2.get_lat().value(), 0.523599, 0.001);
    BOOST_CHECK_CLOSE(point2.get_lon().value(), 0.523598776, 0.001);
    BOOST_CHECK_CLOSE(point2.get_dist().value(), 1.0, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point2.get_lat()), quantity<si::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_lon()), quantity<si::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(spherical_representation_operators)

BOOST_AUTO_TEST_CASE(spherical_representation_addition_operator)
{
    auto point1 = make_spherical_representation(15.0 * bud::degrees, 30.0 * bud::degrees, 10.0 * meters);
    auto point2 = make_spherical_representation(30.0 * bud::degrees, 45.0 * bud::degrees, 20.0 * meters);

    auto sum = make_spherical_representation(point1 + point2);

    BOOST_CHECK_CLOSE(sum.get_lat().value(), 26.097805456, 0.001);
    BOOST_CHECK_CLOSE(sum.get_lon().value(), 39.826115507, 0.001);
    BOOST_CHECK_CLOSE(sum.get_dist().value(), 29.6909332103, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(sum.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(sum.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(sum.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(spherical_representation_arithmetic_functions)

// BOOST_AUTO_TEST_CASE(spherical_representation_cross_product)
// {
//     auto point1 = make_spherical_representation(3.0 * bud::degrees, 50.0 * bud::degrees, 40.0 * meters);
//     auto point2 = make_spherical_representation(30.0 * bud::degrees, 45.0 * bud::degrees, 14.0 * meters);

//     auto result = cross(point1, point2);

//     BOOST_CHECK_CLOSE(result.get_lat().value(), -143.4774246228, 0.001);
//     BOOST_CHECK_CLOSE(result.get_lon().value(), 45.186034054587, 0.001);
//     BOOST_CHECK_CLOSE(result.get_dist().value(), 195.39050840581, 0.001);

//     //checking whether quantity stored is as expected or not
//     BOOST_TEST((std::is_same<decltype(result.get_lat()), quantity
//         <bu::multiply_typeof_helper<si::length, si::length>::type>>::value));
//     BOOST_TEST((std::is_same<decltype(result.get_lon()), quantity
//         <bu::multiply_typeof_helper<si::length, si::length>::type>>::value));
//     BOOST_TEST((std::is_same<decltype(result.get_dist()), quantity
//         <bu::multiply_typeof_helper<si::length, si::length>::type>>::value));
// }

BOOST_AUTO_TEST_CASE(spherical_representation_dot_product)
{
    auto point1 = make_spherical_representation(3.0 * bud::degrees, 50.0 * bud::degrees, 40.0 * meters);
    auto point2 = make_spherical_representation(30.0 * bud::degrees, 45.0 * bud::degrees, 14.0 * meters);

    auto result = dot(point1, point2);

    BOOST_CHECK_CLOSE(result.value(), 524.807154, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(result), quantity
        <bu::multiply_typeof_helper<si::length, si::length>::type>>::value));
}

// BOOST_AUTO_TEST_CASE(spherical_representation_unit_vector)
// {
//     auto point1 = make_spherical_representation(25.0 * bud::degrees, 30.0 * bud::degrees, 90.0*meter);

//     auto result = boost::astronomy::coordinate::unit_vector(point1);

//     BOOST_CHECK_CLOSE(result.get_lat().value(), 25.0, 0.001);
//     BOOST_CHECK_CLOSE(result.get_lon().value(), 30.0, 0.001);
//     BOOST_CHECK_CLOSE(result.get_dist().value(), 1, 0.001);

//     //checking whether quantity stored is as expected or not
//     BOOST_TEST((std::is_same<decltype(result.get_lat()), quantity<bud::plane_angle>>::value));
//     BOOST_TEST((std::is_same<decltype(result.get_lon()), quantity<bud::plane_angle>>::value));
//     BOOST_TEST((std::is_same<decltype(result.get_dist()), quantity<si::length>>::value));
// }

BOOST_AUTO_TEST_CASE(spherical_representation_magnitude)
{
    auto point1 = make_spherical_representation(25.0 * bud::degrees, 36.0 * bud::degrees, 9.0 * meters);

    auto result = boost::astronomy::coordinate::magnitude(point1);

    BOOST_CHECK_CLOSE(result.value(), 9, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(result), quantity<si::length>>::value));

}

BOOST_AUTO_TEST_CASE(spherical_representation_sum)
{
    auto point1 = make_spherical_representation(15.0 * bud::degrees, 30.0 * bud::degrees, 10.0 * meters);
    auto point2 = make_spherical_representation(30.0 * bud::degrees, 45.0 * bud::degrees, 20.0 * meters);

    auto result = boost::astronomy::coordinate::sum(point1, point2);

    BOOST_CHECK_CLOSE(result.get_lat().value(), 26.097805456, 0.001);
    BOOST_CHECK_CLOSE(result.get_lon().value(), 39.826115507, 0.001);
    BOOST_CHECK_CLOSE(result.get_dist().value(), 29.6909332103, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(result.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(result.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(result.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_representation_mean)
{
    auto point1 = make_spherical_representation(15.0 * bud::degrees, 30.0 * bud::degrees, 10.0 * meter);
    auto point2 = make_spherical_representation(30.0 * bud::degrees, 45.0 * bud::degrees, 20.0 * meter);

    auto result = boost::astronomy::coordinate::mean(point1, point2);

    BOOST_CHECK_CLOSE(result.get_lat().value(), 26.097805456543, 0.001);
    BOOST_CHECK_CLOSE(result.get_lon().value(), 39.826115384099, 0.001);
    BOOST_CHECK_CLOSE(result.get_dist().value(), 14.845466643593, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(result.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(result.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(result.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_SUITE_END()


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_FROBENIUS_NORM_INCLUDE
#define MTL_FROBENIUS_NORM_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/is_row_major.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/property_map.hpp>
#include <boost/numeric/mtl/operation/max_of_sums.hpp>
#include <boost/numeric/mtl/operation/squared_abs.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace mat {

/// Frobenius norm, i.e. square root of sum of squares of all entries: \f$\sqrt{\sum_i \sum_j|a_{ij}|^2}\f$.
template <typename Matrix>
typename RealMagnitude<typename Collection<Matrix>::value_type>::type
inline frobenius_norm(const Matrix& matrix)
{
    vampir_trace<3010> tracer;
    using std::sqrt; using std::abs; using math::zero;
    namespace traits = mtl::traits;
    typename traits::const_value<Matrix>::type     value(matrix);

    typedef typename Collection<Matrix>::value_type   value_type;
    typedef typename RealMagnitude<value_type>::type  real_type;
    real_type ref, sum= zero(ref);

    typedef typename traits::range_generator<tag::major, Matrix>::type     cursor_type;
    typedef typename traits::range_generator<tag::nz, cursor_type>::type   icursor_type;

    for (cursor_type cursor = begin<tag::major>(matrix), cend = end<tag::major>(matrix); cursor != cend; ++cursor)
	for (icursor_type icursor = begin<tag::nz>(cursor), icend = end<tag::nz>(cursor); icursor != icend; ++icursor)
	    sum+= squared_abs(value(*icursor));
    return sqrt(sum);
}

}} // namespace mtl::matrix

#endif // MTL_FROBENIUS_NORM_INCLUDE


 /* Copyright (c) 2016, Michal Startek
  *
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without modification,
  * are permitted provided that the following conditions are met:
  *
  *    * Redistributions of source code must retain the above copyright notice,
  *      this list of conditions and the following disclaimer.
  *    * Redistributions in binary form must reproduce the above copyright notice,
  *      this list of conditions and the following disclaimer in the documentation
  *      and/or other materials provided with the distribution.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER
  * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */

#include <random>
#include <boost/random.hpp>
#include <boost/random/random_device.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/binomial_distribution.hpp>
#include <unordered_map>

#define DO_CLOCK(str) { std::cout << str << ": " << ((double)(clock() - timer))/CLOCKS_PER_SEC << std::endl; timer = clock(); }

std::random_device rd;
std::mt19937 gen(rd());
boost::random::mt19937 rng(rd());

std::uniform_real_distribution<double> stdunif(0.0, 1.0);

inline double beta_1_b(double b, std::mt19937 rdev = gen)
{
	return 1.0 - pow(stdunif(rdev), 1.0/b);
}

inline int binom(int tries, double succ_prob, boost::random::mt19937 rdev = rng)
{
	if (succ_prob >= 1.0)
		return tries;
	boost::random::binomial_distribution<> bd(tries, succ_prob);
	boost::variate_generator<boost::mt19937&, boost::random::binomial_distribution<> > var_b(rdev, bd);
	return var_b();
}

void sample(const double* probs, unsigned int* samplespace, unsigned int samples, double switchover = 1.0, double total_prob = 1.0, std::mt19937 crdev = gen, boost::random::mt19937 boost_rdev = rng)
/* Generates a random sample from population 0..n-1 with probabilities probs, and saves it to
 * table sample (which must be of size equal to samples parameter). Parameter switchover controls
 * preference between beta and binomial modes.
 */
{
	double pprob = 0.0;
	double cprob = 0.0;
	unsigned int pidx = 0;
	unsigned int sampleidx = 0;
	while(samples > 0)
	{
		pprob += probs[pidx];
		while(((pprob - cprob) * samples / (total_prob - cprob)) < switchover)
		{
			cprob += beta_1_b(samples, crdev) * (total_prob - cprob);
			while(pprob < cprob)
				pprob += probs[++pidx];
			samplespace[sampleidx++] = pidx;
			samples--;
			if(samples == 0)
				break;
		}
		if(samples == 0)
			break;
		unsigned int nrtaken = binom(samples, (pprob-cprob)/(total_prob-cprob)), boost_rdev;
		for(unsigned int i=0; i<nrtaken; i++)
			samplespace[sampleidx++] = pidx;
		samples -= nrtaken;
		pidx++;
		cprob = pprob;
	}
}

void sample_cntr(const double* probs, unsigned int* sample, unsigned int popsize, unsigned int samples, double switchover = 1.0)
/* Generates a random sample from population 0..n-1 with probabilities probs, and saves counts to
 * the table sample (which must be of size popsize). Parameter switchover controls
 * preference between beta and binomial modes.
 */
{
	memset(sample, 0, sizeof(int)*popsize);
        double pprob = 0.0;
        double cprob = 0.0;
        unsigned int pidx = 0;
        while(samples > 0)
        {
                pprob += probs[pidx];
                while(((pprob - cprob) * samples / (1.0 - cprob)) < switchover)
                {
                        cprob += beta_1_b(samples) * (1.0 - cprob);
                        while(pprob < cprob)
                                pprob += probs[++pidx];
			sample[pidx] += 1;
                        samples--;
                        if(samples == 0)
                                break;
                }
                if(samples == 0)
                        break;
                unsigned int nrtaken = binom(samples, (pprob-cprob)/(1.0-cprob));
		sample[pidx] += nrtaken;
                samples -= nrtaken;
                pidx++;
                cprob = pprob;
        }
}

std::unordered_map<int, int>* sample_ht(const double* probs, unsigned int samples, double switchover)
/* Generates a random sample from population 0..n-1 with probabilities probs, and returns a hashtable
 * containing the counts. Parameter switchover controls preference between beta and binomial modes.
 */
{
	std::unordered_map<int, int>* sampl_multiset = new std::unordered_map<int, int>();
        double pprob = 0.0;
        double cprob = 0.0;
        unsigned int pidx = 0;
        while(samples > 0)
        {
                pprob += probs[pidx];
                while(((pprob - cprob) * samples / (1.0 - cprob)) < switchover)
                {
                        cprob += beta_1_b(samples) * (1.0 - cprob);
                        while(pprob < cprob)
                                pprob += probs[++pidx];
			if(sampl_multiset->find(pidx) == sampl_multiset->end())
				(*sampl_multiset)[pidx] = 1;
			else
				(*sampl_multiset)[pidx] += 1;
                        samples--;
                        if(samples == 0)
                                break;
                }
                if(samples == 0)
                        break;
                unsigned int nrtaken = binom(samples, (pprob-cprob)/(1.0-cprob));
		if(nrtaken > 0)
		{
			if(sampl_multiset->find(pidx) == sampl_multiset->end())
				(*sampl_multiset)[pidx] = nrtaken;
			else
				(*sampl_multiset)[pidx] += nrtaken;
		}
                samples -= nrtaken;
                pidx++;
                cprob = pprob;
        }
        return sampl_multiset;

}

/*
 * The following is a multithreaded implementation of the above
 */

#ifdef THREADS
#include <pthread.h>
#include <sstream>

struct threadargs_t
{
        const double* probs;
	unsigned int probsstart;
	unsigned int probsend;
        unsigned int* samplespace;
        unsigned int totalsamples;
        double switchover;
        double* fragment_probs;
	unsigned int* fragment_sizes;
        unsigned int thread_id;
	pthread_barrier_t* barrier;
	unsigned int no_threads;
	unsigned int pidx;
};

template<typename T> void print_array(T* array, unsigned int size)
{
	std::cout << "-------------------------------------" << std::endl;
	for(int ii=0; ii<size; ii++)
		std::cout << array[ii] << " ";
	std::cout << std::endl << "-------------------------------------" << std::endl;
}

void sample_threadfunc(threadargs_t* args)
{
	clock_t timer = clock();
	std::random_device rd;
	std::mt19937 gen(rd());
	boost::random::mt19937 rng(rd());

	if(args->thread_id == 0)
		DO_CLOCK("RNG init");

	double myprobs = 0.0;
	const double* mprobs = args->probs;
	unsigned int mprobsstart = args->probsstart;
	unsigned int mprobsend = args->probsend;
	for (int ii = mprobsstart; ii < mprobsend; ii++)
		myprobs += args->probs[ii];

	if(args->thread_id == 0)
		DO_CLOCK("After MT init")

	args->fragment_probs[args->thread_id] = myprobs;

	int leader = pthread_barrier_wait(args->barrier);

	if(args->thread_id == 0)
		DO_CLOCK("First barrier");

	if(leader == PTHREAD_BARRIER_SERIAL_THREAD)
		/* Deliberately not using switchover arg. Even if input has weird distribution which justifies it,
		 * this distribution is different. */
		sample_cntr(args->fragment_probs, args->fragment_sizes, args->no_threads, args->totalsamples);

	pthread_barrier_wait(args->barrier);

	if(args->thread_id == 0)
		DO_CLOCK("Sample_cntr, second barrier");

	/* Probably faster to compute this in each thread from scratch, like this,
	 * than to synchronise again just to compute this... */
	unsigned int* samplespace = args->samplespace;
	for(int ii=0; ii<args->thread_id; ii++)
		samplespace += args->fragment_sizes[ii];

	unsigned int samples = args->fragment_sizes[args->thread_id];
	double switchover = args->switchover;

	/* Sadly, not *quite* the same as the single-threaded version above... */

	#if 0
	double correction = 1.0/myprobs;
        double pprob = 0.0;
        double cprob = 0.0;
	const double* probs = args->probs;

        unsigned int pidx = args->probsstart;
        unsigned int sampleidx = 0;
        while(samples > 0)
        {
                pprob += probs[pidx];
                while(((pprob - cprob) * samples / (myprobs - cprob)) < switchover)
                {
                        cprob += beta_1_b(samples, gen) * (myprobs - cprob);
                        while(pprob < cprob)
                                pprob += probs[++pidx]*correction;
                        samplespace[sampleidx++] = pidx;
                        samples--;
                        if(samples == 0)
                                break;
                }
                if(samples == 0)
                        break;
                unsigned int nrtaken = binom(samples, (pprob-cprob)/(myprobs-cprob), rng);
                for(unsigned int i=0; i<nrtaken; i++)
                        samplespace[sampleidx++] = pidx;
                samples -= nrtaken;
                pidx++;
                cprob = pprob;
        }
	#endif

	std::stringstream s;
	s << "Calling sample with args: probs: " << args->probs+args->probsstart << " samplespace: " << samplespace << " samples: " << samples << " switchover: " << args->switchover << " totalprobs: " << myprobs << std::endl;
	std::cout << s.str();
	sample(args->probs+args->probsstart, samplespace, samples, args->switchover, myprobs);

	if(args->thread_id == 0)
	        DO_CLOCK("After main loop");

}

void sample_multithreaded(const double* probs, unsigned int* samplespace, unsigned int popsize, unsigned int samples, double switchover = 1.0, unsigned int no_threads = 1)
{
	clock_t timer = clock();

        if(1 || no_threads == 1)
	{
		std::cout << "Calling sample with args: probs: " << probs << " samplespace: " << samplespace << " samples: " << samples << " switchover: " << switchover << " totalprobs: 1.0"  << std::endl;
                //return sample(probs, samplespace, samples, switchover);
		sample(probs, samplespace, samples, switchover);
		DO_CLOCK("Single-threaded")
	}
        threadargs_t* threadargs = new threadargs_t[no_threads];
        double* fragment_probs = new double[no_threads];
	unsigned int* fragment_sizes = new unsigned int[no_threads];

        pthread_barrier_t barrier;
        pthread_barrier_init(&barrier, nullptr, no_threads);

        pthread_t* thread_ids = new pthread_t[no_threads];

	unsigned int perthread = popsize / no_threads;
	if(popsize % no_threads > 0)
		perthread++;

	DO_CLOCK("MT_init");
        for(unsigned int ii=0; ii<no_threads; ii++)
        {
                threadargs[ii].thread_id = ii;
		threadargs[ii].probs = probs;
		threadargs[ii].probsstart = perthread*ii;
		threadargs[ii].probsend = std::min(perthread*(ii+1), popsize);
		threadargs[ii].samplespace = samplespace;
		threadargs[ii].totalsamples = samples;
		threadargs[ii].switchover = 1.0;
		threadargs[ii].fragment_probs = fragment_probs;
		threadargs[ii].fragment_sizes = fragment_sizes;
		threadargs[ii].barrier = &barrier;
		threadargs[ii].no_threads = no_threads;

		assert(pthread_create(&thread_ids[ii], NULL, (void* (*)(void*)) sample_threadfunc, &threadargs[ii]) == 0);

        }
	DO_CLOCK("MT: spawned threads");

	for(unsigned int ii=0; ii<no_threads; ii++)
	{
		pthread_join(thread_ids[ii], nullptr);
	}
	DO_CLOCK("Multithreaded body");

	delete[] threadargs;
	delete[] fragment_probs;
	delete[] fragment_sizes;
	delete[] thread_ids;
	pthread_barrier_destroy(&barrier);

}
#endif /* THREADS */

int main(int argc, char** argv)
{
	int popsize = atoi(argv[1]);
	int samplesize = atoi(argv[2]);
	int threads = atoi(argv[3]);

	double* probs = new double[popsize];
	unsigned int* samplespace = new unsigned int[samplesize];

	double csum = 0.0;
	for (int ii=0; ii<popsize; ii++)
	{
		probs[ii] = stdunif(gen);
		csum += probs[ii];
	}
	for (int ii=0; ii<popsize; ii++)
		probs[ii] /= csum;

	clock_t timer = clock();
	sample_multithreaded(probs, samplespace, popsize, samplesize, 1.0, threads);
	std::cout << ((double)(clock() - timer))/CLOCKS_PER_SEC << std::endl;

//	for (int ii=0; ii<samplesize; ii++)
//		std::cout << samplespace[ii] << std::endl;

	delete[] probs;
	delete[] samplespace;

}


#ifdef MEX

#include <igl/copyleft/cgal/signed_distance_isosurface.h>
#include <igl/copyleft/offset_surface.h>
#include <igl/matlab/validate_arg.h>
#include <igl/matlab/MexStream.h>
#include <igl/matlab/mexErrMsgTxt.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/C_STR.h>

#include <mex.h>
#include <Eigen/Dense>
#include <iostream>

#include <cstring>

enum ContouringMethod
{
  CONTOURING_METHOD_MARCHING_CUBES = 0,
  CONTOURING_METHOD_CGAL = 1,
  NUM_CONTOURING_METHOD = 2,
};

void parse_rhs(
  const int nrhs,
  const mxArray *prhs[],
  Eigen::MatrixXd & IV,
  Eigen::MatrixXi & IF,
  double & level,
  double & angle_bound,
  double & radius_bound,
  double & distance_bound,
  igl::SignedDistanceType & type,
  ContouringMethod & contouring_method,
  int & grid_size)
{
  using namespace std;
  using namespace igl;
  using namespace igl::matlab;
  using namespace Eigen;
  mexErrMsgTxt(nrhs >= 2, "The number of input arguments must be >=2.");

  const int dim = mxGetN(prhs[0]);
  mexErrMsgTxt(dim == 3,
    "Mesh vertex list must be #V by 3 list of vertex positions");

  parse_rhs_double(prhs,IV);
  parse_rhs_index(prhs+1,IF);

  // defaults
  level = 0.0;
  angle_bound = 28.0;
  double bbd = 1.0;
  // Radius and distance in terms of fraction of bbd
  if(IV.size() > 0)
  {
    bbd = (IV.colwise().maxCoeff()-IV.colwise().minCoeff()).norm();
  }
  radius_bound = 0.02*bbd;
  distance_bound = 0.02*bbd;
  type = SIGNED_DISTANCE_TYPE_DEFAULT;
  contouring_method = CONTOURING_METHOD_MARCHING_CUBES;
  grid_size = 40;

  {
    int i = 2;
    while(i<nrhs)
    {
      mexErrMsgTxt(mxIsChar(prhs[i]),"Parameter names should be strings");
      // Cast to char
      const char * name = mxArrayToString(prhs[i]);
      if(strcmp("Level",name) == 0)
      {
        validate_arg_double(i,nrhs,prhs,name);
        validate_arg_scalar(i,nrhs,prhs,name);
        level = (double)*mxGetPr(prhs[++i]);
      }else if(strcmp("GridSize",name) == 0)
      {
        validate_arg_double(i,nrhs,prhs,name);
        validate_arg_scalar(i,nrhs,prhs,name);
        grid_size = (int)*mxGetPr(prhs[++i]);
      }else if(strcmp("AngleBound",name) == 0)
      {
        validate_arg_double(i,nrhs,prhs,name);
        validate_arg_scalar(i,nrhs,prhs,name);
        angle_bound = (double)*mxGetPr(prhs[++i]);
      }else if(strcmp("RadiusBound",name) == 0)
      {
        validate_arg_double(i,nrhs,prhs,name);
        validate_arg_scalar(i,nrhs,prhs,name);
        radius_bound = ((double)*mxGetPr(prhs[++i])) * bbd;
      }else if(strcmp("DistanceBound",name) == 0)
      {
        validate_arg_double(i,nrhs,prhs,name);
        validate_arg_scalar(i,nrhs,prhs,name);
        distance_bound = ((double)*mxGetPr(prhs[++i])) * bbd;
      }else if(strcmp("ContouringMethod",name) == 0)
      {
        validate_arg_char(i,nrhs,prhs,name);
        const char * contouring_name = mxArrayToString(prhs[++i]);
        if(strcmp("cgal",contouring_name)==0)
        {
          contouring_method = CONTOURING_METHOD_CGAL;
        }else if(strcmp("marching_cubes",contouring_name)==0)
        {
          contouring_method = CONTOURING_METHOD_MARCHING_CUBES;
        }
      }else if(strcmp("SignedDistanceType",name) == 0)
      {
        validate_arg_char(i,nrhs,prhs,name);
        const char * type_name = mxArrayToString(prhs[++i]);
        if(strcmp("pseudonormal",type_name)==0)
        {
          type = igl::SIGNED_DISTANCE_TYPE_PSEUDONORMAL;
        }else if(strcmp("winding_number",type_name)==0)
        {
          type = igl::SIGNED_DISTANCE_TYPE_WINDING_NUMBER;
        }else if(strcmp("default",type_name)==0)
        {
          type = igl::SIGNED_DISTANCE_TYPE_DEFAULT;
        }else if(strcmp("unsigned",type_name)==0)
        {
          type = igl::SIGNED_DISTANCE_TYPE_UNSIGNED;
        }else
        {
          mexErrMsgTxt(false,C_STR("Unknown SignedDistanceType: "<<type_name));
        }
      }else
      {
        mexErrMsgTxt(false,"Unknown parameter");
      }
      i++;
    }
  }

  if(type != igl::SIGNED_DISTANCE_TYPE_UNSIGNED)
  {
    mexErrMsgTxt(dim == mxGetN(prhs[1]),
      "Mesh \"face\" simplex size must equal dimension");
  }
}

void mexFunction(
  int nlhs, mxArray *plhs[],
  int nrhs, const mxArray *prhs[])
{
  using namespace std;
  using namespace Eigen;
  using namespace igl;
  using namespace igl::matlab;
  using namespace igl::copyleft::cgal;

  igl::matlab::MexStream mout;
  std::streambuf *outbuf = cout.rdbuf(&mout);
  //mexPrintf("Compiled at %s on %s\n",__TIME__,__DATE__);

  MatrixXd IV,V;
  MatrixXi IF,F;
  double level,angle_bound,radius_bound,distance_bound;
  SignedDistanceType type;
  ContouringMethod contouring_method;
  int grid_size;
  parse_rhs(
    nrhs,prhs,
    IV,IF,
    level,angle_bound,radius_bound,distance_bound,type,
    contouring_method, grid_size);
  Eigen::MatrixXd GV;
  Eigen::RowVector3i side;
  Eigen::VectorXd S;
  switch(contouring_method)
  {
    default:
    case CONTOURING_METHOD_MARCHING_CUBES:
      {
        igl::copyleft::offset_surface(IV,IF,level,grid_size,type,V,F,GV,side,S);
      }
      break;
    case CONTOURING_METHOD_CGAL:
      signed_distance_isosurface(
        IV,IF,level,angle_bound,radius_bound,distance_bound,type,V,F);
      break;
  }
  switch(nlhs)
  {
    default:
    {
      mexErrMsgTxt(false,"Too many output parameters.");
    }
    case 5:
    {
      prepare_lhs_double(S,plhs+4);
      // Fall through
    }
    case 4:
    {
      prepare_lhs_double(side,plhs+3);
      // Fall through
    }
    case 3:
    {
      prepare_lhs_double(GV,plhs+2);
      // Fall through
    }
    case 2:
    {
      prepare_lhs_index(F,plhs+1);
      // Fall through
    }
    case 1:
    {
      prepare_lhs_double(V,plhs+0);
      // Fall through
    }
    case 0: break;
  }

  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
}

#endif


/******************************************************************************

  Implementation of the original Hough transform for 2D line tracking.

  Reference papers:

  P.V.C. Hough, "Machine Analysis of Bubble Chamber Pictures",
  Proc. Int. Conf. High Energy Accelerators and Instrumentation, 1959.
  (U.S. Patent 3,069,654)

  Duda, R. O. and P. E. Hart, "Use of the Hough Transformation to Detect
  Lines and Curves in Pictures," Comm. ACM, Vol. 15, pp. 11–15, 1972.

  Copyright (c) 2013
  Dzmitry Hlindzich <hlindzich@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.

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

*******************************************************************************/

#ifndef HOUGH_TRANSFORM_HPP_F8fEE77D_37A4_4DFB_83ED_CF832D40772D_
#define HOUGH_TRANSFORM_HPP_F8fEE77D_37A4_4DFB_83ED_CF832D40772D_

#include <cmath>
#include <utility>
#include <algorithm>
#include <vector>
#include <boost/assert.hpp>
#include <boost/math/constants/constants.hpp>

#include "bo/core/raw_image_2d.hpp"

namespace bo {
namespace recognition {

template <typename RealType>
class HoughTransform
{
public:

    typedef bo::RawImage2D<float> Image;
    // Pair (rho, theta) used in the transform.
    typedef std::pair<RealType, RealType> RhoTheta;
    typedef std::vector<RhoTheta> RhoThetas;
    // Pixel in the transformed image. First element corresponds to
    // theta, the second one to rho.
    typedef std::pair<std::size_t, std::size_t> HoughPixel;
    typedef std::vector<HoughPixel> HoughPixels;

    HoughTransform(const Image &image): input_image_(image),
        max_vote_(0)
    {
        std::size_t w = image.width();
        std::size_t h = image.height();

        BOOST_ASSERT(w > 0 && h > 0);

        max_rho_ = std::sqrt(w * w + h * h);
        min_rho_ = -w;

        max_theta_ = boost::math::constants::pi<RealType>();
        min_theta_ = RealType(0);
    }

    // Returns the Hough transform (in the rho-theta parametrization) of the points
    // from the input image whose values exceed the given threshold. Represents the
    // result as a 2D image of size (output_width * output_height).
    // The resulted image covers the following rectangle in the rho-theta space:
    // width = values of theta [0, Pi], height = values of rho [-w, sqrt(w^2 + h^2)],
    // where w and h are the width and the height of the input image correspondingly.
    Image compute(RealType threshold, std::size_t output_width, std::size_t output_height)
    {
        BOOST_ASSERT(output_height > 0 && output_width > 0);

        max_vote_ = 0;

        // Compute the output image resolution.
        theta_scaling_ = static_cast<RealType>(output_width) / (max_theta_ - min_theta_);
        rho_scaling_ = static_cast<RealType>(output_height - 1) / (max_rho_ - min_rho_);

        // Allocate the output image.
        bo::RawImage2D<RealType> hough(output_width, output_height, 0);

        std::size_t w = input_image_.width();
        std::size_t h = input_image_.height();

        // Compute sinusoids in the rho-theta space for each point of the input image
        // whose value is greater that the threshold.
        for (std::size_t i = 0; i < w; ++i)
            for (std::size_t j = 0; j < h; ++j)
            {
                if (input_image_(i,j) > threshold)
                {
                    // Accumulate the sinusoid.
                    for (std::size_t x = 0; x < output_width; ++x)
                    {
                        RealType theta = min_theta_ + x / theta_scaling_;
                        RealType rho = i * std::cos(theta) + j * std::sin(theta);

                        std::size_t y = round(rho_scaling_ * (rho - min_rho_));

                        RealType vote = ++hough(x, y);

                        if(max_vote_ < vote)
                        {
                            max_vote_ = vote;
                        }
                    }
                }
            }

        return hough;
    }

    // Returns the image with the lines reconstructed from the given Hough parameters.
    Image reconstruct_lines(const RhoThetas &parameters)
    {
        const RealType kEpsilon(0.001);

        // Allocate the output image.
        std::size_t w = input_image_.width();
        std::size_t h = input_image_.height();
        Image line_image(w, h, 0);

        for (typename RhoThetas::const_iterator it = parameters.begin();
             it != parameters.end(); ++it)
        {
            RealType rho = it->first;
            RealType theta = it->second;

            RealType sint = std::sin(theta);
            RealType cost = std::cos(theta);

            RealType t1, t2;

            // Find the min (t1) and max (t2) parametric values of the line
            // segment within the image.
            if (std::abs(sint) < kEpsilon)
            {
                t1 = 0;
                t2 = h;
            }
            else if (std::abs(cost) < kEpsilon)
            {
                t1 = -w;
                t2 = 0;
            }
            else
            {
                RealType tc = rho * cost / sint;
                t1 = t2 = tc;

                tc = -rho * sint / cost;
                t1 = std::min(t1, tc);
                t2 = std::max(t2, tc);

                tc = (rho * cost - w) / sint;
                t1 = std::min(t1, tc);
                t2 = std::max(t2, tc);

                tc = (h - rho * sint) / cost;
                t1 = std::min(t1, tc);
                t2 = std::max(t2, tc);
            }

            RealType delta_t = RealType(1);
            RealType t = t1;

            // Draw the segment of the line.
            while (t < t2)
            {
                std::size_t x = round(rho * cost - t * sint);
                std::size_t y = round(rho * sint + t * cost);

                if (x >= 0 && x < w && y >= 0 && y < h)
                {
                    line_image(x, y) = 1;
                }

                t += delta_t;
            }
        }

        return line_image;
    }

    // Returns the image with the lines reconstructed from the given subset of pixels
    // from the Hough image returned by compute() method.
    Image reconstruct_lines(const HoughPixels &pixels)
    {
        RhoThetas parameters;

        // Transform the pixels into rho-theta parameters.
        for (HoughPixels::const_iterator it = pixels.begin(); it != pixels.end(); ++it)
        {
            RealType rho = min_rho_ + it->second / rho_scaling_;
            RealType theta = min_theta_ + it->first / theta_scaling_;

            parameters.push_back(RhoTheta(rho, theta));
        }

        return reconstruct_lines(parameters);
    }

    // Returns the image with the lines detected in the input image. Only the points
    // whose values exceed the given threshold are considered. The number of the
    // detected lines is defined by quantity parameter. Only the lines with the
    // number of votes not less than quantity * (maximal vote in the accumulator)
    // are reconstructed.
    Image reconstruct_lines(RealType threshold, RealType quantity,
                            std::size_t accu_width = 1024,
                            std::size_t accu_height = 1024)
    {
        BOOST_ASSERT(quantity >= RealType(0) && quantity <= RealType(1));

        // Compute the accumulator.
        Image hough = compute(threshold, accu_width, accu_height);

        RealType accu_threshold = max_vote_ * quantity;

        RhoThetas detected;

        // Collect the best parameters from the accumulator.
        for (std::size_t i = 0; i < accu_width; ++i)
            for (std::size_t j = 0; j < accu_height; ++j)
            {
                if (hough(i, j) >= accu_threshold)
                {
                    RealType rho = min_rho_ + j / rho_scaling_;
                    RealType theta = min_theta_ + i / theta_scaling_;

                    detected.push_back(RhoTheta(rho, theta));
                }
            }

        return reconstruct_lines(detected);
    }

    RealType get_rho_scaling()
    {
        return rho_scaling_;
    }

    RealType get_theta_scaling()
    {
        return theta_scaling_;
    }

protected:

    inline std::size_t round(RealType x) const
    {
        return static_cast<std::size_t>(std::floor(x + 0.5));
    }

    Image input_image_;

    RealType max_rho_;
    RealType min_rho_;
    RealType max_theta_;
    RealType min_theta_;

    RealType theta_scaling_;
    RealType rho_scaling_;

    RealType max_vote_;
};

} // namespace recognition
} // namespace bo

#endif // HOUGH_TRANSFORM_HPP_F8fEE77D_37A4_4DFB_83ED_CF832D40772D_


#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <Eigen/Eigen>
#include <queue>

using namespace Eigen;
using namespace std;
ros::Publisher twist_pub, debug_pub;
const double interval = 1000.0;
const double dt = 0.01;

int main(int argc, char **argv) {
    ros::init(argc, argv, "visual_ekf_test");
    ros::NodeHandle n("~");

    twist_pub = n.advertise<geometry_msgs::Vector3Stamped>("/visual_ekf/test_twist", 100);
    debug_pub = n.advertise<geometry_msgs::PoseStamped>("/visual_ekf/test_pose_debug", 100);

    ros::Rate r(1 / dt);

    Quaterniond start(1, 0, 0, 0);
    Quaterniond end(1, 2, 3, 100);
    end = end.normalized();

    Quaterniond q = end * start.conjugate();
    Quaterniond q_state = start;
    AngleAxisd R(q);
    double theta  = R.angle();
    Vector3d axis = R.axis();

    auto t_start = ros::Time::now();
    double t_total = interval * dt;
    // theta = theta / t_total;

    while (ros::ok()) {
        auto cur_t = ros::Time::now();
        if ((cur_t.toSec() - t_start.toSec()) < t_total) {
            geometry_msgs::Vector3Stamped twist;
            twist.header.stamp = cur_t;
            twist.vector.x = 0;
            twist.vector.y = 0;
            twist.vector.z = 0;
            twist_pub.publish(twist);

            geometry_msgs::PoseStamped pose;
            pose.header.stamp = cur_t;
            pose.pose.orientation.w = start.w();
            pose.pose.orientation.x = start.x();
            pose.pose.orientation.y = start.y();
            pose.pose.orientation.z = start.z();
            debug_pub.publish(pose);
        }
        else if ((cur_t.toSec() - t_start.toSec()) < t_total * 2) {
            geometry_msgs::Vector3Stamped twist;
            twist.header.stamp = cur_t;
            twist.vector.x = theta * axis(0);
            twist.vector.y = theta * axis(1);
            twist.vector.z = theta * axis(2);
            twist_pub.publish(twist);

            // AngleAxisd dR(d_theta, axis);
            Quaterniond dq;
            dq.w() = cos(0.5 * theta * dt);
            dq.x() = sin(0.5 * theta * dt) * axis(0);
            dq.y() = sin(0.5 * theta * dt) * axis(1);
            dq.z() = sin(0.5 * theta * dt) * axis(2);
            q_state = (q_state * dq).normalized();

            geometry_msgs::PoseStamped pose;
            pose.header.stamp = cur_t;
            pose.pose.orientation.w = q_state.w();
            pose.pose.orientation.x = q_state.x();
            pose.pose.orientation.y = q_state.y();
            pose.pose.orientation.z = q_state.z();
            debug_pub.publish(pose);
        }

        r.sleep();
        ros::spinOnce();
    }
}


#include <SmurffCpp/Types.h>
#include <Eigen/IterativeLinearSolvers>

#include <SmurffCpp/Utils/MatrixUtils.h>
#include <SmurffCpp/Utils/Error.h>
#include <SmurffCpp/Utils/counters.h>

#include <SmurffCpp/SideInfo/SparseSideInfo.h>
#include "linop.h"

namespace smurff {
namespace linop {
  class AtA;
} }

namespace Eigen {
namespace internal {
  // AtA looks-like a SparseMatrix, so let's inherits its traits:
  template<>
  struct traits<smurff::linop::AtA> :  public Eigen::internal::traits<smurff::SparseMatrix>
  {};
}
}

namespace smurff
{
namespace linop
{

// Example of a matrix-free wrapper from a user type to Eigen's compatible type
// For the sake of simplicity, this example simply wrap a Eigen::SparseMatrix.
class AtA : public Eigen::EigenBase<AtA>
{
public:
  // Required typedefs, constants, and method:
  typedef float_type Scalar;
  typedef float_type RealScalar;
  typedef int StorageIndex;
  enum
  {
    ColsAtCompileTime = Eigen::Dynamic,
    MaxColsAtCompileTime = Eigen::Dynamic,
    IsRowMajor = true
  };
  Index outerSize() const { return m_A.cols(); }
  Index innerSize() const { return m_A.cols(); }
  Index rows()      const { return m_A.cols(); }
  Index cols()      const { return m_A.cols(); }
  template <typename Rhs>
  Eigen::Product<AtA, Rhs, Eigen::AliasFreeProduct> operator*(const Eigen::MatrixBase<Rhs> &x) const
  {
    return Eigen::Product<AtA, Rhs, Eigen::AliasFreeProduct>(*this, x.derived());
  }
  // Custom API:
  AtA(const SparseMatrix &A, const SparseMatrix &At, double reg) : m_A(A), m_At(At), m_reg(reg) {}

  const SparseMatrix &m_A;
  const SparseMatrix &m_At;
  double m_reg;
};

} // namespace linop
} // namespace smurff

// Implementation of AtA * Eigen::DenseVector though a specialization of internal::generic_product_impl:
namespace Eigen {
namespace internal {
  template<typename Rhs>
  struct generic_product_impl<smurff::linop::AtA, Rhs, SparseShape, DenseShape, GemvProduct> // GEMV stands for matrix-vector
  : generic_product_impl_base<smurff::linop::AtA,Rhs,generic_product_impl<smurff::linop::AtA,Rhs> >
  {
    typedef typename Product<smurff::linop::AtA,Rhs>::Scalar Scalar;
    template<typename Dest>
    static void scaleAndAddTo(Dest& dst, const smurff::linop::AtA& lhs, const Rhs& rhs, const Scalar& alpha)
    {
      // This method should implement "dst += alpha * lhs * rhs" inplace,
      dst += alpha * ((lhs.m_At * (lhs.m_A * rhs)) + lhs.m_reg * rhs);
    }
  };
}
}

namespace smurff
{
namespace linop
{

inline void makeSymmetric(Matrix &A)
{
  A = A.selfadjointView<Eigen::Lower>();
}

inline void AtA_mul_B(Matrix& out, const SparseSideInfo& A, double reg, const Matrix& B) {
  out.noalias() = (A.Ft * (A.F * B)) + reg * B;
}

//
//-- Solves the system (K' * K + reg * I) * X = B for X for m right-hand sides
//   K = d x n matrix
//   I = n x n identity
//   X = n x m matrix
//   B = n x m matrix
//
int solve_blockcg_1block(Matrix & X, const SparseSideInfo& K, double reg, Matrix & B, double tol, bool throw_on_cholesky_error) {
  // initialize
  const int nfeat = B.rows();
  const int nrhs  = B.cols();
  double tolsq = tol*tol;

  if (nfeat != K.cols()) {THROWERROR("B.rows() must equal K.cols()");}

  Vector norms(nrhs), inorms(nrhs);
  norms.setZero();
  inorms.setZero();
  #pragma omp parallel for schedule(static)
  for (int rhs = 0; rhs < nrhs; rhs++)
  {
    double sumsq = 0.0;
    for (int feat = 0; feat < nfeat; feat++)
    {
      sumsq += B(feat, rhs) * B(feat, rhs);
    }
    norms(rhs)  = std::sqrt(sumsq);
    inorms(rhs) = 1.0 / norms(rhs);
  }
  Matrix R(nfeat, nrhs);
  Matrix P(nfeat, nrhs);
  Matrix Ptmp(nfeat, nrhs);
  X.setZero();
  // normalize R and P:
  #pragma omp parallel for schedule(static) collapse(2)
  for (int feat = 0; feat < nfeat; feat++)
  {
    for (int rhs = 0; rhs < nrhs; rhs++)
    {
      R(feat, rhs) = B(feat, rhs) * inorms(rhs);
      P(feat, rhs) = R(feat, rhs);
    }
  }
  Matrix* RtR = new Matrix(nrhs, nrhs);
  Matrix* RtR2 = new Matrix(nrhs, nrhs);

  Matrix   KP(nfeat, nrhs);
  Matrix KPtP(nrhs, nrhs);
  Matrix A;
  Matrix Psi;

  //A_mul_At_combo(*RtR, R);
  *RtR = R.transpose() * R;
  makeSymmetric(*RtR);

  const int nblocks = (int)ceil(nfeat / 64.0);

  // CG iteration:
  int iter = 0;
  for (iter = 0; iter < 1000; iter++) {
    // KP = K * P
    ////double t1 = tick();
    AtA_mul_B(KP, K, reg, P);
    ////double t2 = tick();

    KPtP = KP.transpose() * P;
    auto chol_KPtP = KPtP.llt();
    THROWERROR_ASSERT_MSG(!throw_on_cholesky_error || chol_KPtP.info() != Eigen::NumericalIssue, "Cholesky Decomposition failed! (Numerical Issue)");
    THROWERROR_ASSERT_MSG(!throw_on_cholesky_error || chol_KPtP.info() != Eigen::InvalidInput, "Cholesky Decomposition failed! (Invalid Input)");
    A = chol_KPtP.solve(*RtR);
    ////double t3 = tick();

    #pragma omp parallel for schedule(guided)
    for (int block = 0; block < nblocks; block++)
    {
      int row = block * 64;
      int brows = std::min(64, nfeat - row);
      // X += A' * P
      X.block(row, 0, brows, nrhs).noalias() += P.block(row, 0, brows, nrhs) * A;
      // R -= A' * KP
      R.block(row, 0, brows, nrhs).noalias() -= KP.block(row, 0, brows, nrhs) * A;
    }
    ////double t4 = tick();

    // convergence check:
    //A_mul_At_combo(*RtR2, R);
    *RtR2 = R.transpose() * R;
    makeSymmetric(*RtR2);

    Vector d = RtR2->diagonal();
    // std::cout << "[ iter " << iter << "] " << std::scientific << d.transpose() << " (max: " << d.maxCoeff() << " > " << tolsq << ")" << std::endl;
    //std::cout << iter << ":" << std::scientific << d.transpose() << std::endl;
    if ( (d.array() < tolsq).all()) {
      break;
    }

    // Psi = (R R') \ R2 R2'
    auto chol_RtR = RtR->llt();
    THROWERROR_ASSERT_MSG(!throw_on_cholesky_error || chol_RtR.info() != Eigen::NumericalIssue, "Cholesky Decomposition failed! (Numerical Issue)");
    THROWERROR_ASSERT_MSG(!throw_on_cholesky_error || chol_RtR.info() != Eigen::InvalidInput, "Cholesky Decomposition failed! (Invalid Input)");
    Psi  = chol_RtR.solve(*RtR2);
    ////double t5 = tick();

    // P = R + Psi' * P (P and R are already transposed)
    #pragma omp parallel for schedule(guided)
    for (int block = 0; block < nblocks; block++)
    {
      int row = block * 64;
      int brows = std::min(64, nfeat - row);
      Matrix xtmp(brows, nrhs);
      xtmp = P.block(row, 0, brows, nrhs) * Psi;
      P.block(row, 0, brows, nrhs) = R.block(row, 0, brows, nrhs) + xtmp;
    }

    // R R' = R2 R2'
    std::swap(RtR, RtR2);
    ////double t6 = tick();
    ////double t_total = 0.01 * (t6-t1);
    ////printf("t2-t1 = %.3f, t3-t2 = %.3f, t4-t3 = %.3f, t5-t4 = %.3f, t6-t5 = %.3f\n", t2-t1, t3-t2, t4-t3, t5-t4, t6-t5);
    ////printf("t2-t1 = %.3f, t3-t2 = %.3f, t4-t3 = %.3f, t5-t4 = %.3f, t6-t5 = %.3f\n",
    ////  (t2-t1)/(t_total), (t3-t2)/(t_total), (t4-t3)/(t_total), (t5-t4)/(t_total), (t6-t5)/(t_total));
  }

  if (iter == 1000)
  {
    Vector d = RtR2->diagonal().cwiseSqrt();
    std::cerr << "warning: block_cg: could not find a solution in 1000 iterations; residual: ["
              << d.transpose() << " ].all() > " << tol << std::endl;
  }

  // unnormalizing X:
  #pragma omp parallel for schedule(static) collapse(2)
  for (int feat = 0; feat < nfeat; feat++)
  {
    for (int rhs = 0; rhs < nrhs; rhs++)
    {
      X(feat, rhs) *= norms(rhs);
    }
  }
  delete RtR;
  delete RtR2;
  return iter;
}

/** good values for solve_blockcg are blocksize=32 an excess=8 */
int solve_blockcg(Matrix & X, const SparseSideInfo& K, double reg, Matrix & B, double tol, const int blocksize, const int excess, bool throw_on_cholesky_error) {
  if (B.cols() <= excess + blocksize) {
    return solve_blockcg_1block(X, K, reg, B, tol, throw_on_cholesky_error);
  }
  // split B into blocks of size <blocksize> (+ excess if needed)
  Matrix Xblock, Bblock;
  int max_iter = 0;
  for (int i = 0; i < B.cols(); i += blocksize) {
    int ncols = blocksize;
    if (i + ncols + excess >= B.cols()) {
      ncols = B.cols() - i;
    }
    Bblock.resize(B.rows(), ncols);
    Xblock.resize(X.rows(), ncols);

    Bblock = B.block(0, i, B.rows(), ncols);
    int niter = solve_blockcg_1block(Xblock, K, reg, Bblock, tol, throw_on_cholesky_error);
    max_iter = std::max(niter, max_iter);
    X.block(0, i, X.rows(), ncols) = Xblock;
  }

  return max_iter;
}

int solve_blockcg_eigen(Matrix & X, const SparseSideInfo& K, double reg, Matrix & B, double tol, bool throw_on_cholesky_error)
{
   COUNTER("eigen_cg");
   linop::AtA A(K.F, K.Ft, reg);
   Eigen::ConjugateGradient<linop::AtA, Eigen::Lower | Eigen::Upper, Eigen::IdentityPreconditioner> cg;
   cg.setTolerance(tol);
   cg.compute(A);
   X = cg.solve(B);
   return cg.iterations();
}

}}


#include "b3.h"
#include "descriptor.h"
#include "test_structure.h"
#include "gtest/gtest.h"
#include <Eigen/Dense>
#include <cmath>
#include <iostream>

TEST_F(StructureTest, RotationTest) {

  // Choose arbitrary rotation angles.
  double xrot = 1.28;
  double yrot = -3.21;
  double zrot = 0.42;

  // Define rotation matrices.
  Eigen::MatrixXd Rx{3, 3}, Ry{3, 3}, Rz{3, 3}, R{3, 3};
  Rx << 1, 0, 0, 0, cos(xrot), -sin(xrot), 0, sin(xrot), cos(xrot);
  Ry << cos(yrot), 0, sin(yrot), 0, 1, 0, -sin(yrot), 0, cos(yrot);
  Rz << cos(zrot), -sin(zrot), 0, sin(zrot), cos(zrot), 0, 0, 0, 1;
  R = Rx * Ry * Rz;

  Eigen::MatrixXd rotated_pos = positions * R.transpose();
  Eigen::MatrixXd rotated_cell = cell * R.transpose();

  // Define descriptors.
  descriptor_settings[2] = 2;
  B3 descriptor = B3(radial_string, cutoff_string, radial_hyps, cutoff_hyps,
                     descriptor_settings);

  std::vector<Descriptor *> descriptors;
  descriptors.push_back(&descriptor);

  Structure struc1 = Structure(cell, species, positions, cutoff, descriptors);
  Structure struc2 =
      Structure(rotated_cell, species, rotated_pos, cutoff, descriptors);

  // Check that B1 is rotationally invariant.
  double d1, d2, diff;
  double tol = 1e-10;

  for (int n = 0; n < struc1.descriptors[0].n_descriptors; n++) {
    d1 = struc1.descriptors[0].descriptors[0](0, n);
    d2 = struc2.descriptors[0].descriptors[0](0, n);
    diff = d1 - d2;
    EXPECT_LE(abs(diff), tol);
  }
}

  // TEST_F(DescriptorTest, SingleBond) {
  //   // Check that B1 descriptors match the corresponding elements of the
  //   // single bond vector.
  //   double d1, d2, diff;
  //   double tol = 1e-10;

  //   desc1.compute(env1);
  //   desc2.compute(env2);

  //   for (int n = 0; n < no_desc; n++) {
  //     d1 = desc1.descriptor_vals(n);
  //     d2 = desc1.single_bond_vals(n);
  //     diff = d1 - d2;
  //     EXPECT_LE(abs(diff), tol);
  //   }
// }

// TEST_F(DescriptorTest, CentTest) {
//   double finite_diff, exact, diff;
//   double tolerance = 1e-5;

//   desc1.compute(env1);
//   desc2.compute(env2);

//   // Perturb the coordinates of the central atom.
//   for (int m = 0; m < 3; m++) {
//     positions_3 = positions_1;
//     positions_3(0, m) += delta;
//     struc3 = Structure(cell, species, positions_3);
//     env3 = LocalEnvironment(struc3, 0, rcut);
//     env3.many_body_cutoffs = many_body_cutoffs;
//     env3.compute_indices();
//     desc3.compute(env3);

//     // Check derivatives.
//     for (int n = 0; n < no_desc; n++) {
//       finite_diff =
//           (desc3.descriptor_vals(n) - desc1.descriptor_vals(n)) / delta;
//       exact = desc1.descriptor_force_dervs(m, n);
//       diff = abs(finite_diff - exact);
//       EXPECT_LE(diff, tolerance);
//     }
//   }

//   int lmax = 8;
//   desc4.compute(env1);
//   desc5.compute(env2);
//   no_desc = desc4.descriptor_vals.rows();

//   // Perturb the coordinates of the central atom.
//   for (int m = 0; m < 3; m++) {
//     positions_3 = positions_1;
//     positions_3(0, m) += delta;
//     struc3 = Structure(cell, species, positions_3);
//     env3 = LocalEnvironment(struc3, 0, rcut);
//     env3.many_body_cutoffs = many_body_cutoffs;
//     env3.compute_indices();
//     desc6.compute(env3);

//     // Check derivatives.
//     for (int n = 0; n < no_desc; n++) {
//       finite_diff =
//           (desc6.descriptor_vals(n) - desc4.descriptor_vals(n)) / delta;
//       exact = desc4.descriptor_force_dervs(m, n);
//       diff = abs(finite_diff - exact);
//       EXPECT_LE(diff, tolerance);
//     }
//   }
// }

// TEST_F(DescriptorTest, EnvTest) {
//   double finite_diff, exact, diff;
//   double tolerance = 1e-5;

//   desc1.compute(env1);
//   desc2.compute(env2);

//   // Perturb the coordinates of the environment atoms.
//   for (int p = 1; p < noa; p++) {
//     for (int m = 0; m < 3; m++) {
//       positions_3 = positions_1;
//       positions_3(p, m) += delta;
//       struc3 = Structure(cell, species, positions_3);
//       env3 = LocalEnvironment(struc3, 0, rcut);
//       env3.many_body_cutoffs = many_body_cutoffs;
//       env3.compute_indices();
//       desc3.compute(env3);

//       // Check derivatives.
//       for (int n = 0; n < no_desc; n++) {
//         finite_diff =
//             (desc3.descriptor_vals(n) - desc1.descriptor_vals(n)) / delta;
//         exact = desc1.descriptor_force_dervs(p * 3 + m, n);
//         diff = abs(finite_diff - exact);
//         EXPECT_LE(diff, tolerance);
//       }
//     }
//   }

//   int lmax = 8;
//   desc4.compute(env1);
//   desc5.compute(env2);
//   no_desc = desc1.descriptor_vals.rows();

//   // Perturb the coordinates of the environment atoms.
//   for (int p = 1; p < noa; p++) {
//     for (int m = 0; m < 3; m++) {
//       positions_3 = positions_1;
//       positions_3(p, m) += delta;
//       struc3 = Structure(cell, species, positions_3);
//       env3 = LocalEnvironment(struc3, 0, rcut);
//       env3.many_body_cutoffs = many_body_cutoffs;
//       env3.compute_indices();
//       desc6.compute(env3);

//       // Check derivatives.
//       for (int n = 0; n < no_desc; n++) {
//         finite_diff =
//             (desc6.descriptor_vals(n) - desc5.descriptor_vals(n)) / delta;
//         exact = desc4.descriptor_force_dervs(p * 3 + m, n);
//         diff = abs(finite_diff - exact);
//         EXPECT_LE(diff, tolerance);
//       }
//     }
//   }
// }

// TEST_F(DescriptorTest, StressTest) {
//   int stress_ind = 0;
//   double finite_diff, exact, diff;
//   double tolerance = 1e-5;

//   desc1.compute(env1);
//   desc2.compute(env2);

//   // Test all 6 independent strains (xx, xy, xz, yy, yz, zz).
//   for (int m = 0; m < 3; m++) {
//     for (int n = m; n < 3; n++) {
//       cell_2 = cell;
//       positions_2 = positions_1;

//       // Perform strain.
//       cell_2(0, m) += cell(0, n) * delta;
//       cell_2(1, m) += cell(1, n) * delta;
//       cell_2(2, m) += cell(2, n) * delta;
//       for (int k = 0; k < noa; k++) {
//         positions_2(k, m) += positions_1(k, n) * delta;
//       }

//       struc2 = Structure(cell_2, species, positions_2);
//       env2 = LocalEnvironment(struc2, 0, rcut);
//       env2.many_body_cutoffs = many_body_cutoffs;
//       env2.compute_indices();
//       desc2.compute(env2);

//       // Check stress derivatives.
//       for (int p = 0; p < no_desc; p++) {
//         finite_diff =
//             (desc2.descriptor_vals(p) - desc1.descriptor_vals(p)) / delta;
//         exact = desc1.descriptor_stress_dervs(stress_ind, p);
//         diff = abs(finite_diff - exact);
//         EXPECT_LE(diff, tolerance);
//       }

//       stress_ind++;
//     }
//   }

//   int lmax = 8;
//   desc4.compute(env1);
//   desc5.compute(env2);
//   no_desc = desc1.descriptor_vals.rows();
//   stress_ind = 0;

//   // Test all 6 independent strains (xx, xy, xz, yy, yz, zz).
//   for (int m = 0; m < 3; m++) {
//     for (int n = m; n < 3; n++) {
//       cell_2 = cell;
//       positions_2 = positions_1;

//       // Perform strain.
//       cell_2(0, m) += cell(0, n) * delta;
//       cell_2(1, m) += cell(1, n) * delta;
//       cell_2(2, m) += cell(2, n) * delta;
//       for (int k = 0; k < noa; k++) {
//         positions_2(k, m) += positions_1(k, n) * delta;
//       }

//       struc2 = Structure(cell_2, species, positions_2);
//       env2 = LocalEnvironment(struc2, 0, rcut);
//       env2.many_body_cutoffs = many_body_cutoffs;
//       env2.compute_indices();
//       desc5.compute(env2);

//       // Check stress derivatives.
//       for (int p = 0; p < no_desc; p++) {
//         finite_diff =
//             (desc5.descriptor_vals(p) - desc4.descriptor_vals(p)) / delta;
//         exact = desc4.descriptor_stress_dervs(stress_ind, p);
//         diff = abs(finite_diff - exact);
//         EXPECT_LE(diff, tolerance);
//       }

//       stress_ind++;
//     }
//   }
// }


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2019 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_0.txt)

#include <boost/multiprecision/cpp_bin_float.hpp>
#ifdef TEST_MPFR
#include <boost/multiprecision/mpfr.hpp>
#endif
#ifdef TEST_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif
#include "test.hpp"

template <class T>
void test()
{
   T d = 360;
   for (int i = 2; i >= -2; --i)
   {
      T x = i * d;
      T y = remainder(x, d);
      if (y == 0)
         BOOST_CHECK_EQUAL(signbit(y), signbit(x));
      if (i == 0)
      {
         x = -x;
         y = remainder(x, d);
         if (y == 0)
            BOOST_CHECK_EQUAL(signbit(y), signbit(x));
      }
   }
}

int main()
{
   test<boost::multiprecision::cpp_bin_float_50>();
   // No signed zero:
   //test<boost::multiprecision::cpp_dec_float_50>();
   //test<boost::multiprecision::mpf_float_50>();
#ifdef TEST_MPFR
   test<boost::multiprecision::mpfr_float_50>();
#endif
#ifdef TEST_FLOAT128
   test<boost::multiprecision::float128>();
#endif
   return boost::report_errors();
}


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-trigonometric
    This function object converts degree to radian.

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

    @par Example:

      @snippet inrad.cpp inrad

    @par Possible output:

      @snippet inrad.txt inrad

  **/
  IEEEValue inrad(IEEEValue const& x);
} }
#endif

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

#endif


#include "tracer.hpp"
#include "initial_conditions/initial_conditions.hpp"
#include "dynamics/sound.hpp"
#include "observers/observer.hpp"
#define BOOST_TEST_MODULE sound_test
#include <boost/test/unit_test.hpp>

using zero_vec = boost::numeric::ublas::zero_vector<double>;

// make an observer that checks that analytic and numeric solutins coincide

// x(t) = (1+y0 - sqrt(1+t^2)/2)t + sinh^-1(t)/2
// y(t) = 1 - sqrt(1+t^2) + y0

class CheckSoundObserver final: public ThreadLocalObserver
{
public:
	/// create an observer and specify the time interval for saving the particle's position
	CheckSoundObserver( );

	/// d'tor
	~CheckSoundObserver() = default;

	// standard observer functions
	// for documentation look at observer.hpp
	bool watch( const State& state, double t ) override;
	void startTrajectory(const InitialCondition& start, std::size_t trajectory) override;
	void save( std::ostream& ) override {}
	std::shared_ptr<ThreadLocalObserver> clone() const override;
	void combine(ThreadLocalObserver&) override {}

	double x0;
	double y0;
};

CheckSoundObserver::CheckSoundObserver() : ThreadLocalObserver("tlo") { }

void CheckSoundObserver::startTrajectory( const InitialCondition& start, std::size_t )
{
    x0 = start.getState().getPosition()[0];
    y0 = start.getState().getPosition()[1];
}

bool CheckSoundObserver::watch( const State& state, double t )
{
    double ax = (1 + y0 - std::sqrt(1+t*t)/2)*t + std::asinh(t)/2 + x0;
    double ay = 1 - std::sqrt( 1 + t*t ) + y0;
    double nx = state.getPosition()[0];
    double ny = state.getPosition()[1];
	BOOST_CHECK_CLOSE( ax, nx, 1e-9 );
	BOOST_CHECK_CLOSE( ay, ny, 1e-9 );
	return true;
}

std::shared_ptr<ThreadLocalObserver> CheckSoundObserver::clone() const
{
	return std::make_shared<CheckSoundObserver>( );
}

BOOST_AUTO_TEST_SUITE(sound_trace_tests)

BOOST_AUTO_TEST_CASE(gradient)
{
    Potential potential(2, 1, 256);
    default_grid g(2, 256);
    g.setAccessMode(TransformationType::PERIODIC);
    for(auto& data : g)
        data = 0;
    potential.setDerivative(std::vector<int>{0,0}, g.clone(), "velocity1");
    potential.setDerivative(std::vector<int>{1,0}, g.clone(), "velocity1");
    potential.setDerivative(std::vector<int>{0,1}, g.clone(), "velocity1");
    potential.setDerivative(std::vector<int>{1,0}, g.clone(), "velocity0");
    for(auto& data : g)
        data = 1;
    potential.setDerivative(std::vector<int>{0,1}, g.clone(), "velocity0");
    for(auto ind = g.getIndex(); ind.valid(); ++ind)
    {
        g(ind) = ind[1] / 256.;
    }
    potential.setDerivative(std::vector<int>{0,0}, g.clone(), "velocity0");

	std::unique_ptr<RayDynamics> dynamics(new Sound(potential, false, false));
	auto tracer = std::make_shared<Tracer>( potential, std::move(dynamics));
	tracer->setMaxThreads(1);
	tracer->addObserver( CheckSoundObserver().clone() );

	auto generator = createInitialConditionGenerator( 2, std::vector<std::string>{"planar"} );
	init_cond::InitialConditionConfiguration config;
	config.setParticleCount(1000).setEnergyNormalization(true).setSupport(potential.getSupport())
            .setOffset(zero_vec(2));
	tracer->trace( generator, config );
	std::cout << "FINNISHED\n";
}

BOOST_AUTO_TEST_SUITE_END()


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2014 Jose Aparicio

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#ifndef quantlib_tcopula_policy_hpp
#define quantlib_tcopula_policy_hpp

#include <ql/errors.hpp>
#include <ql/utilities/disposable.hpp>
#include <ql/experimental/math/convolvedstudentt.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <boost/bind.hpp>
#include <vector>

namespace QuantLib {

    /*! \brief Student-T Latent Model's copula policy.

    Describes the copula of a set of normalized Student-T independent random
    factors to be fed into the latent variable model.
    The latent model requires the independent variables to be of unit variance
    so the policy expects the factors coefficients to be as usual and the T
    variables to be normalized, the normalization is performed by the policy.
    To normalize the random variables they are divided by the square root of
    the variance of each T (\f$ \frac{\nu}{\nu-2}\f$)
    */
    class TCopulaPolicy {
    public:
        /*! Stores the parameters defining the factors random variable
        T-distributions. As it is now the latent models are restricted to
        having the same distribution for all idiosyncratic factors, so only
        one parameter is needed for them.
        */
        typedef
            struct {
                std::vector<Integer> tOrders;
            } initTraits;

        /*! Delayed initialization of the distribution parameters and caches.
        To be called by the latent model. */
        /* \todo
        Explore other constructors, with different vector dimensions, defining
        simpler combinations (only one correlation, only one variable) might
        simplify memory.
        */
        explicit TCopulaPolicy(
            const std::vector<std::vector<Real> >& factorWeights =
                std::vector<std::vector<Real> >(),
            const initTraits& vals = initTraits());

        //! Number of independent random factors.
        Size numFactors() const {
            return latentVarsInverters_.size() + varianceFactors_.size() - 1;
        }

        //! returns a copy of the initialization arguments
        //... better to have a cache?
        initTraits getInitTraits() const {
            initTraits data;
            data.tOrders.resize(distributions_.size());
            for (Size i=0; i<distributions_.size(); ++i) {
                data.tOrders[i] = static_cast<Integer>(
                    distributions_[i].degrees_of_freedom());
            }
            return data;
        }
        const std::vector<Real>& varianceFactors() const {
            return varianceFactors_;
        }
        /*! Cumulative probability of a given latent variable.
            The iVariable parameter is the index of the requested variable.
        */
        Probability cumulativeY(Real val, Size iVariable) const {
    #if defined(QL_EXTRA_SAFETY_CHECKS)
            QL_REQUIRE(iVariable < latentVarsCumul_.size(),
                "Latent variable index out of bounds.");
    #endif
            return latentVarsCumul_[iVariable](val);
        }
        //! Cumulative probability of the idiosyncratic factors (all the same)
        Probability cumulativeZ(Real z) const {
            return boost::math::cdf(distributions_.back(), z /
                varianceFactors_.back());
        }
        /*! Probability density of a given realization of values of the systemic
          factors (remember they are independent).
          Intended to be used in numerical integration of an arbitrary function
          depending on those values.
        */
        Probability density(const std::vector<Real>& m) const {
    #if defined(QL_EXTRA_SAFETY_CHECKS)
            QL_REQUIRE(m.size() == distributions_.size()-1,
                "Incompatible sample and latent model sizes");
    #endif
            Real prodDensities = 1.;
            for(Size i=0; i<m.size(); i++)
                prodDensities *= boost::math::pdf(distributions_[i],
                    m[i] /varianceFactors_[i]) /varianceFactors_[i];
                 // accumulate lambda
            return prodDensities;
        }
        /*! Returns the inverse of the cumulative distribution of the (modelled)
          latent variable (as indexed by iVariable). Involves the convolution
          of the factors' distributions.
        */
        Real inverseCumulativeY(Probability p, Size iVariable) const {
    #if defined(QL_EXTRA_SAFETY_CHECKS)
            QL_REQUIRE(iVariable < latentVarsCumul_.size(),
                "Latent variable index out of bounds.");
    #endif
            return latentVarsInverters_[iVariable](p);
        }
        /*! Returns the inverse of the cumulative distribution of the
        idiosincratic factor. The LM here is limited to all idiosincratic
        factors following the same distribution.
        */
        Real inverseCumulativeZ(Probability p) const {
            return boost::math::quantile(distributions_.back(), p)
                * varianceFactors_.back();
        }
        /*! Returns the inverse of the cumulative distribution of the
          systemic factor iFactor.
        */
        Real inverseCumulativeDensity(Probability p, Size iFactor) const {
    #if defined(QL_EXTRA_SAFETY_CHECKS)
            QL_REQUIRE(iFactor < distributions_.size()-1,
                "Random factor variable index out of bounds.");
    #endif
            return boost::math::quantile(distributions_[iFactor], p)
                * varianceFactors_[iFactor];
        }
        //to use this (by default) version, the generator must be a uniform one.
        Disposable<std::vector<Real> >
            allFactorCumulInverter(const std::vector<Real>& probs) const;
    private:
        mutable std::vector<boost::math::students_t_distribution<> >
            distributions_;
        mutable std::vector<Real> varianceFactors_;
        mutable std::vector<CumulativeBehrensFisher> latentVarsCumul_;
        mutable std::vector<InverseCumulativeBehrensFisher>
            latentVarsInverters_;
    };

}

#endif


/*
(**************************************************************************)
(*                                                                        *)
(*                                Schifra                                 *)
(*                Reed-Solomon Error Correcting Code Library              *)
(*                                                                        *)
(* Release Version 0.0.1                                                  *)
(* http://www.schifra.com                                                 *)
(* Copyright (c) 2000-2020 Arash Partow, All Rights Reserved.             *)
(*                                                                        *)
(* The Schifra Reed-Solomon error correcting code library and all its     *)
(* components are supplied under the terms of the General Schifra License *)
(* agreement. The contents of the Schifra Reed-Solomon error correcting   *)
(* code library and all its components may not be copied or disclosed     *)
(* except in accordance with the terms of that agreement.                 *)
(*                                                                        *)
(* URL: http://www.schifra.com/license.html                               *)
(*                                                                        *)
(**************************************************************************)
*/

/*
   Description: This example will demonstrate the use of the Reed-Solomon
                erasure channel coding capabilities in a threaded context.
                One must note that the number of threads should not exceed
                the architecture's ability to efficiently and productively
                run the threads. A simple limiting strategy would be not to
                have more threads than the number of available cores on the
                processor.
*/

#include <cstddef>
#include <iostream>
#include <string>

#include <boost/bind.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/thread/thread.hpp>

#include "schifra_galois_field.hpp"
#include "schifra_galois_field_polynomial.hpp"
#include "schifra_sequential_root_generator_polynomial_creator.hpp"
#include "schifra_reed_solomon_encoder.hpp"
#include "schifra_reed_solomon_decoder.hpp"
#include "schifra_reed_solomon_block.hpp"
#include "schifra_erasure_channel.hpp"
#include "schifra_ecc_traits.hpp"
#include "schifra_utilities.hpp"

const std::size_t round_count = 100;

template <typename Encoder, typename Decoder>
class erasure_process
{
public:

   erasure_process(const unsigned int& process_id,
                   schifra::galois::field& field,
                   schifra::galois::field_polynomial& generator_polynomial,
                   const std::size_t& generator_polynomial_index)
   : process_id_(process_id),
     total_time_(0.0),
     encoder_(field,generator_polynomial),
     decoder_(field,generator_polynomial_index)
   {}

   erasure_process& operator=(const erasure_process& ep)
   {
      process_id_ = ep.process_id_;
      total_time_ = ep.total_time_;
      return *this;
   }

   double time() { return total_time_; }

   void execute()
   {
      schifra::traits::equivalent_encoder_decoder<Encoder,Decoder>();

      const std::size_t code_length = Encoder::trait::code_length;
      const std::size_t fec_length  = Encoder::trait::fec_length;
      const std::size_t data_length = Encoder::trait::data_length;
      const std::size_t stack_size  = Encoder::trait::code_length;
      const std::size_t data_size   = stack_size * data_length;

      typedef schifra::reed_solomon::block<code_length,fec_length> block_type;

      block_type block_stack[stack_size];
      unsigned char send_data[data_size];
      unsigned char recv_data[data_size];

      schifra::utils::timer timer;
      total_time_ = 0.0;

      for (std::size_t k = 0; k < round_count; ++k)
      {
         /* Populate block stack with data */
         for (std::size_t i = 0; i < data_size; ++i)
         {
            send_data[i] = static_cast<unsigned char>((i * 3 + 7 * k) & 0xFF);
         }

         schifra::reed_solomon::copy<unsigned char,code_length,fec_length,stack_size>(send_data,data_size,block_stack);

         timer.start();

         schifra::reed_solomon::erasure_channel_stack_encode<code_length,fec_length>(encoder_,block_stack);

         /* Add Erasures - Simulate network packet loss (e.g: UDP) */
         schifra::reed_solomon::erasure_locations_t missing_row_index;
         missing_row_index.clear();

         for (std::size_t i = 0; i < fec_length; ++i)
         {
            std::size_t missing_index = (k + (i * 4)) % stack_size;
            block_stack[missing_index].clear();
            missing_row_index.push_back(missing_index);
         }

         schifra::reed_solomon::erasure_channel_stack_decode<code_length,fec_length>(decoder_,missing_row_index,block_stack);

         schifra::reed_solomon::copy<unsigned char,code_length,fec_length,stack_size>(block_stack,recv_data);

         timer.stop();
         total_time_ += timer.time();

         for (std::size_t i = 0; i < data_size; ++i)
         {
            if (recv_data[i] != send_data[i])
            {
               std::cout << "[" << process_id_ << "] Error: Final block stack comparison failed! stack: " << i << std::endl;
               return;
            }
         }
      }
   }

private:

   unsigned int process_id_;
   double total_time_;
   Encoder encoder_;
   Decoder decoder_;
};

int main()
{
   /* Finite Field Parameters */
   const std::size_t field_descriptor                =   8;
   const std::size_t generator_polynomial_index      = 120;
   const std::size_t generator_polynomial_root_count = 128;

   /* Reed Solomon Code Parameters */
   const std::size_t code_length = 255;
   const std::size_t fec_length  = 128;
   const std::size_t data_length = code_length - fec_length;

   /* Instantiate Finite Field and Generator Polynomials */
   schifra::galois::field field(field_descriptor,
                                schifra::galois::primitive_polynomial_size06,
                                schifra::galois::primitive_polynomial06);

   schifra::galois::field_polynomial generator_polynomial(field);

   if (
        !schifra::make_sequential_root_generator_polynomial(field,
                                                            generator_polynomial_index,
                                                            generator_polynomial_root_count,
                                                            generator_polynomial)
      )
   {
      std::cout << "Error - Failed to create sequential root generator!" << std::endl;
      return 1;
   }

   typedef schifra::reed_solomon::encoder<code_length,fec_length>              encoder_type;
   typedef schifra::reed_solomon::erasure_code_decoder<code_length,fec_length> decoder_type;

   typedef erasure_process<encoder_type,decoder_type> erasure_process_type;
   typedef boost::shared_ptr<erasure_process_type>    erasure_process_ptr_type;

   const unsigned int max_thread_count = 4; // number of functional cores.

   std::vector<erasure_process_ptr_type> erasure_process_list;

   boost::thread_group threads;

   for (unsigned int i = 0; i < max_thread_count; ++i)
   {
      erasure_process_list.push_back(erasure_process_ptr_type(new
                                     erasure_process_type
                                     (
                                       i,
                                       field,
                                       generator_polynomial,
                                       generator_polynomial_index
                                     )));

      threads.create_thread(boost::bind(&erasure_process_type::execute,erasure_process_list[i]));
   }

   threads.join_all();

   double time = -1.0;

   /* Determine the process with the longest running time. */
   for (std::size_t i = 0; i < erasure_process_list.size(); ++i)
   {
      time = ((time < erasure_process_list[i]->time()) ? erasure_process_list[i]->time() : time);
   }

   double mbps = (max_thread_count * round_count * 8.0 * code_length * data_length) / (1048576.0 * time);

   std::cout << "Blocks decoded: " << max_thread_count * round_count * code_length << "\tTime: " << time <<"sec\tRate: " << mbps << "Mbps" << std::endl;

   return 0;
}


#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl;

    const unsigned n= 10;
    compressed2D<double>                         A(n, n);
    dense2D<int, mat::parameters<col_major> > B(n, n);
    morton_dense<double, 0x555555f0>             C(n, n), D(n, n);

    mat::laplacian_setup(A, 2, 5);
    mat::hessian_setup(B, 1); mat::hessian_setup(C, 2.0); mat::hessian_setup(D, 3.0);

    D+= A - 2 * B + C;

    std::cout << "The matrices are: A=\n" << A << "B=\n" << B << "C=\n" << C << "D=\n" << D;

    return 0;
}


// Std includes
#include <vector>
#include <iostream>
// Thirdparties includes
#include <Eigen/Dense>
// Lib includes
#include "fl0w/kinematic.h"

const unsigned int DIM = 3;

using TypeScalar = double;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
using TypeMatrix = Eigen::Matrix<TypeScalar, DIM, DIM>;
template<typename... Args>
using TypeRef = Eigen::Ref<Args...>;
using TypeFlow = fl0w::kinematic::Kinematic<TypeVector, TypeMatrix, TypeRef, std::vector>;

void print(const TypeFlow& flow, const TypeVector& x, const TypeScalar& t) {
    std::cout << std::endl;
    std::cout << "flow.getVelocity(" << x.transpose() << ", " << t << ") -> " << flow.getVelocity(x, t).transpose() << std::endl;
    std::cout << "flow.getVorticity(" << x.transpose() << ", " << t << ") -> " << flow.getVorticity(x, t).transpose() << std::endl;
    std::cout << "flow.getAcceleration(" << x.transpose() << ", " << t << ") -> " << flow.getAcceleration(x, t).transpose() << std::endl;
    std::cout << std::endl;
}

int main () {
    TypeFlow flow;
    flow.init();
    std::cout << "k " << (*flow.sK)[0] << std::endl;
    std::cout << "k " << (*flow.sK)[1] << std::endl;
    std::cout << "k " << (*flow.sK)[2] << std::endl;
    std::cout << "k " << (*flow.sK)[3] << std::endl;
    std::cout << "n0 " << (*flow.sK)[0].norm() << std::endl;
    std::cout << "nl " << (*flow.sK)[(*flow.sK).size()-1].norm() << std::endl;
    std::cout << "a " << flow.a[0] << std::endl;
    std::cout << "b " << flow.b[0] << std::endl;
    std::cout << "o " << flow.omega[0] << std::endl;
    TypeVector x;
    double t;
    // Init
    x << 0.0, 0.0, 0.0;
    t = 0.0;
    print(flow, x, t);
    //x << 10.0, -10.0, 40.0;
    //t = 10.0;
    //print(flow, x, t);
    //x << 1.0, -3.0, -1000.0;
    //t = 40.0;
    //print(flow, x, t);
}