// Copyright 2015 Clemson University
// Authors: Bradley S. Meyer
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <iostream>
#include <fstream>
#include <boost/graph/adjacency_list.hpp>

#include "../include/rank_spanning_branchings.hpp"

struct print_branching
{

  print_branching(){}

  template <typename BranchingGraph>
  bool operator()( BranchingGraph& bg )
  {

    typedef
      typename
        boost::property_map<BranchingGraph, boost::edge_weight_t>::const_type
        WeightMap;

    WeightMap w;
    typename boost::property_traits<WeightMap>::value_type weight;

    std::cout << "Branching:";

    weight = 0;

    BGL_FORALL_EDGES_T( e, bg, BranchingGraph )
    {

      std::cout << " (" << boost::source( e, bg ) << "," <<
        boost::target( e, bg ) << ")";

      weight += get( w, e );

    }

    std::cout << std::endl << "  Weight: " << weight << std::endl << std::endl;

    return true;

  }

};

int
main()
{
  typedef boost::adjacency_list < boost::vecS, boost::vecS, boost::directedS,
    boost::no_property, boost::property < boost::edge_weight_t, int > > Graph;
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  typedef boost::graph_traits < Graph >::edge_descriptor Edge;
#endif
  typedef std::pair<int, int> E;

  const int num_nodes = 4;
  E edge_array[] = { E(0, 1), E(0, 2), E(0, 3), E(1, 2), E(2, 1), E(2, 3),
    E(3, 2), E(1, 3), E(3, 1)
  };
  int weights[] = { 5, 1, 1, 11, 10, 5, 8, 4, 9 };

  std::size_t num_edges = sizeof(edge_array) / sizeof(E);
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  Graph g(num_nodes);
  boost::property_map<Graph, boost::edge_weight_t>::type weightmap =
    get(edge_weight, g);
  for (std::size_t j = 0; j < num_edges; ++j) {
    Edge e; bool inserted;
    boost::tie(e, inserted) = add_edge(edge_array[j].first, edge_array[j].second, g);
    weightmap[e] = weights[j];
  }
#else
  Graph g(edge_array, edge_array + num_edges, weights, num_nodes);
#endif

  // Rank branchings in descending order of weight (use default
  // weight comparison).

  std::cout << std::endl;

  std::cout << "Spanning branchings in descending order of weight:"
            << std::endl << std::endl;

  rsb::rank_spanning_branchings(
    g,
    print_branching()
  );

  std::cout << std::endl;

  // Rank branchings in ascending order of weight (use supplied
  // weight comparison).

  std::cout << "Spanning branchings in ascending order of weight:"
            << std::endl << std::endl;

  rsb::rank_spanning_branchings(
    g,
    print_branching(),
    boost::distance_compare( std::greater<int>() )
  );

  return EXIT_SUCCESS;
}


/*
 * Copyright (c) Nuno Alves de Sousa 2019
 *
 * Use, modification and distribution is subject to the Boost Software License,
 * Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */
#ifndef FIXTURE_DGEMM_H
#define FIXTURE_DGEMM_H

#include <cstdint>
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <new>

#include <celero/Celero.h>

#include <Eigen/Eigen>

#include <mkl.h>

#include <random_vector.hpp>
#include <debug.hpp>

using EigenMatrix = Eigen::MatrixXd;
using MKLMatrix = double*;

enum class ProgressionPolicy {linear, geometric, semilogGemm, semilogAdd};

/**
 * Creates a set of linearly increasing matrix dimensions
 * @param numberOfTests Total number of tests
 * @param increment Increment value for the next set of matrix dimensions
 * @return A problemSpace (i.e. the set of matrix dimensions)
 */
inline std::vector<celero::TestFixture::ExperimentValue>
linearProgression(int numberOfTests, int increment)
{
    std::vector<celero::TestFixture::ExperimentValue> problemSpace;

    // Generate a set of matrix dimensions
    for (int i = 0; i < numberOfTests; ++i) {
        // matrix dimensions as a function of the test number
        problemSpace.push_back(static_cast<std::int64_t>(increment*(i+1)));
    }
    return problemSpace;
}

/**
 * Creates a set of matrix dimensions that follow a geometric progression
 * @param numberOfTests Total number of tests
 * @param increment Common ration for the geometric progression
 * @return A problemSpace (i.e. the set of matrix dimensions)
 */
inline std::vector<celero::TestFixture::ExperimentValue>
geometricProgression(int numberOfTests, int increment)
{
    // Start Value = 1 and Iterations = 0 (default)
    std::vector<celero::TestFixture::ExperimentValue> problemSpace{1, 0};

    // Generate a set of matrix dimensions
    for (int i = 0; i < numberOfTests; ++i) {
        // matrix dimensions as a function of the test number
        problemSpace.push_back(static_cast<std::int64_t>(problemSpace.back().Value * increment));
    }
    return problemSpace;
}

/**
 * Creates a set of matrix dimensions to create a semilog plot when
 * benchmarking gdemm functions (faster decay of benchmark iterations)
 * @param numberOfTests Total number of tests
 * @return A problemSpace (i.e. the set of matrix dimensions)
 */
inline std::vector<celero::TestFixture::ExperimentValue>
semilogGemmProgression(int numberOfTests)
{
    std::vector<celero::TestFixture::ExperimentValue> problemSpace;

    std::int64_t matrixDim = 1;
    for (int i = 1; i <= numberOfTests; ++i) {
        auto orderMag = static_cast<std::int64_t>
            (std::floor(std::log10(matrixDim)));
        matrixDim += static_cast<std::int64_t>(std::pow(10, orderMag));

        // Adjust iterations of the problemSpace according to matrix dimensions
        static std::int64_t iterations;
        switch (matrixDim) {
            case      2: iterations = 100; break;
            case    100: iterations = 25;  break;
            case   1000: iterations = 5;   break;
            case 10'000: iterations = 3;   break;
        }
        problemSpace.push_back({matrixDim, iterations});
    }
    return problemSpace;
}

/**
 * Creates a set of matrix dimensions to create a semilog plot when
 * benchmarking matrix addition (slower decay of benchmark iterations)
 * @param numberOfTests Total number of tests
 * @return A problemSpace (i.e. the set of matrix dimensions)
 */
inline std::vector<celero::TestFixture::ExperimentValue>
semilogAddProgression(int numberOfTests)
{
    std::vector<celero::TestFixture::ExperimentValue> problemSpace;

    std::int64_t matrixDim = 1;
    for (int i = 1; i <= numberOfTests; ++i) {
        auto orderMag = static_cast<std::int64_t>
        (std::floor(std::log10(matrixDim)));
        matrixDim += static_cast<std::int64_t>(std::pow(10, orderMag));

        // Adjust iterations of the problemSpace according to matrix dimensions
        static std::int64_t iterations;
        switch (matrixDim) {
            case      2: iterations = 100; break;
            case   1000: iterations = 75;   break;
        }
        problemSpace.push_back({matrixDim, iterations});
    }
    return problemSpace;
}

/// Base class for all fixtures ralated to matrix operations
/// \tparam policy The ProgressionPolicy for the benchmark fixture
template<ProgressionPolicy policy>
class MatrixFixture: public celero::TestFixture
{
public:
    MatrixFixture() = default;

    /// The problem space is a set of matrix dimensions
    std::vector<celero::TestFixture::ExperimentValue> getExperimentValues() const override
    {
        switch (policy) {
            case ProgressionPolicy::linear:
                return linearProgression(numLinearProgressionTests
                                        ,increment);
            case ProgressionPolicy::geometric:
                return geometricProgression(numGeometricProgressionTests
                                           ,increment);
            case ProgressionPolicy::semilogGemm:
                return semilogGemmProgression(numSemilogGemmProgressionTests);
            case ProgressionPolicy::semilogAdd:
                return semilogAddProgression(numSemilogAddProgressionTests);
        }
    }

protected:
    // Helper setter
    void updateMatrixDim(std::int64_t newDim)
    {
        matrixDim = newDim;
        matrixSize = matrixDim * matrixDim;
    }

    // Helper generator according to current matrix dimensions
    std::vector<double> makeRandomMatrixData()
    {
        return makeRandomVector(matrixSize, dataMin, dataMax);
    }

    std::int64_t matrixDim;
    std::int64_t matrixSize;

private:
    static constexpr int increment = 25;

    static constexpr int numLinearProgressionTests = 100;
    static constexpr int numGeometricProgressionTests = 100;
    static constexpr int numSemilogGemmProgressionTests = 35;

    // Fewer tests because of additional memory requirements (matrix copies)
    static constexpr int numSemilogAddProgressionTests = 35;

    static constexpr double dataMin = 0.0;
    static constexpr double dataMax = 1.0;
};

/// This MKL fixture allocates aligned buffers
template<ProgressionPolicy policy = ProgressionPolicy::linear>
class MKLFixture: public MatrixFixture<policy>
{
public:
    MKLFixture() = default;

    /// Before each run build matrices of random integers
    void setUp(const celero::TestFixture::ExperimentValue& experimentValue) override
    {
        // Update matrix dimensions based on the current experiment
        this->updateMatrixDim(experimentValue.Value);

        // Initialize MKL matrices
        mA = allocate_dmatrix(this->matrixSize);
        mC = allocate_dmatrix(this->matrixSize);

        auto matrixData = this->makeRandomMatrixData();

        std::copy(matrixData.begin(), matrixData.end(), mA);
        std::fill(mC, mC + this->matrixSize, 0);

        MKL_DEBUG(mA, this->matrixDim, this->matrixDim);
        MKL_DEBUG(mC, this->matrixDim, this->matrixDim);
    }

    // Clear MKL matrices on tearDown and do not include deallocation time
    void tearDown() override
    {
        mkl_free(mA);
        mkl_free(mC);
    }

protected:
    // Helper allocation function
    virtual double* allocate_dmatrix(std::int64_t matrixSize)
    {
        auto ptr =
            static_cast<double*>
                (mkl_malloc(matrixSize*sizeof(double), this->align));
        if (!ptr) {
            std::cerr << "error: cannot allocate matrices\n";
            throw std::bad_alloc{};
        }
        return ptr;
    }

    MKLMatrix mA = nullptr;
    MKLMatrix mC = nullptr;

    static constexpr int align = 64; // default alignment on 64-byte bounndary
};

/// This MKL fixture with copies for mkl_domatadd
template<ProgressionPolicy policy = ProgressionPolicy::linear>
class MKLFixtureB: public MKLFixture<policy>
{
public:
    MKLFixtureB() = default;

    /// Before each run build matrices of random integers
    void setUp(const celero::TestFixture::ExperimentValue& experimentValue) override
    {
        // Update matrix dimensions and allocate mA
        MKLFixture<policy>::setUp(experimentValue);

        // Allocate additional space for mAcopy and mC
        mB = allocate_dmatrix(this->matrixSize);
        std::copy(this->mA, this->mA + this->matrixSize, mB);
        MKL_DEBUG(mB, this->matrixDim, this->matrixDim);
    }

    // Clear MKL matrices on tearDown and do not include deallocation time
    void tearDown() override
    {
        MKLFixture<policy>::tearDown();
        mkl_free(mB);
    }

protected:
    // Helper allocation function
    virtual double* allocate_dmatrix(std::int64_t matrixSize)
    {
        auto ptr =
             static_cast<double*>(mkl_malloc(matrixSize*sizeof(double), this->align));
        if (!ptr) {
            throw std::bad_alloc{};
        }
        return ptr;
    }

    MKLMatrix mB = nullptr;
};

template<ProgressionPolicy policy = ProgressionPolicy::linear>
class EigenFixture: public MatrixFixture<policy>
{
public:
    EigenFixture() = default;

    /// Before each run build matrices of random data and setup threading
    void setUp(const celero::TestFixture::ExperimentValue& experimentValue) override
    {
        if constexpr (numberOfThreads != 0) {
            // Eigen threading
            Eigen::setNbThreads(numberOfThreads);
        }

        // Update matrix dimensions based on the current experiment
        this->updateMatrixDim(experimentValue.Value);

        // Initialize Eigen matrix
        auto matrixData = this->makeRandomMatrixData();
        eA = Eigen::Map<EigenMatrix>(matrixData.data()
                                    ,this->matrixDim
                                    ,this->matrixDim);
        EIGEN_DEBUG(eA);
    }

protected:
    EigenMatrix eA;

    static constexpr int numberOfThreads = 4;
};

#endif //FIXTURE_DGEMM_H


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  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_ARCH_COMMON_GENERIC_FUNCTION_IS_DENORMAL_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_IS_DENORMAL_HPP_INCLUDED

#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/constant/false.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/logical.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( is_denormal_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::arithmetic_<A0> >
                          )
  {
    using result = bs::as_logical_t<A0>;
    BOOST_FORCEINLINE  result  operator() ( A0 ) const BOOST_NOEXCEPT
    {
      return False<result>();
    }
  };
  BOOST_DISPATCH_OVERLOAD ( is_denormal_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE  bs::as_logical_t<A0> operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      return bitwise_and(is_nez(a0), is_less(bs::abs(a0), Smallestposval<A0>()));
    }
  };
} } }

