#include <iostream>

#include <ros/ros.h>
#include <ros/package.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Image.h>

#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <Eigen/Core>

#include "opencv2/opencv.hpp"
#include "opencv/cxeigen.hpp"

#include "../camera_models/include/EquidistantCamera.h"
#include "../camera_models/include/PinholeCamera.h"

cv::Mat cv_T_;
CameraPtr cameraptr_;
namespace sc
{

    void TranScanToPoints(const sensor_msgs::LaserScanConstPtr& scan_in, std::vector<Eigen::Vector3d>& Points)
    {
        // http://wiki.ros.org/laser_geometry
        size_t n_pts = scan_in->ranges.size ();
        Eigen::ArrayXXd ranges (n_pts, 2);
        Eigen::ArrayXXd output (n_pts, 2);
        Eigen::ArrayXXd co_sine_map (n_pts, 2);

//        std::cout << "---------- read scan -----------"<<std::endl;
        for (size_t i = 0; i < n_pts; ++i)
        {
            // std::cout << scan_in->ranges[i]<< " "<<scan_in->angle_min + (double) i * scan_in->angle_increment <<std::endl;
            ranges (i, 0) = (double) scan_in->ranges[i];
            ranges (i, 1) = (double) scan_in->ranges[i];

            co_sine_map (i, 0) = cos (scan_in->angle_min + (double) i * scan_in->angle_increment);
            co_sine_map (i, 1) = sin (scan_in->angle_min + (double) i * scan_in->angle_increment);
        }

        output = ranges * co_sine_map;

        for (size_t i = 0; i < n_pts; ++i) {

            // TODO: range_cutoff threashold
            double range_cutoff = 30.0;
            const float range = scan_in->ranges[i];
            if (range < range_cutoff && range >= scan_in->range_min)
            {
                Points.push_back(Eigen::Vector3d(output (i, 0), output (i, 1), 0) );
            }
        }

    }

    void showScanCallback(const sensor_msgs::LaserScanConstPtr &scan_in,
                          const sensor_msgs::ImageConstPtr &image_in) {

        std::vector<Eigen::Vector3d> p_l;
        TranScanToPoints(scan_in, p_l);

        Eigen::Matrix4d Tlc;
        Tlc.setZero();

        if(!cv_T_.empty())
            cv::cv2eigen(cv_T_, Tlc);
        else{
            std::cerr <<" You Do not have calibra result Tlc. We use a Identity matrix." << std::endl;
            Tlc.setIdentity();
        }

        Eigen::Matrix3d Rlc = Tlc.block(0,0,3,3);
        Eigen::Vector3d tlc(Tlc(0,3),Tlc(1,3),Tlc(2,3));

        Eigen::Matrix3d Rcl = Rlc.transpose();
        Eigen::Vector3d tcl = - Rcl * tlc;

//        std::cout << " Tlc: \n"<< Tlc<<std::endl;

        std::vector<Eigen::Vector3d> p_c;
        std::vector<cv::KeyPoint> keypoints;

        int n_pts = scan_in->ranges.size();
        for (int i = 0; i < n_pts; i++) {
           // std::cout << p_l[i].transpose() << std::endl;
            p_c.emplace_back(Rcl * p_l[i] + tcl);
        }

        std::vector< cv::Point2f > pixel;
        std::vector< cv::Point3f> pts;

        for (int i = 0; i < n_pts; i++) {
            double X = p_c[i].x();
            double Y = p_c[i].y();
            double Z = p_c[i].z();

            pts.push_back( cv::Point3f(X,Y,Z) );

        }
        cv::Mat r = cv::Mat::zeros(3,1,CV_64F);
        cv::Mat t = cv::Mat::zeros(3,1,CV_64F);
        cameraptr_->projectPoints(pts, r, t,pixel);
//        cv::projectPoints(pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), cvK_, cvD_, pixel);

        cv::Mat img_src = cv_bridge::toCvShare(image_in, "bgr8")->image;

        for (size_t j = 0; j < pixel.size(); ++j) {
            cv::circle(img_src, pixel[j],1, cv::Scalar(0,255,0),1);
        }
        cv::imshow("show", img_src);
        std::string name = std::to_string(image_in->header.stamp.toSec());
        std::string path = ros::package::getPath("lasercamcal_ros");
        path = path + "/../output/image/" + name + ".png";
        if(!cv::imwrite(path.c_str(),img_src))
            ROS_INFO("can not write %s", path.c_str());
        cv::waitKey(10);
    }

} // sc namespace end

template <typename T>
T readParam(ros::NodeHandle &n, std::string name)
{
    T ans;
    std::cout << name <<std::endl;
    if (n.getParam(name, ans))
    {
        ROS_INFO_STREAM("Loaded " << name << ": " << ans);
    }
    else
    {
        ROS_ERROR_STREAM("Failed to load " << name);
        n.shutdown();
    }
    return ans;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "show_scan");
    ros::NodeHandle nh;
    ros::NodeHandle pnh("~");

    /// ==========================================
    std::string config_file;
    config_file = readParam<std::string>(pnh, "config_file");

    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        std::cerr << "ERROR: Wrong path to settings" << std::endl;
        return 0;
    } else
    {
        std::cout << "READ CONFIG_FILE...\n";
    }

    std::string scan_topic_name;
    std::string img_topic_name;
    std::string bag_path;
    std::string save_path;

    fsSettings["scan_topic_name"] >> scan_topic_name;
    fsSettings["img_topic_name"] >> img_topic_name;
    fsSettings["savePath"] >> save_path;
    fsSettings["bag_path"] >> bag_path;

    std::string sModelType;
    fsSettings["model_type"] >> sModelType;

    cameraptr_ = nullptr;
    if (sModelType.compare("KANNALA_BRANDT") == 0)
    {
        EquidistantCamera::Parameters paras;
        paras.readFromYamlFile(config_file);
        cameraptr_ = CameraPtr (new EquidistantCamera(paras));
        ROS_INFO("LOAD KANNALA BRANDT CAMERA!");
    }
        // todo:: PIN HOLE MODEL
    else
    {
        PinholeCamera::Parameters paras;
        paras.readFromYamlFile(config_file);
        cameraptr_ = CameraPtr (new PinholeCamera(paras));
    }

    fsSettings.release();

    std::cout <<"\t"<<scan_topic_name << std::endl;
    std::cout <<"\t"<<img_topic_name << std::endl;
    std::cout <<"\t"<<bag_path << std::endl;

    std::string fn = save_path + "result.yaml";
    fsSettings.open(fn,cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        std::cerr << "You should run calibra tool first to generate result.yaml in your savePath." << std::endl;
        return 0;
    }
    fsSettings["extrinsicTlc"] >> cv_T_;
    std::cout <<"Load extrinsicTlc:\n"<<cv_T_<<std::endl;
    fsSettings.release();

    std::cout << "END CONFIG_FILE\n" << std::endl;
/// ==========================================

    message_filters::Subscriber<sensor_msgs::LaserScan> scan_sub(nh, scan_topic_name, 10);
    message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, img_topic_name, 10);

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::LaserScan, sensor_msgs::Image> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), scan_sub, image_sub);
    sync.registerCallback(boost::bind(&sc::showScanCallback, _1, _2));
    std::cout << "start spin.." << std::endl;
    ros::spin();

    return 0;
}


#include "coela_utility/src/misc_math.h"

#include "coela_core/src/ccd_image.h"
#include "coela_core/src/image_utils.h"
#include "coela_core/src/ds9_region.h"

#include "coela_luckypipe/src/ccd_dataset_info.h"
#include "coela_luckypipe/src/camera_config_info.h"

#include "coela_luckypipe/src/gain_utils.h"
#include "coela_luckypipe/src/single_ccd_filters.h"
#include "coela_luckypipe/src/image_cleanup.h"
#include "coela_luckypipe/src/frame_info.h"
#include "coela_luckypipe/src/clean_and_register_subroutines.h"
#include "coela_analysis/src/psf_generation.h"

//#include "../threading/threading_tools.h"
#include <tbb/tick_count.h>
#include <tbb/task_scheduler_init.h>
#include <boost/filesystem.hpp>
#include <iostream>
#include <fstream>
#include <stdexcept>
#include "getoptpp/getopt_pp.h"

using namespace coela;
using namespace std;
using tbb::tick_count;

struct ProgramOptionSet {
    string dataset_info_filename;
    CcdDatasetInfo dataset;
};

void print_usage_and_exit()
{
    exit(0);
}

ProgramOptionSet load_options_from_command_line(int argc, char** argv)
{
    if (argc != 2) { print_usage_and_exit(); }
    ProgramOptionSet opts;
    opts.dataset_info_filename=argv[1];
    opts.dataset = CcdDatasetInfo(opts.dataset_info_filename);
    return opts;
}

struct RowHists10bit {
    RowHists10bit(size_t width, int min_val) {
        pixel_hists.resize(width);
        for (size_t i=0; i!=pixel_hists.size(); ++i) {
            pixel_hists[i].set_min_value(min_val);
        }
    }

    void clear() {
        for (size_t i=0; i!=pixel_hists.size(); ++i) {
            pixel_hists[i].clear();
        }
    }
    vector<HistogramContainer10bit> pixel_hists;
};

void process_n_rows(vector<FrameInfo>& frame_vec,
                    PixelArray2d<float>& bias_frm,
                    vector<RowHists10bit>& row_hist_vec,
                    const PixelRange& crop_box,
                    const PixelRange& temporal_debias_box,
                    size_t first_row,
                    size_t n_rows_to_process,
                    size_t n_frames_to_process
                   )
{
    if ((first_row + n_rows_to_process -1) > bias_frm.range().y_dim()) {
        throw runtime_error("!!!");
    }

    for (size_t i=0; i!=row_hist_vec.size(); ++i) {
        row_hist_vec[i].clear();
    }
    cout << n_rows_to_process << "rows starting at " << first_row << endl;

    HistogramContainer14bit single_frame_hist;

    for (size_t i=0; i!=n_frames_to_process; ++i) {
        cout<<"\rFrame "<<i<<" of "<< n_frames_to_process<<flush;
        FrameInfo& frm= frame_vec[i];
        CcdImage<float> img;

        img.pix = PixelArray2d<float>::load_from_file(frm.file_path,
                   frm.header_byte_offset);
        img = CcdImage<float>::sub_image(img, crop_box);

        single_frame_hist.clear();
        if (frm.bias_pedestal== 0.0) {
            frm.bias_pedestal =
                image_cleanup::determine_bias_pedestal_from_box_in_raw_image(
                    img.pix,
                    temporal_debias_box,
                    single_frame_hist);
        }

        img.pix -= frm.bias_pedestal;

        for (size_t offset = 0; offset!= n_rows_to_process; ++offset) {
            size_t row_num = first_row + offset;
            RowHists10bit& rowhist = row_hist_vec[offset];
            for (size_t x=1; x<=img.pix.range().x_dim(); ++x) {
                rowhist.pixel_hists[x-1].count_rounded_value(img.pix(x, row_num));
            }
        }
    }
    cout<<endl;

    for (size_t offset = 0; offset!= n_rows_to_process; ++offset) {
        size_t row_num = first_row + offset;

        for (size_t x=1; x<=bias_frm.range().x_dim(); ++x) {
            bias_frm(x, row_num) =
                image_cleanup::determine_histogram_bias_pedestal_via_thresholded_centroid(
                    row_hist_vec[offset].pixel_hists[x-1]);
        }
    }
}

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

    ProgramOptionSet opts = load_options_from_command_line(argc, argv) ;
    const CcdDatasetInfo& cdi(opts.dataset);
    cout<<"CCD output dir: "<<cdi.CCD_outputdir<<endl;

    CameraConfigInfo camconf(cdi.default_camera_config_file);

    vector<FrameInfo> frame_vec(
        FrameInfo::get_frames_vec(cdi.CCD_inputdir,
                                  cdi.in_filestem,
                                  cdi.extension,
                                  cdi.ccd_id,
                                  cdi.dataset_output_base_dir));

    FrameInfo::set_bias_pedestal_estimates(frame_vec, 0.0);

    string output_folder=cdi.CCD_outputdir+"/calibration_data/";
    boost::filesystem::create_directories(output_folder);

    const size_t n_frames_to_process_limit = 3000;
    const int n_frames_to_process = min(n_frames_to_process_limit, frame_vec.size());

    size_t n_rows_to_process_at_once = 500;

    CcdImage<float> first;
    FrameInfo& frm= frame_vec.front();

    first.pix = PixelArray2d<float>::load_from_file(frm.file_path, frm.header_byte_offset);
    first.initialize_CCD_grid_for_raw_data();
    PixelRange crop_box = camconf.get_calibration_info_for_CCD_id(
                              frm.ccd_id).cropped_PixelRange;
    first = CcdImage<float>::sub_image(first, crop_box);

    CcdBoxRegion temporal_debias_rgn =
        camconf.get_calibration_info_for_CCD_id(
            frm.ccd_id).default_temporal_debiasing_histogram_region;

    PixelRange temporal_debias_box =
        first.CCD_grid.corresponding_pixel_region(temporal_debias_rgn).bounded_pixels();

    //============================================================================================
    //Per pixel bias estimation:
    cerr<<"Allocating pixel hists...";
    vector<RowHists10bit> row_hist_vec;

    for (size_t i=0; i!=n_rows_to_process_at_once; ++i) {
        row_hist_vec.push_back(RowHists10bit(first.pix.range().x_dim(), -300));
    }

    cerr<<"Done"<<endl;
    cout<<endl;

    CcdImage<float> per_pixel_bias_estimates(first);
    per_pixel_bias_estimates.pix.assign(0);
    //----------------------------------------------------------------------------------
    size_t first_row=1;
    while (n_rows_to_process_at_once!=0) {
        if (first_row + n_rows_to_process_at_once -1 <= first.pix.range().y_dim()) {
            process_n_rows(frame_vec,
                           per_pixel_bias_estimates.pix,
                           row_hist_vec,
                           crop_box,
                           temporal_debias_box,
                           first_row,
                           n_rows_to_process_at_once,
                           n_frames_to_process);

            per_pixel_bias_estimates.write_to_file(output_folder+"per_pixel_bias_frame.fits");
            first_row+=n_rows_to_process_at_once;

        } else {
            n_rows_to_process_at_once--;
        }

    }

    //============================================================================================
    //----------------------------------------------------------------------------------
    //----------------------------------------------------------------------------------

    FrameInfo::write_list_to_file(frame_vec, output_folder+"frame_list.txt");

    double uniform_bias_est =
        FrameInfo::get_median_bias_pedestal_estimate(frame_vec);
    cout<<"Uniform bias est: "<< uniform_bias_est<<endl;

