/*
 * index_of_dispersion_filter.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <dials/algorithms/image/filter/index_of_dispersion_filter.h>

namespace dials { namespace algorithms { namespace boost_python {

  using namespace boost::python;

  template <typename FloatType>
  void index_of_dispersion_filter_wrapper(const char *name) {
    typedef IndexOfDispersionFilter<FloatType> IndexOfDispersionFilterType;

    class_<IndexOfDispersionFilterType>(name, no_init)
      .def(init<const af::const_ref<FloatType, af::c_grid<2> > &, int2>(
        (arg("image"), arg("size"))))
      .def("index_of_dispersion", &IndexOfDispersionFilterType::index_of_dispersion)
      .def("mean", &IndexOfDispersionFilterType::mean)
      .def("sample_variance", &IndexOfDispersionFilterType::sample_variance);
  }

  template <typename FloatType>
  void index_of_dispersion_filter_masked_wrapper(const char *name) {
    typedef IndexOfDispersionFilterMasked<FloatType> IndexOfDispersionFilterType;

    class_<IndexOfDispersionFilterType>(name, no_init)
      .def(init<const af::const_ref<FloatType, af::c_grid<2> > &,
                const af::const_ref<int, af::c_grid<2> > &,
                int2,
                int>((arg("image"), arg("mask"), arg("size"), arg("min_count"))))
      .def("index_of_dispersion", &IndexOfDispersionFilterType::index_of_dispersion)
      .def("mean", &IndexOfDispersionFilterType::mean)
      .def("sample_variance", &IndexOfDispersionFilterType::sample_variance)
      .def("mask", &IndexOfDispersionFilterType::mask)
      .def("count", &IndexOfDispersionFilterType::count);
  }

  template <typename FloatType>
  IndexOfDispersionFilter<FloatType> make_index_of_dispersion_filter(
    const af::const_ref<FloatType, af::c_grid<2> > &image,
    int2 size) {
    return IndexOfDispersionFilter<FloatType>(image, size);
  }

  template <typename FloatType>
  IndexOfDispersionFilterMasked<FloatType> make_index_of_dispersion_filter_masked(
    const af::const_ref<FloatType, af::c_grid<2> > &image,
    const af::const_ref<int, af::c_grid<2> > &mask,
    int2 size,
    int min_size) {
    return IndexOfDispersionFilterMasked<FloatType>(image, mask, size, min_size);
  }

  template <typename FloatType>
  void index_of_dispersion_filter_suite() {
    def("index_of_dispersion_filter",
        &make_index_of_dispersion_filter<FloatType>,
        (arg("image"), arg("kernel")));

    def("index_of_dispersion_filter",
        &make_index_of_dispersion_filter_masked<FloatType>,
        (arg("image"), arg("mask"), arg("kernel"), arg("min_count")));
  }

  void export_index_of_dispersion_filter() {
    index_of_dispersion_filter_wrapper<float>("IndexOfDispersionFilterFloat");
    index_of_dispersion_filter_wrapper<double>("IndexOfDispersionFilterDouble");
    index_of_dispersion_filter_masked_wrapper<float>(
      "IndexOfDispersionFilterMaskedFloat");
    index_of_dispersion_filter_masked_wrapper<double>(
      "IndexOfDispersionFilterMaskedDouble");

    index_of_dispersion_filter_suite<float>();
    index_of_dispersion_filter_suite<double>();
  }

}}}  // namespace dials::algorithms::boost_python


// Copyright 2012 Christian Henning
// 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)

/// \brief Unit test for hsl and hsv color spaces

#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/toolbox/color_spaces/hsl.hpp>
#include <boost/gil/extension/toolbox/color_spaces/hsv.hpp>

#include <boost/test/unit_test.hpp>

using namespace std;
using namespace boost;
using namespace gil;

BOOST_AUTO_TEST_SUITE( hsl_hsv_test_suite )

BOOST_AUTO_TEST_CASE( hsl_hsv_test )
{
    {
        rgb8_pixel_t p( 128, 0, 128 );

        hsl32f_pixel_t h;

        color_convert( p, h );
        color_convert( h, p );
    }

    {
        size_t width  = 640;
        size_t height = 480;

        hsl32f_image_t hsl_img( width, height );
        hsv32f_image_t hsv_img( width, height );

        for( size_t y = 0; y < height; y++ )
        {
            hsl32f_view_t::x_iterator hsl_x_it = view( hsl_img ).row_begin( y );
            hsv32f_view_t::x_iterator hsv_x_it = view( hsv_img ).row_begin( y );

            float v = static_cast<float>( height -  y )
                    / height;

            for( size_t x = 0; x < width; x++ )
            {
                float hue = ( x + 1.f ) / width;

                hsl_x_it[x] = hsl32f_pixel_t( hue, 1.0, v );
                hsv_x_it[x] = hsv32f_pixel_t( hue, 1.0, v );
            }
        }
    }

    {
        rgb8_image_t rgb_img( 640, 480 );
        fill_pixels( view(rgb_img), rgb8_pixel_t( 255, 128, 64 ));
        hsl32f_image_t hsl_img( view( rgb_img ).dimensions() );

        copy_pixels( color_converted_view<hsl32f_pixel_t>( view( rgb_img ))
                    , view( hsl_img ));
    }

    {
        rgb8_image_t rgb_img( 640, 480 );
        fill_pixels( view(rgb_img), rgb8_pixel_t( 255, 128, 64 ));
        hsv32f_image_t hsv_img( view( rgb_img ).dimensions() );

        copy_pixels( color_converted_view<hsv32f_pixel_t>( view( rgb_img ))
                    , view( hsv_img ));
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include <fstream>
#include <vector>
#include <string>

#include <rw/math/Random.hpp>
#include <rw/math/Vector3D.hpp>
#include <rwlibs/task/GraspTask.hpp>
#include <rwlibs/algorithms/kdtree/KDTree.hpp>
#include <rwlibs/algorithms/kdtree/KDTreeQ.hpp>

#include <boost/program_options/options_description.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/program_options/option.hpp>
#include <boost/program_options/parsers.hpp>

#include "util.hpp"

using namespace std;
using namespace rw::core;
using namespace rw::math;
using namespace rwlibs::task;
using namespace rwlibs::algorithms;
using namespace boost::program_options;

variables_map init(int argc, char** argv){
    options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("output,o", value<string>()->default_value("out.xml"), "the output file.")
        ("angle", value<double>()->default_value(8.0), "Standard deviation of angle in input in degree.")
        ("dist", value<double>()->default_value(0.003), "Standard deviation of distance in input.")
        ("input", value<string>(), "The input grasp task file that should be filtered.")
    ;
    positional_options_description optionDesc;
    optionDesc.add("input",-1);

    variables_map vm;
    //store(parse_command_line(argc, argv, desc), vm);
    store(command_line_parser(argc, argv).
              options(desc).positional(optionDesc).run(), vm);
    notify(vm);

    // write standard welcome, status
    if (vm.count("help")) {
        cout << "Usage:\n\n"
                  << "\t" << argv[0] <<" [options] -o<outfile> <expFile1> <expFile2> <...> <expFileN> \n"
                  << "\n";
        cout << desc << "\n";
        abort();
    }
    return vm;
}

int main_jaj(int argc, char** argv);
int main_lpe(int argc, char** argv);

rw::core::Ptr< std::map<int,std::vector<int> > > calculateRegions(const std::vector<Transform3D<> >& data, double dist, double angle){
    typedef boost::tuple<int,int> KDTreeValue; // (transform index, region index)
    typedef KDTreeQ<KDTreeValue> NNSearch;

    // now build a kdtree with all end configurations

    std::vector<NNSearch::KDNode> nodes;
    for(std::size_t i = 0; i < data.size(); i++) {
        Transform3D<> k = data[i];
        EAA<> r( k.R());
        //Vector3D<> r = k.R()*Vector3D<>::z();
        Q key(6, k.P()[0],k.P()[1],k.P()[2], r[0],r[1],r[2]);
		const int iInt = boost::numeric_cast<int>(i);
        nodes.push_back(NNSearch::KDNode(key, KDTreeValue(iInt, -1)));
    }

    std::cout << "Nodes created, building tree.. " << std::endl;
    NNSearch* nntree = NNSearch::buildTree(nodes);
    // todo estimate the average distance between neighbors
    std::cout << "Tree build, finding regions" << std::endl;
    std::map<int, bool > regions;
    int freeRegion = 0;
    std::list<const NNSearch::KDNode*> result;
    Q diff(6, dist, dist, dist, angle, angle, angle);
    // find neighbors and connect them
    for(NNSearch::KDNode &n : nodes) {
        KDTreeValue &val = n.value;
        // check if the node is allready part of a region
        if(val.get<1>() >=0)
            continue;

        result.clear();
        nntree->nnSearchRect(n.key-diff, n.key+diff, result);
        int currentIndex = -1;
        // first see if any has an id
        for(const NNSearch::KDNode* nn : result) {
            KDTreeValue nnval = nn->value;
            if(nnval.get<1>() >=0){
                currentIndex = nnval.get<1>();
                break;
            }
        }
        if( currentIndex<0 ){
            currentIndex=freeRegion;
            //std::cout << "Adding " << freeRegion << std::endl;
            regions[freeRegion] = true;
            freeRegion++;
        }
        val.get<1>() = currentIndex;
        for(const NNSearch::KDNode* nn : result) {
            KDTreeValue nnval = nn->value;
            if(nnval.get<1>() >=0 && nnval.get<1>()!=currentIndex){

                //std::cout << "Merging regions " << currentIndex << "<--" << nnval.get<1>() << std::endl;

                // merge all previously defined nnval.get<1>() into freeRegion
                regions[nnval.get<1>()] = false;
                for(NNSearch::KDNode &npro : nodes) {
                    KDTreeValue &npval = npro.value;
                    // check if the node is allready part of a region
                    if(npval.get<1>() == nnval.get<1>())
                        npval.get<1>()=currentIndex;
                }
            }
        }

    }

    // now print region information
    std::vector<int> validRegions;
    typedef std::map<int,bool>::value_type mapType;
    for(mapType val : regions) {
        if(val.second==true){
            validRegions.push_back(val.first);
        }
    }

    std::cout << "Nr of detected regions: " << validRegions.size() << std::endl;
    std::map<int,std::vector<int> >* statMap = new std::map<int,std::vector<int> >();

    for(NNSearch::KDNode &n : nodes) {
        KDTreeValue &val = n.value;
        // check if the node is allready part of a region
        (*statMap)[val.get<1>()].push_back( val.get<0>() );
    }
    return ownedPtr( statMap );
}

int main(int argc, char** argv)
{
	static const unsigned int SEED = static_cast<unsigned int>(time(NULL));
    Random::seed(SEED);
    srand (SEED);
    variables_map vm = init(argc, argv);

    double angle = vm["angle"].as<double>()*Deg2Rad;
    double dist = vm["dist"].as<double>();

    std::string input = vm["input"].as<std::string>();
    std::string output = vm["output"].as<std::string>();
    char line[1000];
    std::vector<Transform3D<> > inputStartCfgs, inputEndCfgs;
    std::ifstream in(input.c_str());
    in.getline(line,1000);
    in.getline(line,1000);
    char tmpc;
    while(!in.eof() ){
        in.getline(line,400);
        std::istringstream istr(line);
        Vector3D<float> pos_s, pos_e;
        EAA<float> rot_s, rot_e;

        sscanf(line,"%f %c %f %c %f %c %f %c %f %c %f %c %f %c %f %c %f %c %f %c %f %c %f",
               &pos_s[0], &tmpc, &pos_s[1], &tmpc, &pos_s[2], &tmpc, &rot_s[0], &tmpc, &rot_s[1], &tmpc, &rot_s[2], &tmpc,
               &pos_e[0], &tmpc, &pos_e[1], &tmpc, &pos_e[2], &tmpc, &rot_e[0], &tmpc, &rot_e[1], &tmpc, &rot_e[2]);

        if(pos_e[2]<0)
            continue;

        //std::cout << pos_e << std::endl;
        inputStartCfgs.push_back( cast<double>(Transform3D<float>(pos_s, rot_s)));
        inputEndCfgs.push_back( cast<double>(Transform3D<float>(pos_e, rot_e)));
        if( rot_e.angle() > 180*Deg2Rad )
            std::cout << "larger" << std::endl;
    }
    std::cout << "Size of intput: " << inputStartCfgs.size() << std::endl;

    Ptr< std::map<int, std::vector<int> > > statMap = calculateRegions(inputEndCfgs, dist, angle);

    std::cout << "Map stats: " << std::endl;
    std::ofstream fstr(output.c_str());
    fstr.precision(16);
    fstr << " added region, first int in each line represents the region in witch it belong\n";
    fstr << " \n";
    typedef std::map<int,std::vector<int> >::value_type mapType2;
    for(mapType2 val : *statMap) {
        if(val.second.size() > 4)
            std::cout << "Region stat: " << val.first << ":" << val.second.size() ;
        Vector3D<> dir(0,0,0);
        double angle = 0;
        int count = 0;

        for(int idx : val.second) {
            count ++;
            Transform3D<> s = inputStartCfgs[ idx ];
            EAA<> sr( s.R() );
            //Unused: Vector3D<> srz = s.R()*Vector3D<>::z();
            Transform3D<> e = inputEndCfgs[ idx ];
            EAA<> er( e.R() );
            //Unused: Vector3D<> erz = e.R()*Vector3D<>::z();
            dir += er.axis();
            angle += er.angle();

            Transform3D<> aTb = inverse( s )*e;
            Vector3D<> abp = aTb.P();
            EAA<> abe(aTb.R() );
            RPY<> abr(aTb.R() );
            Vector3D<> aby = aTb.R()*Vector3D<>::z();

            fstr << val.first << " ; "
                    << s.P()[0] << " ; " << s.P()[1] << " ; " << s.P()[2] << ";"
                    << sr[0] << " ; " << sr[1] << " ; " << sr[2] << " ; "
                    //<< sr.axis()[0] << " ; " << sr.axis()[1] << " ; " << sr.axis()[2] << " ; " << sr.angle() << ";"

                    //<< srz[0] << " ; " << srz[1] << " ; " << srz[2] << " ; " << sr.angle() << ";"
                    << e.P()[0] << " ; " << e.P()[1] << " ; " << e.P()[2] << ";"
                    << er[0] << " ; " << er[1] << " ; " << er[2] << ";"
                    //<< er.axis()[0] << " ; " << er.axis()[1] << " ; " << er.axis()[2] << ";" << er.angle() << ";"
                    << abp[0] << " ; " << abp[1] << " ; " << abp[2] << ";"
                    << abe[0] << " ; " << abe[1] << " ; " << abe[2] << ";"
                    << abr(0) << " ; " << abr(1) << " ; " << abr(2) << ";"
                    << aby[0] << " ; " << aby[1] << " ; " << aby[2] << ";"
                    << "\n";
                    //<< erz[0] << " ; " << erz[1] << " ; " << erz[2] << ";" << er.angle() << "\n";
        }
        if(val.second.size() > 4){
            EAA<> rot( normalize(dir), angle/count);

            std::cout << "\t" << rot.toRotation3D()*Vector3D<>::z() << std::endl;

        }
    }
    fstr.close();

    return 0;
}

/**
 * We need to choose a number of grasps
 */