#endif


#include <ros/ros.h>

#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

#include <gps_convert_utils.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/NavSatFix.h>

#include <Eigen/Eigen>

Eigen::Vector3d first_pose;
bool first_pose_flag = false;

ros::Publisher pub_odom;

void GPSHandler(const sensor_msgs::NavSatFix::ConstPtr& rtk_msg) {
  double latitude = rtk_msg->latitude;
  double longitude = rtk_msg->longitude;
  double altitude = rtk_msg->altitude;

  // 转化为utm坐标系
  UTMCoor utmcoor;
  LatLonToUTMXY(latitude / 180.0 * M_PI, longitude / 180.0 * M_PI, 51, utmcoor);
  Eigen::Vector3d gps_utm(utmcoor.x, utmcoor.y, altitude);
  if (!first_pose_flag) {
    first_pose = gps_utm;
    first_pose_flag = true;
  }

  gps_utm -= first_pose;

  nav_msgs::Odometry odom_msg;
  odom_msg.header.frame_id = "/map";
  odom_msg.header.stamp = ros::Time::now();
  odom_msg.pose.pose.position.x = gps_utm(0);
  odom_msg.pose.pose.position.y = gps_utm(1);
  odom_msg.pose.pose.position.z = gps_utm(2);
  odom_msg.pose.covariance[0] = rtk_msg->position_covariance[0];
  odom_msg.pose.covariance[7] = rtk_msg->position_covariance[4];
  odom_msg.pose.covariance[14] = rtk_msg->position_covariance[8];

  pub_odom.publish(odom_msg);
}

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

  ros::Publisher pub_gps_path = n.advertise<nav_msgs::Path>("/gps_path", 10);
  pub_odom = n.advertise<nav_msgs::Odometry>("/gps_odom", 10);

  std::string bag_path = "/home/hkw/rosbag/rs_ruby_mti_680_new.bag";
  std::string gps_topic = "/gnss";

  rosbag::Bag bag;
  bag.open(bag_path, rosbag::bagmode::Read);

  std::vector<std::string> topics;
  topics.push_back(gps_topic);

  rosbag::View view_;
  rosbag::View view_full;
  view_full.addQuery(bag);
  ros::Time time_init = view_full.getBeginTime();
  ros::Time time_finish = view_full.getEndTime();

  view_.addQuery(bag, rosbag::TopicQuery(topics), time_init, time_finish);
  if (view_.size() == 0) {
    ROS_ERROR("No messages to play on specified topics.  Exiting.");
    ros::shutdown();
    return 0;
  }

  ros::Rate rate(10);
  for (const rosbag::MessageInstance& m : view_) {
    ros::Time ros_bag_time = m.getTime();
    if (m.getTopic() == gps_topic) {
      sensor_msgs::NavSatFix::ConstPtr gps_msg =
          m.instantiate<sensor_msgs::NavSatFix>();
      if (gps_msg != NULL) {
        GPSHandler(gps_msg);
        ros::spinOnce();
        rate.sleep();
      }
    }
  }

  return 0;
}


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
* \file     gl_shapes.cpp
* \author   Collin Johnson
*
* Definition of a number of functions for drawing various primitive shapes using OpenGL.
*/

#include <ui/common/gl_shapes.h>
#include <ui/common/ui_color.h>
#include <math/angle_range.h>
#include <core/point.h>
#include <core/multivariate_gaussian.h>
#include <math/geometry/arc.h>
#include <math/geometry/circle.h>
#include <math/geometry/rectangle.h>
#include <math/geometry/polygon.h>
#include <boost/range/iterator_range.hpp>
#include <GL/gl.h>

namespace vulcan
{
namespace ui
{

const float ARROW_ANGLE        = 150.0f;   // degrees of offset for the lines forming the point of the arrow
// The arrow ratios are how long the arrow head is vs. the length
const float SMALL_ARROW_RATIO  = 0.1f;
const float MEDIUM_ARROW_RATIO = 0.2f;
const float LARGE_ARROW_RATIO  = 0.3f;

void draw_filled_partial_ellipse(Point<float>  center,
                                 float               xAxisRadius,
                                 float               yAxisRadius,
                                 math::angle_range_t range,
                                 int                 numSegments);
void draw_line_partial_ellipse(Point<float>  center,
                               float               xAxisRadius,
                               float               yAxisRadius,
                               math::angle_range_t range,
                               float               lineWidth,
                               int                 numSegments);
void draw_arrow(const Point<float>& start, float length, float direction, float lineWidth, float ratio);
void draw_arrow_polygon(const Point<float>& start,
                        float length,
                        float direction,
                        float lineWidth,
                        float ratio,
                        bool filled);
template <typename PointIter>
void gl_draw_filled_polygon_impl(PointIter begin, PointIter end);

template <typename PointIter>
void gl_draw_line_polygon_impl(PointIter begin, PointIter end, float lineWidth);

void gl_draw_filled_circle(const Point<float>& center, float radius, int numSegments)
{
    gl_draw_filled_ellipse(center, radius, radius, numSegments);
}

void gl_draw_line_circle(const Point<float>& center, float radius, float width, int numSegments)
{
    gl_draw_line_ellipse(center, radius, radius, width, numSegments);
}

void gl_draw_filled_arc(const math::Arc<float>& arc, int numSegments)
{
    draw_filled_partial_ellipse(arc.center(), arc.radius(), arc.radius(), arc.range(), numSegments);
}

void gl_draw_line_arc(const math::Arc<float>& arc, float width, int numSegments)
{
    draw_line_partial_ellipse(arc.center(), arc.radius(), arc.radius(), arc.range(), width, numSegments);

    // Draw line segments connecting the center of the arc to the curved portion
    glLineWidth(width);
    glBegin(GL_LINES);
    glVertex2f(arc.center().x, arc.center().y);
    glVertex2f(arc.center().x + (arc.radius() * std::cos(arc.range().start)),
               arc.center().y + (arc.radius() * std::sin(arc.range().start)));
    glVertex2f(arc.center().x, arc.center().y);
    glVertex2f(arc.center().x + (arc.radius() * std::cos(arc.range().start + arc.range().extent)),
               arc.center().y + (arc.radius() * std::sin(arc.range().start + arc.range().extent)));
    glEnd();
}

void gl_draw_filled_circle(const math::Circle<float>& circle, int numSegments)
{
    gl_draw_filled_circle(circle.center(), circle.radius(), numSegments);
}

void gl_draw_line_circle(const math::Circle<float>& circle, float width, int numSegments)
{
    gl_draw_line_circle(circle.center(), circle.radius(), width, numSegments);
}

void gl_draw_filled_ellipse(const Point<float>& center, float xAxisRadius, float yAxisRadius, int numSegments)
{
    draw_filled_partial_ellipse(center, xAxisRadius, yAxisRadius, math::angle_range_t(0.0f, 2*M_PI), numSegments);
}

void gl_draw_line_ellipse(const Point<float>& center, float xAxisRadius, float yAxisRadius, float width, int numSegments)
{
    draw_line_partial_ellipse(center, xAxisRadius, yAxisRadius, math::angle_range_t(0.0f, 2*M_PI), width, numSegments);
}

void gl_draw_filled_polygon(const std::vector<Point<float>>& vertices)
{
    gl_draw_filled_polygon_impl(vertices.begin(), vertices.end());
}

void gl_draw_line_polygon(const std::vector<Point<float>>& vertices, float width)
{
    gl_draw_line_polygon_impl(vertices.begin(), vertices.end(), width);
}

void gl_draw_filled_polygon(const math::Polygon<double>& polygon)
{
    gl_draw_filled_polygon_impl(polygon.begin(), polygon.end());
}

void gl_draw_line_polygon(const math::Polygon<double>& polygon, float width)
{
    gl_draw_line_polygon_impl(polygon.begin(), polygon.end(), width);
}

void gl_draw_filled_rectangle(const math::Rectangle<float>& rect)
{
    glBegin(GL_QUADS);
    glVertex2f(rect.bottomLeft.x,  rect.bottomLeft.y);
    glVertex2f(rect.bottomRight.x, rect.bottomRight.y);
    glVertex2f(rect.topRight.x,    rect.topRight.y);
    glVertex2f(rect.topLeft.x,     rect.topLeft.y);
    glEnd();
}

void gl_draw_line_rectangle(const math::Rectangle<float>& rect, float width)
{
    glLineWidth(width);
    glBegin(GL_LINE_LOOP);
    glVertex2f(rect.bottomLeft.x,  rect.bottomLeft.y);
    glVertex2f(rect.bottomRight.x, rect.bottomRight.y);
    glVertex2f(rect.topRight.x,    rect.topRight.y);
    glVertex2f(rect.topLeft.x,     rect.topLeft.y);
    glEnd();
}

void gl_draw_filled_triangle(const Point<float>& center, float width, float height, float orientation)
{
    glPushMatrix();
    glTranslatef(center.x, center.y, 0.0f);
    glRotatef(orientation*180.0f/M_PI, 0.0f, 0.0f, 1.0f);
    glBegin(GL_TRIANGLES);
    glVertex2f(-height/2.0f, -width/2.0f);
    glVertex2f(height/2.0f, 0.0f);
    glVertex2f(-height/2.0f, width/2.0f);
    glEnd();
    glPopMatrix();
}

void gl_draw_line_triangle(const Point<float>& center, float width, float height, float orientation, float lineWidth)
{
    glLineWidth(lineWidth);
    glPushMatrix();
    glTranslatef(center.x, center.y, 0.0f);
    glRotatef(orientation*180.0f/M_PI, 0.0f, 0.0f, 1.0f);
    glBegin(GL_LINE_LOOP);
    glVertex2f(-height/2.0f, -width/2.0f);
    glVertex2f(height/2.0f, 0.0f);
    glVertex2f(-height/2.0f, width/2.0f);
    glEnd();
    glPopMatrix();
}

void gl_draw_small_arrow(const Point<float>& start, float length, float direction, float lineWidth)
{
    draw_arrow(start, length, direction, lineWidth, SMALL_ARROW_RATIO);
}

void gl_draw_medium_arrow(const Point<float>& start, float length, float direction, float lineWidth)
{
    draw_arrow(start, length, direction, lineWidth, MEDIUM_ARROW_RATIO);
}

void gl_draw_large_arrow(const Point<float>& start, float length, float direction, float lineWidth)
{
    draw_arrow(start, length, direction, lineWidth, LARGE_ARROW_RATIO);
}

void gl_draw_small_arrow_polygon_line(const Point<float>& start, float length, float direction, float lineWidth)
{
    draw_arrow_polygon(start, length, direction, lineWidth, SMALL_ARROW_RATIO, false);
}

void gl_draw_medium_arrow_polygon_line(const Point<float>& start, float length, float direction, float lineWidth)
{
    draw_arrow_polygon(start, length, direction, lineWidth, MEDIUM_ARROW_RATIO, false);
}

void gl_draw_large_arrow_polygon_line(const Point<float>& start, float length, float direction, float lineWidth)
{
    draw_arrow_polygon(start, length, direction, lineWidth, LARGE_ARROW_RATIO, false);
}

void gl_draw_small_arrow_polygon_filled(const Point<float>& start, float length, float direction)
{
    draw_arrow_polygon(start, length, direction, 0.0f, SMALL_ARROW_RATIO, true);
}

void gl_draw_medium_arrow_polygon_filled(const Point<float>& start, float length, float direction)
{
    draw_arrow_polygon(start, length, direction, 0.0f, MEDIUM_ARROW_RATIO, true);
}

void gl_draw_large_arrow_polygon_filled(const Point<float>& start, float length, float direction)
{
    draw_arrow_polygon(start, length, direction, 0.0f, LARGE_ARROW_RATIO, true);
}

void gl_draw_gaussian_distribution(const MultivariateGaussian& gaussian, float numSigma, const GLColor& color, float width)
{
    /*
    * A Gaussian distribution will be drawn as the following:
    *
    *   - A low-alpha filled-in ellipse
    *   - Lines along the major and minor axis
    *   - A solid line bounding the ellipse
    */

    const int NUM_SEGMENTS = 36;

    Vector eigenvalues;
    Matrix eigenvectors;

    Point<float> center(gaussian.getMean()(0), gaussian.getMean()(1));
    Matrix xyCov = gaussian.getCovariance().submat(arma::span(0,1), arma::span(0,1));

    arma::eig_sym(eigenvalues, eigenvectors, xyCov);

    float theta = atan2(eigenvectors(1, 0), eigenvectors(0, 0));

    // The eigenvalues are sigma^2, so scale them to represent the desired number of sigmas for the ellipses
    eigenvalues = numSigma * arma::sqrt(eigenvalues);

    glPushMatrix();

    glTranslatef(center.x, center.y, 0.0f);
    glRotatef(theta*180.0f/M_PI, 0.0f, 0.0f, 1.0f);

    color.set(0.33f);
    gl_draw_filled_ellipse(Point<float>(0.0f, 0.0f), eigenvalues(0), eigenvalues(1), NUM_SEGMENTS);

    color.set();

    glLineWidth(width);
    glBegin(GL_LINES);
    glVertex2f(-eigenvalues(0), 0.0f);
    glVertex2f(eigenvalues(0), 0.0f);

    glVertex2f(0.0f, -eigenvalues(1));
    glVertex2f(0.0f, eigenvalues(1));
    glEnd();

    gl_draw_line_ellipse(Point<float>(0.0f, 0.0f), eigenvalues(0), eigenvalues(1), width, NUM_SEGMENTS);

    glPopMatrix();
}

void draw_filled_partial_ellipse(Point<float>  center,
                                 float               xAxisRadius,
                                 float               yAxisRadius,
                                 math::angle_range_t range,
                                 int                 numSegments)
{
    // Use the triangle fan to easily draw the circle by incrementing the next vertex by the segment interval
    float rotationStep = range.extent / numSegments;

    glBegin(GL_TRIANGLE_FAN);

    glVertex2f(center.x, center.y);

    // Need to do <= numSegments because the first vertex needs to be drawn a second time in order to close the circle
    for(int n = 0; n <= numSegments; ++n)
    {
        glVertex2f(center.x + xAxisRadius*cos(n*rotationStep + range.start),
                   center.y + yAxisRadius*sin(n*rotationStep + range.start));
    }

    glEnd();
}

void draw_line_partial_ellipse(Point<float>  center,
                               float               xAxisRadius,
                               float               yAxisRadius,
                               math::angle_range_t range,
                               float               lineWidth,
                               int                 numSegments)
{
    float rotationStep = range.extent / numSegments;

    glLineWidth(lineWidth);
    glBegin(GL_LINE_STRIP);

    // Need to do <= numSegments because the first vertex needs to be drawn a second time in order to close the circle
    for(int n = 0; n <= numSegments; ++n)
    {
        glVertex2f(center.x + xAxisRadius*cos(n*rotationStep + range.start),
                   center.y + yAxisRadius*sin(n*rotationStep + range.start));
    }

    glEnd();
}

void draw_arrow(const Point<float>& start, float length, float direction, float lineWidth, float ratio)
{
    float headLength = length * ratio;

    glLineWidth(lineWidth);
    glPushMatrix();
    glTranslatef(start.x, start.y, 0.0f);
    glRotatef(direction*180.0/M_PI, 0.0f, 0.0f, 1.0f);
    glBegin(GL_LINES);

    // Body of the arrow
    glVertex2f(0.0f, 0.0f);
    glVertex2f(length, 0.0f);

    // Left arrow
    glVertex2f(length, 0.0f);
    glVertex2f(length - headLength*std::cos(ARROW_ANGLE), headLength*std::sin(ARROW_ANGLE));

    // Right arrow
    glVertex2f(length, 0.0f);
    glVertex2f(length - headLength*std::cos(ARROW_ANGLE), -headLength*std::sin(ARROW_ANGLE));

    glEnd();
    glPopMatrix();
}

void draw_arrow_polygon(const Point<float>& start,
                        float length,
                        float direction,
                        float lineWidth,
                        float ratio,
                        bool filled)
{
    // The body of the array is 2/3 of the length
    // The height of the head is 1/3 the length
    const float headLength = length * ratio;
    const float height = headLength * 0.8f;
    const float bodyLength = length - headLength;
    const float bodyHeight = height / 2.0f;

    math::Rectangle<float> body(Point<float>(0.0f, -bodyHeight), Point<float>(bodyLength, bodyHeight));
    Point<float> triangleBottom(bodyLength, -height);
    Point<float> triangleTop(bodyLength, height);
    Point<float> triangleRight(length, 0.0f);

    glPushMatrix();
    glTranslatef(start.x, start.y, 0.0f);
    glRotatef(direction * 180.0 / M_PI, 0.0, 0.0, 1.0);

    if(filled)
    {
        gl_draw_filled_rectangle(body);
        glBegin(GL_TRIANGLES);
        glVertex2f(triangleTop.x, triangleTop.y);
        glVertex2f(triangleRight.x, triangleRight.y);
        glVertex2f(triangleBottom.x, triangleBottom.y);
        glEnd();
    }
    else
    {

        glLineWidth(lineWidth);
        glBegin(GL_LINE_LOOP);
        glVertex2f(body.bottomRight.x, body.bottomRight.y);
        glVertex2f(body.bottomLeft.x, body.bottomLeft.y);
        glVertex2f(body.topLeft.x, body.topLeft.y);
        glVertex2f(body.topRight.x, body.topRight.y);
        glVertex2f(triangleTop.x, triangleTop.y);
        glVertex2f(triangleRight.x, triangleRight.y);
        glVertex2f(triangleBottom.x, triangleBottom.y);
        glEnd();
    }

    glPopMatrix();
}

template <typename PointIter>
void gl_draw_filled_polygon_impl(PointIter begin, PointIter end)
{
    glBegin(GL_POLYGON);

    for(auto vertex : boost::make_iterator_range(begin, end))
    {
        glVertex2f(vertex.x, vertex.y);
    }

    glEnd();
}

template <typename PointIter>
void gl_draw_line_polygon_impl(PointIter begin, PointIter end, float lineWidth)
{
    glLineWidth(lineWidth);
    glBegin(GL_LINE_LOOP);

    for(auto vertex : boost::make_iterator_range(begin, end))
    {
        glVertex2f(vertex.x, vertex.y);
    }

    glEnd();
}

} // namespace ui
} // namespace vulcan