//
    CcdImage<float> standard_col_bias_frm =
        clean_and_register_subroutines::
        estimate_column_bias_pattern(
            frame_vec, output_folder,
            crop_box,
            false,
            temporal_debias_box,
            n_frames_to_process,
            7);
//
    standard_col_bias_frm.write_to_file(output_folder +"std_col_bias_frame.fits");

    CcdImage<float> row_bias_frm =
        clean_and_register_subroutines::
        estimate_row_bias_pattern(
            frame_vec, output_folder,
            crop_box,
            standard_col_bias_frm,
            n_frames_to_process,
            7);
//
    row_bias_frm.write_to_file(output_folder +"row_bias_frame.fits");

//----------------------------------------------------------------------------------
    CcdImage<float> blank_frame(per_pixel_bias_estimates);
    blank_frame.pix.assign(0.0);

    CcdImage<double> raw_avg =
        clean_and_register_subroutines::create_debiased_average(
            frame_vec,
            crop_box,
            first.CCD_grid.image_outline_,
            blank_frame,
            n_frames_to_process,
            7);

    CcdImage<double>pixel_debiased_avg(raw_avg);
    pixel_debiased_avg.pix -= PixelArray2d<double>(per_pixel_bias_estimates.pix);
    pixel_debiased_avg.write_to_file(output_folder+"pixel_debiased_avg.fits");

    CcdImage<float> combined_bias_frame(standard_col_bias_frm);
    combined_bias_frame.pix += row_bias_frm.pix;

    CcdImage<double> algorithm_debiased_avg(raw_avg);
//        clean_and_register_subroutines::create_debiased_average(
//            frame_vec,
//            crop_box,
//            first.CCD_grid.image_outline_,
//            combined_bias_frame,
//            n_frames_to_process,
//            7);

    algorithm_debiased_avg.pix -= PixelArray2d<double>(combined_bias_frame.pix);
    algorithm_debiased_avg.write_to_file(output_folder+"algorithm_debiased_avg.fits");

//----------------------------------------------------------------------------------
    CcdImage<float> diff_after_col_debias(per_pixel_bias_estimates);
    diff_after_col_debias.pix -= standard_col_bias_frm.pix;
    diff_after_col_debias.write_to_file(output_folder+"col_db_bias_diff.fits");

    CcdImage<float> diff_after_col_row_db(diff_after_col_debias);
    diff_after_col_row_db.pix -= row_bias_frm.pix;
    diff_after_col_row_db.write_to_file(output_folder + "col_and_row_db_bias_diff.fits");

    return (0);
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 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_FUNCTION_IS_POSITIVE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_POSITIVE_HPP_INCLUDED

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

 /*!

    @ingroup group-predicates
    Function object implementing is_positive capabilities

    Returns @ref True if x is positive else @ref False.

    This function differs from @ref is_gtz for floating point arguments,
    because @ref Zero is positive but not greater than 0, and @ref Mzero is
    not positive and not greater than 0, It's probably @ref is_gtz that
    you want.

    @par Semantic:

    @code
    as_logical_t<T> r = is_positive(x);
    @endcode

    is similar to:

    @code
    if T is signed
      as_logical_t<T> r = bitofsign(x) == 0;
    else
      as_logical_t<T> r = True ;
    @endcode

    @par Note:

    Mzero is the floating point 'minus zero',
    i.e. all bits are @ref Zero but the sign bit.
    Such a value is treated as @ref Zero by ieee standards.

    behaviour of is_positive on @ref Nan is undefined.

    @see is_negative,  Mzero, bitofsign

  **/
  const boost::dispatch::functor<tag::is_positive_> is_positive = {};
} }
#endif

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

#endif


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

#include <sstream>
#include "tester.h"
#include <dlib/svm_threaded.h>
#include <dlib/rand.h>

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

    logger dlog("test.learning_to_track");

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

    struct detection_dense
    {
        typedef struct track_dense track_type;
        matrix<double,0,1> measurements;
    };

    struct track_dense
    {
        typedef matrix<double,0,1> feature_vector_type;

        track_dense()
        {
            time_since_last_association = 0;
        }

        void get_similarity_features(const detection_dense det, feature_vector_type& feats) const
        {
            feats = abs(last_measurements - det.measurements);
        }

        void update_track(const detection_dense det)
        {
            last_measurements = det.measurements;
            time_since_last_association = 0;
        }

        void propagate_track()
        {
            ++time_since_last_association;
        }

        matrix<double,0,1> last_measurements;
        unsigned long time_since_last_association;
    };

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

    struct detection_sparse
    {
        typedef struct track_sparse track_type;
        matrix<double,0,1> measurements;
    };

    struct track_sparse
    {
        typedef std::vector<std::pair<unsigned long,double> > feature_vector_type;

        track_sparse()
        {
            time_since_last_association = 0;
        }

        void get_similarity_features(const detection_sparse det, feature_vector_type& feats) const
        {
            matrix<double,0,1> temp = abs(last_measurements - det.measurements);
            feats.clear();
            for (long i = 0; i < temp.size(); ++i)
                feats.push_back(make_pair(i, temp(i)));
        }

        void update_track(const detection_sparse det)
        {
            last_measurements = det.measurements;
            time_since_last_association = 0;
        }

        void propagate_track()
        {
            ++time_since_last_association;
        }

        matrix<double,0,1> last_measurements;
        unsigned long time_since_last_association;
    };

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

    dlib::rand rnd;
    const long num_objects = 4;
    const long num_properties = 6;
    std::vector<matrix<double,0,1> > object_properties(num_objects);

    void initialize_object_properties()
    {
        rnd.set_seed("23ja2oirfjaf");
        for (unsigned long i = 0; i < object_properties.size(); ++i)
            object_properties[i] = randm(num_properties,1,rnd);
    }

    template <typename detection>
    detection sample_detection_from_sensor(long object_id)
    {
        DLIB_CASSERT(object_id < num_objects,
            "You can't ask to sample a detection from an object that doesn't exist.");
        detection temp;
        // Set the measurements equal to the object's true property values plus a little bit of
        // noise.
        temp.measurements = object_properties[object_id] + randm(num_properties,1,rnd)*0.1;
        return temp;
    }

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

    template <typename detection>
    std::vector<std::vector<labeled_detection<detection> > > make_random_tracking_data_for_training()
    {
        typedef std::vector<labeled_detection<detection> > detections_at_single_time_step;
        typedef std::vector<detections_at_single_time_step> track_history;

        track_history data;

        // At each time step we get a set of detections from the objects in the world.
        // Simulate 100 time steps worth of data where there are 3 objects present.
        const int num_time_steps = 100;
        for (int i = 0; i < num_time_steps; ++i)
        {
            detections_at_single_time_step dets(3);
            // sample a detection from object 0
            dets[0].det = sample_detection_from_sensor<detection>(0);
            dets[0].label = 0;

            // sample a detection from object 1
            dets[1].det = sample_detection_from_sensor<detection>(1);
            dets[1].label = 1;

            // sample a detection from object 2
            dets[2].det = sample_detection_from_sensor<detection>(2);
            dets[2].label = 2;

            randomize_samples(dets, rnd);
            data.push_back(dets);
        }

        // Now let's imagine object 1 and 2 are gone but a new object, object 3 has arrived.
        for (int i = 0; i < num_time_steps; ++i)
        {
            detections_at_single_time_step dets(2);
            // sample a detection from object 0
            dets[0].det = sample_detection_from_sensor<detection>(0);
            dets[0].label = 0;

            // sample a detection from object 3
            dets[1].det = sample_detection_from_sensor<detection>(3);
            dets[1].label = 3;

            randomize_samples(dets, rnd);
            data.push_back(dets);
        }

        return data;
    }

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

    template <typename detection>
    std::vector<detection> make_random_detections(long num_dets)
    {
        DLIB_CASSERT(num_dets <= num_objects,
            "You can't ask for more detections than there are objects in our little simulation.");

        std::vector<detection> dets(num_dets);
        for (unsigned long i = 0; i < dets.size(); ++i)
        {
            dets[i] = sample_detection_from_sensor<detection>(i);
        }
        randomize_samples(dets, rnd);
        return dets;
    }

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

    template <typename detection>
    void test_tracking_stuff()
    {
        print_spinner();

        typedef std::vector<labeled_detection<detection> > detections_at_single_time_step;
        typedef std::vector<detections_at_single_time_step> track_history;
        std::vector<track_history> data;
        data.push_back(make_random_tracking_data_for_training<detection>());
        data.push_back(make_random_tracking_data_for_training<detection>());
        data.push_back(make_random_tracking_data_for_training<detection>());
        data.push_back(make_random_tracking_data_for_training<detection>());
        data.push_back(make_random_tracking_data_for_training<detection>());

        structural_track_association_trainer trainer;
        trainer.set_c(1000);
        track_association_function<detection> assoc = trainer.train(data);

        double test_val = test_track_association_function(assoc, data);
        DLIB_TEST_MSG( test_val == 1, test_val);
        test_val = cross_validate_track_association_trainer(trainer, data, 5);
        DLIB_TEST_MSG ( test_val == 1, test_val);

        typedef typename detection::track_type track;
        std::vector<track> tracks;

        std::vector<detection> dets = make_random_detections<detection>(3);
        assoc(tracks, dets);
        DLIB_TEST(tracks.size() == 3);

        dets = make_random_detections<detection>(3);
        assoc(tracks, dets);
        DLIB_TEST(tracks.size() == 3);

        dets = make_random_detections<detection>(3);
        assoc(tracks, dets);
        DLIB_TEST(tracks.size() == 3);

        dets = make_random_detections<detection>(4);
        assoc(tracks, dets);
        DLIB_TEST(tracks.size() == 4);

        dets = make_random_detections<detection>(3);
        assoc(tracks, dets);
        DLIB_TEST(tracks.size() == 4);
        unsigned long total_miss = 0;
        for (unsigned long i = 0; i < tracks.size(); ++i)
            total_miss += tracks[i].time_since_last_association;
        DLIB_TEST(total_miss == 1);

        dets = make_random_detections<detection>(3);
        assoc(tracks, dets);
        DLIB_TEST(tracks.size() == 4);
        total_miss = 0;
        unsigned long num_zero = 0;
        for (unsigned long i = 0; i < tracks.size(); ++i)
        {
            total_miss += tracks[i].time_since_last_association;
            if (tracks[i].time_since_last_association == 0)
                ++num_zero;
        }
        DLIB_TEST(total_miss == 2);
        DLIB_TEST(num_zero == 3);

        ostringstream sout;
        serialize(assoc, sout);

        istringstream sin(sout.str());
        deserialize(assoc, sin);
        DLIB_TEST( test_track_association_function(assoc, data) == 1);
    }

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

    class test_learning_to_track : public tester
    {
    public:
        test_learning_to_track (
        ) :
            tester ("test_learning_to_track",
                "Runs tests on the assignment learning code.")
        {}

        void perform_test (
        )
        {
            initialize_object_properties();
            for (int i = 0; i < 3; ++i)
            {
                dlog << LINFO << "run test_tracking_stuff<detection_dense>()";
                test_tracking_stuff<detection_dense>();
                dlog << LINFO << "run test_tracking_stuff<detection_sparse>()";
                test_tracking_stuff<detection_sparse>();
            }
        }
    } a;

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

}