int main_lpe(int argc, char** argv)
{
    typedef std::pair<GraspSubTask*, GraspTarget*> Value;
    typedef KDTreeQ<Value> NNSearch;

	static const unsigned int SEED = static_cast<unsigned int>(time(NULL));
	Random::seed(SEED);
	srand(SEED);
    variables_map vm = init(argc, argv);

    std::string grasptask_file_out = vm["output"].as<std::string>();
    double angleThres = vm["angle"].as<double>()*Deg2Rad;
    std::string input = vm["input"].as<string>();
    std::string output = vm["output"].as<string>();
    //Unused: int count = vm["samples"].as<int>();
    GraspTask::Ptr gtask = GraspTask::load( input );

    // first we build a search tree to efficiently search for targets in 6d
    std::vector<NNSearch::KDNode*> simnodes;
    NNSearch *nntree = buildKDTree_pos_eaa(gtask,simnodes);

    Q diff(7, 0.01, 0.01, 0.01, angleThres, angleThres, angleThres,angleThres);

    std::vector< Value > selGrasps;
    std::list<const NNSearch::KDNode*> result;

    for(std::size_t i=0;i<simnodes.size(); i++){
        // find the node with the highest quality
        double q_max = -100;
        NNSearch::KDNode *n_max = simnodes[0];
        for(NNSearch::KDNode *nptr : simnodes) {
            NNSearch::KDNode &n = *nptr;
            Value &val = n.value;
            if(val.second==NULL)
                continue;
            if(val.second->getResult()==NULL)
                continue;
            Q qual = val.second->getResult()->qualityAfterLifting;
            if(qual.size()==0)
                continue;
            if(q_max<qual[0]){
                q_max = qual[0];
                n_max = &n;
            }
        }
        if(q_max<-90)
            break;
        // we stop the sampling at a certain threshold 30% success
        if(q_max<0.3)
            break;

        // add the max node
        Value &v = n_max->value;
        selGrasps.push_back( v );
        v.second = NULL;

        // use the max node as a sample point and remove all neighboring nodes
        result.clear();
        nntree->nnSearchRect( n_max->key-diff, n_max->key+diff, result);
        for(const NNSearch::KDNode* res_n : result) {
            NNSearch::KDNode* r_n = (NNSearch::KDNode*) res_n;
            r_n->value.second = NULL;
        }
    }
    std::cout << "GENRATED: "<< selGrasps.size() << " from " << simnodes.size() << std::endl;

    // convert selectGrasps to a graspTask
    GraspTask res = *gtask;
    res.getSubTasks().clear();
    for(Value& taskpair : selGrasps) {
        GraspSubTask stask = *taskpair.first;
        stask.targets.clear();
        stask.targets.push_back( *taskpair.second );
        res.addSubTask(stask);
    }

    GraspTask::saveRWTask(&res, output + ".rwtask.xml");
    //std::cout << "COVERAGE: " << (((double)samples_t)/(double)samples_f)*100.0 << "%" << std::endl;
    return 0;
}

/**
 * We need to choose a number of grasps
 */
int main_jaj(int argc, char** argv)
{
    typedef std::pair<GraspSubTask*, GraspTarget*> Value;
    typedef KDTreeQ<Value> NNSearch;

	static const unsigned int SEED = static_cast<unsigned int>(time(NULL));
	Random::seed(SEED);
	srand(SEED);
    variables_map vm = init(argc, argv);

	std::string grasptask_file_out = vm["output"].as<std::string>();
	double angleThres = vm["angle"].as<double>()*Deg2Rad;
    std::string input = vm["input"].as<string>();
    std::string output = vm["output"].as<string>();
    int count = vm["samples"].as<int>();
	GraspTask::Ptr gtask = GraspTask::load( input );

	// first we build a search tree to efficiently search for targets orientet in a certain direction
	NNSearch *nntree_normal = buildKDTree_eaa(gtask);

	// now we sample the normals with random orientations
	Q diff(4, angleThres, angleThres, angleThres, angleThres);
	typedef std::pair<GraspSubTask*, GraspTarget*> Value;

	std::vector< Value > selGrasps;
	std::list<const NNSearch::KDNode*> result;
	int samples_f = 0, samples_t = 0;
	while(selGrasps.size() < (std::size_t)count) {
        result.clear();
        EAA<> eaa( Math::ranRotation3D<double>() );
        //Vector3D<> k = Math::ranRotation3D<double>()*Vector3D<>::z();
        Vector3D<> k = eaa.axis();
        Q key(3, k[0], k[1], k[2], eaa.angle());
        nntree_normal->nnSearchRect(key-diff,key+diff, result);
        samples_f++;
        if(result.size()==0)
            continue;
        samples_t++;
        int idx = Math::ranI(0,boost::numeric_cast<int>(result.size()));
        if((std::size_t)idx == result.size())
            idx--;
        std::list<const NNSearch::KDNode*>::iterator i = result.begin();
        //std::cout << idx << "==" <<  result.size() << std::endl;
        if(idx>0)
            std::advance(i, idx);
        const NNSearch::KDNode* node = *i;
        Value v = node->value;
        selGrasps.push_back( v );
        std::cout << result.size() << "    \r" << std::flush;
	}
	// print all failures

	// convert selectGrasps to a graspTask
	GraspTask res = *gtask;
	res.getSubTasks().clear();
	for(Value& taskpair : selGrasps) {
	    GraspSubTask stask = *taskpair.first;
	    stask.targets.clear();
	    stask.targets.push_back( *taskpair.second );
	    res.addSubTask(stask);
	}

	GraspTask::saveRWTask(&res, output + ".rwtask.xml");
	std::cout << "COVERAGE: " << (((double)samples_t)/(double)samples_f)*100.0 << "%" << std::endl;
	return 0;
}