//   Copyright 2015 Patrick Putnam
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
#ifndef CLOTHO_WEIGHT_DISTRIBUTION_HELPER_HPP_
#define CLOTHO_WEIGHT_DISTRIBUTION_HELPER_HPP_

#include <boost/property_tree/ptree.hpp>
#include <boost/random/normal_distribution.hpp>
#include "clotho/random/normal_distribution_parameter.hpp"

namespace clotho {
namespace genetics {

template < class T >
struct weight_distribution_helper;

template < >
struct weight_distribution_helper< double > {
    typedef boost::random::normal_distribution< double > type;
    typedef typename type::param_type                   result_type;

    static result_type  makeParameter( boost::property_tree::ptree & config ) {
        normal_distribution_parameter< double > param( config );
        return result_type( param.m_mean, param.m_sigma );
    }
};

template < >
struct weight_distribution_helper< float > {
    typedef boost::random::normal_distribution< float >     type;
    typedef typename type::param_type                       result_type;

    static result_type  makeParameter( boost::property_tree::ptree & config ) {
        normal_distribution_parameter< float > param( config );
        return result_type( param.m_mean, param.m_sigma );
    }
};

}   // namespace genetics
}   // namespace clotho

#endif  // CLOTHO_WEIGHT_DISTRIBUTION_HELPER_HPP_


#include <vector>
#include <tuple>

#include <Eigen/Core>

namespace tsp {
typedef Eigen::MatrixXd distance_matrix_t;
typedef std::vector<int> path_t;
typedef Eigen::Matrix<uint8_t, Eigen::Dynamic, Eigen::Dynamic> ordered_neighbors_t;

distance_matrix_t randomDistanceMatrix(int N);
/// Compute nearest neighbor from i
ordered_neighbors_t neighborMatrix(const distance_matrix_t& d);
double evaluateCost(const distance_matrix_t& d, const path_t& path);

namespace heuristic_nearest {
std::tuple<double, path_t> solve (const distance_matrix_t& d);
}
namespace dynamic_programming {
std::tuple<double, path_t> solveWithBound (const distance_matrix_t& d,
    double costUpperBound,
    bool allowPruning = true);

inline std::tuple<double, path_t> solve (const distance_matrix_t& d,
    bool allowPruning = true)
{
  return solveWithBound(d, std::numeric_limits<double>::infinity(), allowPruning);
}

std::tuple<double, path_t> solveWithHeuristic (const distance_matrix_t& d,
    bool allowPruning = true);

}
namespace brute_force {
std::tuple<double, path_t> solve (const distance_matrix_t& d);
}
namespace approximative_kopt {
void swap2opt(path_t& path, int i, int k);
bool swap3opt(const distance_matrix_t& d, path_t& path, int i, int j, int k);
std::tuple<double, path_t> solve2opt (const distance_matrix_t& d, path_t initialGuess = path_t());
std::tuple<double, path_t> solve3opt (const distance_matrix_t& d, path_t initialGuess = path_t());
}
}


/**
 * @author     : Zhao Chonyyao (cyzhao@zju.edu.cn)
 * @date       : 2021-04-30
 * @description: embedded elasticity finite element method problem
 * @version    : 1.0
 */
#include <memory>
#include <iomanip>
#include <boost/property_tree/ptree.hpp>

#include "Common/error.h"

// TODO: possible bad idea of having dependence to model in problem module
#include "Model/fem/elas_energy.h"
#include "Model/fem/mass_matrix.h"

#include "Problem/energy/basic_energy.h"
#include "Io/io.h"
#include "Geometry/extract_surface.imp"
#include "Geometry/interpolate.h"
#include "libigl/include/igl/readOBJ.h"

#include "embedded_elas_fem_problem.h"