#include <pangolin/pangolin.h>
#include <pangolin/gl/glvbo.h>
#include <pangolin/gl/glsl.h>
#include <random>

#include <Eigen/Eigen>

const char* shader_vert = R"glsl(
    #version 150 core
    in vec3 pos;
    uniform mat4 T_co;

    void main()
    {
        gl_Position = T_co * vec4(pos, 1.0);
    }
)glsl";

const char* shader_geom = R"glsl(
    #version 150 core

    layout(points) in;
    layout(triangle_strip, max_vertices = 4) out;
    uniform float rad;
    uniform mat4 Proj;
    out vec2 uv;

    void main()
    {
        gl_Position = Proj * (gl_in[0].gl_Position + vec4(-rad, -rad, 0.0, 0.0));
        uv = vec2(-1,-1);
        EmitVertex();
        gl_Position = Proj * (gl_in[0].gl_Position + vec4(-rad, +rad, 0.0, 0.0));
        uv = vec2(-1,+1);
        EmitVertex();
        gl_Position = Proj * (gl_in[0].gl_Position + vec4(+rad, -rad, 0.0, 0.0));
        uv = vec2(+1,-1);
        EmitVertex();
        gl_Position = Proj * (gl_in[0].gl_Position + vec4(+rad, +rad, 0.0, 0.0));
        uv = vec2(+1,+1);
        EmitVertex();
        EndPrimitive();
    }
)glsl";

const char* shader_frag = R"glsl(
    #version 150 core
    in  vec2 uv;
    out vec4 outColor;
    uniform float weight;

    void main()
    {
        float sigma = 1.0/2.5;
        float sigma_sq = sigma*sigma;
        float dist_sq = uv[0]*uv[0] + uv[1]*uv[1];
        float prob = 1.0 - weight*exp(-dist_sq / (2*sigma_sq));
        outColor = vec4(1.0, 1.0, 1.0, prob);
    }
)glsl";

int main( int /*argc*/, char** /*argv*/ )
{
    using namespace pangolin;

    pangolin::CreateWindowAndBind("Main",640,480);
    glEnable(GL_DEPTH_TEST);
    glDepthFunc(GL_ALWAYS);
    glEnable( GL_BLEND );
    glBlendFunc(GL_ZERO, GL_SRC_ALPHA);
    glClearColor(1.0,1.0,1.0,1.0);

    // Define Projection and initial ModelView matrix
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(640,480,420,420,320,240,0.2,100),
        pangolin::ModelViewLookAt(-2,2,-2, 0,0,0, pangolin::AxisY)
    );

    // Create Interactive View in window
    pangolin::Handler3D handler(s_cam);
    pangolin::View& d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, 0.0, 1.0, -640.0f/480.0f)
            .SetHandler(&handler);

    std::random_device rd{};
    std::mt19937 gen{rd()};

    Eigen::Matrix<Eigen::Vector3f,Eigen::Dynamic,Eigen::Dynamic> data(1000,1000);
    std::normal_distribution<> d{0.0,1.0};
    for(int r=0; r < data.rows(); ++r) {
        for(int c=0; c < data.cols(); ++c) {
//            const float x = d(gen);
//            const float y = d(gen);
//            const float z = d(gen);
//            data(r,c) = Eigen::Vector3f(x,y,z);

            const float x = 6*M_PI * r / (float)data.rows();
            const float y = 6*M_PI * c / (float)data.cols();
            data(r,c) = Eigen::Vector3f(x,y,sin(x)+cos(y));
        }
    }

    GlBuffer vbo(GlArrayBuffer, data.rows()*data.cols(), GL_FLOAT, 3, GL_STATIC_DRAW);
    vbo.Upload(data.data(), data.rows()*data.cols() * sizeof(Eigen::Vector3f));

    GlSlProgram prog;
    prog.AddShader(GlSlVertexShader, shader_vert);
    prog.AddShader(GlSlGeometryShader, shader_geom);
    prog.AddShader(GlSlFragmentShader, shader_frag);
    prog.Link();
//    prog.Bind();

    // Specify layout of point data

    pangolin::Var<float> rad("geom.rad", 0.01);
    pangolin::Var<float> weight("geom.weight", 0.1);

    while( !pangolin::ShouldQuit() )
    {
        // Clear screen and activate view to render into
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);

        {
            prog.Bind();
            prog.SetUniform("rad", rad.Get());
            prog.SetUniform("weight", weight.Get());
            prog.SetUniform("T_co", s_cam.GetModelViewMatrix() );
            prog.SetUniform("Proj", s_cam.GetProjectionMatrix() );
            GLint posAttrib = prog.GetAttributeHandle("pos");
            glEnableVertexAttribArray(posAttrib);
            vbo.Bind();
            glVertexAttribPointer(posAttrib, 3, GL_FLOAT, GL_FALSE, 0, 0);
            RenderVbo(vbo, GL_POINTS);
            vbo.Unbind();
            glDisableVertexAttribArray(posAttrib);
            prog.Unbind();
        }

        // Swap frames and Process Events
        pangolin::FinishFrame();
    }

    return 0;
}


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/1-50/47/problem47.hpp"

BOOST_AUTO_TEST_SUITE( Problem47 )

    BOOST_AUTO_TEST_CASE( Example1 ) {
        auto res = problems::problem47::solve(2, 2);
        BOOST_CHECK_EQUAL(res, 14);
    }

    BOOST_AUTO_TEST_CASE( Example2 ) {
        auto res = problems::problem47::solve(3, 3);
        BOOST_CHECK_EQUAL(res, 644);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem47::solve();
        BOOST_CHECK_EQUAL(res, 134043);
    }

BOOST_AUTO_TEST_SUITE_END()


/*=============================================================================
    Copyright (c) 2001-2006 Joel de Guzman
    Copyright (c) 2007 Dan Marsden

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
==============================================================================*/
#include <boost/detail/lightweight_test.hpp>
#include <boost/fusion/container/vector/vector.hpp>
#include <boost/fusion/adapted/mpl.hpp>
#include <boost/fusion/sequence/io/out.hpp>
#include <boost/fusion/sequence/intrinsic/at.hpp>
#include <boost/fusion/container/generation/make_vector.hpp>
#include <boost/fusion/algorithm/iteration/fold.hpp>
#include <boost/fusion/algorithm/iteration/accumulate.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/vector.hpp>

#include <boost/type_traits/remove_const.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/type_traits/is_reference.hpp>

#include <string>

using boost::mpl::if_;
using boost::mpl::int_;
using boost::is_same;

struct add_ints_only
{
    template<typename T>
    struct result;

    template <typename State, typename T>
    struct result<add_ints_only(State, T)>
    {
        typedef typename boost::remove_const<
            typename boost::remove_reference<State>::type>::type type;
    };

    template <typename State, typename T>
    State
    operator()(State const& state, T const& /*x*/) const
    {
        return state;
    }

    int
    operator()(int state, int x) const
    {
        return x + state;
    }
};

struct count_ints
{
    template<typename T>
    struct result;

    template <typename CountT, typename T>
    struct result<count_ints(CountT, T)>
    {
        typedef typename boost::remove_const<
            typename boost::remove_reference<CountT>::type>::type state;
        typedef typename boost::remove_const<
            typename boost::remove_reference<T>::type>::type elem;

        typedef typename
            if_<
                is_same<elem, int>
              , typename boost::mpl::next<state>::type
              , state
            >::type
        type;
    };

    template <typename CountT, typename T>
    typename result<count_ints(CountT, T)>::type
    operator()(CountT const&, T const&) const
    {
        typedef typename result<count_ints(CountT, T)>::type result_;
        return result_();
    }
};

struct appender
{
    typedef std::string result_type;

    std::string operator()(std::string const& str, char c) const
    {
        return str + c;
    }
};

struct lvalue_adder
{
    template<typename Sig>
    struct result;

    template<typename T0, typename T1>
    struct result<lvalue_adder(T0, T1&)>
    {
        // Second argument still needs to support rvalues - see definition of fusion::fold
        typedef T1 type;
    };

    template<typename T0, typename T1>
    T1 operator()(T0 const& lhs, T1& rhs) const
    {
        return lhs + rhs;
    }
};

int add(int lhs, int rhs)
{
    return lhs + rhs;
}

int
main()
{
    using namespace boost::fusion;
    namespace fusion = boost::fusion;

    {
        typedef vector<int, char, int, double> vector_type;
        vector_type v(12345, 'x', 678910, 3.36);
        int result = fold(v, 0, add_ints_only());
        std::cout << result << std::endl;
        BOOST_TEST(result == 12345+678910);
    }

    {
        typedef vector<int> vector_type;
        vector_type v(12345);

        int n = fusion::fold(v, int_<0>(), count_ints());
        std::cout << n << std::endl;
        BOOST_TEST(n == 1);
    }

    {
        typedef vector<int, char, int, double, int> vector_type;
        vector_type v(12345, 'x', 678910, 3.36, 8756);

        int n = fusion::fold(v, int_<0>(), count_ints());
        std::cout << n << std::endl;
        BOOST_TEST(n == 3);
    }

    {
        typedef boost::mpl::vector<int, char, int, double, int> mpl_vec;
        int n = fusion::fold(mpl_vec(), int_<0>(), count_ints());
        std::cout << n << std::endl;
        BOOST_TEST(n == 3);
    }

    {
        BOOST_TEST(fusion::fold(fusion::make_vector('a','b','c','d','e'), std::string(""), appender())
                   == "abcde");
    }

    {
        vector<int, int> vec(1,2);
        BOOST_TEST(fusion::fold(vec, 0, lvalue_adder()) == 3);
    }

    {
        vector<int, int> vec(1,2);
        BOOST_TEST(fusion::fold(vec, 0, add) == 3);
    }

    {
        typedef vector<int, char, int, double> vector_type;
        vector_type v(12345, 'x', 678910, 3.36);
        int result = accumulate(v, 0, add_ints_only());
        std::cout << result << std::endl;
        BOOST_TEST(result == 12345+678910);
    }

    {
        typedef vector<int> vector_type;
        vector_type v(12345);

        int n = fusion::accumulate(v, int_<0>(), count_ints());
        std::cout << n << std::endl;
        BOOST_TEST(n == 1);
    }

    {
        typedef vector<int, char, int, double, int> vector_type;
        vector_type v(12345, 'x', 678910, 3.36, 8756);

        int n = fusion::accumulate(v, int_<0>(), count_ints());
        std::cout << n << std::endl;
        BOOST_TEST(n == 3);
    }

    {
        typedef boost::mpl::vector<int, char, int, double, int> mpl_vec;
        int n = fusion::accumulate(mpl_vec(), int_<0>(), count_ints());
        std::cout << n << std::endl;
        BOOST_TEST(n == 3);
    }

    {
        BOOST_TEST(fusion::accumulate(fusion::make_vector('a','b','c','d','e'), std::string(""), appender())
                   == "abcde");
    }

    {
        vector<int, int> vec(1,2);
        BOOST_TEST(fusion::accumulate(vec, 0, add) == 3);
    }

    return boost::report_errors();
}