#include <boost/format.hpp>
#include "biggles/tools/max_creation.hpp"
#include "biggles/model.hpp"
#include <iostream>
#include <sstream>
//#include <stdlib.h>

// include this last to stop pre-processor macros breaking things
extern "C" {
#include <ccan/tap/tap.h>
}

using namespace biggles;

void test_proc1() {
    clutter_ptr clutter_p(new clutter_t());
    const time_stamp max_time = 20;
    size_t total_obs = 0;
    for (time_stamp ts = 0; ts < max_time; ++ts) {
        int num_obs = sampling::uniform_int(1, 10);
        total_obs += size_t(num_obs);
        for (int i = 0; i < num_obs; ++i) {
            clutter_p->insert(new_obs(sampling::uniform_real(0.f, 10.f), sampling::uniform_real(0.f, 10.f), ts));
        }
    }
    partition::track_collection_ptr tracks_p(new track_collection());
    partition_ptr_t start_partition_ptr(new partition(tracks_p, clutter_p));
    partition_ptr_t end_partition_ptr;
    max_creation(start_partition_ptr, end_partition_ptr);
    diag("num obs = %zu", total_obs);
    ok1(end_partition_ptr->observation_count() == total_obs);
    diag("%zu clutter, %zu tracks", end_partition_ptr->clutter().size(), end_partition_ptr->tracks().size());
    BOOST_FOREACH(const boost::shared_ptr<const track>& t, end_partition_ptr->tracks()) {
        diag("duration = %zu", t->duration());
    }

    ok1(end_partition_ptr->volume() == start_partition_ptr->volume());

    model::parameters paras;
    model::clutter_rate(paras) = 2.0f;
    model::birth_rate(paras) = 0.2f;
    model::observation_probability(paras) = 0.8f;
    model::survival_probability(paras) = 0.98;
    model::observation_error_covariance(paras) = Eigen::Matrix2f::Identity() * 0.04f;
    model::process_noise_covariance(paras) = detail::initQ();

    model::log_partition_given_parameters_and_data_density(*end_partition_ptr, paras);
}

int main(int argc, char** argv)
{
    //plan_tests(15);
    plan_no_plan();

    test_proc1();

    return exit_status();
}


#define CATCH_CONFIG_MAIN

#include "CALPHADSpeciesPhaseGibbsEnergy.h"

#include "catch.hpp"

#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <iomanip>
#include <iostream>
#include <string>
#include <vector>

namespace pt = boost::property_tree;