namespace PhysIKA {
using namespace std;
using namespace Eigen;
using namespace igl;

template <typename T>
using MAT = Eigen::Matrix<T, -1, -1>;
template <typename T>
using VEC = Eigen::Matrix<T, -1, 1>;

template <typename T>
embedded_elas_problem_builder<T>::embedded_elas_problem_builder(const T* x, const boost::property_tree::ptree& pt)
    : pt_(pt)
{
    //TODO: need to check exception
    using Vector3T               = Eigen::Matrix<T, 3, 1>;
    const string filename        = pt.get<string>("filename");
    const string filename_coarse = pt.get<string>("filename_coarse");
    cout << "filename " << filename << "\n filename_coarse " << filename_coarse << endl;
    MAT<T>   nods;
    MatrixXi cells;

    MAT<T>   nods_coarse;
    MatrixXi cells_coarse;

    string type = pt.get<string>("type", "tet");
    if (type == "vox")
        type = "hex";
    string type_coarse = pt.get<string>("type_coarse", type);
    if (type_coarse == "vox")
        type_coarse = "hex";
    cout << "mesh type is " << type << endl;
    cout << "coarse type is " << type_coarse << endl;
    if (type_coarse == "tet")
    {
        IF_ERR(exit, mesh_read_from_vtk<T, 4>(filename_coarse.c_str(), nods_coarse, cells_coarse));
    }
    else if (type_coarse == "hex")
    {
        exit_if(mesh_read_from_vtk<T, 8>(filename_coarse.c_str(), nods_coarse, cells_coarse));
    }
    else if (type_coarse == "hybrid")
    {
        cout << "read hybrid mesh " << filename << endl;
        exit_if(mesh_read_from_vtk<T, 8>(filename_coarse.c_str(), nods_coarse));
    }
    else
    {
        // error_msg("type:<%s> is not supported.", type.c_str());
    }
    cout << "number of cells is " << cells_coarse.cols() << endl;
#if 1
    if (type_coarse == "hybrid")
    {
        Vector3T nods_min = nods_coarse.col(0);
        Vector3T nods_max = nods_coarse.col(1);
        for (size_t i = 0; i < 3; ++i)
        {
            nods_min(i) = nods_coarse.row(i).minCoeff();
            nods_max(i) = nods_coarse.row(i).maxCoeff();
        }
        Matrix<T, 3, 8> nods_coarsest = Matrix<T, 3, 8>::Ones();
        //set z
        nods_coarsest.block<1, 4>(2, 0) *= nods_min(2);
        nods_coarsest.block<1, 4>(2, 4) *= nods_max(2);
        //set y
        Vector4i y_min{ { 2, 3, 6, 7 } };
        Vector4i y_max{ { 0, 1, 4, 5 } };
        nods_coarsest(1, y_min) *= nods_min(1);
        nods_coarsest(1, y_max) *= nods_max(1);
        //set x
        Vector4i x_min{ { 0, 3, 4, 7 } };
        Vector4i x_max{ { 1, 2, 5, 6 } };
        nods_coarsest(0, x_min) *= nods_min(0);
        nods_coarsest(0, x_max) *= nods_max(0);

        nods_coarse = nods_coarsest;

        cout << nods_coarse << endl;
        cells_coarse.resize(8, 1);
        cells_coarse.col(0).setLinSpaced(8, 0, 7);
        Eigen::MatrixXi hexs2tets = hex_2_tet(cells_coarse);
        cells_coarse              = hexs2tets;

        cout << cells_coarse << endl;
        type_coarse = "tet";
    }
#endif
    if (filename.rfind(".obj") != string::npos)
    {
        readOBJ(filename.c_str(), nods, cells);
        nods.transposeInPlace();
        cells.transposeInPlace();
    }
    else
    {
        //TODO: need to check file reading error
        if (type == "tet")
        {
            IF_ERR(exit, mesh_read_from_vtk<T, 4>(filename.c_str(), nods, cells));
        }
        else if (type == "hex")
        {
            exit_if(mesh_read_from_vtk<T, 8>(filename.c_str(), nods, cells));
        }
        //else if (type == "hybrid") {
        //	cout << "read hybrid mesh " << filename << endl;
        //	exit_if(mesh_read_from_vtk<T, 8>(filename.c_str(), nods));
        //

        //}
        else
        {
            // error_msg("type:<%s> is not supported.", type.c_str());
        }
    }

    if (cells.size() == 0)
        cells.resize(4, 0);
    if (cells_coarse.size() == 0)
        cells_coarse.resize(4, 0);
    interp_pts_in_tets<T, 3>(nods, cells, nods_coarse, fine_to_coarse_coef_);
    //interp_pts_in_point_cloud<T, 3>(nods_coarse, nods, coarse_to_fine_coef_);

    if (type_coarse == "tet")
        interp_pts_in_tets<T, 3>(nods_coarse, cells_coarse, nods, coarse_to_fine_coef_);
    else
    {
        //interp_pts_in_point_cloud<T, 3>(nods_coarse, nods, coarse_to_fine_coef_);
        Eigen::MatrixXi hexs2tets = hex_2_tet(cells_coarse);
        cout << "size of hexs2tets " << hexs2tets.rows() << " " << hexs2tets.cols() << endl;
        interp_pts_in_tets<T, 3>(nods_coarse, hexs2tets, nods, coarse_to_fine_coef_);
    }

    const size_t num_nods = nods_coarse.cols();
    cout << "V" << nods_coarse.rows() << " " << nods_coarse.cols() << endl
         << "T " << cells_coarse.rows() << " " << cells_coarse.cols() << endl;
    if (x != nullptr)
    {
        nods        = Map<const MAT<T>>(x, nods.rows(), nods.cols());
        nods_coarse = nods * fine_to_coarse_coef_;
    }

    cout << "Boundary Box :\n"
         << nods_coarse.rowwise().minCoeff() << endl
         << nods_coarse.rowwise().maxCoeff() << endl;

    REST_  = nods;
    cells_ = cells;
    /*Matrix<T, -1, -1> nods_temp = nods_coarse;*/
    fine_verts_num_ = REST_.cols();

    auto         phy_paras = pt.get_child("physics");
    const T      rho       = phy_paras.get<T>("rho", 20);
    const T      Young     = phy_paras.get<T>("Young", 2000.0);
    const T      poi       = phy_paras.get<T>("poi", 0.3);
    const T      gravity   = phy_paras.get<T>("gravity", 9.8);
    const T      dt        = phy_paras.get<T>("dt", 0.01);
    const T      w_pos     = phy_paras.get<T>("w_pos", 1e6);
    const size_t num_frame = phy_paras.get<size_t>("num_frames", 100);

    //read fixed points
    vector<size_t> cons(0);
    const string   cons_file_path = pt.get<string>("cons", "");
    /*if(cons_file_path != "")
    IF_ERR(exit, read_fixed_verts_from_csv(cons_file_path.c_str(), cons));*/
    cout << "constrint " << cons.size() << " points" << endl;

    //calc mass vector
    Matrix<T, -1, 1> mass_vec(num_nods);
    // calc_mass_vector<T>(nods, cells, rho, mass_vec);
    if (type_coarse == "tet")
        mass_calculator<T, 3, 4, 1, 1, basis_func, quadrature>(nods_coarse, cells_coarse, rho, mass_vec);
    else if (type_coarse == "hex")
        mass_calculator<T, 3, 8, 1, 2, basis_func, quadrature>(nods_coarse, cells_coarse, rho, mass_vec);

    cout << "build energy" << endl;
    int ELAS = 0;
    int GRAV = 1;
    int KIN  = 2;
    int POS  = 3;
    if (pt_.get<string>("solver_type") == "explicit")
        POS = 2;

    ebf_.resize(POS + 1);
    {
        const string csttt_type = phy_paras.get<string>("csttt", "linear");

        gen_elas_energy_intf<T>(type_coarse, csttt_type, nods_coarse, cells_coarse, Young, poi, ebf_[ELAS], &elas_intf_);
        /* nods_coarse = nods_temp;*/
        // to lowercase.
        char axis  = pt.get<char>("grav_axis", 'y') | 0x20;
        ebf_[GRAV] = make_shared<gravity_energy<T, 3>>(num_nods, 1, gravity, mass_vec, axis);
        kinetic_   = make_shared<momentum<T, 3>>(nods_coarse.data(), num_nods, mass_vec, dt);

        if (pt_.get<string>("solver_type") == "implicit")
            ebf_[KIN] = kinetic_;

        ebf_[POS] = make_shared<position_constraint<T, 3>>(nods_coarse.data(), num_nods, w_pos, cons);
    }
    cout << "set up energy done." << endl;

    //set constraint

    enum constraint_type
    {
        COLL
    };
    cbf_.resize(COLL + 1);
    collider_  = nullptr;
    cbf_[COLL] = collider_;

    shared_ptr<Problem<T, 3>> pb      = make_shared<Problem<T, 3>>(ebf_[0], nullptr);
    auto                      dat_str = make_shared<dat_str_core<T, 3>>(pb->Nx() / 3, pt.get<bool>("hes_is_const", false));
    compute_hes_pattern(pb->energy_, dat_str);
    ebf_[0]->Hes(nods_coarse.data(), dat_str);
    SparseMatrix<T> K = dat_str->get_hes();

    embedded_interp_ = make_shared<embedded_interpolate<T>>(nods_coarse, coarse_to_fine_coef_, fine_to_coarse_coef_, K, 0.586803 / 2);

    if (pt_.get<string>("solver_type") == "explicit")
    {
        Map<Matrix<T, -1, 1>> position(nods_coarse.data(), nods_coarse.size());
        semi_implicit_ = make_shared<semi_implicit<T>>(dt, mass_vec, position);
    }
    cout << "init problem done." << endl;
}

template <typename T>
std::shared_ptr<Problem<T, 3>> embedded_elas_problem_builder<T>::build_problem() const
{
    cout << "assemble energy" << endl;
    shared_ptr<Functional<T, 3>> energy;
    try
    {
        energy = build_energy_t<T, 3>(ebf_);
    }
    catch (std::exception& e)
    {
        cerr << e.what() << endl;
        exit(EXIT_FAILURE);
    }

    shared_ptr<Constraint<T>> constraint;
    cout << "assemble constraint" << endl;
    bool all_null = true;
    for (auto& c : cbf_)
        if (c != nullptr)
            all_null = false;
    if (all_null)
    {
        constraint = nullptr;
        cout << "WARNGING: No hard constraints." << endl;
    }
    else
    {
        try
        {
            constraint = build_constraint_t<T>(cbf_);
        }
        catch (std::exception& e)
        {
            cerr << e.what() << endl;
            exit(EXIT_FAILURE);
        }
    }
    exit_if(constraint != nullptr && energy->Nx() != constraint->Nx(), "energy and constraint has different dimension.");
    return make_shared<Problem<T, 3>>(energy, constraint);
}

template <typename T>
int embedded_elas_problem_builder<T>::update_problem(const T* x, const T* v)
{
    embedded_interp_->update_verts(x, fine_verts_num_);
    const Eigen::Matrix<T, -1, -1>& verts = embedded_interp_->get_verts();

    IF_ERR(return, kinetic_->update_location_and_velocity(verts.data(), v));
    if (collider_ != nullptr)
        IF_ERR(return, collider_->update(verts.data()));
    return 0;
}

template class embedded_elas_problem_builder<double>;

template class embedded_elas_problem_builder<float>;

}  // namespace PhysIKA


// Copyright Stephan T. Lavavej, http://nuwen.net .
// Distributed under the Boost Software License, Version 1.0.
// See accompanying file LICENSE_1_0.txt or copy at
// http://boost.org/LICENSE_1_0.txt .

#ifndef PHAM_RANDOM_HH
#define PHAM_RANDOM_HH

#include "compiler.hh"

#ifdef NUWEN_PLATFORM_MSVC
    #pragma once
#endif

#include "clock.hh"
#include "time.hh"
#include "typedef.hh"
#include "vector.hh"

#include "external_begin.hh"
    #include <boost/random/mersenne_twister.hpp>
    #include <boost/utility.hpp>
#include "external_end.hh"

namespace nuwen {
    namespace random {
        class twister : public boost::noncopyable {
        public:
            inline twister();

            inline uc_t   random_uc();
            inline us_t   random_us();
            inline ul_t   random_ul();
            inline ull_t  random_ull();
            inline float  random_float_0_1();
            inline double random_double_0_1();

        private:
            boost::mt19937 m_mt;
        };
    }
}

namespace pham {
    inline nuwen::ul_t make_seed() {
        using namespace nuwen;

        const ull_t ctr = clock_ctr();

        const ul_t a = static_cast<ul_t>(ctr >> 32);
        const ul_t b = static_cast<ul_t>(ctr & 0xFFFFFFFFUL);

        const ull_t uqh = ull_from_sll(current_qh_time());

        const ul_t c = static_cast<ul_t>(uqh >> 32);
        const ul_t d = static_cast<ul_t>(uqh & 0xFFFFFFFFUL);

        return a ^ b ^ c ^ d;
    }
}

inline nuwen::random::twister::twister() : m_mt(pham::make_seed()) { }

inline nuwen::uc_t nuwen::random::twister::random_uc() {
    return static_cast<nuwen::uc_t>(m_mt() & 0xFF);
}

inline nuwen::us_t nuwen::random::twister::random_us() {
    return static_cast<nuwen::us_t>(m_mt() & 0xFFFF);
}

inline nuwen::ul_t nuwen::random::twister::random_ul() {
    return m_mt();
}

inline nuwen::ull_t nuwen::random::twister::random_ull() {
    ull_t ret = m_mt();
    ret <<= 32;
    ret |= m_mt();

    return ret;
}

inline float nuwen::random::twister::random_float_0_1() {
    return static_cast<float>(random_ul()) / static_cast<float>(0xFFFFFFFFUL);
}

inline double nuwen::random::twister::random_double_0_1() {
    return static_cast<double>(random_ull()) / static_cast<double>(0xFFFFFFFFFFFFFFFFULL);
}

#endif // Idempotency


////////////////////////////////////////////////////////////////////////////////
//  Copyright (c) 2021 Andreas Wagner.
//
//  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)
////////////////////////////////////////////////////////////////////////////////

#ifndef TUMORMODELS_UPWINDING_FORMULAS_LINEARIZED_FLOW_HPP
#define TUMORMODELS_UPWINDING_FORMULAS_LINEARIZED_FLOW_HPP

#include <Eigen/Dense>
#include <cassert>

#include "vessel_formulas.hpp"

namespace macrocirculation {

namespace linearized {

/*! @brief Calculates the upwinding at an inner boundary.
 *
 * @param alpha The \f$ \sqrt{\frac{C}{L}} \f$ factor for the linearized characteristics.
 * @param p_l   The pressure at the boundary of the left cell.
 * @param q_l   The flux at the boundary of the left cell.
 * @param p_r   The pressure at the boundary of the right cell.
 * @param q_r   The flux at the boundary of the right cell.
 * @param p_up  The upwinded pressure.
 * @param q_up  The upwinded flux.
 */
inline void inner_boundary(double alpha, double p_l, double q_l, double p_r, double q_r, double &p_up, double &q_up) {
  q_up = 0.5 * (q_l + q_r + alpha * (p_l - p_r));
  p_up = (alpha * p_l + q_l  - q_up) / alpha;
}

/*! @brief Calculates the upwinding at an nfurcation.
 *
 * @param p         The boundary pressure values of each vessel at a common vertex.
 * @param q         The boundary flux values of each vessel at a common vertex.
 * @param params    The physical edge parameters near the nfurcation.
 * @param sigma     The vessel normals (+1 if the vessel points to the vertex, -1 if not).
 * @param p_up      The upwinded pressure values.
 * @param q_up      The upwinded flux values.
 */
inline void nfurcation_boundary(const std::vector<double> &p,
                                const std::vector<double> &q,
                                const std::vector<VesselParameters> &params,
                                const std::vector<double> &sigma,
                                std::vector<double> &p_up,
                                std::vector<double> &q_up) {
  // all vectors need to have the same size:
  assert(p.size() == q.size());
  assert(p_up.size() == p.size());
  assert(q_up.size() == p.size());
  assert(params.size() == p.size());
  assert(sigma.size() == p.size());

  const size_t N = p.size();

  // dof-ordering: (p_1, q_1, p_2, q_2, ... p_N, q_N)
  Eigen::MatrixXd mat(p.size() + q.size(), p.size() + q.size());
  Eigen::VectorXd rhs(p.size() + q.size());

  mat.setZero();

  // vector containing the \f$ \sqrt{ \frac{C}{L} } \f$ factors:
  std::vector<double> alpha;
  for (auto &param : params) {
    alpha.push_back(std::sqrt(linear::get_C(param) / linear::get_L(param)));
  }

  // constrain the upwinded fluxes to zero:
  for (int k = 0; k < N; k += 1) {
    mat(0, 2 * k + 1) = sigma[k];
  }
  rhs(0) = 0;

  // the characteristics should be equal
  for (int k = 0; k < N; k += 1) {
    mat(1 + k, 2 * k) = 0.5 * alpha[k] * sigma[k];
    mat(1 + k, 2 * k + 1) = 0.5;
    rhs(1 + k) = 0.5 * alpha[k] * sigma[k] * p[k] + 0.5 * q[k];
  }

  // the pressures should be equal
  for (int k = 1; k < N; k += 1) {
    mat(N + k, 2 * k) = 1.;
    mat(N + k, 2 * (k - 1)) = -1.;
    rhs(N + k) = 0.;
  }

  Eigen::VectorXd result = mat.fullPivLu().solve(rhs);

  for (int k = 0; k < N; k += 1) {
    p_up[k] = result[2 * k];
    q_up[k] = result[2 * k + 1];
  }
}

} // namespace linearized

} // namespace macrocirculation