// superblocks.hpp
// Superblocks - Version 0.4.3

#include <iostream>
#include <string>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

using namespace std;
using namespace boost;

// Seed Types:
#define SEED_WITH_BLOCK_HEIGHT 1
#define SEED_WITH_PREVIOUS_BLOCK_HASH 2

// Luck Types:
#define LUCK_WITH_LESS_THAN_X 1
#define LUCK_WITH_MORE_THAN_X 2
#define LUCK_WITH_MORE_THAN_X_AND_LESS_THAN_Y 3

// Secondary Luck Types:
#define SECONDARY_REWARD_PLUS_RAND_MINUS_X 1
#define SECONDARY_REWARD_PLUS_NEW_RAND_MINUS_X 2

class Superblocks {

  public:

    int debug;
    clock_t cstart, cend;

    string coin;
    string coin_name;

    int hash_length;

    int seed_type[LUCK_STEPS];

    int cutout_start[LUCK_STEPS];
    int cutout_length[LUCK_STEPS];

    int rnd_range_low[LUCK_STEPS];
    int rnd_range_high[LUCK_STEPS];

    int secondary_rand[LUCK_STEPS]; // do secondary rand step?
    int secondary_rand_type[LUCK_STEPS];
    int secondary_rand_low[LUCK_STEPS];
    int secondary_rand_high[LUCK_STEPS];
    int secondary_rand_x[LUCK_STEPS];
    int secondary_cutout_start[LUCK_STEPS];
    int secondary_cutout_length[LUCK_STEPS];

    int luck_type[LUCK_STEPS];

    int luck_x[LUCK_STEPS];
    int luck_y[LUCK_STEPS];

    int reward[LUCK_STEPS];

    void init();

    int getnextblockreward( const char* hash ); // in superblocks.cpp

    int getsecondaryreward( int seed, int step ); // in superblocks.cpp

    int /*static*/ generateMTRandom(unsigned int s, int range_low, int range_high); // in utils.cpp

    long hex2long(const char* hexString); // in utils.cpp

    long gethashcutout( int step, const char* hash ); // in utils.cpp

    void timer_start(); // in utils.cpp

    float timer_end(); // in utils.cpp

	string getseedtype( int type )
	{
		switch( type ) {
			case 1: return "SEED_WITH_BLOCK_HEIGHT";
			case 2: return "SEED_WITH_PREVIOUS_BLOCK_HASH";
			default: return "error";
		}
	}
	string getlucktype( int type )
	{
		switch( type ) {
			case 1: return "LUCK_WITH_LESS_THAN_X";
			case 2: return "LUCK_WITH_MORE_THAN_X";
			case 3: return "LUCK_WITH_MORE_THAN_X_AND_LESS_THAN_Y";
			default: return "error";
		}
	}
	string getsecondarylucktype( int type )
	{
		switch( type ) {
			case 1: return "SECONDARY_REWARD_PLUS_RAND_MINUS_X";
			default: return "error";
		}
	}

};


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

#include "odometry_measurement.h"

#include <Eigen/Core>
#include <Eigen/Geometry>

namespace g2o {

VelocityMeasurement::VelocityMeasurement() : _measurement(0., 0.), _dt(0.) {}

VelocityMeasurement::VelocityMeasurement(number_t vl, number_t vr, number_t dt)
    : _measurement(vl, vr), _dt(dt) {}

MotionMeasurement::MotionMeasurement() : _measurement(0., 0., 0.), _dt(0.) {}

MotionMeasurement::MotionMeasurement(number_t x, number_t y, number_t theta,
                                     number_t dt)
    : _measurement(x, y, theta), _dt(dt) {}

MotionMeasurement::MotionMeasurement(const Vector3& m, number_t dt)
    : _measurement(m), _dt(dt) {}

VelocityMeasurement OdomConvert::convertToVelocity(const MotionMeasurement& m) {
  if (fabs(m.theta()) > 1e-7) {
    const number_t translation = std::hypot(m.x(), m.y());
    const number_t R = translation / (2 * sin(m.theta() / 2));
    number_t w = 0.;
    if (fabs(m.dt()) > 1e-7) w = m.theta() / m.dt();

    const number_t vl = (2. * R * w - w) / 2.;
    const number_t vr = w + vl;

    return VelocityMeasurement(vl, vr, m.dt());
  } else {
    number_t vl, vr;
    if (fabs(m.dt()) > 1e-7)
      vl = vr = std::hypot(m.x(), m.y()) / m.dt();
    else
      vl = vr = 0.;
    return VelocityMeasurement(vl, vr, m.dt());
  }
}

MotionMeasurement OdomConvert::convertToMotion(const VelocityMeasurement& v,
                                               number_t l) {
  number_t x, y, theta;
  if (fabs(v.vr() - v.vl()) > 1e-7) {
    number_t R = l * 0.5 * ((v.vl() + v.vr()) / (v.vr() - v.vl()));
    number_t w = (v.vr() - v.vl()) / l;

    theta = w * v.dt();
    Rotation2D rot(theta);
    Vector2 icc(0, R);
    Vector2 motion = (rot * (Vector2(-1. * icc))) + icc;
    x = motion.x();
    y = motion.y();
  } else {
    number_t tv = 0.5 * (v.vr() + v.vl());
    theta = 0.;
    x = tv * v.dt();
    y = 0.;
  }

  return MotionMeasurement(x, y, theta, v.dt());
}

}  // namespace g2o


/*
 * =====================================================================================
 *
 *       Filename:  Composition.cpp
 *
 *    Description:
 *
 *        Version:  1.0
 *        Created:  4/23/2012 10:12:49 PM
 *       Revision:  none
 *       Compiler:  msvc
 *
 *         Author:  HAN HU, hh1985@bu.edu
 *   Organization:  Bioinformatics Program, Boston University
 *
 * =====================================================================================
 */
#include "GAGPL/CHEMISTRY/Composition.h"
#include <boost/lexical_cast.hpp>
#include <stdexcept>
#include <locale>

namespace  gag
{
	double Composition::getMass() const
	{
		std::map<std::string, int>::const_iterator iter1 = _composition.begin();
		double mass = 0.0;
		for(; iter1 != _composition.end(); iter1++)
		{
			const Isotope& iso = _ptable.getIsotopeByRelativeShift(iter1->first);
			// TBD: support for multiple isotopes.
			// std::vector<Isotope>::iterator iter2 = e.isotopes.begin();
			mass += (*iter1).second * iso.mass;
		}
		return mass;
	}

	std::string Composition::getCompositionString() const
	{
		std::string compo_string;

		// Generate the string.
		for(std::map<std::string, int>::const_iterator iter = _composition.begin(); iter != _composition.end(); iter++)
		{
			if(iter->second == 1)
				compo_string.append(iter->first);
			else if(iter->second > 1)
			{
				compo_string.append(iter->first);
				compo_string.append(boost::lexical_cast<std::string>(iter->second));
			} else {
				throw 32751;
			}
		}
		return compo_string;
	}

	void Composition::addElement(const std::string& elem, const int& num)
	{
		// If the string has been contained in _composition, simply add it up.
		// otherwise create a new one.
		std::map<std::string, int>::iterator iter = _composition.find(elem);

		if(iter != _composition.end())
		{
			iter->second += num;
		} else
		{
			_composition.insert(std::make_pair(elem, num));
		}

		//updateMass();
		//updateString();
	}

	void Composition::update(const std::string& str)
  {
    // Split the composition into a linear combination of Elements.
    // and do format examination.
		if(str.length() == 0)
			throw 8888;

		std::locale loc;
		std::string temp_str;
		std::string temp_num;

		for(size_t i=0; i<str.length(); i++)
		{
			// Only three types of character are allowed:
			// Uppercase letter, lowercase letter and digits (0-9)

			// Better to add exception control of the data type here.

			if(std::isupper(str[i],loc))
			{
				temp_str=str[i];

				// Dump the string and number into composition.
				if(i == str.length()-1 || std::isupper(str[i+1],loc))
				{
					(*this).addElement(temp_str, 1);

					temp_str.clear();
					//temp_num.clear();

				}

			} else if(std::islower(str[i],loc)) // There will always be only one lowercase letter.
			{

				temp_str.push_back(str[i]);

				// If the lowercase letter is the last one or the next is not a digit (should be uppercase letter)
				if(i == str.length()-1 || isupper(str[i+1],loc))
				{
					(*this).addElement(temp_str, 1);
					temp_str.clear();
				}

			} else if(std::isdigit(str[i],loc))
			{
				temp_num.push_back(str[i]);

				// It is unlikely that temp_num still has length 0. But be careful of that.
				if(i == str.length()-1 || isupper(str[i+1],loc))
				{
					(*this).addElement(temp_str, boost::lexical_cast<int>(temp_num));

					temp_str.clear();
					temp_num.clear();
				}
			}
		}

		//updateMass();
		//updateString();

		//return _composition;

  }

	void Composition::add(const Composition& cp)
  {

		const std::map<std::string, int>& cp_map = cp.get();

		for(std::map<std::string, int>::const_iterator iter1 = cp_map.begin(); iter1 != cp_map.end(); iter1++)
    {

			std::map<std::string, int>::iterator iter2 = _composition.find((*iter1).first);

      if(iter2 != _composition.end())
				(*iter2).second += (*iter1).second;

			else
				_composition.insert(std::make_pair(iter1->first,iter1->second));
    }

		//updateMass();
		//updateString();

  }

	void Composition::deduct(const Composition& cp)
  {

    const std::map<std::string, int>& cp_map = cp.get();

		for(std::map<std::string, int>::const_iterator iter1 = cp_map.begin(); iter1 != cp_map.end(); iter1++)
    {

			std::map<std::string, int>::iterator iter2 = _composition.find((*iter1).first);

			if(iter2 != _composition.end() && (*iter1).second < (*iter2).second)
        (*iter2).second -= (*iter1).second;
      else if((*iter1).second == (*iter2).second)
        _composition.erase(iter2);
      else
				throw std::runtime_error("Error: the deducted composition is invalid!");
    }

		//updateMass();
		//updateString();
  }

  // Update the value of _compostion.
  void Composition::add(const std::string& compo)
  {
		Composition cp(compo);
		this->add(cp);

  }

	void Composition::deduct(const std::string& compo)
  {
		Composition cp(compo);
		this->deduct(cp);
  }

	void Composition::clear()
	{
		_composition.clear();
		//_mass = 0.0;
		//_compo_string.clear();
	}

	Composition::Composition(const std::string& str)
		: _ptable(PeriodicTable::Instance()), _composition()/*, _mass(0.0), _compo_string() */
	{
		// Update _composition and _mass
		//_ptable.load();
		(*this).update(str);
	}

	Composition::Composition()
		: _ptable(PeriodicTable::Instance()), _composition()/*, _mass(0.0), _compo_string()*/
	{
		//_ptable.load();
	}

	Composition& Composition::operator=(const Composition& rhs)
	{
		if(this != &rhs)
		{
			_composition = rhs._composition; // Copy constructor for map.
			//_mass = rhs._mass;
		}

		return *this;
	}

	size_t Composition::getMaxNumVariants() const
	{
		std::map<std::string, int>::const_iterator it = _composition.begin();

		size_t max_num = 0;

		for(; it!=_composition.end(); it++)
		{
			max_num += it->second * _ptable.getMaxRelativeShift(it->first);
		}

		return max_num;
	}

	double Composition::getAverageMass() const
	{
		double avg_mass(0.0);
		std::map<std::string, int>::const_iterator it = _composition.begin();

		for(; it!=_composition.end(); it++)
		{
			avg_mass += _ptable.getElementBySymbol(it->first).getAverageMass() * it->second;
		}

		return avg_mass;
	}
}


/*
 This program is free software; you can redistribute it and/or modify it under
 the terms of the European Union Public Licence - EUPL v.1.1 as published by
 the European Commission.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE. See the European Union Public Licence - EUPL v.1.1
 for more details.

 You should have received a copy of the European Union Public Licence - EUPL v.1.1
 along with this program.

 Further information about the European Union Public Licence - EUPL v.1.1 can
 also be found on the world wide web at http://ec.europa.eu/idabc/eupl
 */