// Read CALPHAD database for Ni
// Compute a few energies as a function of temperature
// near melting temperature
TEST_CASE("CALPHAD Gibbs energy", "[gibbs energy]")
{
    std::cout << std::setprecision(12);

    // Ni melting temperature
    double temperature = 1728.;

    // read calphad database
    std::string calphad_filename("../thermodynamic_data/calphadAuNi.json");
    pt::ptree calphad_db;
    std::cout << "Read " << calphad_filename << std::endl;
    try
    {
        pt::read_json(calphad_filename, calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    std::cout << "File content:" << std::endl;
    for (auto& v : calphad_db)
        std::cout << v.first << std::endl;

    // Ni is species "B" in calphadAuNi.json
    std::cout << "Read species B data" << std::endl;
    pt::ptree spB;
    try
    {
        spB = calphad_db.get_child("SpeciesB");
    }
    catch (std::exception& e)
    {
        std::cerr << "Read species: exception caught: " << e.what()
                  << std::endl;
    }

    pt::ptree liqB;
    try
    {
        liqB = spB.get_child("PhaseL");
    }
    catch (std::exception& e)
    {
        std::cerr << "Read phase: exception caught: " << e.what() << std::endl;
    }

    pt::ptree solB;
    try
    {
        solB = spB.get_child("PhaseA");
    }
    catch (std::exception& e)
    {
        std::cerr << "Read phase: exception caught: " << e.what() << std::endl;
    }

    // liquid
    Thermo4PFM::CALPHADSpeciesPhaseGibbsEnergy energyNiLiquid;
    energyNiLiquid.initialize("NiL", liqB);

    double el = energyNiLiquid.fenergy(temperature);
    std::cout << "Ni liquid, Energy at T = " << temperature << " : " << el
              << std::endl;

    // solid
    Thermo4PFM::CALPHADSpeciesPhaseGibbsEnergy energyNiSolid;
    energyNiSolid.initialize("NiS", solB);

    double es = energyNiSolid.fenergy(temperature);
    std::cout << "Ni solid, Energy at T = " << temperature << " : " << es
              << std::endl;

    // at the meting temperature, energies in liquid and solid should be equal
    CHECK(es == Approx(el).margin(0.2));

    double belowT = temperature - 1.;
    el            = energyNiLiquid.fenergy(belowT);
    std::cout << "Ni liquid, Energy at T = " << belowT << " : " << el
              << std::endl;
    es = energyNiSolid.fenergy(belowT);
    std::cout << "Ni solid, Energy at T = " << belowT << " : " << es
              << std::endl;

    // below  the meting temperature, energies in solid should be lower than
    // in liquid
    CHECK(es < el - 10.);

    double aboveT = temperature + 1.;
    el            = energyNiLiquid.fenergy(aboveT);
    std::cout << "Ni liquid, Energy at T = " << aboveT << " : " << el
              << std::endl;
    es = energyNiSolid.fenergy(aboveT);
    std::cout << "Ni solid, Energy at T = " << aboveT << " : " << es
              << std::endl;
    CHECK(el < es - 8.);
}


#include "software/world/field.h"

#include <boost/circular_buffer.hpp>

#include "shared/constants.h"
#include "software/geom/rectangle.h"
#include "software/util/time/timestamp.h"

Field::Field(double field_x_length, double field_y_length, double defense_x_length,
             double defense_y_length, double goal_y_length, double boundary_buffer_size,
             double center_circle_radius, const Timestamp &timestamp,
             unsigned int buffer_size)
    : field_x_length_(field_x_length),
      field_y_length_(field_y_length),
      defense_x_length_(defense_x_length),
      defense_y_length_(defense_y_length),
      goal_y_length_(goal_y_length),
      // While not explicitly given by SSL-Vision, the goals are typically
      // deep enough to fit a single robot
      goal_x_length_(ROBOT_MAX_RADIUS_METERS * 2),
      boundary_buffer_size_(boundary_buffer_size),
      center_circle_radius_(center_circle_radius)
{
    // Set the size of the Timestamp history buffer
    last_update_timestamps.set_capacity(buffer_size);

    updateTimestamp(timestamp);
}

void Field::updateDimensions(const Field &new_field_data)
{
    field_x_length_        = new_field_data.xLength();
    field_y_length_        = new_field_data.yLength();
    defense_y_length_      = new_field_data.defenseAreaYLength();
    defense_x_length_      = new_field_data.defenseAreaXLength();
    goal_y_length_         = new_field_data.goalYLength();
    boundary_buffer_size_  = new_field_data.boundaryYLength();
    center_circle_radius_  = new_field_data.centerCircleRadius();
    last_update_timestamps = new_field_data.getTimestampHistory();
}

void Field::updateDimensions(double field_x_length, double field_y_length,
                             double defense_x_length, double defense_y_length,
                             double goal_y_length, double boundary_buffer_size,
                             double center_circle_radius, const Timestamp &timestamp)
{
    field_x_length_       = field_x_length;
    field_y_length_       = field_y_length;
    defense_y_length_     = defense_y_length;
    defense_x_length_     = defense_x_length;
    goal_y_length_        = goal_y_length;
    boundary_buffer_size_ = boundary_buffer_size;
    center_circle_radius_ = center_circle_radius;
    updateTimestamp(timestamp);
}

double Field::xLength() const
{
    return field_x_length_;
}

double Field::yLength() const
{
    return field_y_length_;
}

double Field::totalXLength() const
{
    return field_x_length_ + (2 * boundary_buffer_size_);
}

double Field::totalYLength() const
{
    return field_y_length_ + (2 * boundary_buffer_size_);
}

double Field::goalYLength() const
{
    return goal_y_length_;
}

double Field::goalXLength() const
{
    return goal_x_length_;
}

double Field::defenseAreaYLength() const
{
    return defense_y_length_;
}

double Field::defenseAreaXLength() const
{
    return defense_x_length_;
}

Rectangle Field::friendlyDefenseArea() const
{
    return Rectangle(
        Point(-field_x_length_ * 0.5, defense_y_length_ / 2.0),
        Point(-field_x_length_ * 0.5 + defense_x_length_, -defense_y_length_ / 2.0));
}

Rectangle Field::enemyDefenseArea() const
{
    return Rectangle(
        Point(field_x_length_ * 0.5, defense_y_length_ / 2.0),
        Point(field_x_length_ * 0.5 - defense_x_length_, -defense_y_length_ / 2.0));
}

Rectangle Field::friendlyHalf() const
{
    return Rectangle(friendlyCornerNeg(), Point(0, friendlyCornerPos().y()));
}

Rectangle Field::enemyHalf() const
{
    return Rectangle(Point(0, enemyCornerNeg().y()), enemyCornerPos());
}

Rectangle Field::fieldLines() const
{
    return Rectangle(friendlyCornerNeg(), enemyCornerPos());
}

Rectangle Field::fieldBoundary() const
{
    Point neg_x_neg_y_corner(-totalXLength() / 2, -totalYLength() / 2);
    Point pos_x_pos_y_corner(totalXLength() / 2, totalYLength() / 2);
    return Rectangle(neg_x_neg_y_corner, pos_x_pos_y_corner);
}

double Field::centerCircleRadius() const
{
    return center_circle_radius_;
}

Circle Field::centerCircle() const
{
    return Circle(Point(0, 0), centerCircleRadius());
}

Point Field::centerPoint() const
{
    return Point(0, 0);
}

Point Field::friendlyGoal() const
{
    return Point(-xLength() / 2.0, 0.0);
}

Point Field::enemyGoal() const
{
    return Point(xLength() / 2.0, 0.0);
}

Point Field::penaltyEnemy() const
{
    return Point(enemyGoal().x() - defenseAreaXLength(), enemyGoal().y());
}

Point Field::penaltyFriendly() const
{
    return Point(friendlyGoal().x() + defenseAreaXLength(), friendlyGoal().y());
}

Point Field::friendlyCornerPos() const
{
    return Point(friendlyGoal().x(), yLength() / 2.0);
}

Point Field::friendlyCornerNeg() const
{
    return Point(friendlyGoal().x(), -yLength() / 2.0);
}

Point Field::enemyCornerPos() const
{
    return Point(enemyGoal().x(), yLength() / 2);
}

Point Field::enemyCornerNeg() const
{
    return Point(enemyGoal().x(), -yLength() / 2);
}

Point Field::friendlyGoalpostPos() const
{
    return Point(friendlyGoal().x(), goalYLength() / 2.0);
}

Point Field::friendlyGoalpostNeg() const
{
    return Point(friendlyGoal().x(), -goalYLength() / 2.0);
}

Point Field::enemyGoalpostPos() const
{
    return Point(enemyGoal().x(), goalYLength() / 2.0);
}

Point Field::enemyGoalpostNeg() const
{
    return Point(enemyGoal().x(), -goalYLength() / 2.0);
}

double Field::boundaryYLength() const
{
    return boundary_buffer_size_;
}

bool Field::pointInFriendlyDefenseArea(const Point p) const
{
    return friendlyDefenseArea().containsPoint(p);
}

bool Field::pointInEnemyDefenseArea(const Point p) const
{
    return enemyDefenseArea().containsPoint(p);
}

bool Field::pointInFieldLines(const Point &p) const
{
    return fieldLines().containsPoint(p);
}

boost::circular_buffer<Timestamp> Field::getTimestampHistory() const
{
    return last_update_timestamps;
}

Timestamp Field::getMostRecentTimestamp() const
{
    return last_update_timestamps.front();
}

void Field::updateTimestamp(Timestamp time_stamp)
{
    // Check if the timestamp buffer is empty
    if (last_update_timestamps.empty())
    {
        last_update_timestamps.push_front(time_stamp);
    }
    // Check that the new timestamp is not older than the most recent timestamp
    else if (time_stamp < Field::getMostRecentTimestamp())
    {
        throw std::invalid_argument(
            "Error: Attempt tp update Field state with old Timestamp");
    }
    else
    {
        last_update_timestamps.push_front(time_stamp);
    }
}

bool Field::pointInEntireField(const Point &p) const
{
    Rectangle entire_field = Rectangle(Point(-totalXLength() / 2, -totalYLength() / 2),
                                       Point(totalXLength() / 2, totalYLength() / 2));
    return entire_field.containsPoint(p);
}

bool Field::operator==(const Field &other) const
{
    return this->field_y_length_ == other.field_y_length_ &&
           this->field_x_length_ == other.field_x_length_ &&
           this->goal_y_length_ == other.goal_y_length_ &&
           this->defense_y_length_ == other.defense_y_length_ &&
           this->defense_x_length_ == other.defense_x_length_ &&
           this->boundary_buffer_size_ == other.boundary_buffer_size_ &&
           this->center_circle_radius_ == other.center_circle_radius_;
}

bool Field::operator!=(const Field &other) const
{
    return !(*this == other);
}


#include <algorithm>
#include <cassert>
#include <cmath>
#include <set>
#include <stdexcept>
#include <utility>
#include <boost/scoped_ptr.hpp>
#include <opengm/inference/lpcplex.hxx>
#include <opengm/datastructures/marray/marray.hxx>

#include "pgmlink/pgm.h"
#include "pgmlink/hypotheses.h"
#include "pgmlink/log.h"
#include "pgmlink/reasoner_pgm.h"
#include "pgmlink/pgm_chaingraph.h"
#include "pgmlink/traxels.h"

//#include <ostream>

using namespace std;

namespace pgmlink {
  ////
  //// class Chaingraph
  ////

  Chaingraph::~Chaingraph() {
    if(optimizer_ != NULL) {
	delete optimizer_;
	optimizer_ = NULL;
    }

    delete builder_;
    builder_ = NULL;
  }

double Chaingraph::forbidden_cost() const {
  return builder_->forbidden_cost();
}

bool Chaingraph::with_constraints() const {
  return with_constraints_;
}

void Chaingraph::formulate( const HypothesesGraph& hypotheses ) {
    LOG(logDEBUG) << "Chaingraph::formulate: entered";
    reset();

    // build the model
    linking_model_ = boost::shared_ptr<pgm::chaingraph::Model>(builder_->build(hypotheses));

    // refine the model with hard constraints
    pgm::OpengmLPCplex::Parameter param;
    param.verbose_ = true;
    param.integerConstraint_ = true;
    param.epGap_ = ep_gap_;
    param.timeLimit_ = cplex_timeout_;
    LOG(logDEBUG) << "Chaingraph::formulate ep_gap = " << param.epGap_;
    pgm::OpengmLPCplex* cplex = new pgm::OpengmLPCplex(*(linking_model_->opengm_model), param);
    optimizer_ = cplex; // opengm::Inference optimizer_

    if (with_constraints_) {
      LOG(logDEBUG) << "Chaingraph::formulate: add_constraints";
      builder_->add_hard_constraints( *linking_model_ , hypotheses, *cplex );
    }

    if (fixed_detections_) {
      LOG(logDEBUG) << "Chaingraph::formulate: fix_detections";
      builder_->fix_detections( *linking_model_, hypotheses, *cplex );
    }
}

void Chaingraph::infer() {
    opengm::InferenceTermination status = optimizer_->infer();
    if(status != opengm::NORMAL) {
        throw std::runtime_error("GraphicalModel::infer(): optimizer terminated unnormally");
    }
}

void Chaingraph::conclude( HypothesesGraph& g ) {
    // extract solution from optimizer
  vector<pgm::OpengmLPCplex::LabelType> solution;
    opengm::InferenceTermination status = optimizer_->arg(solution);
    if(status != opengm::NORMAL) {
	throw runtime_error("GraphicalModel::infer(): solution extraction terminated unnormally");
    }

    // add 'active' properties to graph
    g.add(node_active()).add(arc_active());
    property_map<node_active, HypothesesGraph::base_graph>::type& active_nodes = g.get(node_active());
    property_map<arc_active, HypothesesGraph::base_graph>::type& active_arcs = g.get(arc_active());

    // write state after inference into 'active'-property maps
    if(builder_->has_detection_vars()) {
      for(pgm::chaingraph::Model::node_var_map::const_iterator it = linking_model_->var_of_node().begin(); it != linking_model_->var_of_node().end(); ++it) {
	bool state = false;
	if(solution[it->second] == 1) state = true;
	active_nodes.set(it->first, state);
      }
    } else {
      // if we have no detection vars we set all nodes to active by default
      active_nodes.setAll(true);
    }
    for(pgm::chaingraph::Model::arc_var_map::const_iterator it = linking_model_->var_of_arc().begin(); it != linking_model_->var_of_arc().end(); ++it) {
	bool state = false;
	if(solution[it->second] == 1) state = true;
	active_arcs.set(it->first, state);
    }
}

  const pgm::OpengmModel* Chaingraph::get_graphical_model() const {
    return linking_model_->opengm_model.get();
  }

  const Chaingraph::node_var_map& Chaingraph::get_node_map() const {
    return linking_model_->var_of_node();
  }

  const Chaingraph::arc_var_map& Chaingraph::get_arc_map() const {
    return linking_model_->var_of_arc();
  }

void Chaingraph::reset() {
    if(optimizer_ != NULL) {
	delete optimizer_;
	optimizer_ = NULL;
    }
}

} /* namespace pgmlink */


#ifndef ODF_TRANSFORMATION_PROCESS_HPP
#define ODF_TRANSFORMATION_PROCESS_HPP
#include <boost/math/special_functions/sinc.hpp>
#include "basic_process.hpp"
#include "basic_voxel.hpp"

#include "mapping/fa_template.hpp"
extern fa_template fa_template_imp;

class ReadDWIData : public BaseProcess{
public:
    virtual void init(Voxel&) {}
    virtual void run(Voxel& voxel, VoxelData& data)
    {
        data.space.resize(voxel.dwi_data.size());
        for (unsigned int index = 0; index < data.space.size(); ++index)
            data.space[index] = voxel.dwi_data[index][data.voxel_index];
    }
    virtual void end(Voxel&,gz_mat_write&) {}
};

void calculate_shell(const std::vector<float>& sorted_bvalues,
                     std::vector<unsigned int>& shell);
class BalanceScheme : public BaseProcess{
    std::vector<float> trans;
    unsigned int new_q_count;
    unsigned int old_q_count;
private:
    Voxel* stored_voxel;
    std::vector<image::vector<3,float> > old_bvectors;
    std::vector<float> old_bvalues;
public:
    BalanceScheme(void):stored_voxel(0){}

    virtual void init(Voxel& voxel)
    {
        if(!voxel.scheme_balance)
            return;
        std::vector<unsigned int> shell;
        calculate_shell(voxel.bvalues,shell);
        unsigned int b_count = voxel.bvalues.size();
        unsigned int total_signals = 0;

        tessellated_icosahedron new_dir;
        new_dir.init(6);

        std::vector<image::vector<3,float> > new_bvectors;
        std::vector<float> new_bvalues;

        // if b0
        if(voxel.bvalues.front() == 0.0)
        {
            trans.resize(b_count);
            trans[0] = 1;
            new_bvectors.resize(1);
            new_bvalues.resize(1);
            total_signals += 1;
        }

        for(unsigned int shell_index = 0;shell_index < shell.size();++shell_index)
        {
            unsigned int from = shell[shell_index];
            unsigned int to = (shell_index + 1 == shell.size() ? b_count:shell[shell_index+1]);
            unsigned int num = to-from;

            //calculate averaged angle distance
            double averaged_angle = 0.0;
            for(unsigned int i = from;i < to;++i)
            {
                double max_cos = 0.0;
                for(unsigned int j = from;j < to;++j)
                {
                    if(i == j)
                        continue;
                    double cur_cos = std::fabs(std::cos(voxel.bvectors[i]*voxel.bvectors[j]));
                    if(cur_cos > 0.998)
                        continue;
                    max_cos = std::max<double>(max_cos,cur_cos);
                }
                averaged_angle += std::acos(max_cos);
            }
            averaged_angle /= num;

            //calculate averaged b_value
            double avg_b = image::mean(voxel.bvalues.begin()+from,voxel.bvalues.begin()+to);
            unsigned int trans_old_size = trans.size();
            trans.resize(trans.size() + new_dir.half_vertices_count*b_count);
            for(unsigned int i = 0; i < new_dir.half_vertices_count;++i)
            {
                std::vector<double> t(b_count);
                double effective_b = 0.0;
                for(unsigned int j = from;j < to;++j)
                {
                    double angle = std::acos(std::min<double>(1.0,std::fabs(new_dir.vertices[i]*voxel.bvectors[j])));
                    angle/=averaged_angle;
                    t[j] = std::exp(-2.0*angle*angle); // if the angle == 1, then weighting = 0.135
                    effective_b += t[j]*voxel.bvalues[j];
                }
                double sum_t = std::accumulate(t.begin(),t.end(),0.0);
                image::multiply_constant(t,avg_b/1000.0/sum_t);
                std::copy(t.begin(),t.end(),trans.begin() + trans_old_size + i * b_count);
                new_bvalues.push_back(effective_b/sum_t);
                new_bvectors.push_back(new_dir.vertices[i]);
            }
            total_signals += new_dir.half_vertices_count;
        }

        old_q_count = voxel.bvalues.size();
        new_q_count = total_signals;
        voxel.bvalues.swap(new_bvalues);
        voxel.bvectors.swap(new_bvectors);
        new_bvalues.swap(old_bvalues);
        new_bvectors.swap(old_bvectors);
        stored_voxel = &voxel;

    }
    virtual void run(Voxel& voxel, VoxelData& data)
    {

        if(!voxel.scheme_balance)
            return;
        if(stored_voxel)// restored btalbe here in case user terminate the recon
        {
            stored_voxel = 0;
            voxel.bvalues = old_bvalues;
            voxel.bvectors = old_bvectors;
        }
        std::vector<float> new_data(new_q_count);
        data.space.swap(new_data);
        image::mat::vector_product(trans.begin(),new_data.begin(),data.space.begin(),image::dyndim(new_q_count,old_q_count));
    }
};

struct GeneralizedFA
{
    float operator()(const std::vector<float>& odf)
    {
        float m1 = 0.0;
        float m2 = 0.0;
        std::vector<float>::const_iterator iter = odf.begin();
        std::vector<float>::const_iterator end = odf.end();
        for (;iter != end; ++iter)
        {
            float t = *iter;
            m1 += t;
            m2 += t*t;
        }
        m1 *= m1;
        m1 /= ((float)odf.size());
        if (m2 == 0.0)
            return 0.0;
        return std::sqrt(((float)odf.size())/((float)odf.size()-1.0)*(m2-m1)/m2);
    }
};

const unsigned int odf_block_size = 20000;
struct OutputODF : public BaseProcess
{
protected:
    std::vector<std::vector<float> > odf_data;
    std::vector<unsigned int> odf_index_map;
public:
    virtual void init(Voxel& voxel)
    {
        odf_data.clear();
        if (voxel.need_odf)
        {
            unsigned int total_count = 0;
            odf_index_map.resize(voxel.mask.size());
            for (unsigned int index = 0;index < voxel.mask.size();++index)
                if (voxel.mask[index])
                {
                    odf_index_map[index] = total_count;
                    ++total_count;
                }
            try
            {
                std::vector<unsigned int> size_list;
                while (1)
                {

                    if (total_count > odf_block_size)
                    {
                        size_list.push_back(odf_block_size);
                        total_count -= odf_block_size;
                    }
                    else
                    {
                        size_list.push_back(total_count);
                        break;
                    }
                }
                odf_data.resize(size_list.size());
                for (unsigned int index = 0;index < odf_data.size();++index)
                    odf_data[index].resize(size_list[index]*(voxel.ti.half_vertices_count));
            }
            catch (...)
            {
                odf_data.clear();
                throw std::runtime_error("Memory not enough for creating an ODF containing fib file.");
            }
        }

    }
    virtual void run(Voxel& voxel,VoxelData& data)
    {

        if (voxel.need_odf && data.fa[0] + 1.0 != 1.0)
        {
            unsigned int odf_index = odf_index_map[data.voxel_index];
            std::copy(data.odf.begin(),data.odf.end(),
                      odf_data[odf_index/odf_block_size].begin() + (odf_index%odf_block_size)*(voxel.ti.half_vertices_count));
        }

    }
    virtual void end(Voxel& voxel,gz_mat_write& mat_writer)
    {

        if (!voxel.need_odf)
            return;
        {
            set_title("output odfs");
            for (unsigned int index = 0;index < odf_data.size();++index)
            {
                if (!voxel.odf_deconvolusion)
                    image::divide_constant(odf_data[index],voxel.z0);
                std::ostringstream out;
                out << "odf" << index;
                mat_writer.write(out.str().c_str(),&*odf_data[index].begin(),
                                      voxel.ti.half_vertices_count,
                                      odf_data[index].size()/(voxel.ti.half_vertices_count));
            }
            odf_data.clear();
        }

    }
};

struct ODFLoader : public BaseProcess
{
    std::vector<unsigned int> index_mapping1;
    std::vector<unsigned int> index_mapping2;
public:
    virtual void init(Voxel& voxel)
    {
        //voxel.qa_scaling must be 1
        voxel.z0 = 0.0;
        index_mapping1.resize(voxel.mask.size());
        index_mapping2.resize(voxel.mask.size());
        int voxel_index = 0;
        for(unsigned int i = 0;i < voxel.template_odfs.size();++i)
        {
            for(unsigned int j = 0;j < voxel.template_odfs[i].size();j += voxel.ti.half_vertices_count)
            {
                int k_end = j + voxel.ti.half_vertices_count;
                bool is_odf_zero = true;
                for(int k = j;k < k_end;++k)
                    if(voxel.template_odfs[i][k] != 0.0)
                    {
                        is_odf_zero = false;
                        break;
                    }
                if(!is_odf_zero)
                    for(;voxel_index < index_mapping1.size();++voxel_index)
                        if(voxel.mask[voxel_index] != 0)
                            break;
                if(voxel_index >= index_mapping1.size())
                    break;
                index_mapping1[voxel_index] = i;
                index_mapping2[voxel_index] = j;
                ++voxel_index;
            }
        }
    }
    virtual void run(Voxel& voxel, VoxelData& data)
    {
        int cur_index = data.voxel_index;
        std::copy(voxel.template_odfs[index_mapping1[cur_index]].begin() +
                  index_mapping2[cur_index],
                  voxel.template_odfs[index_mapping1[cur_index]].begin() +
                  index_mapping2[cur_index]+data.odf.size(),data.odf.begin());

    }
    virtual void end(Voxel& voxel,gz_mat_write& mat_writer)
    {
        if (voxel.need_odf)
        {
            set_title("output odfs");
            for (unsigned int index = 0;index < voxel.template_odfs.size();++index)
            {
                std::ostringstream out;
                out << "odf" << index;
                mat_writer.write(out.str().c_str(),&*voxel.template_odfs[index].begin(),
                                      voxel.ti.half_vertices_count,
                                      voxel.template_odfs[index].size()/(voxel.ti.half_vertices_count));
            }
        }
        mat_writer.write("trans",&*voxel.param,4,4);
    }
};

// for normalization
class RecordQA  : public BaseProcess
{
public:
    virtual void init(Voxel& voxel)
    {
        voxel.qa_map.resize(voxel.dim);
        std::fill(voxel.qa_map.begin(),voxel.qa_map.end(),0.0);
    }
    virtual void run(Voxel& voxel, VoxelData& data)
    {
        voxel.qa_map[data.voxel_index] = data.fa[0];
    }
    virtual void end(Voxel&,gz_mat_write&)
    {

    }
};

double base_function(double theta);
struct SaveFA : public BaseProcess
{
protected:
    std::vector<float> iso,gfa;
    std::vector<std::vector<float> > fa;
    std::vector<std::vector<float> > rdi;
protected:
    float z0;
public:
    virtual void init(Voxel& voxel)
    {

        fa.resize(voxel.max_fiber_number);
        for (unsigned int index = 0;index < voxel.max_fiber_number;++index)
            fa[index].resize(voxel.dim.size());
        gfa.clear();
        gfa.resize(voxel.dim.size());
        iso.clear();
        iso.resize(voxel.dim.size());
        if(voxel.output_rdi)
        {
            float sigma = voxel.param[0]; //optimal 1.24
            for(float L = 0.2f;L <= sigma;L+= 0.2f)
            {
                rdi.push_back(std::vector<float>());
                rdi.back().resize(voxel.dim.size());
            }
        }
        if(voxel.csf_calibration)
        {
            std::vector<unsigned short> data(voxel.dwi_data[0],voxel.dwi_data[0]+voxel.dim.size());
            std::sort(data.begin(),data.end());
            // CSF selected at 125,000 mm^3
            int size = 125000.0f/(voxel.vs[0]*voxel.vs[1]*voxel.vs[2]);
            float water_b0 = *(data.end()-size);

            float sigma = voxel.param[0]; //optimal 1.24
            // numerically estimate free water ODF
            unsigned int odf_size = voxel.ti.half_vertices_count;
            std::vector<float> dwi(voxel.bvalues.size()),odf(odf_size);
            for(int i = 0;i < dwi.size();++i)
                dwi[i] = water_b0*std::exp(-voxel.bvalues[i]*0.003f); // free water diffusivity

            std::vector<float> sinc_ql(odf_size*voxel.bvalues.size());
            // calculate reconstruction matrix
            for (unsigned int j = 0,index = 0; j < odf_size; ++j)
                for (unsigned int i = 0; i < voxel.bvalues.size(); ++i,++index)
                    sinc_ql[index] = voxel.bvectors[i]*
                                 image::vector<3,float>(voxel.ti.vertices[j])*
                                   std::sqrt(voxel.bvalues[i]*0.01506);

            for (unsigned int index = 0; index < sinc_ql.size(); ++index)
                sinc_ql[index] = voxel.r2_weighted ?
                             base_function(sinc_ql[index]*sigma):
                             boost::math::sinc_pi(sinc_ql[index]*sigma);

            image::mat::vector_product(&*sinc_ql.begin(),&*dwi.begin(),&*odf.begin(),
                                    image::dyndim(odf.size(),dwi.size()));
            z0 = image::mean(odf.begin(),odf.end());
        }

    }
    virtual void run(Voxel& voxel, VoxelData& data)
    {
        iso[data.voxel_index] = data.min_odf;
        gfa[data.voxel_index] = GeneralizedFA()(data.odf);
        for (unsigned int index = 0;index < voxel.max_fiber_number;++index)
            fa[index][data.voxel_index] = data.fa[index];
        if(voxel.output_rdi)
            for (unsigned int index = 0;index < data.rdi.size();++index)
                rdi[index][data.voxel_index] = data.rdi[index];
    }
    virtual void end(Voxel& voxel,gz_mat_write& mat_writer)
    {
        set_title("output data");
        mat_writer.write("gfa",&*gfa.begin(),1,gfa.size());
        if(voxel.csf_calibration)
            voxel.z0 = z0;
        if(voxel.z0 + 1.0 == 1.0)
            voxel.z0 = 1.0;
        mat_writer.write("z0",&voxel.z0,1,1);

        image::divide_constant(iso,voxel.z0);
        mat_writer.write("iso",&*iso.begin(),1,iso.size());

        for (unsigned int index = 0;index < voxel.max_fiber_number;++index)
        {
            image::divide_constant(fa[index],voxel.z0);
            std::ostringstream out;
            out << index;
            std::string num = out.str();
            std::string fa_str = "fa";
            fa_str += num;
            set_title(fa_str.c_str());
            mat_writer.write(fa_str.c_str(),&*fa[index].begin(),1,fa[index].size());
        }

        // output normalized qa
        {
            float max_qa = 0.0;
            for (unsigned int i = 0;i < voxel.max_fiber_number;++i)
                max_qa = std::max<float>(*std::max_element(fa[i].begin(),fa[i].end()),max_qa);

            if(max_qa != 0.0)
            for (unsigned int index = 0;index < voxel.max_fiber_number;++index)
            {
                image::divide_constant(fa[index],max_qa);
                std::ostringstream out;
                out << index;
                std::string num = out.str();
                std::string fa_str = "nqa";
                fa_str += num;
                set_title(fa_str.c_str());
                mat_writer.write(fa_str.c_str(),&*fa[index].begin(),1,fa[index].size());
            }
        }

        if(voxel.output_rdi)
        {
            for(unsigned int i = 0;i < rdi.size();++i)
                image::divide_constant(rdi[i],voxel.z0);
            float L = 0.2f;
            for(unsigned int i = 0;i < rdi.size();++i,L += 0.2f)
            {
                std::ostringstream out;
                out.precision(2);
                out << "rdi" << std::setfill('0') << std::setw(2) << int(L*10) << "L";
                mat_writer.write(out.str().c_str(),&*rdi[i].begin(),1,rdi[i].size());
            }
            for(unsigned int i = 0;i < rdi[0].size();++i)
            for(unsigned int j = 0;j < rdi.size();++j)
                rdi[j][i] = rdi[rdi.size()-1][i]-rdi[j][i];
            L = 0.2f;
            for(unsigned int i = 0;i < rdi.size();++i,L += 0.2f)
            {
                std::ostringstream out2;
                out2.precision(2);
                out2 << "nrdi" << std::setfill('0') << std::setw(2) << int(L*10) << "L";
                mat_writer.write(out2.str().c_str(),&*rdi[i].begin(),1,rdi[i].size());
            }
        }
    }
};

struct SaveDirIndex : public BaseProcess
{
protected:
    std::vector<std::vector<short> > findex;
public:
    virtual void init(Voxel& voxel)
    {

        findex.resize(voxel.max_fiber_number);
        for (unsigned int index = 0;index < voxel.max_fiber_number;++index)
            findex[index].resize(voxel.dim.size());
    }
    virtual void run(Voxel& voxel, VoxelData& data)
    {

        for (unsigned int index = 0;index < voxel.max_fiber_number;++index)
            findex[index][data.voxel_index] = data.dir_index[index];
    }
    virtual void end(Voxel& voxel,gz_mat_write& mat_writer)
    {
        for (unsigned int index = 0;index < voxel.max_fiber_number;++index)
        {
            std::ostringstream out;
            out << index;
            std::string num = out.str();
            std::string index_str = "index";
            index_str += num;
            set_title(index_str.c_str());
            mat_writer.write(index_str.c_str(),&*findex[index].begin(),1,findex[index].size());
        }
    }
};

struct SaveDir : public BaseProcess
{
protected:
    std::vector<std::vector<float> > dir;
public:
    virtual void init(Voxel& voxel)
    {

        dir.resize(voxel.max_fiber_number);
        for (unsigned int index = 0;index < voxel.max_fiber_number;++index)
            dir[index].resize(voxel.dim.size()*3);
    }
    virtual void run(Voxel& voxel, VoxelData& data)
    {

        unsigned int dir_index = data.voxel_index;
        for (unsigned int index = 0;index < voxel.max_fiber_number;++index)
            std::copy(data.dir[index].begin(),data.dir[index].end(),dir[index].begin() + dir_index + dir_index + dir_index);
    }
    virtual void end(Voxel& voxel,gz_mat_write& mat_writer)
    {
        for (unsigned int index = 0;index < voxel.max_fiber_number;++index)
        {
            std::ostringstream out;
            out << index;
            std::string num = out.str();
            std::string index_str = "dir";
            index_str += num;
            set_title(index_str.c_str());
            mat_writer.write(index_str.c_str(),&*dir[index].begin(),1,dir[index].size());
        }
    }
};

struct SearchLocalMaximum
{
    std::vector<std::vector<unsigned short> > neighbor;
    std::map<float,unsigned short,std::greater<float> > max_table;
    void init(Voxel& voxel)
    {

        unsigned int half_odf_size = voxel.ti.half_vertices_count;
        unsigned int faces_count = voxel.ti.faces.size();
        neighbor.resize(voxel.ti.half_vertices_count);
        for (unsigned int index = 0;index < faces_count;++index)
        {
            short i1 = voxel.ti.faces[index][0];
            short i2 = voxel.ti.faces[index][1];
            short i3 = voxel.ti.faces[index][2];
            if (i1 >= half_odf_size)
                i1 -= half_odf_size;
            if (i2 >= half_odf_size)
                i2 -= half_odf_size;
            if (i3 >= half_odf_size)
                i3 -= half_odf_size;
            neighbor[i1].push_back(i2);
            neighbor[i1].push_back(i3);
            neighbor[i2].push_back(i1);
            neighbor[i2].push_back(i3);
            neighbor[i3].push_back(i1);
            neighbor[i3].push_back(i2);
        }
    }
    void search(const std::vector<float>& old_odf)
    {
        max_table.clear();
        for (unsigned int index = 0;index < neighbor.size();++index)
        {
            float value = old_odf[index];
            bool is_max = true;
            std::vector<unsigned short>& nei = neighbor[index];
            for (unsigned int j = 0;j < nei.size();++j)
            {
                if (value < old_odf[nei[j]])
                {
                    is_max = false;
                    break;
                }
            }
            if (is_max)
                max_table[value] = (unsigned short)index;
        }
    }
};

struct DetermineFiberDirections : public BaseProcess
{
    SearchLocalMaximum lm;
    std::mutex mutex;
public:
    virtual void init(Voxel& voxel)
    {
        lm.init(voxel);
    }

    virtual void run(Voxel& voxel,VoxelData& data)
    {
        data.min_odf = *std::min_element(data.odf.begin(),data.odf.end());
        std::lock_guard<std::mutex> lock(mutex);
        lm.search(data.odf);
        std::map<float,unsigned short,std::greater<float> >::const_iterator iter = lm.max_table.begin();
        std::map<float,unsigned short,std::greater<float> >::const_iterator end = lm.max_table.end();
        for (unsigned int index = 0;iter != end && index < voxel.max_fiber_number;++index,++iter)
        {
            data.dir_index[index] = iter->second;
            data.fa[index] = iter->first - data.min_odf;
        }
    }
};

struct ScaleZ0ToMinODF : public BaseProcess
{
    float max_min_odf;
public:
    virtual void init(Voxel& voxel)
    {
        voxel.z0 = 0.0;
    }

    virtual void run(Voxel& voxel,VoxelData& data)
    {
        if(data.min_odf > voxel.z0)
            voxel.z0 = data.min_odf;
    }
};

#endif//ODF_TRANSFORMATION_PROCESS_HPP


/*
 * signals.cpp
 *
 *  Created on: Apr 9, 2015
 *      Author: sujiwo
 */

#include <iostream>
#include <ros/ros.h>
#include "Rate.h"
#include "vector_map.h"
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/CameraInfo.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Pose.h>
#include <signal.h>
#include <cstdio>
#include "Math.h"
#include <Eigen/Eigen>
#include "road_wizard/Signals.h"
#include <runtime_manager/adjust_xy.h>

static std::string camera_id_str;

static constexpr uint32_t SUBSCRIBE_QUEUE_SIZE = 1000;

static int adjust_proj_x = 0;
static int adjust_proj_y = 0;

typedef struct {
  double thiX;
  double thiY;
  double thiZ;
} Angle;

static VectorMap vmap;
static Angle cameraOrientation; // camera orientation = car's orientation

static Eigen::Vector3f position;
static Eigen::Quaternionf orientation;
static  float fx,
  fy,
  imageWidth,
  imageHeight,
  cx,
  cy;
static tf::StampedTransform trf;

#define SignalLampRadius 0.3

/* Callback function to shift projection result */
void adjust_xyCallback (const runtime_manager::adjust_xy::ConstPtr& config_msg)
{
  adjust_proj_x = config_msg->x;
  adjust_proj_y = config_msg->y;
}

void cameraInfoCallback (const sensor_msgs::CameraInfo::ConstPtr camInfoMsg)
{
  fx = static_cast<float>(camInfoMsg->P[0]);
  fy = static_cast<float>(camInfoMsg->P[5]);
  imageWidth = camInfoMsg->width;
  imageHeight = camInfoMsg->height;
  cx = static_cast<float>(camInfoMsg->P[2]);
  cy = static_cast<float>(camInfoMsg->P[6]);
}

/* convert degree value into 0 to 360 range */
static double setDegree0to360(double val)
{
  if (val < 0.0f) {
    return (val + 360.0f);
  }
  else if (360.0f < val) {
    return (val - 360.0f);
  }

  return val;
}

static void get_cameraRollPitchYaw(double* roll,
                                   double* pitch,
                                   double* yaw)
{
  geometry_msgs::Pose cameraPose;
  cameraPose.position.x    = (double)(position.x());
  cameraPose.position.y    = (double)(position.y());
  cameraPose.position.z    = (double)(position.z());
  cameraPose.orientation.x = (double)(orientation.x());
  cameraPose.orientation.y = (double)(orientation.y());
  cameraPose.orientation.z = (double)(orientation.z());
  cameraPose.orientation.w = (double)(orientation.w());

  tf::Quaternion quat;

  tf::quaternionMsgToTF(cameraPose.orientation, quat);
  tf::Matrix3x3(quat).getRPY(*roll, *pitch, *yaw);

  /* convert from radian to degree */
  *roll  = setDegree0to360(*roll  * 180.0f / M_PI);
  *pitch = setDegree0to360(*pitch * 180.0f / M_PI);
  *yaw   = setDegree0to360(*yaw   * 180.0f / M_PI);
}

/*
  check if lower < val < upper
  This function also considers circulation
*/
static bool isRange(const double lower, const double upper, const double val)
{
  if (lower <= upper) {
    if (lower < val && val < upper) {
      return true;
    }
  }
  else {
    if (val < upper || lower < val) {
      return true;
    }
  }

  return false;
}

void getTransform (Eigen::Quaternionf &ori, Point3 &pos)
{
  static tf::TransformListener listener;

  // target_frame    source_frame
  ros::Time now = ros::Time();
  listener.waitForTransform (camera_id_str, "map", now, ros::Duration(10.0));
  listener.lookupTransform (camera_id_str, "map", now, trf);

  tf::Vector3 &p = trf.getOrigin();
  tf::Quaternion o = trf.getRotation();
  pos.x()=p.x(); pos.y()=p.y(); pos.z()=p.z();
  ori.w()=o.w(); ori.x()=o.x(); ori.y()=o.y(); ori.z()=o.z();
}

Point3 transform (const Point3 &psrc, tf::StampedTransform &tfsource)
{
  tf::Vector3 pt3 (psrc.x(), psrc.y(), psrc.z());
  tf::Vector3 pt3s = tfsource * pt3;
  return Point3 (pt3s.x(), pt3s.y(), pt3s.z());
}

/*
 * Project a point from world coordinate to image plane
 */
bool project2 (const Point3 &pt, int &u, int &v, bool useOpenGLCoord=false)
{
  float nearPlane = 1.0;
  float farPlane = 200.0;
  Point3 _pt = transform (pt, trf);
  float _u = _pt.x()*fx/_pt.z() + cx;
  float _v = _pt.y()*fy/_pt.z() + cy;

  u = static_cast<int>(_u);
  v = static_cast<int>(_v);
  if ( u < 0 || imageWidth < u || v < 0 || imageHeight < v || _pt.z() < nearPlane || farPlane < _pt.z() ) {
    u = -1, v = -1;
    return false;
  }

  if (useOpenGLCoord) {
    v = imageHeight - v;
  }

  return true;
}

void echoSignals2 (ros::Publisher &pub, bool useOpenGLCoord=false)
{
  int countPoint = 0;
  road_wizard::Signals signalsInFrame;

  for (unsigned int i=1; i<=vmap.signals.size(); i++) {
    Signal signal = vmap.signals[i];
    int pid = vmap.vectors[signal.vid].pid;

    Point3 signalcenter = vmap.getPoint(pid);
    Point3 signalcenterx (signalcenter.x(), signalcenter.y(), signalcenter.z()+SignalLampRadius);

    int u, v;
    if (project2 (signalcenter, u, v, useOpenGLCoord) == true) {
      countPoint++;
      // std::cout << u << ", " << v << ", " << std::endl;

      int radius;
      int ux, vx;
      project2 (signalcenterx, ux, vx, useOpenGLCoord);
      radius = (int)distance (ux, vx, u, v);

      road_wizard::ExtractedPosition sign;
      sign.signalId = signal.id;

      sign.u = u + adjust_proj_x; // shift project position by configuration value from runtime manager
      sign.v = v + adjust_proj_y; // shift project position by configuration value from runtime manager

      sign.radius = radius;
      sign.x = signalcenter.x(), sign.y = signalcenter.y(), sign.z = signalcenter.z();
      sign.hang = vmap.vectors[signal.vid].hang; // hang is expressed in [0, 360] degree
      sign.type = signal.type, sign.linkId = signal.linkid;
      sign.plId = signal.plid;

      /* convert signal's horizontal angle to yaw */
      double reversed_signalYaw = setDegree0to360(sign.hang + 180.0f);

      get_cameraRollPitchYaw(&cameraOrientation.thiX,
                             &cameraOrientation.thiY,
                             &cameraOrientation.thiZ);

      // std::cout << "signal : " << reversed_signalYaw << ", car : " << cameraOrientation.thiZ << std::endl;

      /*
        check whether this signal is oriented to the camera
        interested signals have below condition orientation:
        (camera_orientation - 70deg) < (signal_orientation + 180deg) < (camera_orientatin + 70deg)
      */
      double conditionRange_lower = setDegree0to360(cameraOrientation.thiZ - 70);
      double conditionRange_upper = setDegree0to360(cameraOrientation.thiZ + 70);

      // std::cout << "lower: " << conditionRange_lower << ", upper: " << conditionRange_upper << std::endl;

      if (isRange(conditionRange_lower, conditionRange_upper, reversed_signalYaw)) {
        signalsInFrame.Signals.push_back (sign);
      }
    }
  }

  signalsInFrame.header.stamp = ros::Time::now();
  pub.publish (signalsInFrame);

  // printf ("There are %d out of %u signals in frame\n", countPoint, static_cast<unsigned int>(vmap.signals.size()));
}

void interrupt (int s)
{
  ros::shutdown();
  exit(1);
}

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

  ros::init(argc, argv, "feat_proj", ros::init_options::NoSigintHandler);
  ros::NodeHandle rosnode;
  ros::NodeHandle private_nh("~");
  std::string cameraInfo_topic_name;
  private_nh.param<std::string>("camera_info_topic", cameraInfo_topic_name, "/camera/camera_info");

  /* get camera ID */
  camera_id_str = cameraInfo_topic_name;
  camera_id_str.erase(camera_id_str.find("/camera/camera_info"));
  if (camera_id_str == "/") {
    camera_id_str = "camera";
  }

  /* load vector map */
  ros::Subscriber sub_point     = rosnode.subscribe("vector_map_info/point_class",
                                                    SUBSCRIBE_QUEUE_SIZE,
                                                    &VectorMap::load_points,
                                                    &vmap);
  ros::Subscriber sub_line      = rosnode.subscribe("vector_map_info/line_class",
                                                    SUBSCRIBE_QUEUE_SIZE,
                                                    &VectorMap::load_lines,
                                                    &vmap);
  ros::Subscriber sub_lane      = rosnode.subscribe("vector_map_info/lane",
                                                    SUBSCRIBE_QUEUE_SIZE,
                                                    &VectorMap::load_lanes,
                                                    &vmap);
  ros::Subscriber sub_vector    = rosnode.subscribe("vector_map_info/vector_class",
                                                    SUBSCRIBE_QUEUE_SIZE,
                                                    &VectorMap::load_vectors,
                                                    &vmap);
  ros::Subscriber sub_signal    = rosnode.subscribe("vector_map_info/signal",
                                                    SUBSCRIBE_QUEUE_SIZE,
                                                    &VectorMap::load_signals,
                                                    &vmap);
  ros::Subscriber sub_whiteline = rosnode.subscribe("vector_map_info/white_line",
                                                    SUBSCRIBE_QUEUE_SIZE,
                                                    &VectorMap::load_whitelines,
                                                    &vmap);
  ros::Subscriber sub_dtlane    = rosnode.subscribe("vector_map_info/dtlane",
                                                    SUBSCRIBE_QUEUE_SIZE,
                                                    &VectorMap::load_dtlanes,
                                                    &vmap);

  /* wait until loading all vector map is completed */
  ros::Rate wait_rate(1);
  while(vmap.points.empty() || vmap.lines.empty() || vmap.whitelines.empty() ||
        vmap.lanes.empty() || vmap.dtlanes.empty() || vmap.vectors.empty() || vmap.signals.empty())
    {
      ros::spinOnce();
      wait_rate.sleep();
    }

  vmap.loaded = true;
  std::cout << "all vector map loaded." << std::endl;

  ros::Subscriber cameraInfoSubscriber = rosnode.subscribe (cameraInfo_topic_name, 100, cameraInfoCallback);
  ros::Subscriber adjust_xySubscriber  = rosnode.subscribe("/config/adjust_xy", 100, adjust_xyCallback);
  //  ros::Subscriber ndtPoseSubscriber    = rosnode.subscribe("/current_pose", 10, ndtPoseCallback);
  ros::Publisher  signalPublisher      = rosnode.advertise <road_wizard::Signals> ("roi_signal", 100);
  signal (SIGINT, interrupt);

  Rate loop (25);
  while (true) {

    ros::spinOnce();

    try {
      getTransform (orientation, position);
    } catch (tf::TransformException &exc) {
    }

    echoSignals2 (signalPublisher, false);
    loop.sleep();
  }

}


#ifndef STAN_MATH_PRIM_MAT_PROB_INV_WISHART_LOG_HPP
#define STAN_MATH_PRIM_MAT_PROB_INV_WISHART_LOG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/prob/inv_wishart_lpdf.hpp>
#include <boost/math/tools/promotion.hpp>

namespace stan {
namespace math {

/**
 * The log of the Inverse-Wishart density for the given W, degrees
 * of freedom, and scale matrix.
 *
 * The scale matrix, S, must be k x k, symmetric, and semi-positive
 * definite.
 *
 * @deprecated use inverse_wishart_lpdf
 *
 * @param W A scalar matrix
 * @param nu Degrees of freedom
 * @param S The scale matrix
 * @return The log of the Inverse-Wishart density at W given nu and S.
 * @throw std::domain_error if nu is not greater than k-1
 * @throw std::domain_error if S is not square, not symmetric, or not
 * semi-positive definite.
 * @tparam T_y Type of scalar.
 * @tparam T_dof Type of degrees of freedom.
 * @tparam T_scale Type of scale.
 */
template <bool propto, typename T_y, typename T_dof, typename T_scale>
return_type_t<T_y, T_dof, T_scale> inv_wishart_log(
    const Eigen::Matrix<T_y, Eigen::Dynamic, Eigen::Dynamic>& W,
    const T_dof& nu,
    const Eigen::Matrix<T_scale, Eigen::Dynamic, Eigen::Dynamic>& S) {
  return inv_wishart_lpdf<propto, T_y, T_dof, T_scale>(W, nu, S);
}

/**
 * @deprecated use inverse_wishart_lpdf
 */
template <typename T_y, typename T_dof, typename T_scale>
inline return_type_t<T_y, T_dof, T_scale> inv_wishart_log(
    const Eigen::Matrix<T_y, Eigen::Dynamic, Eigen::Dynamic>& W,
    const T_dof& nu,
    const Eigen::Matrix<T_scale, Eigen::Dynamic, Eigen::Dynamic>& S) {
  return inv_wishart_lpdf<T_y, T_dof, T_scale>(W, nu, S);
}

}  // namespace math
}  // namespace stan
#endif


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2018.
//
// This software is released under a three-clause BSD license:
//  * 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.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// 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 ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS 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.
//
// --------------------------------------------------------------------------
// $Maintainer: Timo Sachsenberg $
// $Authors: Marc Sturm $
// --------------------------------------------------------------------------

// STL
#include <iostream>
#include <cmath>

// OpenMS
#include <OpenMS/VISUAL/AxisTickCalculator.h>
#include <OpenMS/MATH/MISC/MathFunctions.h>

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

using namespace std;

namespace OpenMS
{
  using namespace Math;

  void AxisTickCalculator::calcGridLines(double x1, double x2, GridVector & grid)
  {
    grid.clear();

    if (boost::math::isnan(x1) || boost::math::isnan(x2))
      return;

    if (x1 > -0.0001 && x1 < 0.0001)
    {
      x1 = 0.0001;
    }

    double dx = x2 - x1;

    if (dx < 0.0000001)
    {
      return;
    }
    double epsilon = dx / 200;

    double sDecPow = floor(log10(dx));
    double sDec = pow(10.0, sDecPow);

    UInt n_max_big_gridlines = (UInt)floor(dx / sDec);

    std::vector<double> big;
    double currGL = ceilDecimal(x1, (UInt)sDecPow);

    // big grid lines
    while (currGL < (x2 + epsilon))
    {
      big.push_back(currGL);
      currGL += sDec;
    }
    grid.push_back(big);

    // only one major grid line
    if (n_max_big_gridlines <= 3)
    {
      std::vector<double> small;
      currGL = grid[0][0] - sDec * 9 / 10;
      while (currGL < (x2 + epsilon))
      {
        // check if in visible area
        if (currGL > x1)
        {
          // check for overlap with big gridlines
          bool overlap = false;
          for (Size i = 0; i != big.size(); ++i)
          {
            if (fabs(big[i] - currGL) < epsilon)
            {
              overlap = true;
            }
          }
          if (!overlap)
          {
            small.push_back(currGL);
          }
        }
        currGL += sDec / 10;
      }
      grid.push_back(small);
      return;
    }

    // four or more major grid lines
    std::vector<double> small;
    currGL = grid[0][0] - sDec / 2;
    while (currGL < (x2 + epsilon))
    {
      if (currGL > x1)
      {
        small.push_back(currGL);
      }
      currGL += sDec;
    }

    grid.push_back(small);
  }

  void AxisTickCalculator::calcLogGridLines(double x1, double x2, GridVector & grid)
  {
    grid.clear();
    double scalValues[8];
    scalValues[0] = log10(2.0);
    scalValues[1] = log10(3.0);
    scalValues[2] = log10(4.0);
    scalValues[3] = log10(5.0);
    scalValues[4] = log10(6.0);
    scalValues[5] = log10(7.0);
    scalValues[6] = log10(8.0);
    scalValues[7] = log10(9.0);
    double dx = x2 - x1;

    if (dx < 0.00000001)
    {
      return;
    }

    Int x1ceil = (Int)floor(x1);
    Int x2floor = (Int)ceil(x2);
    std::vector<double> big;
    for (Int i = x1ceil; i != x2floor; ++i)
    {
      big.push_back(i);
    }
    grid.push_back(big);
    std::vector<double> small;
    for (Size i = 0; i != grid[0].size(); ++i)
    {
      double currGL = grid[0][i];
      for (Int j = 0; j != 8; ++j)
      {
        if (currGL + scalValues[j] > x2)
        {
          break;
        }
        small.push_back(currGL + scalValues[j]);
      }
    }
    grid.push_back(small);
  }

} //namespace


/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/*                                                                  */
/*                      Early Plume Microphysics                    */
/*                              (EPM)                               */
/*                                                                  */
/* Integrate Header File                                            */
/*                                                                  */
/* Author               : Thibaud M. Fritz                          */
/* Time                 : 9/27/2018                                 */
/* File                 : Integrate.hpp                             */
/*                                                                  */
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

#ifndef INTEGRATE_H_INCLUDED
#define INTEGRATE_H_INCLUDED

#include <iostream>
#include <cmath>
#include <vector>
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>
#include "omp.h"

#include "Util/ForwardDecl.hpp"
#include "Util/PhysConstant.hpp"
#include "Util/PhysFunction.hpp"
#include "Core/Interface.hpp"
#include "Core/Parameters.hpp"
#include "Core/Monitor.hpp"
#include "Core/Aircraft.hpp"
#include "Core/Emission.hpp"
#include "AIM/Coagulation.hpp"
#include "AIM/Nucleation.hpp"
#include "AIM/Aerosol.hpp"
#include "odeSolver.hpp"

namespace EPM
{

    static const int EPM_SUCCESS = 1;
    static const int EPM_FAILURE = 0;

    /* Vortex sinking timescales, taken from Unterstrasser et al., 2008 */
    const RealDouble t_Vortex_0 = 8.00E+00;
    const RealDouble t_Vortex_1 = 1.10E+02;

    /* Dilution timescales for a B747, taken from:
     * B. Kärcher, "A trajectory box model for aircraft exhaust plumes", Journal of Geophysical Research, 1995 */
    const RealDouble t_0 = 1.00E-04; /* [s] */
    const RealDouble t_j = 1.00E-02; /* [s] */
    const RealDouble t_1 = 8.00E+00; /* [s] */
    const RealDouble t_2 = 6.60E+01; /* [s] */

    const RealDouble m = 2.0;
    const RealDouble n = 50.0;
    const RealDouble Cv = 3.0;

    /* Engine exit plane characteristics for a B747, taken from:
     * B. Kärcher, "A trajectory box model for aircraft exhaust plumes", Journal of Geophysical Research, 1995 */
    /* Engine exit core area im m^2 */
    const RealDouble Ac0 = 0.604;
    /* Engine exit core velocity in m/s */
    const RealDouble uc0 = 475.7;
    /* Engine exit core temperature in K */
    /* const RealDouble Tc0 = 547.3; */
    /* RealDouble Tc0 */
    /* Engine exit bypass area in m^2 */
    /* const RealDouble Ab0 = 1.804; */
    /* RealDouble Ab0 */

    int Integrate( RealDouble &temperature_K, RealDouble pressure_Pa, RealDouble relHumidity_w, RealDouble varArray[], \
                   RealDouble fixArray[], RealDouble aerArray[][2], const Aircraft &AC, const Emission &EI, \
                   RealDouble &Ice_rad, RealDouble &Ice_den, RealDouble &Soot_den, RealDouble &H2O_mol, \
                   RealDouble &SO4g_mol, RealDouble &SO4l_mol, AIM::Aerosol &SO4Aer, AIM::Aerosol &IceAer, \
                   RealDouble &Area, RealDouble &Ab0, RealDouble &Tc0, const bool CHEMISTRY );
    int RunMicrophysics( RealDouble &temperature_K, RealDouble pressure_Pa, RealDouble relHumidity_w, \
                         RealDouble varArray[], RealDouble fixArray[], RealDouble aerArray[][2], \
                         const Aircraft &AC, const Emission &EI, RealDouble delta_T_ad, RealDouble delta_T, \
                         RealDouble &Ice_rad, RealDouble &Ice_den, RealDouble &Soot_den, RealDouble &H2O_mol, \
                         RealDouble &SO4g_mol, RealDouble &SO4l_mol, AIM::Aerosol &SO4Aer, AIM::Aerosol &IceAer, \
                         RealDouble &Area, RealDouble &Ab0, RealDouble &Tc0, const bool CHEMISTRY );
    RealDouble dT_Vortex( const RealDouble time, const RealDouble delta_T, bool deriv = 0 );
    RealDouble dilutionRatio( const RealDouble time );
    RealDouble depositionRate( const RealDouble r, const RealDouble T, const RealDouble P, const RealDouble H2O, \
                               const RealDouble r_0,  const RealDouble theta );
    void odeRHS( const Vector_1D &x, Vector_1D &dxdt, const RealDouble t = 0.0 );
    bool isFreezable( const RealDouble r, const RealDouble T, const RealDouble H2O, const RealDouble r0 );
    RealDouble condensationRate( const RealDouble r, const RealDouble T, const RealDouble P, const RealDouble H2O, \
                                 const RealDouble theta );

}

#endif /* INTEGRATE_H_INCLUDED */


//  Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Basic sanity check that header <boost/math/tools/series.hpp>
// #includes all the files that it needs to.
//
#include <boost/math/tools/series.hpp>
//
// Note this header includes no other headers, this is
// important if this test is to be meaningful:
//
#include "test_compile_result.hpp"

struct Functor
{
   typedef double result_type;
   double operator()();
};
#define U double

Functor func;
boost::uintmax_t uim = 0;

void compile_and_link_test()
{
   check_result<Functor::result_type>(boost::math::tools::sum_series<Functor>(func, i));
   check_result<Functor::result_type>(boost::math::tools::sum_series<Functor>(func, i, uim));
   check_result<Functor::result_type>(boost::math::tools::sum_series<Functor, U>(func, i, d));
   check_result<Functor::result_type>(boost::math::tools::sum_series<Functor, U>(func, i, uim, d));
   check_result<Functor::result_type>(boost::math::tools::kahan_sum_series<Functor>(func, i));
   check_result<Functor::result_type>(boost::math::tools::kahan_sum_series<Functor>(func, i, uim));
}