#endif //TUMORMODELS_UPWINDING_FORMULAS_LINEARIZED_FLOW_HPP


#ifndef __num_t_hpp__
#define __num_t_hpp__

/**
 * @file
 *
 * @copyright 2013 Recalcitrant Software, LLC.
 * @license Distributed under the Boost Software License,
 * Version 1.0. See included LICENSE.md or
 * http://www.boost.org/LICENSE_1_0.txt for the complete
 * license.
 */

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>

using int_t = boost::multiprecision::number
<
   boost::multiprecision::cpp_int_backend<>,
   boost::multiprecision::expression_template_option::et_off
>;

using num_t = boost::multiprecision::number
<
   boost::multiprecision::cpp_dec_float<1024>,
   boost::multiprecision::expression_template_option::et_off
>;

#endif


#ifndef STAN_MATH_PRIM_FUN_MODIFIED_BESSEL_FIRST_KIND_HPP
#define STAN_MATH_PRIM_FUN_MODIFIED_BESSEL_FIRST_KIND_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <boost/math/special_functions/bessel.hpp>

namespace stan {
namespace math {

/**
 *
   \f[
   \mbox{modified\_bessel\_first\_kind}(v, z) =
   \begin{cases}
     I_v(z) & \mbox{if } -\infty\leq z \leq \infty \\[6pt]
     \textrm{error} & \mbox{if } z = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{modified\_bessel\_first\_kind}(v, z)}{\partial z} =
   \begin{cases}
     \frac{\partial\, I_v(z)}{\partial z} & \mbox{if } -\infty\leq z\leq \infty
 \\[6pt] \textrm{error} & \mbox{if } z = \textrm{NaN} \end{cases} \f]

   \f[
     {I_v}(z) = \left(\frac{1}{2}z\right)^v\sum_{k=0}^\infty
 \frac{\left(\frac{1}{4}z^2\right)^k}{k!\Gamma(v+k+1)} \f]

     \f[
     \frac{\partial \, I_v(z)}{\partial z} = I_{v-1}(z)-\frac{v}{z}I_v(z)
     \f]
 *
 */
template <typename T2>
inline T2 modified_bessel_first_kind(int v, const T2 z) {
  check_not_nan("modified_bessel_first_kind", "z", z);

  return boost::math::cyl_bessel_i(v, z);
}

}  // namespace math
}  // namespace stan

#endif


// Copyright (c) 2018 Evan S Weinberg
// Test bounds on the residual of sequential solves

#include <iostream>
#include <iomanip>
#include <cmath>
#include <string>
#include <sstream>
#include <complex>
#include <random>

// Borrow dense matrix eigenvalue routines.
#include <Eigen/Dense>

#include "blas/generic_vector.h"

#include "square_wilson.h"
#include "inverters/generic_bicgstab.h"

using namespace std;
using namespace Eigen;

typedef Matrix<std::complex<double>, Dynamic, Dynamic, ColMajor> cMatrix;

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

  complex<double> *gauge_links;

  // Set output precision to be long.
  cout << setprecision(10);

  // Inversion info
  inversion_info invif;

  // RNG related things.
  std::mt19937 generator (1337u); // RNG, 1337u is the seed.
  double inv_variance = 6.0; // inverse of variance for gaussian non-compact U(1) links.

  // Basic information about the lattice.
  int length = 24;
  double m_sq = 0.001;

  // max iter
  int max_iter = 100000;

  // Set tolerances below.I'll nonetheless type the proof up tomorrow, but in any case, I'm feeling good

  // Some start-up.
  int volume = length*length;
  int wilson_volume = 2*volume;

  // Create a random compact U(1) link.
  gauge_links = allocate_vector<complex<double>>(2*length*length);
  gaussian_real(gauge_links, 2*length*length, generator, 1.0/inv_variance);
  polar(gauge_links, 2*length*length);

  // Structure which gets passed to the function.
  laplace_gauged_struct lapstr_gauged;
  lapstr_gauged.length = length;
  lapstr_gauged.m_sq = m_sq;
  lapstr_gauged.gauge_links = gauge_links;

  ///////////////////////////////////////////////////
  // GET THE CONDITION NUMBER OF THE WILSON MATRIX //
  ///////////////////////////////////////////////////

  // Need to get the sqrt of the largest and smallest eigenvalue of the normal op

  double condition_number = 0;

  {
    complex<double> *rhs_cplx;
    complex<double> *inter_cplx;

    // Vectors.
    rhs_cplx = allocate_vector<complex<double>>(wilson_volume);
    inter_cplx = allocate_vector<complex<double>>(wilson_volume);

    // Zero out the vector.
    zero_vector(rhs_cplx, 2*length*length);
    zero_vector(inter_cplx, 2*length*length);

    // Allocate a sufficiently gigantic matrix.
    cMatrix mat_cplx = cMatrix::Zero(wilson_volume, wilson_volume);

    // Form matrix elements. This is where it's important that
    // dMatrix and cMatrix are column major.
    // I should probably make this safer by using a "Map".
    for (int i = 0; i < wilson_volume; i++)
    {
      // Set a point on the rhs for a matrix element.
      zero_vector(rhs_cplx, wilson_volume);
      rhs_cplx[i] = 1.0;
      zero_vector(inter_cplx, wilson_volume);
      square_wilson_gauged(inter_cplx, rhs_cplx, &lapstr_gauged);

      // Where we put the result of the matrix element.
      complex<double>* mptr = &(mat_cplx(i*wilson_volume));

      square_wilson_dagger_gauged(mptr, inter_cplx, &lapstr_gauged);
    }

    SelfAdjointEigenSolver<cMatrix> eigsolve_cplx(volume);
    eigsolve_cplx.compute(mat_cplx);

    double largest_eval = real(eigsolve_cplx.eigenvalues()(wilson_volume-1));
    double smallest_eval = real(eigsolve_cplx.eigenvalues()(0));
    std::cout << " Largest singular value: " << sqrt(largest_eval) << "\n";
    std::cout << "Smallest singular value: " << sqrt(smallest_eval) << "\n";

    condition_number = sqrt(largest_eval/smallest_eval);
    std::cout << "       Condition number: " << condition_number << "\n";

    deallocate_vector(&rhs_cplx);
    deallocate_vector(&inter_cplx);
  }

  ///////////////////////////////////
  // PERFORM SEQUENTIAL INVERSIONS //
  ///////////////////////////////////

  // truly desired tolerance.
  double eps = 1e-10;

  // tolerances
  //double eps_1 = 1e-3;
  //double eps_2 = 1e-14;

  // bound
  double eps_1 = eps/2;
  double eps_2 = eps/(2*condition_number);

  {
    // Get a rhs b.
    complex<double>* b;
    b = allocate_vector<complex<double>>(wilson_volume);
    gaussian_real(b, wilson_volume, generator, 1.0/inv_variance);

    double bnorm = sqrt(norm2sq(b, wilson_volume));

    // We want to solve M^dag M x = b.
    // Define Y == M^dag, X == M
    // Solve Y X x = b sequentially.
    // First, define y == X x and solve Y y = b.
    std::cout << "\nSolve Y y = b\n";
    complex<double>* y;
    y = allocate_vector<complex<double>>(wilson_volume);
    zero_vector(y, wilson_volume);

    invif = minv_vector_bicgstab(y, b, wilson_volume, max_iter, eps_1, square_wilson_dagger_gauged, &lapstr_gauged);
    if (invif.success == true)
    {
      printf("  Algorithm %s took %d iterations to reach a tolerance of %.8e.\n", invif.name.c_str(), invif.iter, sqrt(invif.resSq)/bnorm);
    }

    // Next, solve M x = y
    double ynorm = sqrt(norm2sq(y, wilson_volume));
    std::cout << "\nSolve X x = y\n";
    complex<double>* x;
    x = allocate_vector<complex<double>>(wilson_volume);
    zero_vector(x, wilson_volume);

    invif = minv_vector_bicgstab(x, y, wilson_volume, max_iter, eps_2, square_wilson_gauged, &lapstr_gauged);
    if (invif.success == true)
    {
      printf("  Algorithm %s took %d iterations to reach a tolerance of %.8e.\n", invif.name.c_str(), invif.iter, sqrt(invif.resSq)/ynorm);
    }

    // Do the verify.
    complex<double>* tmp;
    tmp = allocate_vector<complex<double>>(wilson_volume);
    zero_vector(tmp, wilson_volume);

    complex<double>* check;
    check = allocate_vector<complex<double>>(wilson_volume);
    zero_vector(check, wilson_volume);

    // Apply M^dag M
    square_wilson_gauged(tmp, x, &lapstr_gauged);
    square_wilson_dagger_gauged(check, tmp, &lapstr_gauged);
    double rel_res = sqrt(diffnorm2sq(check, b, wilson_volume))/bnorm;
    printf("\nThe normal op tolerance is %.8e.\n", rel_res);

    // Check the equality.
    printf("\nThis should be below %.8e.\n", eps_1 + condition_number*eps_2);

    deallocate_vector(&b);
    deallocate_vector(&y);
    deallocate_vector(&x);
    deallocate_vector(&tmp);
    deallocate_vector(&check);

  }

  //////////////
  // CLEAN UP //
  //////////////

  // Free the lattice.
  //delete[] lattice;

  deallocate_vector(&gauge_links);
  return 0;
}


#pragma once

#include <cmath>
#include <numeric>
#include <vector>
#include <limits>
#include <iomanip>
#include <fstream>
#include <iostream>
using namespace std;

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Sparse>
using namespace Eigen;

#include <boost/version.hpp>
#include <boost/limits.hpp>
#include <boost/filesystem.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/numeric/conversion/bounds.hpp>

#include <boost/config.hpp>
#include <boost/program_options.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/detail/config_file.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/algorithm/string.hpp>
namespace po = boost::program_options;

#ifndef UTIL_H
#define UTIL_H

const double Inf = numeric_limits<double>::infinity();
const double pi = boost::math::constants::pi<double>();
const double log_2 = boost::math::constants::ln_two<double>();
const double log_pi = log(boost::math::constants::pi<double>());
const double pi_180 = pi / 180.0;

typedef Eigen::SparseMatrix<double> SpMat; // Declares a column-major sparse matrix type of double
typedef Eigen::Triplet<double> Tri;

class Params {
public:

    Params( );
    ~Params( );
    Params(const string &params_file, const long seed_from_command_line);
    bool check_input_params( ) const;

    friend ostream& operator<<(ostream& out, const Params& params);

    long seed;
    bool testing;
    bool diploid;
    string datapath, mcmcpath, prevpath, gridpath, olderpath;
    double qEffctHalfInterval, mEffctHalfInterval;
    double mrateMuLowerBound, qrateMuLowerBound, mrateMuUpperBound, qrateMuUpperBound;
    double mSeedsProposalS2, mSeedsProposalS2x, mSeedsProposalS2y;
    double qSeedsProposalS2, qSeedsProposalS2x, qSeedsProposalS2y;
    double qEffctProposalS2, mEffctProposalS2, mrateMuProposalS2, qrateMuProposalS2;
    double momegaProposalS2, qomegaProposalS2;
    double mnegBiProb, qnegBiProb;
    double qVoronoiPr;
    double min_omegam, max_omegam;
    double min_omegaq, max_omegaq;
    double temp;
    double lowerBound, upperBound, genomeSize;
    int numMCMCIter, numBurnIter, numThinIter;
    int nDemes, nIndiv, mnegBiSize, qnegBiSize;
    string distance;
};

double get_bootstrap_var(const MatrixXi &Sims, VectorXd cvec, const VectorXi &indiv2deme, int nb, int alpha, int beta);
double poisln(const MatrixXd &expectedIBD, const MatrixXd &observedIBDCnt, const MatrixXd &ceffective, const MatrixXd &cMatrix);
VectorXd split(const string &line);
double mvgammaln(const double a, const int p);
double max(double a, double b);
MatrixXd pairwise_distance(const MatrixXd &X, const MatrixXd &Y);
MatrixXd readMatrixXd(const string &filename);
double trace_AxB(const MatrixXd &A, const MatrixXd &B);
void getWeights(VectorXd &w, VectorXd &x);
double median(vector<double> &v);