/*
 ------ Copyright (C) 2010 STA Steering Board (space.trajectory.analysis AT gmail.com) ----
 ------------------ Author: Ricardo Noriega  ----------------------------------------------
 ------------------ email: ricardonor@gmail.com  ------------------------------------------
Totally changed by Guillermo in May 2011 due to the numerioud bugs in the original code that could not calculate properly and crashed STA.
Commenting properly all methods to allow the poor new guy that will update the code

 */

#include <QDebug>

#include <math.h>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include "commanalysis.h"
#include "Main/findDataFolder.h"
#include "Locations/environmentdialog.h"
#include "Payloads/receiverPayloadDialog.h"
#include <Astro-Core/surfaceVelocity.h>

#ifdef Q_WS_MAC
#include <CoreFoundation/CFBundle.h>
#endif

//Definition of constructors, the default one contains a warning.

using namespace Eigen;

bool tracking=true;//This allows to set a tracking system for the calculations, so the antenna of the groundstation follows the spacecraft!

CommAnalysis::CommAnalysis()
{
    qDebug()<< "Warning: CommAnalysis object created without any participant to make the calculations";
}

CommAnalysis::CommAnalysis(ScenarioTransmitterPayloadType *transmitter, ScenarioReceiverPayloadType *receiver, ScenarioGroundStationEnvironment* environment, PropagatedScenario* propagatedScenario, int indexSC, int indexGS, int indexMA, bool flagTX, bool flagRX):m_transmitter(transmitter), m_receiver(receiver), m_propagatedScenario(propagatedScenario), m_environment(environment), m_indexSC(indexSC), m_indexGS(indexGS), m_indexMA(indexMA), m_flagTX(flagTX), m_flagRX(flagRX)
{
    // Nothing to be done
}

//Default destructor

CommAnalysis::~CommAnalysis()
{
    // Noting to be done
}

double lightSpeed=SPEED_OF_LIGHT;
double Pi=mypi;

////////////////////////////////////////////// FUNCTIONS ////////////////////////////////////////////////////////

/** Calculates the Doppler shift due to motion between a receiver and a emmitter
  * The calculation is done for a given participant a a given mission arc of this participant
  * Accepts as parameters the lenght of the list that actually corresponds to each of the
  *
  * \return a QList of double numbers containing all doppler shifts in time
  *
  */
QList<double> CommAnalysis::getDopplerShiftList(int numberOfRows)
{

    QList<double> myDopplerShiftList;
    double dopplerShift;

    for(int i=0; i<numberOfRows; i++)
    {
        double frequency=m_transmitter->Budget()->FrequencyBand();
        double t=m_propagatedScenario->spaceObjects().at(m_indexSC)->mission().at(m_indexMA)->trajectorySampleTime(i);

        bool visibility;
        if(m_propagatedScenario->groundObjects().at(m_indexGS)->elevationAngle(m_propagatedScenario->spaceObjects().at(m_indexSC),t)>=5.0*DEG2RAD)
        {
            visibility=true;
            sta::StateVector samples;

            samples=m_propagatedScenario->spaceObjects().at(m_indexSC)->mission().at(m_indexMA)->trajectorySample(i);

            double latitudeGS=m_propagatedScenario->groundObjects().at(m_indexGS)->latitude;
            double longitudeGS=m_propagatedScenario->groundObjects().at(m_indexGS)->longitude;
            double altitudeGS=m_propagatedScenario->groundObjects().at(m_indexGS)->altitude;

            latitudeGS=DEG2RAD*(latitudeGS);
            longitudeGS=DEG2RAD*(longitudeGS);

            longitudeGS=longitudeGS+getGreenwichHourAngle(m_propagatedScenario->spaceObjects().at(m_indexSC)->mission().at(m_indexMA)->trajectorySampleTime(i)+2400000.5);

            Vector3d normPosition(cos(latitudeGS) * cos(longitudeGS),
                                  cos(latitudeGS) * sin(longitudeGS),
                                  sin(latitudeGS));

            Vector3d stationPos=normPosition.cwise() * STA_SOLAR_SYSTEM->lookup("Earth")->radii() + normPosition * altitudeGS;

            Vector3d toSpacecraft = (samples.position - stationPos).normalized();

            Vector3d velSat=samples.velocity;
            surfaceVelocity earthVel;
            double speedGS=earthVel.earthStationLinearVelocity(latitudeGS);
            Vector3d velGS(speedGS*sin(longitudeGS),speedGS*cos(longitudeGS),0);
            toSpacecraft.normalize();

            double radialVelToGS = ((velGS-velSat).dot(toSpacecraft)) * 1000; //The result is in Km/sec so I convert it to m/s

            dopplerShift=radialVelToGS*frequency/lightSpeed;

            myDopplerShiftList.append(dopplerShift);

        }
        else
        {
            visibility=false;
            myDopplerShiftList.append(0.0);
        }
    }

    return myDopplerShiftList;
} ///////////////////////////////////////// end of the method //////////////////////////////////////////

/** Calculates the free space loss due to motion between a receiver and a emmitter
  * The calculation is done for a given participant a a given mission arc of this participant
  * Accepts as parameters the lenght of the list that actually corresponds to each of the
  *
  * \return a QList of double numbers containing all free space losses in time
  *
  */
QList<double> CommAnalysis::getFreeSpaceLossList(int numberOfRows)
{
    QList<double> myFreeSpaceLossList;
    double eachFreeSpaceLoss, eachFreeSpaceLossIndBs;

    //The inputs for this function will be the frequency and the range between both participants
    double frequency = m_transmitter->Budget()->FrequencyBand();
    double range, t;

    for(int i=0; i<numberOfRows; i++)
    {
        t = m_propagatedScenario->spaceObjects().at(m_indexSC)->mission().at(m_indexMA)->trajectorySampleTime(i);
        range = m_propagatedScenario->groundObjects().at(m_indexGS)->getRange(m_propagatedScenario->spaceObjects().at(m_indexSC), t) * 1000;  // In meters

        bool visibility;
        // The following condition shall be upgraded to make it gereric (not only 5.0*DEG2GRAD times)
        if (m_propagatedScenario->groundObjects().at(m_indexGS)->elevationAngle(m_propagatedScenario->spaceObjects().at(m_indexSC),t) >= 5.0*DEG2RAD)
        {
            visibility=true;
            //mjdList.append(t);
            //range=range*1000;
            //rangeList.append(range);//In metres
            eachFreeSpaceLoss = pow((4 * Pi * range * frequency / lightSpeed), 2);  // This formula is defined to be in the denominator
                                                                                // of the C/No when it is expressed in natural units
            eachFreeSpaceLossIndBs = 10*log10(eachFreeSpaceLoss); //This value shall be preceded by a minus when it is in decibels
            myFreeSpaceLossList.append(eachFreeSpaceLossIndBs);
        }
        else
        {
            visibility=false;
            myFreeSpaceLossList.append(0.0);
        }

    }

    return myFreeSpaceLossList;
} ///////////////////////////////////////// end of the method //////////////////////////////////////////

double CommAnalysis::OxygenSpecificAttenuation(double frequency)
{ //THE FREQUENCY HAS TO BE IN GHZ!!!!!!!

    double oxSpecAtt;

    oxcheck = m_environment->OxChoice();

    if(oxcheck==QString("true"))
    {

        oxSpecAtt=((7.1/(pow(frequency,2)+0.36))+(4.5/(pow((frequency-57),2)+0.98)))*pow(frequency,2)/1000;  //dB/Km
        /*oxSpecAtt=(7.1/(pow(frequency,2)+0.36));
        oxSpecAtt=oxSpecAtt+(4.5/(pow((frequency-57),2)+0.98));
        oxSpecAtt=oxSpecAtt*pow(frequency,2)/1000;*/
    }
    else if(oxcheck==QString("false"))
    {
        oxSpecAtt=0;
    }

    return oxSpecAtt;
} ///////////////////////////////////////// end of the method //////////////////////////////////////////

/** Calculates the ????????????
  * For applications which require an annual mean value, the surface water vapour densities exceeded
  *  for 50% of the year, shown for quick reference in Fig. 1, ITU-R P.836-3, should be used
  *
  * \return a double
  *
  */
double CommAnalysis::WaterVapourSpecificAttenuation(double frequency, double latitude, double longitude)
{
    double wpSpecAtt;
    double waterDensity;

    wvcheck=m_environment->WaterVapourChoice();

    QString whereIsTheDataFolder = findDataFolder();

    if(wvcheck=="true")
    {
        QString pathLat(whereIsTheDataFolder + "/" + "data/maps/surfwv_lat.txt"); QFile fileLat(pathLat);
        QString pathLon(whereIsTheDataFolder + "/" + "data/maps/surfwv_lon.txt"); QFile fileLon(pathLon);
        QString pathWaterVapour(whereIsTheDataFolder + "/" + "data/maps/surfwv_50.txt"); QFile fileWaterVapour(pathWaterVapour);

        if((!fileLat.exists()) || (!fileLon.exists()) || (!fileWaterVapour.exists()))
        {
            qDebug()<<" file error: surfwv_lat.txt does not exist";
        }
        else // In this case we found the file and we can read it
        {
            fileLat.open(QIODevice::ReadOnly);
            QTextStream latStream(&fileLat);
            double latPoint;

            int i=0;
            do
            {
                latStream>>latPoint;
                i++;
            } while(latPoint>latitude);

            fileLat.close();
            if(longitude<0) longitude=longitude+360;

            fileLon.open(QIODevice::ReadOnly);
            QTextStream lonStream(&fileLon);
            double lonPoint;

            int k;
            for(k=0; k<i; k++)
            {
                lonStream>>lonPoint;
            }

            int j=0;
            do
            {
                lonStream>>lonPoint;
                j++;
            } while(lonPoint<longitude);

            fileLon.close();

            fileWaterVapour.open(QIODevice::ReadOnly);
            QTextStream waterVapourStream(&fileWaterVapour);

            int z=0;
            int target=i+j;

            for(z; z<target; z++)
            {
                waterVapourStream>>waterDensity;
            }

            wpSpecAtt=(0.067+(3/(pow(((frequency)-22.3),2)+7.3)))*waterDensity*pow((frequency),2)/10000; //This result is in dB/Km
        }
    }
    else if (wvcheck=="false")
    {
        wpSpecAtt=0.0;
    }

    return wpSpecAtt;
} ///////////////////////////////////////// end of the method //////////////////////////////////////////

/** Calculates the atmospheric attenuation
  * This takes the elevation angle of the participant that is on the ground station.
  * Satellites cannot carry antennas with tracking systems since there is not attitude control in STA
  *
  * \return a double
  *
  */
QList<double> CommAnalysis::getAtmosphericAttenuation(double frequency, double wpSpecAtt, double oxSpecAtt, int numberOfRows)
{
    QList<double> myAtmosphericAttenuationList;
    double eachAtmosphericAttenuation;

    double h0 = 6;//equivalent height of a uniform medium for oxygen calculations (Km)
    double hw; //equivalent height for water vapour calculations
    frequency = frequency / 1000000000;
    hw = 2.2 + (3 / (pow((frequency - 22.3),2) + 3)); // In Km
    double elevationAngle;

    for(int i=0; i<numberOfRows; i++)
    {
        double t=m_propagatedScenario->spaceObjects().at(m_indexSC)->mission().at(m_indexMA)->trajectorySampleTime(i);

        if(tracking==true)
        {
            elevationAngle = m_propagatedScenario->groundObjects().at(m_indexGS)->elevationAngle(m_propagatedScenario->spaceObjects().at(m_indexSC),t);
        }
        else if(tracking==false)
        {
            if(m_flagTX==false && m_flagRX==true)
                elevationAngle = m_transmitter->Transmitter()->PointingDirection()->elevation();
            else if(m_flagRX==false && m_flagTX==true)
                elevationAngle = m_receiver->Receiver()->PointingDirection()->elevation();
        }

        bool visibility;

        if(m_propagatedScenario->groundObjects().at(m_indexGS)->elevationAngle(m_propagatedScenario->spaceObjects().at(m_indexSC),t)>=5.0*DEG2RAD)
        {
            visibility=true;
            //elevationAngleList.append(elevationAngle);
            //oxAttList.append(h0*oxSpecAtt*exp(-(hw/h0))/sin(elevationAngle));
            //wvAttList.append((hw*wpSpecAtt)/sin(elevationAngle));
            eachAtmosphericAttenuation = (((h0 * oxSpecAtt * exp(-(hw / h0))) + (hw * wpSpecAtt))) / sin(elevationAngle);
            myAtmosphericAttenuationList.append(eachAtmosphericAttenuation);
        }
        else
        {
            visibility=false;
            myAtmosphericAttenuationList.append(0.0);
        }
    }

    return myAtmosphericAttenuationList;

} ///////////////////////////////////////// end of the method //////////////////////////////////////////