bool dlmcell(const string &filename, const VectorXd &sizes, const vector<double> &array);
void removeRow(MatrixXd &matrix, const int rowToRemove);
void removeElem(VectorXd &vector, const int elemToRemove);
void insertRow(MatrixXd &mat, const VectorXd &row);
void insertElem(VectorXd &vec, const double &elem);

double dnegbinln(const int k, const int size, const double prob);
double dinvgamln(const double x, const double shape, const double scale);
double dmvnormln(const VectorXd &x, const VectorXd &mu, const MatrixXd &sigma);
double dtrnormln(const double x, const double mu, const double sigma2, const double bnd);

VectorXd slice(const VectorXd &A, const VectorXi &I);
MatrixXd slice(const MatrixXd &A, const VectorXi &R, const VectorXi &C);

MatrixXd greatcirc_dist(const MatrixXd &X, const MatrixXd &Y);
MatrixXd euclidean_dist(const MatrixXd &X, const MatrixXd &Y);

#endif


#include <boost/test/unit_test.hpp>

#include "Core/ArrowUtilities.h"
#include "../plotter/Matplotlib/Plot.h"
#include "../learn/Learn.h"
#include "../learn/SKLearn.h"

BOOST_AUTO_TEST_CASE(DoubleColumnNumpyRoundtrip)
{
    auto col = toColumn<std::optional<double>>({ 1.0, 2.0, std::nullopt, 3.0 });
    auto nar = columnToNpArr(*col);
    auto col2 = npArrayToColumn(nar, col->name());
    BOOST_CHECK(col->Equals(*col2));
}

struct RegressionFixture
{
    double coef = 2.0;
    double intercept = 1.0;
    double linearMap(double x) { return coef * x + intercept; };

    std::vector<double> xsVector{ 1.0, 2.0, 3.0, 4.0, 5.0 };
    std::vector<double> ysVector = transformToVector(xsVector, [this](double x) { return linearMap(x); });

    std::vector<double> newSample{ 20.0 };

    std::shared_ptr<arrow::Table> xs = tableFromVectors(xsVector);
    std::shared_ptr<arrow::Column> ys = toColumn(ysVector);

    double predictAt20(pybind11::object model)
    {
        auto predictedAt20 = toVector<double>(*sklearn::predict(model, *tableFromVectors(newSample)));
        return predictedAt20.at(0);
    }
};

BOOST_FIXTURE_TEST_CASE(LinearRegression, RegressionFixture)
{
    auto linReg = sklearn::newLinearRegression();
    BOOST_CHECK_EQUAL((std::string)linReg.get_type().str(), "<class 'sklearn.linear_model.base.LinearRegression'>");
    sklearn::fit(linReg, *xs, *ys);;

    auto inferredCoef = linReg.attr("coef_").cast<double>();
    auto inferredIntercept = linReg.attr("intercept_").cast<double>();
    BOOST_CHECK_EQUAL(inferredCoef, coef);
    BOOST_CHECK_EQUAL(inferredIntercept, intercept);

    BOOST_CHECK_EQUAL(sklearn::score(linReg, *xs, *ys), 1.0);

    auto predictedAt20 = predictAt20(linReg);
    BOOST_CHECK_EQUAL(predictedAt20, linearMap(20));
}

BOOST_FIXTURE_TEST_CASE(LogisticRegression, RegressionFixture)
{
    // TODO: find a better example
    // now just check that functions can be called to obtain whatever results
    auto logReg = sklearn::newLogisticRegression(5.25);
    BOOST_CHECK_EQUAL((std::string)logReg.get_type().str(), "<class 'sklearn.linear_model.logistic.LogisticRegression'>");
    BOOST_CHECK_EQUAL(logReg.attr("C").cast<double>(), 5.25);

    sklearn::fit(logReg, *xs, *ys);
    //BOOST_CHECK_EQUAL(sklearn::score(logReg, *xs, *ys), 1.0);

    // see what got inferred

    auto predictedAt20 = predictAt20(logReg);
    //BOOST_CHECK_EQUAL(predictedAt20, linearMap(20));

}


#define BOOST_TEST_MODULE blas

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <mla/vector/all.h++>

#include <mla/operations/level1/asum.h++>

typedef boost::mpl::list<
	mla::vector::Dense<float>,
	mla::vector::Dense<double>,
	mla::vector::SparseCS<float>,
	mla::vector::SparseCS<double>
> vector_type_list;

BOOST_AUTO_TEST_SUITE(test_boost_level1)

BOOST_AUTO_TEST_CASE_TEMPLATE( blas_level1_test_asum_one_dense, VectorType, vector_type_list )
{
	using namespace mla;

	VectorType x(3);
	x.setValue(0, 1.0f);
	x.setValue(1, 1.0f);
	x.setValue(2, 1.0f);

	typename VectorType::scalar_type value;
	value = asum(x);

	BOOST_CHECK_CLOSE( value, 3.0f, 1.0e-5 );
}

BOOST_AUTO_TEST_CASE_TEMPLATE( blas_level1_test_asum_one_sparse, VectorType, vector_type_list )
{
	using namespace mla;

	VectorType x(5);
	x.setValue(0, 1.0f);
	x.setValue(2, 1.0f);
	x.setValue(4, 1.0f);

	typename VectorType::scalar_type value;
	value = asum(x);

	BOOST_CHECK_CLOSE( value, 3.0f, 1.0e-5 );
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <Eigen/Dense>

namespace boltzmann {

/**
 * @brief Barycentric interpolation formula
 *
 * @param wi
 * @param xi
 */
template <typename NUMERIC = double>
class LagrangePolynomial
{
 public:
  typedef NUMERIC numeric_t;
  typedef Eigen::Matrix<numeric_t, -1, 1> vector_t;

 public:
  /**
   * @brief comput barycentric
   *
   * @param wi   weights from barycentric interpolation formula
   * @param xi   abscissas
   */
  static void compute_weights(vector_t &wi, const vector_t &xi);
  /**
   *
   *
   * @param y    output
   * @param x    evaluation points
   * @param xi   abscissas
   * @param yi   interpolation values
   * @param wi   weights (obtained via compute weights)
   */
  static void evaluate(
      vector_t &y, const vector_t &x, const vector_t &xi, const vector_t &yi, const vector_t &wi);
};

// --------------------------------------------------------------------------------
template <typename NUMERIC>
void
LagrangePolynomial<NUMERIC>::compute_weights(vector_t &wi, const vector_t &xi)
{
  const int N = xi.size();

  assert(wi.size() == xi.size());

  for (int i = 0; i < N; ++i) {
    numeric_t v = 1;
    for (int j = 0; j < N; ++j) {
      if (j != i) v *= (xi[j] - xi[i]);
    }
    wi[i] = 1 / v;
  }
}

// --------------------------------------------------------------------------------
template <typename NUMERIC>
void
LagrangePolynomial<NUMERIC>::evaluate(
    vector_t &y, const vector_t &x, const vector_t &xi, const vector_t &yi, const vector_t &wi)
{
  const numeric_t tol(1e-15);

  vector_t dist(xi.size());

  // number of grid points
  const int N = xi.size();

  // number of evaluation nodes
  const int n = x.size();
  assert(y.size() == x.size());

  // iterate over evaluation points in x(j)
  for (int j = 0; j < n; ++j) {
    // compute distance
    bool done = false;
    for (int k = 0; k < N; ++k) {
      dist(k) = x(j) - xi(k);
      if (std::abs(dist(k)) < tol) {
        y(j) = yi(k);
        done = true;
        break;
      }
    }
    if (done) continue;

    // compute sum
    double denom = 0;
    double numer = 0;

    for (int k = 0; k < N; ++k) {
      double v = (wi[k] / dist[k]);
      denom += v;
      numer += v * yi[k];
    }
    y(j) = numer / denom;
  }
}

}  // end namespace boltzmann


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

#include <iostream>
#include <cmath>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/mtl/recursion/bit_masking.hpp>
#include <boost/numeric/mtl/recursion/predefined_masks.hpp>
#include <boost/numeric/mtl/operation/print.hpp>

using namespace std;

template <typename Matrix>
void print_matrix(Matrix& matrix)
{
    typedef typename mtl::Collection<Matrix>::size_type   size_type;
    using std::cout;
    for (size_type i=0 ; i<num_rows(matrix); i++ ){
	for(size_type j=0; j<num_cols(matrix);  j++ ){
	    cout.fill (' '); cout.width (8); cout.precision (5); cout.flags (ios_base::left);
	    cout << showpoint <<  matrix[i][j] <<"  ";
	}
	cout << endl;
    }
}

template <typename Matrix>
void test(Matrix& matrix, const char* name)
{
    typedef typename mtl::Collection<Matrix>::size_type   size_type;
    matrix= 0;
    {
	mtl::mat::inserter<Matrix> ins(matrix);
	for (size_type i= 0; i < matrix.num_rows(); i++)
	    for (size_type j= 0; j < matrix.num_cols(); j++)
		if ((i + j) & 1)
		    ins(i, j) << i + 2*j;
    }

    std::cout << "\n" << name << "\n";
    print_matrix(matrix);

    mtl::mat::transposed_view<Matrix> trans(matrix);
    std::cout << "Transposed" << "\n";
    print_matrix(trans);

    std::cout << "with <<" << "\n"
	      << trans << "\n";

    std::cout << "with << and formatted" << "\n"
	      << with_format(trans, 7, 4) << "\n";

    Matrix square(5, 5);
    square= matrix * trans;

    std::cout << "squared before:\n" << with_format(trans, 4, 2)
	      << "squared in place:\n" << matrix * trans << "\n";

    // Comparison with FP!!!! :-! Make something better eventually
    //MTL_THROW_IF((matrix * trans)[0][1] != 1.0, mtl::runtime_error("Wrong multiplicatin result!"));
}

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

    dense2D<double>                                dr(5, 7);
    dense2D<double, mat::parameters<col_major> > dc(5, 7);
    morton_dense<double,  morton_mask>             md(5, 7);
    morton_dense<double,  doppled_16_row_mask>     d16r(5, 7);
    compressed2D<double>                           comp(5, 7);

    test(dr, "Dense row major");
    test(dc, "Dense column major");
    test(md, "Morton N-order");
    test(d16r, "Hybrid 16 row-major");
    //test(comp, "compressed2D");

    return 0;
}


#include <arrow/memory_pool.h>
#include <arrow/table.h>
#include <arrow/array.h>
#include <arrow/compute/api_vector.h>

#include <boost/assert.hpp>

#include <iostream>
#include <memory>
#include <limits>
#include <iterator>
#include <algorithm>

#include "attribute_mapper.h"
#include "arrow_utils.h"
#include "arrow_convenience.h"
#include "arrowcube.h"

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

using namespace std;
using namespace arrow;

pair<double, double>
col_bounds(const std::string &colname,
           shared_ptr
 arrow)
{
  shared_ptr<ChunkedArray> col = arrow->GetColumnByName(colname);

  double min_so_far = numeric_limits<double>::max(),
      max_so_far = numeric_limits<double>::min();

  arrow_foreach<DoubleType>(
      col,
      [&min_so_far,
       &max_so_far](double v) {
        min_so_far = std::min(min_so_far, v);
        max_so_far = std::max(max_so_far, v);
      });

  return make_pair(min_so_far, max_so_far);
}

pair<double, double>
col_mean_stdev(
    const std::string &colname,
    shared_ptr
 arrow)
{
  shared_ptr<ChunkedArray> col = arrow->GetColumnByName(colname);

  size_t n = 0;
  double ex = 0.0, exx = 0.0;

  arrow_foreach<DoubleType>(
      col,
      [&n, &ex, &exx](double v) {
        // drop nans
        if (v != v)
          return;
        ++n;
        ex += v;
        exx += v * v;
      });
  ex /= n;
  exx /= n;
  return make_pair(ex, sqrt(exx - ex * ex)); // this is numerically unstable, :shrug:
}

ostream &operator<<(ostream &os, const std::pair<double, double> &v)
{
  return os << "(" << v.first << "," << v.second << ")";
}

template <typename T1, typename T2>
void write_ppm(
    size_t width, size_t height,
    ostream &os,
    const std::vector<T2> &vec,
    T1 closure)
{
  os << "P3\n" << width << " " << height << "\n255\n";
  for (size_t y = 0; y < height; ++y) {
    for (size_t x = 0; x < width; ++x) {
      const T2 &v = vec[y * width + x];
      int r, g, b;
      std::tie(r, g, b) = closure(v);
      os << r << " " << g << " " << b << endl;
    }
  }
}

std::tuple<int, int, int> color(float u)
{
  if (u == 0) {
    return make_tuple(0, 0, 0);
  }
  u *= 3;
  if (u < 1) {
    return make_tuple(int(u * 255), 0, 0);
  }
  if (u < 2) {
    u -= 1;
    return make_tuple(255, int(u * 255), 0);
  }
  u -= 2;
  return make_tuple(255, 255, int(u * 255));
}

template <typename T>
// This should be done more carefully for large data.
shared_ptr

convert_to_fixed(shared_ptr
 input,
                 vector<string> columns,
                 vector<T*> xforms
                 )
{
  vector<shared_ptr<Field> > fields;
  vector<shared_ptr<ChunkedArray> > arrays;

  for (size_t i = 0; i < columns.size(); ++i) {
    string &c = columns[i];
    fields.push_back(field(c, shared_ptr<DataType>(new UInt32Type), false));
    // shared_ptr<Array> array;
    shared_ptr<ChunkedArray> col = input->GetColumnByName(c);

    vector<shared_ptr<Array>> array_vector;
    auto chunks = col->chunks();
    for (auto &chunk: chunks) {
      NumericBuilder<UInt32Type> builder;
      auto float_chunk = std::static_pointer_cast<DoubleArray>(chunk);
      for (size_t j = 0; j < float_chunk->length(); ++j) {
        int32_t v = xforms[i]->convert(float_chunk->Value(j));
        OK_OR_DIE(builder.Append(v));
      }
      shared_ptr<Array> a;
      OK_OR_DIE(builder.Finish(&a));
      array_vector.push_back(a);
    }
    cerr << array_vector.size() << endl;
    cerr << col->chunks().size() << endl;
    shared_ptr<ChunkedArray> ca(new ChunkedArray(array_vector));
    arrays.push_back(ca);
    cerr << ca->length() << endl;
  }

  return Table::Make(
      schema(fields),
      arrays);
}

shared_ptr
 make_address_table(shared_ptr
 arrow)
{
  cerr << "Bounds:" << endl;
  cerr << "  pickup_latitude: " << col_bounds("pickup_latitude", arrow) << endl;
  cerr << "  pickup_longitude: " << col_bounds("pickup_longitude", arrow) << endl;
  cerr << "  dropoff_latitude: " << col_bounds("dropoff_latitude", arrow) << endl;
  cerr << "  dropoff_longitude: " << col_bounds("dropoff_longitude", arrow) << endl;
  cerr << "mean/stdev:" << endl;
  cerr << "  pickup_latitude: " << col_mean_stdev("pickup_latitude", arrow) << endl;
  cerr << "  pickup_longitude: " << col_mean_stdev("pickup_longitude", arrow) << endl;
  cerr << "  dropoff_latitude: " << col_mean_stdev("dropoff_latitude", arrow) << endl;
  cerr << "  dropoff_longitude: " << col_mean_stdev("dropoff_longitude", arrow) << endl;

  const size_t resolution = 256;

  // DoubleAttribute lat( 40.5,  41, resolution);
  // DoubleAttribute lon(-74.25, -73.75, resolution);
  CenterWidthAttribute lat(40.75, 0.25, resolution);
  CenterWidthAttribute lon(-74, 0.25, resolution);

  vector<CenterWidthAttribute*> xforms = {&lat, &lon};
  shared_ptr
 addresses =
      convert_to_fixed<CenterWidthAttribute>(
          arrow,
          { "pickup_latitude", "pickup_longitude" },
          xforms);

  return addresses;
}

void test_with_nyc_pickup_data(std::string filename)
{
  shared_ptr
 arrow = read_feather_table(filename);

  // cerr << "Bounds:" << endl;
  // cerr << "  pickup_latitude: " << col_bounds("pickup_latitude", arrow) << endl;
  // cerr << "  pickup_longitude: " << col_bounds("pickup_longitude", arrow) << endl;
  // cerr << "  dropoff_latitude: " << col_bounds("dropoff_latitude", arrow) << endl;
  // cerr << "  dropoff_longitude: " << col_bounds("dropoff_longitude", arrow) << endl;
  // cerr << "mean/stdev:" << endl;
  // cerr << "  pickup_latitude: " << col_mean_stdev("pickup_latitude", arrow) << endl;
  // cerr << "  pickup_longitude: " << col_mean_stdev("pickup_longitude", arrow) << endl;
  // cerr << "  dropoff_latitude: " << col_mean_stdev("dropoff_latitude", arrow) << endl;
  // cerr << "  dropoff_longitude: " << col_mean_stdev("dropoff_longitude", arrow) << endl;

  const size_t resolution = 256;

  // // DoubleAttribute lat( 40.5,  41, resolution);
  // // DoubleAttribute lon(-74.25, -73.75, resolution);
  // CenterWidthAttribute lat(40.75, 0.25, resolution);
  // CenterWidthAttribute lon(-74, 0.25, resolution);

  // vector<CenterWidthAttribute*> xforms = {&lat, &lon};
  shared_ptr
 addresses = make_address_table(arrow);
      // convert_to_fixed<CenterWidthAttribute>(
      //     arrow,
      //     { "pickup_latitude", "pickup_longitude" },
      //     xforms);

  // cerr << "Hello ?" << endl;
  // arrow_foreach<Int32Type>(
  //     addresses->GetColumnByName("pickup_longitude"),
  //     [](uint32_t v) {
  //       cerr << v << endl;
  //     });
  // cerr << "Hello ?" << endl;

  vector<size_t> counts_2d(resolution * resolution);
  size_t n = 0;
  arrow_foreach<Int32Type>(
      addresses->GetColumnByName("pickup_latitude"),
      addresses->GetColumnByName("pickup_longitude"),
      [&n, &counts_2d](uint32_t i_lat, uint32_t i_lon) {
        ++n;
        i_lat = resolution - 1 - i_lat;
        counts_2d[i_lon + resolution * i_lat]++;
      });
  size_t mx = *std::max_element(counts_2d.begin(), counts_2d.end());
  cerr << n << " " << mx << endl;

  write_ppm(resolution, resolution,
            cout,
            counts_2d,
            [&mx](size_t v) {
              float u = log(v + 1) / log(mx + 1);
              return color(u);
            });
}

struct CountPolicy
{
  int count;
  CountPolicy(): count(0) {};

  inline void add(const RowIterator &row) {
    ++count;
  }
};

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

void test_arrow_cube(std::string filename)
{
  shared_ptr

      arrow = read_feather_table(filename),
      addresses = make_address_table(arrow);

  // nc2::ArrowCube ac(addresses, { "pickup_latitude", "pickup_longitude" });
  // CountPolicy policy;
  // ac.range_query<size_t, CountPolicy>(policy, { make_pair(size_t(0), size_t(256)), make_pair(size_t(0), size_t(256)) });
  // cerr << policy.count << endl;

  // compute::SortOrder order = compute::SortOrder::Ascending;
  compute::SortOptions options({
      compute::SortKey("pickup_latitude", compute::SortOrder::Ascending)
    });
  auto sorted_df = sort_table(addresses, &options);
}

void test_arrow_convenience()
{
  shared_ptr table = make_table(
      {{ "col1", make_chunked_array<UInt32Type>({ 0, 0, 0, 1, 1, 1, 2, 2, 2 }) },
       { "col2", make_chunked_array<UInt32Type>({ 0, 0, 1, 0, 2, 2, 1, 2, 2 }) },
       { "agg",  make_chunked_array<DoubleType>({ 1, 1, 1, 1, 1, 1, 1, 1, 1 }) }
      });

  {
    RowIterator itor({
        ChunkedArrayIterator(table->GetColumnByName("col1")),
        ChunkedArrayIterator(table->GetColumnByName("col2")),
        ChunkedArrayIterator(table->GetColumnByName("agg"))
      });

    do {
      for (size_t i = 0; i < 2; ++i) {
        cerr << itor.cols_[i].value<UInt32Type>() << " ";
      }
      cerr << "agg: " << itor.cols_[2].value<DoubleType>() << endl;
    } while (!itor.next());
  }

  table = CompressAggregation<UInt32Type, DoubleType>::call(
      table, { "col1", "col2" }, { "agg" });

  {
    RowIterator itor({
        ChunkedArrayIterator(table->GetColumnByName("col1")),
        ChunkedArrayIterator(table->GetColumnByName("col2")),
        ChunkedArrayIterator(table->GetColumnByName("agg"))
      });

    do {
      for (size_t i = 0; i < 2; ++i) {
        cerr << itor.cols_[i].value<UInt32Type>() << " ";
      }
      cerr << "agg: " << itor.cols_[2].value<DoubleType>() << endl;
    } while (!itor.next());
  }
}

int main(int argc, char **argv)
{
  const std::string filename = "/Users/cscheid/data/nyc-tlc/feather/yellow_tripdata_2014-02.feather";
  // test_with_nyc_pickup_data(argv[1]);
  // test_arrow_cube(filename);
  test_arrow_convenience();
}


#include <boost/test/unit_test.hpp>

#include "ssmkit/random/generator.hpp"

#include <thread>
#include <array>
#include <random>
#include <algorithm>

using namespace ssmkit;
using namespace std;

BOOST_AUTO_TEST_SUITE(generator_test);

// thread task
void sample(double *r){
  uniform_real_distribution<double> dist;
  *r = dist(ssmkit::random::Generator::get().getGenerator());
}

BOOST_AUTO_TEST_CASE(multithread) {
  constexpr unsigned int n = 100;
  // array to store samples
  array<double, n> samples;
  array<thread, n> thrd;
  unsigned int repeat = 20;

  for (int i=0; i<repeat; i++){
    auto sb = samples.begin();
    generate_n(thrd.begin(), n, [&sb]() { return thread(sample, sb++); });
    for_each(thrd.begin(), thrd.end(), [](auto &t) { t.join(); });

    // check if there is any replicate
    bool ch =
        all_of(samples.begin(), samples.end(), [&samples](const auto &sb) {
          return count_if(samples.begin(), samples.end(), [&sb](const auto &s) {
                   return s == sb;
                 }) == 1 ? true : false;
        });
    BOOST_CHECK(ch);
  }
}

BOOST_AUTO_TEST_SUITE_END();


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/ieee/include/functions/frexp.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/half.hpp>
#include <boost/simd/include/constants/limitexponent.hpp>
#include <boost/simd/include/constants/mindenormal.hpp>
#include <boost/simd/include/constants/minexponent.hpp>
#include <boost/simd/include/constants/halfeps.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/simd/include/constants/smallestposval.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/two.hpp>
#include <boost/simd/include/constants/four.hpp>
#include <boost/simd/include/functions/divides.hpp>
#include <boost/simd/include/functions/ldexp.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/fusion/include/vector_tie.hpp>
#include <boost/simd/sdk/config.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL( frexp0, BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::frexp;
  using boost::simd::ldexp;
  using boost::simd::tag::frexp_;
  using boost::simd::native;

  typedef native<T,BOOST_SIMD_DEFAULT_EXTENSION>            vT;
  typedef typename boost::dispatch::meta::as_integer<vT,signed>::type viT;

  {
    viT e;
    vT  m;
    vT  a = boost::simd::Valmax<vT>();
    frexp(a, m, e);
    NT2_TEST_ULP_EQUAL(m, boost::simd::One<vT>()-boost::simd::Halfeps<vT>(), 1);
    NT2_TEST_EQUAL(e, boost::simd::Limitexponent<vT>());
    NT2_TEST_EQUAL(ldexp(m,e),a);
  }
#ifndef BOOST_SIMD_NO_DENORMALS
  {
    viT e;
    vT  m;
    vT  a = boost::simd::Mindenormal<vT>();
    frexp(a, m, e);
    NT2_TEST_ULP_EQUAL(m, boost::simd::Half<vT>(), 1);
    NT2_TEST_EQUAL(e, boost::simd::Minexponent<vT>()-boost::simd::Nbmantissabits<vT>()+boost::simd::One<viT>());
    NT2_TEST_EQUAL(ldexp(m,e),a);
 }

  {
    viT e;
    vT  m;
    vT  a = boost::simd::Smallestposval<vT>()/boost::simd::Two<vT>();
    frexp(a, m, e);
    NT2_TEST_ULP_EQUAL(m, boost::simd::Half<vT>(), 1);
    NT2_TEST_EQUAL(e, boost::simd::Minexponent<vT>());
    NT2_TEST_EQUAL(ldexp(m,e),a);
 }

  {
    viT e;
    vT  m;
    vT  a = boost::simd::Smallestposval<vT>()/boost::simd::Four<vT>();
    frexp(a, m, e);
    NT2_TEST_ULP_EQUAL(m, boost::simd::Half<vT>(), 1);
    NT2_TEST_EQUAL(e, boost::simd::Minexponent<vT>()-boost::simd::One<viT>());
    NT2_TEST_EQUAL(ldexp(m,e),a);
 }
#endif
}