/** Calculates the rain attenuation
  * This is the position of the groundstation, the parent object should be read
  * This function is following the recommendation ITU-R P.618-10 for Rain Attenuation estimation (section 2.2)
  *
  * \return a double
  *
  */
QList<double>  CommAnalysis::getRainAttenuation(double latitude, double longitude, int numberOfRows)
{
    QList<double> myRainAttenuationList;
    double eachRainAttenuation;

    ////////////////////////////// DATA AND COEFFICIENTS FOR THIS FUNCTION ////////////////////////////////////////
    double akh[4]={-5.3398, -0.3531, -0.23789, -0.94158};
    double bkh[4]={-0.10008, 1.26970, 0.86036, 0.64552};
    double ckh[4]={1.13098, 0.45400, 0.15354, 0.16817};

    double akv[4]={-3.80595, -3.44965, -0.39902, 0.50167};
    double bkv[4]={0.56934, -0.22911, 0.73042, 1.07319};
    double ckv[4]={0.81061, 0.51059, 0.11899, 0.27195};

    double aah[5]={-0.14318, 0.29591, 0.32177, -5.37610, 16.1721};
    double bah[5]={1.82442, 0.77564, 0.63773, -0.96230, -3.29980};
    double cah[5]={-0.55187, 0.19822, 0.13164, 1.47828, 3.43990};

    double aav[5]={-0.07771, 0.56727, -0.20238, -48.2991, 48.5833};
    double bav[5]={2.33840, 0.95545, 1.14520, 0.791669, 0.791459};
    double cav[5]={-0.76284, 0.54039, 0.26809, 0.116226, 0.116479};

    double logf;
    double frequency;

    if(m_flagTX==false && m_flagRX==true)
    {
        logf=log10(m_transmitter->Budget()->FrequencyBand()/1000000000);
        frequency= m_transmitter->Budget()->FrequencyBand()/1000000000;
    }
    else if(m_flagRX==false && m_flagTX==true)
    {
        logf=log10(m_receiver->Budget()->FrequencyBand()/1000000000);
        frequency= m_receiver->Budget()->FrequencyBand()/1000000000;
    }

    double logkh= (akh[0]*exp(-pow((logf-bkh[0])/ckh[0],2)))+(akh[1]*exp(-pow((logf-bkh[1])/ckh[1],2)))+(akh[2]*exp(-pow((logf-bkh[2])/ckh[2],2)))+(akh[3]*exp(-pow((logf-bkh[3])/ckh[3],2)))-(logf*0.18961)+0.71147;
    double logkv= (akv[0]*exp(-pow((logf-bkv[0])/ckv[0],2)))+(akv[1]*exp(-pow((logf-bkv[1])/ckv[1],2)))+(akv[2]*exp(-pow((logf-bkv[2])/ckv[2],2)))+(akv[3]*exp(-pow((logf-bkv[3])/ckv[3],2)))-(logf*0.16398)+0.63297;

    double alphah=(aah[0]*exp(-pow((logf-bah[0])/cah[0],2)))+(aah[1]*exp(-pow((logf-bah[1])/cah[1],2)))+(aah[2]*exp(-pow((logf-bah[2])/cah[2],2)))+(aah[3]*exp(-pow((logf-bah[3])/cah[3],2)))+(aah[4]*exp(-pow((logf-bah[4])/cah[4],2)))+(logf*0.67849)-1.95537;
    double alphav=(aav[0]*exp(-pow((logf-bav[0])/cav[0],2)))+(aav[1]*exp(-pow((logf-bav[1])/cav[1],2)))+(aav[2]*exp(-pow((logf-bav[2])/cav[2],2)))+(aav[3]*exp(-pow((logf-bav[3])/cav[3],2)))+(aav[4]*exp(-pow((logf-bav[4])/cav[4],2)))-(logf*0.053739)+0.83433;

    double kh=pow(10, logkh);

    double kv=pow(10, logkv);

    double tilt;

    if(m_flagTX==false && m_flagRX==true)
        tilt=m_transmitter->Transmitter()->EMproperties()->TiltAngle();
    else if(m_flagRX==false && m_flagTX==true)
        tilt=m_receiver->Receiver()->EMproperties()->TiltAngle();

    //////////////////////////////////////STEP 1: Determination of rain height given in ITU-R P.839//////////////////////////////

    // Find out now the correct path of the files
    QString staResourcesPath = findDataFolder();

    QString pathLat(staResourcesPath + "/" + "data/maps/ESALAT.TXT");
    QFile fileLat(pathLat);

    if(!fileLat.exists())
        qDebug()<<" file error";
    fileLat.open(QIODevice::ReadOnly);

    QTextStream latStream(&fileLat);
    double latPoint;

    int i1=0;
    do
    {
        latStream>>latPoint;
        i1++;

    } while(latPoint>latitude);

    fileLat.close();

    if(longitude<0)
        longitude=longitude+360;

    QString pathLon(staResourcesPath + "/" + "data/maps/ESALON.TXT");

    QFile fileLon(pathLon);

    if(!fileLon.exists())
        qDebug()<<" file error";
    fileLon.open(QIODevice::ReadOnly);

    QTextStream lonStream(&fileLon);
    double lonPoint;

    int k1=0;
    for(k1; k1<i1; k1++)
        lonStream>>lonPoint;

    int j1=0;
    do
    {
        lonStream>>lonPoint;
        j1++;
    } while(lonPoint<longitude);

    fileLon.close();

    QString pathHeight(staResourcesPath + "/" + "data/maps/ESA0HEIGHT.TXT");

    QFile fileHeight(pathHeight);

    if(!fileHeight.exists())
        qDebug()<<" file error";
    fileHeight.open(QIODevice::ReadOnly);

    QTextStream heightStream(&fileHeight);
    double heightPoint;

    int z1=0;
    int target1=i1+j1;

    for(z1; z1<target1; z1++)
        heightStream>>heightPoint;

    double hr;

    hr=heightPoint+0.36; //in Km

    //This step is working properly

    ///////////////////////////////////////////STEP 4: Get the rainfall rate exceeded for 0.01% of an average year//////////////////////////////////////////////

    QString pathLatRain(staResourcesPath + "/" + "data/maps/ESARAIN_LAT_v5.TXT");

    QFile fileLatRain(pathLatRain);

    if(!fileLatRain.exists())
        qDebug()<<" file error";
    fileLatRain.open(QIODevice::ReadOnly);

    QTextStream latStreamRain(&fileLatRain);
    double latPointRain;

    int i2=0;
    do
    {
        latStreamRain>>latPointRain;
        i2++;
    }while(latPointRain>latitude);

    fileLatRain.close();

    if(longitude<0)
        longitude=longitude+360;

    QString pathLonRain(staResourcesPath + "/" + "data/maps/ESARAIN_LON_v5.TXT");

    QFile fileLonRain(pathLonRain);

    if(!fileLonRain.exists())
        qDebug()<<" file error";
    fileLonRain.open(QIODevice::ReadOnly);

    QTextStream lonStreamRain(&fileLonRain);
    double lonPointRain;

    int k2=0;
    for(k2; k2<i2; k2++)
        lonStreamRain>>lonPointRain;

    int j2=0;
    do
    {
        lonStreamRain>>lonPointRain;
        j2++;

    } while(lonPointRain<longitude);
    fileLonRain.close();

    QString pathRain001(staResourcesPath + "/" + "data/maps/R0_01.TXT");

    QFile fileRain001(pathRain001);

    if(!fileRain001.exists())
        qDebug()<<" file error";
    fileRain001.open(QIODevice::ReadOnly);

    QTextStream rain001Stream(&fileRain001);
    double r001;

    int t=0;
    int target2=i2+j2;

    for(t; t<target2; t++)
        rain001Stream>>r001;

    //////////////////////////////////////////STEP 2, 3: Get the slant-path range length, below the rain height and its projection////////////////////////////////////////////

    //////////////////////////////////////////STEP 5: get the Specific Attenuation in dB/Km according to ITU-R P.838-3////////////////////////////////////////////////////////

    ///////////////////////////////////////// STEP 6: Calculate the horizontal reduction factor for 0.01% of the time ////////////////////////////////////////////////////////

    //////////////////////////////////////////STEP 7: Calculate the vertical reduction factor for 0.01% of the time //////////////////////////////////////////////////////////

    //////////////////////////////////////////STEP 8: The effective path length //////////////////////////////////////////////////////////

    //////////////////////////////////////////STEP 9: The predicted attenuation exceeded for 0.01% of an average year //////////////////////////////////////////////////////////

    //////////////////////////////////////////STEP 10: Other percentages //////////////////////////////////////////////////////////////////////////////////////////////////////

    double elevationAngle;
    double finalAtt;

    for(int counter=0; counter<numberOfRows; counter++)
    {
        double t=m_propagatedScenario->spaceObjects().at(m_indexSC)->mission().at(m_indexMA)->trajectorySampleTime(counter);

        if(tracking==true)
        {
            elevationAngle=m_propagatedScenario->groundObjects().at(m_indexGS)->elevationAngle(m_propagatedScenario->spaceObjects().at(m_indexSC),t);
        }
        else if(tracking==false)
        {

            if(m_flagTX==false && m_flagRX==true)
                elevationAngle=m_transmitter->Transmitter()->PointingDirection()->elevation();
            else if(m_flagRX==false && m_flagTX==true)
                elevationAngle=m_receiver->Receiver()->PointingDirection()->elevation();

        } //This takes the elevation angle of the participant that is on the ground station. Satellites cannot carry antennas with tracking systems since there is not attitude control in STA

        bool visibility;
        if(m_propagatedScenario->groundObjects().at(m_indexGS)->elevationAngle(m_propagatedScenario->spaceObjects().at(m_indexSC),t)>=5.0*DEG2RAD)
        {
            visibility=true;
        }
        else
        {
            visibility=false;
        }

        double gsAlt=m_propagatedScenario->groundObjects().at(m_indexGS)->altitude;
        gsAlt=gsAlt/1000;//Now gsAlt is in Km

        if(visibility==true )
        {
            raincheck=m_environment->Rain()->RainChoice();

            if(raincheck=="true")
            {
                double slantPath;
                slantPath=(hr-gsAlt)/sin(elevationAngle);

                double Lg=slantPath*cos(elevationAngle); //Horizontal projection of the slant path length

                //////////////////////// This belongs to step 5 //////////////////////
                double ktot=(kh+kv+((kh-kv)*pow(cos(elevationAngle),2)*cos(2*tilt)))/2;
                double alphatot=((kh*alphah)+(kv*alphav)+((kh*alphah-kv*alphav)*pow(cos(elevationAngle),2)*cos(2*tilt)))/(2*ktot);
                double specAttenuation= ktot*pow(r001, alphatot); //Since it is specific attenuation, the units are dB/Km.

                ////////////////////////// This belongs to step 6 /////////////////////
                double horiz_reduc=1/(1+(0.78*sqrt(Lg*specAttenuation/frequency))-(0.38*(1-exp(-2*Lg))));

                ////////////////////////// This belongs to step 7 ////////////////////////
                double zeta=atan((hr-gsAlt)/(Lg*horiz_reduc));

                double Lr;
                double chi;

                if(zeta>elevationAngle)
                    Lr=(Lg*horiz_reduc)/cos(elevationAngle);
                else
                    Lr=slantPath;

                if(fabs(latitude)<36)
                    chi=36-fabs(latitude);
                else
                    chi=0;

                chi=chi*DEG2RAD;
                double ver_reduc=1/(1+(sqrt(sin(elevationAngle))*((31*(1-exp(-(elevationAngle/(1+chi))))*(sqrt((Lr*specAttenuation))/pow(frequency, 2)))-0.45)));

                ////////////////////////// This belongs to step 8 /////////////////////////
                double Le=Lr*ver_reduc;

                ////////////////////////// This belongs to step 9 /////////////////////////
                double estimatedAtt001=specAttenuation*Le;

                //////////////////////// This belongs to step 10 //////////////////////////
                double beta;
                double percentage=m_environment->Rain()->PercentageExceededLimit();
                if(percentage>1 || fabs(latitude)>=36)
                    beta=0;
                else if(percentage<1 && fabs(latitude)<36 && elevationAngle>=(25*DEG2RAD))
                    beta=-0.005*(fabs(latitude)-36);
                else
                    beta=-0.005*(fabs(latitude)-36)+1.8-(4.25*sin(elevationAngle));

                eachRainAttenuation = estimatedAtt001*pow((percentage/0.01),(-(0.655+(0.033*log(percentage))-(0.045*log(estimatedAtt001))-(beta*sin(elevationAngle)*(1-percentage))))); //In dB!!

            }
            else if(raincheck=="false")
            {
                eachRainAttenuation = 0;
            }

            myRainAttenuationList.append(eachRainAttenuation);

        }
        else
        {
            myRainAttenuationList.append(0.0);
        }

    }

    return myRainAttenuationList;
} ///////////////////////////////////////// end of the method //////////////////////////////////////////