NT2_TEST_CASE_TPL( frexp, BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::frexp;
  using boost::simd::tag::frexp_;
  using boost::simd::native;

  typedef native<T,BOOST_SIMD_DEFAULT_EXTENSION>            vT;
  typedef typename boost::dispatch::meta::as_integer<vT,signed>::type viT;

  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<frexp_(vT)>::type)
                  , (std::pair<vT,viT>)
                  );

  {
    viT e;
    vT  m;

    frexp(boost::simd::One<vT>(), m, e);
    NT2_TEST_EQUAL(m, boost::simd::Half<vT>());
    NT2_TEST_EQUAL(e, boost::simd::One<viT>());
  }

  {
    viT e;
    vT  m;

    m = frexp(boost::simd::One<vT>(), e);
    NT2_TEST_EQUAL(m, boost::simd::Half<vT>());
    NT2_TEST_EQUAL(e, boost::simd::One<viT>());
  }

  {
    viT e;
    vT  m;

    boost::fusion::vector_tie(m,e) = frexp(boost::simd::One<vT>());
    NT2_TEST_EQUAL(m, boost::simd::Half<vT>());
    NT2_TEST_EQUAL(e, boost::simd::One<viT>());
  }

  {
    std::pair<vT,viT> p;

    p = frexp(boost::simd::One<vT>());
    NT2_TEST_EQUAL(p.first  , boost::simd::Half<vT>());
    NT2_TEST_EQUAL(p.second , boost::simd::One<viT>());
  }
}


///////////////////////////////////////////////////////////////
//  Copyright 2011 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_

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error

#if !defined(TEST_MPF_50) && !defined(TEST_BACKEND) && !defined(TEST_CPP_DEC_FLOAT) && !defined(TEST_MPFR_50)
#  define TEST_MPF_50
#  define TEST_MPFR_50
#  define TEST_CPP_DEC_FLOAT

#ifdef _MSC_VER
#pragma message("CAUTION!!: No backend type specified so testing everything.... this will take some time!!")
#endif
#ifdef __GNUC__
#pragma warning "CAUTION!!: No backend type specified so testing everything.... this will take some time!!"
#endif

#endif

#if defined(TEST_MPF_50)
#include <boost/multiprecision/gmp.hpp>
#endif
#if defined(TEST_MPFR_50)
#include <boost/multiprecision/mpfr.hpp>
#endif
#ifdef TEST_BACKEND
#include <boost/multiprecision/concepts/mp_number_archetypes.hpp>
#endif
#ifdef TEST_CPP_DEC_FLOAT
#include <boost/multiprecision/cpp_dec_float.hpp>
#endif

#include "table_type.hpp"
#define TEST_UDT

#include <boost/math/special_functions/ellint_2.hpp>
#include "libs/math/test/test_ellint_2.hpp"

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      ".*",                          // test type(s)
      ".*",                          // test data group
      ".*", 300, 200);               // test function
   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

BOOST_AUTO_TEST_CASE( test_main )
{
   using namespace boost::multiprecision;
   expected_results();
   //
   // Test at:
   // 18 decimal digits: tests 80-bit long double approximations
   // 30 decimal digits: tests 128-bit long double approximations
   // 35 decimal digits: tests arbitrary precision code
   //
#ifdef TEST_MPF_50
   test_spots(number<gmp_float<18> >(), "number<gmp_float<18> >");
   test_spots(number<gmp_float<30> >(), "number<gmp_float<30> >");
   test_spots(number<gmp_float<35> >(), "number<gmp_float<35> >");
   // there should be at least one test with expression templates off:
   test_spots(number<gmp_float<35>, et_off>(), "number<gmp_float<35>, et_off>");
#endif
#ifdef TEST_MPFR_50
   test_spots(number<mpfr_float_backend<18> >(), "number<mpfr_float_backend<18> >");
   test_spots(number<mpfr_float_backend<30> >(), "number<mpfr_float_backend<30> >");
   test_spots(number<mpfr_float_backend<35> >(), "number<mpfr_float_backend<35> >");
#endif
#ifdef TEST_CPP_DEC_FLOAT
   test_spots(number<cpp_dec_float<18> >(), "number<cpp_dec_float<18> >");
   test_spots(number<cpp_dec_float<30> >(), "number<cpp_dec_float<30> >");
   test_spots(number<cpp_dec_float<35, long long, std::allocator<void> > >(), "number<cpp_dec_float<35, long long, std::allocator<void> > >");
#endif
}


// Copyright © 2013 the Search Authors under the MIT license. See AUTHORS for the list of authors.
#pragma once

#include <vector>
#include <boost/optional.hpp>

void fatal(const char*, ...);

template <class Ops, class Elm> class BinHeap {
public:

	// push pushes a new element into the heap in
	// O(lg n) time.
	void push(Elm e) {
		heap.push_back(e);
		Ops::setind(e, heap.size() - 1);
		pullup(heap.size() - 1);
	}

	// pop pops the front element from the heap and
	// returns it in O(lg n) time.
	boost::optional<Elm> pop() {
		if (heap.size() == 0)
			return boost::optional<Elm>();

		Elm res = heap[0];
		if (heap.size() > 1) {
			heap[0] = heap.back();
			heap.pop_back();
			Ops::setind(heap[0], 0);
			pushdown(0);
		} else {
			heap.pop_back();
		}
		Ops::setind(res, -1);

		return boost::optional<Elm>(res);
	}

	// front returns the front element of the heap if it is
	// not empty and an empty option if the heap is empty.
	// O(1) time.
	boost::optional<Elm> front() {
		if (heap.size() == 0)
			return boost::optional<Elm>();
		return boost::optional<Elm>(heap[0]);
	}

	// update updates the element in the heap at the given
	// index.  This should be called whenever the priority
	// of an element changes.  O(lg n) time.
	void update(long i) {
		if (i < 0 || (unsigned int) i >= heap.size())
			fatal("Updating an invalid heap index: %ld, size=%lu\n", i, heap.size());
		i = pullup(i);
		pushdown(i);
	}

	// pushupdate either pushes the element or updates it's
	// position in the queue, given the element and i, it's
	// index value.  Note that the index value must be tracked
	// properly, i.e., i < 0 means that the element is not in
	// the priority queue and i  >= 0 means that the element
	// is at the given index. O(lg n) time.
	void pushupdate(Elm e, long i) {
		if (i  < 0)
			push(e);
		else
			update(i);
	}

	// empty returns true if the heap is empty and
	// false otherwise.
	bool empty() const { return heap.empty(); }

	// clear clears all of the elements from the heap
	// leaving it empty.
	void clear() { heap.clear(); }

 	// size returns the number of entries in the heap.
	long size() const { return heap.size(); }

	// at returns the element of the heap at the given
	// index.
	Elm at(long i) {
		assert (i < size());
		return heap[i];
	}

	// data returns the raw vector used to back the
	// heap.  If you mess with this then you must
	// reinit the heap afterwards to ensure that the
	// heap property still holds.
	std::vector<Elm> &data() { return heap; }

	// reinit reinitialize the heap property in O(n) time.
	void reinit() {
		if (heap.size() <= 0)
			return;

		for (unsigned int i = 0; i < heap.size(); i++)
			Ops::setind(heap[i], i);

		for (long i = heap.size() / 2; ; i--) {
			pushdown(i);
			if (i == 0)
				break;
		}
	}

	// append appends the vector of elements to the heap
	// and ensures that the heap property holds after.
	void append(const std::vector<Elm> &elms) {
		heap.insert(heap.end(), elms.begin(), elms.end());
		reinit();
	}

	long pushdown(long i) {
		long l = left(i), r = right(i);

		long sml = i;
		if (l < size() && Ops::pred(heap[l], heap[i]))
			sml = l;
		if (r < size() && Ops::pred(heap[r], heap[sml]))
			sml = r;

		if (sml != i) {
			swap(sml, i);
			return pushdown(sml);
		}

		return i;
	}

private:
	friend bool binheap_push_test();
	friend bool binheap_pop_test();

	long parent(long i) { return (i - 1) / 2; }

	long left(long i) { return 2 * i + 1; }

	long right(long i) { return 2 * i + 2; }

	long pullup(long i) {
		if (i == 0)
			return i;
		long p = parent(i);
		if (Ops::pred(heap[i], heap[p])) {
			swap(i, p);
			return pullup(p);
		}
		return i;
	}

	void swap(long i, long j) {
		Ops::setind(heap[i], j);
		Ops::setind(heap[j], i);
		Elm tmp = heap[i];
		heap[i] = heap[j];
		heap[j] = tmp;
	}

	std::vector<Elm> heap;
};


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

#include "DRegularLanguage.hpp"
#include <memory>

using namespace FACore;
using namespace std;

template<unsigned int ALPHABET_SIZE>
DRegularLanguage<ALPHABET_SIZE> EmptyLanguage() {
    typedef DRegularLanguage<ALPHABET_SIZE> Language;
    typedef typename Language::Machine Machine;

    shared_ptr<const Machine> automaton( new Machine() );
    // By default, it will be the implementation of the empty language

    return Language(automaton, 0);
}

template<unsigned int ALPHABET_SIZE>
DRegularLanguage<ALPHABET_SIZE> FullLanguage() {
    typedef DRegularLanguage<ALPHABET_SIZE> Language;
    typedef typename Language::Machine Machine;

    Machine* machine = new Machine();

    // By default, it will be the implementation of the empty language
    auto state = machine->AddState(true);

    for(unsigned int c = 0; c < Language::AlphabetSize; c++) {
        machine->SetArc(state, c, state);
    }

    return Language(machine, state);
}

template<class LanguageType>
bool InLanguage(LanguageType &language, std::vector<typename LanguageType::StateId> word) {
    return language.contains(word.begin(), word.end());
}

template<class LanguageType>
bool NotInLanguage(LanguageType &language, std::vector<typename LanguageType::StateId> word) {
    return !language.contains(word.begin(), word.end());
}

BOOST_AUTO_TEST_SUITE( TestDRegularLanguage );

BOOST_AUTO_TEST_CASE( empty_language_alphabet2 )
{
    DRegularLanguage<2> emptyLanguage = EmptyLanguage<2>(); // This is the default

    BOOST_CHECK( NotInLanguage(emptyLanguage, {}) );
    BOOST_CHECK( NotInLanguage(emptyLanguage, {0}) );
    BOOST_CHECK( NotInLanguage(emptyLanguage, {1}) );
    BOOST_CHECK( NotInLanguage(emptyLanguage, {1,0,1}) );
}

BOOST_AUTO_TEST_CASE( empty_language_alphabet3 )
{
    DRegularLanguage<3> emptyLanguage = EmptyLanguage<3>(); // This is the default

    BOOST_CHECK( NotInLanguage(emptyLanguage, {}) );
    BOOST_CHECK( NotInLanguage(emptyLanguage, {0}) );
    BOOST_CHECK( NotInLanguage(emptyLanguage, {1}) );
    BOOST_CHECK( NotInLanguage(emptyLanguage, {1,0,2}) );
}

BOOST_AUTO_TEST_CASE( full_language )
{
    DRegularLanguage<2> emptyLanguage = FullLanguage<2>(); // This is the default

    BOOST_CHECK( InLanguage(emptyLanguage, {}) );
    BOOST_CHECK( InLanguage(emptyLanguage, {0}) );
    BOOST_CHECK( InLanguage(emptyLanguage, {1}) );
    BOOST_CHECK( InLanguage(emptyLanguage, {1,0,1}) );
}

BOOST_AUTO_TEST_CASE( thue_morse )
{
    // The thue morse machine accepts a bit string iff it has an odd number of ones
    DAutomaton<2> *thueMorseMachine = new DAutomaton<2>();
    auto evenState = thueMorseMachine->AddState(false);
    auto oddState = thueMorseMachine->AddState(true);

    thueMorseMachine->SetArc(evenState, 0, evenState);
    thueMorseMachine->SetArc(evenState, 1, oddState);
    thueMorseMachine->SetArc(oddState, 0, oddState);
    thueMorseMachine->SetArc(oddState, 1, evenState);

    DRegularLanguage<2> thueMorseSequence(thueMorseMachine, evenState);

    // Test using the normal as well as integer "contains" function
    BOOST_CHECK( !thueMorseSequence.contains(0) );
    BOOST_CHECK( NotInLanguage(thueMorseSequence, {} ));
    BOOST_CHECK( NotInLanguage(thueMorseSequence, {0} ));
    BOOST_CHECK( NotInLanguage(thueMorseSequence, {0,0} ));

    BOOST_CHECK( !thueMorseSequence.contains(3) );
    BOOST_CHECK( NotInLanguage(thueMorseSequence, {1,1} ));
    BOOST_CHECK( NotInLanguage(thueMorseSequence, {0,1,1} ));

    BOOST_CHECK( !thueMorseSequence.contains(5) );
    BOOST_CHECK( NotInLanguage(thueMorseSequence, {1,0,1} ));

    BOOST_CHECK( thueMorseSequence.contains(1) );
    BOOST_CHECK( InLanguage(thueMorseSequence, {1}) );
    BOOST_CHECK( InLanguage(thueMorseSequence, {0,1}) );

    BOOST_CHECK( thueMorseSequence.contains(2) );
    BOOST_CHECK( InLanguage(thueMorseSequence, {1,0}) );
    BOOST_CHECK( InLanguage(thueMorseSequence, {0,1,0}) );

    BOOST_CHECK( thueMorseSequence.contains(4) );
    BOOST_CHECK( InLanguage(thueMorseSequence, {1,0,0}) );
}

BOOST_AUTO_TEST_SUITE_END();