double CommAnalysis::SystemTempCalculations()
{

    double antennaTemp, skyTemp, groundTemp, rainAttenuation, systemTemp, equivalentTemp, rxFigureNoise, rxFeederLoss, thermoTempFeeder;

    rxFigureNoise=pow(10, m_receiver->Receiver()->SystemTemperature()->RxNoiseFigure()/10);//Now in natural units
    rxFeederLoss=pow(10, m_receiver->Receiver()->FeederLossRx()/10); //Now in natural units
    thermoTempFeeder=m_receiver->Receiver()->SystemTemperature()->ThermoFeeder();

    double elevationAngle;
    antennaTempChoice=m_receiver->Receiver()->SystemTemperature()->choiceTantenna();

    for (int j=0; j<elevationAngleList.length(); j++){

        elevationAngle=elevationAngleList[j];
        if(antennaTempChoice=="calculated")
        {
               if(m_flagRX==true)
                    antennaTemp=290;
                else if(m_flagRX==false)
                {

                        skyTemp=4+275*(1-pow(10,(-atmosphericAttList[j]/10)));
                        rainAttenuation=pow(10, rainAttenuationList[j]/10);

                        if (elevationAngle<=-10*DEG2RAD){ groundTemp=290;}
                        else if (elevationAngle>-10*DEG2RAD && elevationAngle<=0*DEG2RAD){ groundTemp=150;}
                        else if (elevationAngle>0*DEG2RAD && elevationAngle<=10*DEG2RAD){ groundTemp=50;}
                        else if (elevationAngle>10*DEG2RAD && elevationAngle<=90*DEG2RAD){ groundTemp=10;}

                        antennaTemp=(skyTemp/rainAttenuation)+groundTemp+275*(1-(1/rainAttenuation));

                 }
         }else if(antennaTempChoice=="constant"){
             antennaTemp=m_receiver->Receiver()->SystemTemperature()->Tantenna();
         }
        antennaTempList.append(antennaTemp);

        equivalentTemp=290*(rxFigureNoise-1);

    systemTemp=(antennaTemp/rxFeederLoss)+equivalentTemp+(thermoTempFeeder*(1-(1/rxFeederLoss))); //In Kelvin
    systemTempList.append(systemTemp);

    }

  return systemTemp;
} ///////////////////////////////////////// end of the method //////////////////////////////////////////

double CommAnalysis::Modulations(double EbNo)
{ //the EbNo is passed in dB

    QString modType=m_transmitter->Transmitter()->Modulation()->ModulationType();
    double ber, a, b;

    if(modType==QString("BPSK"))
    {
        a=sqrt(EbNo);
        b=erfc(a);
        ber=0.5*b;
    }
    else if (modType=="DE-BPSK")
    {
        a=sqrt(EbNo);
        ber=erfc(a);
    }
    else if (modType=="D-BPSK")
    {
        a=exp((-EbNo));
        ber=0.5*a;
    }
    else if (modType=="QPSK")
    {
        a=sqrt(EbNo);
        b=erfc(a);
        ber=0.5*b;
    }
    else if (modType=="DE-QPSK")
    {
        a=sqrt(EbNo);
        ber=erfc(a);
    }
    else if (modType=="D-QPSK")
    {
        a=exp((-EbNo));
        ber=0.5*a;
    }
    else if (modType=="OQPSK")
    {
        a=sqrt(EbNo);
        b=erfc(a);
        ber=0.5*b;
    }

    return ber;
} ///////////////////////////////////////// end of the method //////////////////////////////////////////

void CommAnalysis::CommReports()
{
    // Find out now the correct path of the files
    QString staResourcesPath = findDataFolder();

    QString firstFile = staResourcesPath + "/" + "reportComm1.txt";
    QString secondFile = staResourcesPath + "/" + "reportComm2.txt";
    QString thirdFile = staResourcesPath + "/" + "reportComm3.txt";

    QFile reportComm1(firstFile);
    QFile reportComm2(secondFile);
    QFile reportComm3(thirdFile);

    //qDebug() << firstFile << endl;
    //qDebug() << secondFile << endl;
    //qDebug() << thirdFile << endl;

    reportComm1.open(QIODevice::WriteOnly|QIODevice::ReadWrite);
    reportComm2.open(QIODevice::WriteOnly|QIODevice::ReadWrite);
    reportComm3.open(QIODevice::WriteOnly|QIODevice::ReadWrite);

    QTextStream streamReportComm1(&reportComm1);
    QTextStream streamReportComm2(&reportComm2);
    QTextStream streamReportComm3(&reportComm3);

    streamReportComm1.setRealNumberPrecision(16);
    streamReportComm2.setRealNumberPrecision(16);
    streamReportComm3.setRealNumberPrecision(16);

    streamReportComm1<<"MJD"<<"\t"<<"EIRP"<<"\t"<<"Rcvd.Freq."<<"\t"<<"Doppler Shift"<<"\t"<<"Rcvd. Power"<<"\t"<<"Flux Density"<<"\t"<<"OverlapBWfactor"<<endl;
    streamReportComm2<<"MJD"<<"\t"<<"FSL"<<"\t"<<"OxLoss"<<"\t"<<"WvLoss"<<"\t"<<"RainLoss"<<"\t"<<"AtmosLoss"<<"\t"<<"PropLoss"<<endl;
    streamReportComm3<<"MJD"<<"\t"<<"G/Y"<<"\t"<<"C/No"<<"\t"<<"C/N"<<"\t"<<"Eb/No"<<"\t"<<"BER"<<endl;

    // Starting now the calculations
    double frequency;

    if(m_flagTX==false && m_flagRX==true)
    {
        frequency = m_transmitter->Budget()->FrequencyBand()/1000000000;
    }
    else if(m_flagRX==false && m_flagTX==true)
    {
        frequency = m_receiver->Budget()->FrequencyBand()/1000000000;
    }

    /////////////// FIXED VALUES THAT COME FROM TRANSMITTER AND RECEIVER //////////////////////
    double potTxDb = 10*log10(m_transmitter->Transmitter()->TransmittingPower());
    double gainTxDb = m_transmitter->Transmitter()->EMproperties()->GainMax();
    double txFeederLossDb = m_transmitter->Transmitter()->FedderLossTx();
    double txDepointingLossDb = m_transmitter->Transmitter()->DepointingLossTx();

    double polLoss, rcvdPower, fluxDensity;

    if (m_transmitter->Transmitter()->EMproperties()->Polarisation()=="rightCircular" && m_receiver->Receiver()->EMproperties()->Polarisation()=="leftCircular")
        polLoss=20;//in dB
    else if (m_receiver->Receiver()->EMproperties()->Polarisation()=="rightCircular" && m_transmitter->Transmitter()->EMproperties()->Polarisation()=="leftCircular")
        polLoss=20;//in dB
    else
    {
        double txAngle = m_transmitter->Transmitter()->EMproperties()->TiltAngle();
        double rxAngle = m_receiver->Receiver()->EMproperties()->TiltAngle();
        polLoss = -20*(log10(cos(fabs(txAngle-rxAngle))));
    }

    double gainRxDb = m_receiver->Receiver()->EMproperties()->GainMax();
    double rxFeederLossDb = m_receiver->Receiver()->FeederLossRx();
    double rxDepointingLossDb = m_receiver->Receiver()->DepointingLossRx();
    double transmittedFrequency = m_transmitter->Budget()->FrequencyBand();

    //Calculate some fix parameters
    double eirp = potTxDb + gainTxDb - (txFeederLossDb + txDepointingLossDb);  //The units are in dBW
    double txBW = m_transmitter->Transmitter()->EMproperties()->BandWidth();
    double rxBW = m_receiver->Receiver()->EMproperties()->BandWidth();
    double txDataRate = m_transmitter->Transmitter()->Modulation()->DataRate();

    double overLapBWfactor;

    if(txBW>=rxBW)
        overLapBWfactor=rxBW/txBW;
    else if(rxBW>txBW)
        overLapBWfactor=txBW/rxBW;

    double propLoss;
    double carrierToNoise;
    double EbNo, BER;
    QList<double> rcvdFrequencyList;
    QList<double> pathLossList;
    QList<double> GoverTList;

    double myJulianDay;
    int myIndex;
    int p;
    int myNumberOfRows = m_propagatedScenario->spaceObjects().at(m_indexSC)->mission().at(m_indexMA)->trajectorySampleCount();  qDebug() << "myNumberOfRows" << myNumberOfRows << endl;

    double receiverFrequency, receiverPower, myGoverT, pathLoss, carrierToNoiseDensity;
    double theRange;

    // Get the list of all Doppler shifts:
    QList<double> myRealDopplerShiftList = CommAnalysis::getDopplerShiftList(myNumberOfRows);  qDebug() << "myRealDopplerShiftListLenght" << myRealDopplerShiftList.length() << endl;
    // Get the list of all free space losses:
    QList<double> myRealFreeSpaceLossList = CommAnalysis::getFreeSpaceLossList(myNumberOfRows);
    double oxAttenuation = CommAnalysis::OxygenSpecificAttenuation(frequency);
    double wpAttenuation = CommAnalysis::WaterVapourSpecificAttenuation(frequency, m_propagatedScenario->groundObjects().at(m_indexGS)->latitude,m_propagatedScenario->groundObjects().at(m_indexGS)->longitude);
    QList<double> myRealAtmosphericAttenuationList = CommAnalysis::getAtmosphericAttenuation(frequency, wpAttenuation, oxAttenuation, myNumberOfRows); qDebug() << "myRealAtmosphericAttenuationListLenght" << myRealAtmosphericAttenuationList.length() << endl;
    QList<double> myRealRainAttenuationList = CommAnalysis::getRainAttenuation(m_propagatedScenario->groundObjects().at(m_indexGS)->latitude,m_propagatedScenario->groundObjects().at(m_indexGS)->longitude, myNumberOfRows);  qDebug() << "myRealRainAttenuationListLengh" << myRealRainAttenuationList.length() << endl;
    double SystemTempCalculations = CommAnalysis::SystemTempCalculations();

    for(myIndex=0; myIndex<myNumberOfRows; myIndex++)
    {
        myJulianDay = m_propagatedScenario->spaceObjects().at(m_indexSC)->mission().at(m_indexMA)->trajectorySampleTime(myIndex);
        receiverFrequency = transmittedFrequency + myRealDopplerShiftList[myIndex];  //qDebug() << receiverFrequency << endl;
        theRange = m_propagatedScenario->groundObjects().at(m_indexGS)->getRange(m_propagatedScenario->spaceObjects().at(m_indexSC), myIndex) * 1000;
        fluxDensity = eirp - (10 * log10(4 * theRange * theRange * Pi)); //qDebug() << fluxDensity << endl;
        pathLoss = myRealRainAttenuationList[myIndex] + myRealAtmosphericAttenuationList[myIndex] + myRealFreeSpaceLossList[myIndex];
        //myGoverT = gainRxDb - (rxFeederLossDb + rxDepointingLossDb + pathLoss) - (10*log10(systemTempList[myIndex]));  //The units are dBHz
        myGoverT = gainRxDb - (rxFeederLossDb + rxDepointingLossDb + pathLoss) - (10*log10(1.0));
        carrierToNoiseDensity = eirp - pathLoss + myGoverT + 228.6 + (10*log10(overLapBWfactor));  //The units are dBHz, the 228.6 factor is in dBW/HzK and it is the Bolztman constant in decibels
        receiverPower = eirp - propLoss;
        carrierToNoise = carrierToNoiseDensity - (10*log10(txBW));
        BER = CommAnalysis::Modulations(EbNo);

        streamReportComm1 << myJulianDay << "\t" << eirp << "\t" << receiverFrequency << "\t" << myRealDopplerShiftList[myIndex] << "\t" << receiverPower << "\t" << fluxDensity << "\t" << overLapBWfactor << endl;
        //streamReportComm2 << myJulianDay << "\t" << myRealFreeSpaceLossList[myIndex] << "\t" << oxAttList[p] << "\t" << wvAttList[myIndex] << "\t" << myRealRainAttenuationList[myIndex] << "\t" << myRealAtmosphericAttenuationList[myIndex] << "\t" << pathLoss << endl;
        streamReportComm2 << myJulianDay << "\t" << myRealFreeSpaceLossList[myIndex] << "\t" << 0.0 << "\t" << 0.0 << "\t" << myRealRainAttenuationList[myIndex] << "\t" << myRealAtmosphericAttenuationList[myIndex] << "\t" << pathLoss << endl;
        streamReportComm3 << myJulianDay << "\t" << myGoverT << "\t" << carrierToNoiseDensity << "\t" << carrierToNoise << "\t" << EbNo << "\t" << BER <<endl;
    }

    reportComm1.close();
    reportComm2.close();
    reportComm3.close();
}


//          Copyright Oliver Kowalke 2009.
// 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 <cstdlib>
#include <stdexcept>
#include <utility>

#include <boost/assert.hpp>
#include <boost/bind.hpp>
#include <boost/optional.hpp>
#include <boost/thread.hpp>

#include "boost/task.hpp"

namespace tsk = boost::tasks;

typedef tsk::static_pool< tsk::unbounded_fifo > pool_type;

int serial_fib( int n)
{
	if( n < 2)
		return n;
	else
		return serial_fib( n - 1) + serial_fib( n - 2);
}

int parallel_fib( int n, int cutof)
{
	if ( n < cutof) return serial_fib( n);
	else
	{
		BOOST_ASSERT( boost::this_task::runs_in_pool() );
		tsk::task< int > t1(
			parallel_fib,
			n - 1,
			cutof);
		tsk::task< int > t2(
			parallel_fib,
			n - 2,
			cutof);
		tsk::handle< int > h1(
			tsk::async(
				boost::move( t1),
				tsk::as_sub_task() ) ) ;
		tsk::handle< int > h2(
			tsk::async(
				boost::move( t2),
				tsk::as_sub_task() ) );
		return h1.get() + h2.get();
	}
}

inline
void submit(
		tsk::spin::unbounded_channel< int > & send,
		tsk::spin::unbounded_channel< std::pair< int , int > > & recv,
		int n)
{
	for ( int i = 0; i <= n; ++i)
		send.put( i);
	send.deactivate();
	boost::optional< std::pair< int , int > > r;
	while ( recv.take( r) )
	{
		BOOST_ASSERT( r);
		printf("fib(%d) == %d\n", r->first, r->second);
	}
}

inline
void calculate(
		tsk::spin::unbounded_channel< int > & recv,
		tsk::spin::unbounded_channel< std::pair< int , int > > & send)
{
	boost::optional< int > n;
	while ( recv.take( n) )
	{
		BOOST_ASSERT( n);
		int r = parallel_fib( * n, 5);
		send.put( std::make_pair( * n, r) );
	}
	send.deactivate();
}

int main( int argc, char *argv[])
{
	try
	{
		pool_type pool( tsk::poolsize( 2) );

		tsk::spin::unbounded_channel< int > buf1;
		tsk::spin::unbounded_channel< std::pair< int , int > > buf2;

		tsk::async(
			tsk::make_task( submit, buf1, buf2, 15),
			pool);
		tsk::async(
			tsk::make_task( calculate, buf1, buf2),
			pool);

		return EXIT_SUCCESS;
	}
	catch ( std::exception const& e)
	{ std::cerr << "exception: " << e.what() << std::endl; }
	catch ( ... )
	{ std::cerr << "unhandled" << std::endl; }

	return EXIT_FAILURE;
}


#pragma once

#include <memory>
#include <Eigen/StdVector>
#include <Eigen/Core>
#include "../geometry/LineSegment/LineSegment2/LineSegment2i/linesegment2i.hh"

namespace bold
{
  class Spatialiser;

  struct LineJunction
  {
    enum class Type
    {
      X,
      T,
      L,
      NONE
    };

    Eigen::Vector2d position;
    Type type;
    double angle;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  };

  class LineJunctionFinder
  {
  public:

    LineJunctionFinder() = default;

    std::vector<LineJunction, Eigen::aligned_allocator<LineJunction>> findLineJunctions(std::vector<LineSegment3d> const& lineSegments);

    Maybe<LineJunction> tryFindLineJunction(LineSegment3d const& segment1, LineSegment3d const& segment2, double distToEndThreshold = 0.2);
  };
}


//------------------------------------------------------------------------------
// \file ProgramStateExample_tests.cpp
//------------------------------------------------------------------------------
#include "Categories/AlgebraicDataTypes/ProgramStateExamples.h"

#include <boost/test/unit_test.hpp>

//using namespace Categories::AlgebraicDataTypes;

//using
  //Categories::AlgebraicDataTypes::ProgramStates::MainPrograms::ProgramTThroughStdOptional;

BOOST_AUTO_TEST_SUITE(Categories)
BOOST_AUTO_TEST_SUITE(AlgebraicDataTypes)
BOOST_AUTO_TEST_SUITE(ProgramStateExamples_tests)

BOOST_AUTO_TEST_SUITE(SumTypesThroughInheritanceExamples)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ProgramStatesThroughInheritanceConstructs)
{
  BOOST_TEST(true);
}

BOOST_AUTO_TEST_SUITE_END() // SumTypesThroughInheritanceExamples

BOOST_AUTO_TEST_SUITE_END() // ProgramStateExamples_tests
BOOST_AUTO_TEST_SUITE_END() // AlgebraicDataTypes
BOOST_AUTO_TEST_SUITE_END() // Categories


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_filtering.hh"

#include <boost/python.hpp>
#include <boost/graph/betweenness_centrality.hpp>

#include "graph.hh"
#include "graph_selectors.hh"
#include "graph_util.hh"

using namespace std;
using namespace boost;
using namespace graph_tool;

template <class Graph, class EdgeBetweenness, class VertexBetweenness>
void normalize_betweenness(const Graph& g,
                           EdgeBetweenness edge_betweenness,
                           VertexBetweenness vertex_betweenness,
                           size_t n)
{
    double vfactor = (n > 2) ? 1.0/((n-1)*(n-2)) : 1.0;
    double efactor = (n > 1) ? 1.0/(n*(n-1)) : 1.0;
    if (std::is_convertible<typename graph_traits<Graph>::directed_category,
                            undirected_tag>::value)
    {
        vfactor *= 2;
        efactor *= 2;
    }

    int i, N = num_vertices(g);
    #pragma omp parallel for default(shared) private(i)   \
        schedule(runtime) if (N > 100)
    for (i = 0; i < N; ++i)
    {
        typename graph_traits<Graph>::vertex_descriptor v = vertex(i, g);
        if (v == graph_traits<Graph>::null_vertex())
            continue;
        put(vertex_betweenness, v, vfactor * get(vertex_betweenness, v));
    }

    typename graph_traits<Graph>::edge_iterator e, e_end;
    for (tie(e, e_end) = edges(g); e != e_end; ++e)
    {
        put(edge_betweenness, *e, efactor * get(edge_betweenness, *e));
    }
}

struct get_betweenness
{
    typedef void result_type;
    template <class Graph, class EdgeBetweenness, class VertexBetweenness>
    void operator()(Graph& g,
                    GraphInterface::vertex_index_map_t index_map,
                    EdgeBetweenness edge_betweenness,
                    VertexBetweenness vertex_betweenness,
                    bool normalize, size_t n) const
    {
        vector<vector<typename graph_traits<Graph>::edge_descriptor> >
            incoming_map(num_vertices(g));
        vector<size_t> distance_map(num_vertices(g));
        vector<typename property_traits<VertexBetweenness>::value_type>
            dependency_map(num_vertices(g));
        vector<size_t> path_count_map(num_vertices(g));
        brandes_betweenness_centrality
            (g, vertex_betweenness, edge_betweenness,
             make_iterator_property_map(incoming_map.begin(), index_map),
             make_iterator_property_map(distance_map.begin(), index_map),
             make_iterator_property_map(dependency_map.begin(), index_map),
             make_iterator_property_map(path_count_map.begin(), index_map),
             index_map);
        if (normalize)
            normalize_betweenness(g, edge_betweenness, vertex_betweenness, n);
    }
};

struct get_weighted_betweenness
{
    typedef void result_type;
    template <class Graph, class EdgeBetweenness, class VertexBetweenness,
              class VertexIndexMap>
        void operator()(Graph& g, VertexIndexMap vertex_index,
                        EdgeBetweenness edge_betweenness,
                        VertexBetweenness vertex_betweenness,
                        boost::any weight_map, bool normalize,
                        size_t n, size_t max_eindex) const
    {
        vector<vector<typename graph_traits<Graph>::edge_descriptor> >
            incoming_map(num_vertices(g));
        vector<typename property_traits<EdgeBetweenness>::value_type>
            distance_map(num_vertices(g));
        vector<typename property_traits<VertexBetweenness>::value_type>
            dependency_map(num_vertices(g));
        vector<size_t> path_count_map(num_vertices(g));

        typename EdgeBetweenness::checked_t weight =
            any_cast<typename EdgeBetweenness::checked_t>(weight_map);

        brandes_betweenness_centrality
            (g, vertex_betweenness, edge_betweenness,
             make_iterator_property_map(incoming_map.begin(), vertex_index),
             make_iterator_property_map(distance_map.begin(), vertex_index),
             make_iterator_property_map(dependency_map.begin(), vertex_index),
             make_iterator_property_map(path_count_map.begin(), vertex_index),
             vertex_index, weight.get_unchecked(max_eindex+1));
        if (normalize)
            normalize_betweenness(g, edge_betweenness, vertex_betweenness, n);
    }
};

void betweenness(GraphInterface& g, boost::any weight,
                 boost::any edge_betweenness,
                 boost::any vertex_betweenness,
                 bool normalize)
{
    if (!belongs<edge_floating_properties>()(edge_betweenness))
        throw ValueException("edge property must be of floating point value"
                             " type");

    if (!belongs<vertex_floating_properties>()(vertex_betweenness))
        throw ValueException("vertex property must be of floating point value"
                             " type");

    if (!weight.empty())
    {
        run_action<>()
            (g, std::bind<>(get_weighted_betweenness(),
                            std::placeholders::_1, g.GetVertexIndex(),
                            std::placeholders::_2,
                            std::placeholders::_3, weight, normalize,
                            g.GetNumberOfVertices(), g.GetMaxEdgeIndex()),
             edge_floating_properties(),
             vertex_floating_properties())
            (edge_betweenness, vertex_betweenness);
    }
    else
    {
        run_action<>()
            (g, std::bind<void>(get_betweenness(), std::placeholders::_1,
                                g.GetVertexIndex(), std::placeholders::_2,
                                std::placeholders::_3, normalize,
                                g.GetNumberOfVertices()),
             edge_floating_properties(),
             vertex_floating_properties())
            (edge_betweenness, vertex_betweenness);
    }
}

struct get_central_point_dominance
{
    template <class Graph, class VertexBetweenness>
    void operator()(Graph& g, VertexBetweenness vertex_betweenness, double& c)
        const
    {
        c = double(central_point_dominance(g, vertex_betweenness));
    }
};

double central_point(GraphInterface& g,
                     boost::any vertex_betweenness)
{
    double c = 0.0;
    run_action<graph_tool::detail::never_reversed>()
        (g, std::bind<>(get_central_point_dominance(), std::placeholders::_1,
                        std::placeholders::_2, std::ref(c)),
         vertex_scalar_properties()) (vertex_betweenness);
    return c;
}

void export_betweenness()
{
    using namespace boost::python;
    def("get_betweenness", &betweenness);
    def("get_central_point_dominance", &central_point);
}