// Copyright (c) 2020-2021 Franz Alt
// This code is licensed under MIT license (see LICENSE.txt for details).

#include <libcvpg/imageproc/scripting/algorithms/k_means.hpp>

#include <chrono>
#include <functional>

#include <boost/asynchronous/continuation_task.hpp>

#include <libcvpg/core/exception.hpp>
#include <libcvpg/core/image.hpp>
#include <libcvpg/imageproc/algorithms/k_means.hpp>
#include <libcvpg/imageproc/scripting/item.hpp>
#include <libcvpg/imageproc/scripting/processing_context.hpp>
#include <libcvpg/imageproc/scripting/detail/compiler.hpp>
#include <libcvpg/imageproc/scripting/detail/handler.hpp>
#include <libcvpg/imageproc/scripting/detail/parser.hpp>

namespace detail {

struct k_means_task :  public boost::asynchronous::continuation_task<std::shared_ptr<cvpg::imageproc::scripting::processing_context> >
{
    k_means_task(std::shared_ptr<cvpg::imageproc::scripting::processing_context> context, std::uint32_t result_id, cvpg::imageproc::scripting::detail::parser::item item)
        : boost::asynchronous::continuation_task<std::shared_ptr<cvpg::imageproc::scripting::processing_context> >("algorithms::k_means_task")
        , m_context(context)
        , m_result_id(result_id)
        , m_item(std::move(item))
    {}

    void operator()()
    {
        try
        {
            auto id = std::any_cast<std::uint32_t>(m_item.arguments.at(0).value());
            auto k = std::any_cast<std::int32_t>(m_item.arguments.at(1).value());
            auto max_iterations = std::any_cast<std::int32_t>(m_item.arguments.at(2).value());
            auto eps = std::any_cast<std::int32_t>(m_item.arguments.at(3).value());

            auto input = m_context->load(id);
            auto parameters = m_context->parameters();

            std::uint32_t cutoff_x = 512;
            std::uint32_t cutoff_y = 512;

            {
                auto it = parameters.find("cutoff_x");

                if (it != parameters.end())
                {
                    cutoff_x = std::any_cast<std::uint32_t>(it->second);
                }
            }

            {
                auto it = parameters.find("cutoff_y");

                if (it != parameters.end())
                {
                    cutoff_y = std::any_cast<std::uint32_t>(it->second);
                }
            }

            if (input.type() == cvpg::imageproc::scripting::item::types::grayscale_8_bit_image)
            {
                auto image = std::any_cast<cvpg::image_gray_8bit>(input.value());

                auto start = std::chrono::system_clock::now();

                boost::asynchronous::create_callback_continuation(
                    [result = this->this_task_result(), context = m_context, result_id = m_result_id, start](auto cont_res) mutable
                    {
                        auto stop = std::chrono::system_clock::now();

                        try
                        {
                            context->store(result_id, std::move(std::get<0>(cont_res).get()), std::chrono::duration_cast<std::chrono::microseconds>(stop - start));

                            result.set_value(context);
                        }
                        catch (...)
                        {
                            result.set_exception(std::current_exception());
                        }

                    },
                    cvpg::imageproc::algorithms::k_means(std::move(image), k, max_iterations, eps)
                );
            }
            else if (input.type() == cvpg::imageproc::scripting::item::types::rgb_8_bit_image)
            {
                auto image = std::any_cast<cvpg::image_rgb_8bit>(input.value());

                auto start = std::chrono::system_clock::now();

                boost::asynchronous::create_callback_continuation(
                    [result = this->this_task_result(), context = m_context, result_id = m_result_id, start](auto cont_res) mutable
                    {
                        auto stop = std::chrono::system_clock::now();

                        try
                        {
                            context->store(result_id, std::move(std::get<0>(cont_res).get()), std::chrono::duration_cast<std::chrono::microseconds>(stop - start));

                            result.set_value(context);
                        }
                        catch (...)
                        {
                            result.set_exception(std::current_exception());
                        }

                    },
                    cvpg::imageproc::algorithms::k_means(std::move(image), k, max_iterations, eps)
                );
            }
        }
        catch (...)
        {
            this->this_task_result().set_exception(std::current_exception());
        }
    }

private:
    std::shared_ptr<cvpg::imageproc::scripting::processing_context> m_context;

    std::uint32_t m_result_id;

    cvpg::imageproc::scripting::detail::parser::item m_item;
};

auto k_means(std::shared_ptr<cvpg::imageproc::scripting::processing_context> context, std::uint32_t result_id, cvpg::imageproc::scripting::detail::parser::item item)
{
    return boost::asynchronous::top_level_callback_continuation<std::shared_ptr<cvpg::imageproc::scripting::processing_context> >(
               k_means_task(context, result_id, std::move(item))
           );
}

} // namespace detail

namespace cvpg::imageproc::scripting::algorithms {

std::string k_means::name() const
{
    return "k_means";
}

std::string k_means::category() const
{
    return "filters/segmentation";
}

std::vector<scripting::item::types> k_means::result() const
{
    return
    {
        scripting::item::types::grayscale_8_bit_image
    };
}

parameter_set k_means::parameters() const
{
    return parameter_set
           ({
               parameter("image", "input image", "", { scripting::item::types::grayscale_8_bit_image, scripting::item::types::rgb_8_bit_image }),
               parameter("k", "amount of clusters", "", scripting::item::types::signed_integer, static_cast<std::int32_t>(1), static_cast<std::int32_t>(255), static_cast<std::int32_t>(1)),
               parameter("max_iterations", "maximum amount of iterations", "", scripting::item::types::signed_integer, static_cast<std::int32_t>(1), static_cast<std::int32_t>(std::numeric_limits<std::int32_t>::max()), static_cast<std::int32_t>(1)),
               parameter("eps", "minimum distance in color space to mark two values as equal", "", scripting::item::types::signed_integer, static_cast<std::int32_t>(1), static_cast<std::int32_t>(441), static_cast<std::int32_t>(1))
           });
}

void k_means::on_parse(std::shared_ptr<detail::parser> parser) const
{
    // all parameters
    {
        std::function<std::uint32_t(std::uint32_t, std::int32_t, std::int32_t, std::int32_t)> fct =
            [parser, parameters = this->parameters()](std::uint32_t image_id, std::int32_t k, std::int32_t max_iterations, std::int32_t eps)
            {
                // find image
                if (!parser)
                {
                    throw cvpg::invalid_parameter_exception("invalid parser");
                }

                auto image = parser->find_item(image_id);

                if (image.arguments.empty())
                {
                    throw cvpg::invalid_parameter_exception("invalid input ID");
                }

                auto input_type = image.arguments.front().type();

                // check parameters
                if (!(input_type == scripting::item::types::grayscale_8_bit_image || input_type == scripting::item::types::rgb_8_bit_image))
                {
                    throw cvpg::invalid_parameter_exception("invalid input");
                }

                if (!parameters.is_valid("k", k))
                {
                    throw cvpg::invalid_parameter_exception("invalid amount of clusters");
                }

                if (!parameters.is_valid("max_iterations", max_iterations))
                {
                    throw cvpg::invalid_parameter_exception("invalid amount of maximum iterations");
                }

                if (!parameters.is_valid("eps", eps))
                {
                    throw cvpg::invalid_parameter_exception("invalid epsilon");
                }

                std::uint32_t result_id = 0;

                switch (input_type)
                {
                    case scripting::item::types::grayscale_8_bit_image:
                    {
                        detail::parser::item result_item
                        {
                            "k_means",
                            {
                                scripting::item(scripting::item::types::grayscale_8_bit_image, image_id),
                                scripting::item(scripting::item::types::signed_integer, k),
                                scripting::item(scripting::item::types::signed_integer, max_iterations),
                                scripting::item(scripting::item::types::signed_integer, eps)
                            }
                        };

                        result_id = parser->register_item(std::move(result_item));

                        break;
                    }

                    case scripting::item::types::rgb_8_bit_image:
                    {
                        detail::parser::item result_item
                        {
                            "k_means",
                            {
                                scripting::item(scripting::item::types::rgb_8_bit_image, image_id),
                                scripting::item(scripting::item::types::signed_integer, k),
                                scripting::item(scripting::item::types::signed_integer, max_iterations),
                                scripting::item(scripting::item::types::signed_integer, eps)
                            }
                        };

                        result_id = parser->register_item(std::move(result_item));

                        break;
                    }

                    default:
                    {
                        // to make the compiler happy ; other input types are not allowed and should be handled above
                        break;
                    }
                }

                if (result_id != 0)
                {
                    parser->register_link(image_id, result_id);
                }

                return result_id;
            };

        parser->register_specification(name(), std::move(fct));
    }

    // default for 'eps' (minimum distance of two values in color space)
    {
        std::function<std::uint32_t(std::uint32_t, std::int32_t, std::int32_t)> fct =
            [parser, parameters = this->parameters()](std::uint32_t image_id, std::int32_t k, std::int32_t max_iterations)
            {
                // find image
                if (!parser)
                {
                    throw cvpg::invalid_parameter_exception("invalid parser");
                }

                auto image = parser->find_item(image_id);

                if (image.arguments.empty())
                {
                    throw cvpg::invalid_parameter_exception("invalid input ID");
                }

                auto input_type = image.arguments.front().type();

                // check parameters
                if (!(input_type == scripting::item::types::grayscale_8_bit_image || input_type == scripting::item::types::rgb_8_bit_image))
                {
                    throw cvpg::invalid_parameter_exception("invalid input");
                }

                if (!parameters.is_valid("k", k))
                {
                    throw cvpg::invalid_parameter_exception("invalid amount of clusters");
                }

                if (!parameters.is_valid("max_iterations", max_iterations))
                {
                    throw cvpg::invalid_parameter_exception("invalid amount of maximum iterations");
                }

                std::uint32_t result_id = 0;

                const std::int32_t eps = 5;

                switch (input_type)
                {
                    case scripting::item::types::grayscale_8_bit_image:
                    {
                        detail::parser::item result_item
                        {
                            "k_means",
                            {
                                scripting::item(scripting::item::types::grayscale_8_bit_image, image_id),
                                scripting::item(scripting::item::types::signed_integer, k),
                                scripting::item(scripting::item::types::signed_integer, max_iterations),
                                scripting::item(scripting::item::types::signed_integer, eps)
                            }
                        };

                        result_id = parser->register_item(std::move(result_item));

                        break;
                    }

                    case scripting::item::types::rgb_8_bit_image:
                    {
                        detail::parser::item result_item
                        {
                            "k_means",
                            {
                                scripting::item(scripting::item::types::rgb_8_bit_image, image_id),
                                scripting::item(scripting::item::types::signed_integer, k),
                                scripting::item(scripting::item::types::signed_integer, max_iterations),
                                scripting::item(scripting::item::types::signed_integer, eps)
                            }
                        };

                        result_id = parser->register_item(std::move(result_item));

                        break;
                    }

                    default:
                    {
                        // to make the compiler happy ; other input types are not allowed and should be handled above
                        break;
                    }
                }

                if (result_id != 0)
                {
                    parser->register_link(image_id, result_id);
                }

                return result_id;
            };

        parser->register_specification(name(), std::move(fct));
    }

    // default for 'max_iterations'
    {
        std::function<std::uint32_t(std::uint32_t, std::int32_t)> fct =
            [parser, parameters = this->parameters()](std::uint32_t image_id, std::int32_t k)
            {
                // find image
                if (!parser)
                {
                    throw cvpg::invalid_parameter_exception("invalid parser");
                }

                auto image = parser->find_item(image_id);

                if (image.arguments.empty())
                {
                    throw cvpg::invalid_parameter_exception("invalid input ID");
                }

                auto input_type = image.arguments.front().type();

                // check parameters
                if (!(input_type == scripting::item::types::grayscale_8_bit_image || input_type == scripting::item::types::rgb_8_bit_image))
                {
                    throw cvpg::invalid_parameter_exception("invalid input");
                }

                if (!parameters.is_valid("k", k))
                {
                    throw cvpg::invalid_parameter_exception("invalid amount of clusters");
                }

                std::uint32_t result_id = 0;

                const std::int32_t max_iterations = 10;
                const std::int32_t eps = 5;

                switch (input_type)
                {
                    case scripting::item::types::grayscale_8_bit_image:
                    {
                        detail::parser::item result_item
                        {
                            "k_means",
                            {
                                scripting::item(scripting::item::types::grayscale_8_bit_image, image_id),
                                scripting::item(scripting::item::types::signed_integer, k),
                                scripting::item(scripting::item::types::signed_integer, max_iterations),
                                scripting::item(scripting::item::types::signed_integer, eps)
                            }
                        };

                        result_id = parser->register_item(std::move(result_item));

                        break;
                    }

                    case scripting::item::types::rgb_8_bit_image:
                    {
                        detail::parser::item result_item
                        {
                            "k_means",
                            {
                                scripting::item(scripting::item::types::rgb_8_bit_image, image_id),
                                scripting::item(scripting::item::types::signed_integer, k),
                                scripting::item(scripting::item::types::signed_integer, max_iterations),
                                scripting::item(scripting::item::types::signed_integer, eps)
                            }
                        };

                        result_id = parser->register_item(std::move(result_item));

                        break;
                    }

                    default:
                    {
                        // to make the compiler happy ; other input types are not allowed and should be handled above
                        break;
                    }
                }

                if (result_id != 0)
                {
                    parser->register_link(image_id, result_id);
                }

                return result_id;
            };

        parser->register_specification(name(), std::move(fct));
    }
}

void k_means::on_compile(std::uint32_t item_id, std::shared_ptr<detail::compiler> compiler) const
{
    auto handler =
        detail::handler(
            [result_id = item_id, item = compiler->get_item(item_id)](std::shared_ptr<processing_context> context)
            {
                return ::detail::k_means(context, result_id, std::move(item));
            });

    compiler->register_handler(item_id, name(), std::move(handler));
}

} // namespace cvpg::imageproc::scripting::algorithms


#include <iostream>
#include <stdio.h>
#include <fstream>
#include <vector>
#include <map>
#include <set>
#include <unordered_set>
#include <unordered_map>
#include <cmath>
// #include <gnuplot-iostream.h>
#include <assert.h>
#include <boost/program_options.hpp>

#include "vector3d.hpp"
#include "swiss_to_lat_lon.hpp"
#include "import_heightmap.hpp"

namespace po = boost::program_options;

bool EQ_DBL(double a, double b){
    return std::abs(a-b)<EPS_DBL;
}

int main(int ac, char** av) {
    //Variables to be assigned by program options
    double height_map_resolution;
    double north_bound;
    double south_bound;
    double east_bound;
    double west_bound;
    int sun_angles;
    int times_per_year;
    int start_day;
    double summer_angle_panel;
    double winter_angle_panel;
    bool use_terrain_normals = true;
    bool compute_best_fixed_angle = false;
    bool compute_best_summer_winter_angle = false;

    std::string input_file;
    std::string output_file;
    bool verbose = false;
    bool elevation_dependant_sun_intensity = false;

    // Declare the supported options.
    po::options_description op_desc("Allowed options");
    op_desc.add_options()
    ("help", "print options table")
    ("input-file,i", po::value<std::string>(&input_file), "File containing height map data in x<space>y<space>z<newline> swiss coordinates format")
    ("output-file-base,o", po::value<std::string>(&output_file)->default_value("data_out"), "Base filname for output files (default data_out)")
    ("resolution,R", po::value<double>(&height_map_resolution)->default_value(200.0), "resolution of data (default: 200.0)")
    ("nmax",po::value<double>(&north_bound)->default_value(1e100), "maximum north coordinate to be treated (default 1e100)")
    ("nmin",po::value<double>(&south_bound)->default_value(-1e100), "minimum north coordinate to be treated (default -1e100)")
    ("emax",po::value<double>(&east_bound)->default_value(1e100), "maximum east coordinate to be treated (default 1e100)")
    ("emin",po::value<double>(&west_bound)->default_value(-1e100), "minimum east coordinate to be treated (default -1e100)")
    ("elevation,E", "Include effect of terrain elevation in the computation of sun intensity")
    ("sunangles,s", po::value<int>(&sun_angles)->default_value(360), "number of angles to compute sunlight from (default 360)")
    ("times,t", po::value<int>(&times_per_year)->default_value(12), "number of times per year to calculate at (default 12)")
    ("start-day, D", po::value<int>(&start_day)->default_value(20), "Day of the year to output first data. (default 20)")
    ("fixed-angle, F", po::value<double>(&summer_angle_panel), "Use fixed angle for solar panel inclination to compute sun intensity.")
    ("summer-angle", po::value<double>(&summer_angle_panel), "Fixed angle for solar panel inclination during summer to compute sun intensity..")
    ("winter-angle", po::value<double>(&winter_angle_panel), "Fixed angle for solar panel inclination during winter to compute sun intensity..")
    ("best-fixed-angle, B", "Use best fixed angle for this latitude to compute sun intensity.")
    ("best-two-season-angles, BSW", "Use best summer and winter angle for this latitude to compute sun intensity.")
    ("terrain-normals, T", "Use terrain normal for each point to compute sun intensity, default option if none of the solar panel angles options are specified.")
    ("verbose, v", "Verbose: output lots of text")
    ;

    po::positional_options_description pd;
    pd.add("input-file", 1).add("output-file", 1);

    po::variables_map vm;
    po::store(po::parse_command_line(ac, av, op_desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout <<"CrunchGeoData [options] [input file] [output base]"<<std::endl<< op_desc << std::endl;
        return 1;
    }
    if(vm.count("fixed-angle")){
        winter_angle_panel = summer_angle_panel;
        use_terrain_normals = false;
    }
    if(vm.count("summer-angle") && !vm.count("winter-angle")){
        std::cout << "Please specify also a winter angle, or use option fixed-angle" << std::endl;
        exit(255);
    }
    if(vm.count("best-fixed-angle")){
        use_terrain_normals = false;
        compute_best_fixed_angle = true;
    }
    if(vm.count("best-two-season-angles")){
        use_terrain_normals = false;
        compute_best_summer_winter_angle = true;
    }

    if (vm.count("elevation")){
        elevation_dependant_sun_intensity = true;
    }
    if (vm.count("verbose")){
        verbose = true;
    }
    if(!vm.count("input-file")){
        std::cout << "Input file must be specified" << std::endl;
        exit(255);
    }

    std::unordered_map<vector3d, std::vector<double>, hash> grid_points;   //grid_points is unordered_map of all points in bounding box with
                                                                            //a vector to hold average sun power for the days sun is computed

    double north_x_max;
    double east_y_max;
    double south_x_min;
    double west_y_min;

    import_heightmap(input_file, south_bound, north_bound, east_bound, west_bound,
                     times_per_year, grid_points ,north_x_max, south_x_min, east_y_max, west_y_min);

    std::pair<double,double> NE = swiss_to_lat_lon(north_x_max+height_map_resolution/2.0, east_y_max+height_map_resolution/2.0);
    std::pair<double,double> SW = swiss_to_lat_lon(south_x_min-height_map_resolution/2.0, west_y_min-height_map_resolution/2.0);

    std::cout << "NE: " << north_x_max <<", " << east_y_max << " SW: "<< south_x_min << " , " << west_y_min <<  std::endl;
    std::cout /*<< std::setprecision(9)*/ << "NE: " << NE.first <<", " << NE.second << " SW: "<< SW.first << ", " << SW.second <<  std::endl;
    std::cout << "Number of points in dataset: " << grid_points.size() << std::endl;

    double average_latitude=((NE.first+SW.first)/2.0)*M_PI/180.0;
    vector3d summer_normal_panel;
    vector3d winter_normal_panel;

    if(!use_terrain_normals){
        if(compute_best_fixed_angle){
        }
        if(compute_best_summer_winter_angle){
        }
        if(average_latitude > 0){  //if latitude is greater than zero, point south
            summer_normal_panel.x = -1*sin(summer_angle_panel*M_PI/180);
            summer_normal_panel.y = 0;
            summer_normal_panel.z = 1*cos(summer_angle_panel*M_PI/180);

            winter_normal_panel.x = -1*sin(winter_angle_panel*M_PI/180);
            winter_normal_panel.y = 0;
            winter_normal_panel.z = 1*cos(winter_angle_panel*M_PI/180);
        }
        else{  //if latitude is less than zero, point north
            summer_normal_panel.x = 1*sin(summer_angle_panel*M_PI/180);
            summer_normal_panel.y = 0;
            summer_normal_panel.z = 1*cos(summer_angle_panel*M_PI/180);

            winter_normal_panel.x = 1*sin(winter_angle_panel*M_PI/180);
            winter_normal_panel.y = 0;
            winter_normal_panel.z = 1*cos(winter_angle_panel*M_PI/180);
        }

    }

    std::vector<std::vector<double> > sun_elevation_angle(times_per_year,std::vector<double>(sun_angles));
    //Elevation angle PHI of sun for a day and hour

    std::vector<std::vector<double> > sun_intensity_day_angle(times_per_year,std::vector<double>(sun_angles));

    std::vector<std::vector <vector3d> > sun_vec_day_angle(times_per_year,std::vector<vector3d>(sun_angles));
    //Unit vectors for sun's direction in day, hour.

    for(int day = 0; day < times_per_year; day++){
        double N=(365.0*day)/times_per_year+start_day;  //We are computing values for the Nth day of the year
        double phi_axis=-asin(0.39779*cos((0.98565*(N+10)+1.914*sin(0.98565*(N-2)*M_PI/180))*M_PI/180));  //Earth axis inclination for day N
        for(int thH=0;thH<sun_angles;thH++){//Angle theta for sun; 0 is North, +90 is West.
            int indTH= thH;
            double thHrad = (180-thH)*M_PI/180.0;  //turn around for sun formula angle
            double phiElv=asin(sin(average_latitude)*sin(phi_axis)+cos(average_latitude)*cos(thHrad)*cos(phi_axis));  //Sun elevation, imperical fomula, see wikipedia

            thHrad = M_PI-thHrad;          //In our coordinate system (North x+, West y+) we must inverse the sun theta. Theta corresponds to sun ray direction.

            vector3d sun_vec(cos(phiElv) * cos(thHrad) , cos(phiElv) * sin(thHrad) , sin(phiElv));  //Unit vector for direction sun ray come from
            sun_elevation_angle[(int)day][indTH]=phiElv;  //stick sun elevation in its datastructure

            sun_vec_day_angle[(int)day][indTH]=sun_vec;  //stick sun vector in its datastructure

            double air_mass_coefficient =sqrt(708.0*708.0*sin(phiElv)*sin(phiElv)+2.0*708.0+1.0)-708.0*sin(phiElv);  //wikipedia
            sun_intensity_day_angle[(int)day][indTH]=pow(0.7,pow(air_mass_coefficient,0.678))/0.7;  //stick sun intensity in its datastructure
        }
    }

    std::vector<double> max_sun_intensity;
    max_sun_intensity.assign(times_per_year, 0);
    std::vector<double> min_sun_intensity;
    min_sun_intensity.assign(times_per_year, 1e12);

    int N=0;

    std::cout <<"   Progress: "<< 100*(N*1.0)/(grid_points.size()*1.0) <<" %    \n";

    for(auto grid_point : grid_points){
        vector3d v = grid_point.first;
        N++;

        if (1) {
            std::cout <<"   Progress: "<< 100.0*(N*1.0)/(grid_points.size()*1.0) <<" %  ("<<N<<")    \r";
            std::cout.flush();
        }
        vector3d vNx(v.x+height_map_resolution*10,v.y,0);            //get points 10xresolution to the north, west, south and east
        vector3d vWx(v.x,v.y+height_map_resolution*10,0);
        vector3d vSx(v.x-height_map_resolution*10,v.y,0);
        vector3d vEx(v.x,v.y-height_map_resolution*10,0);
        auto itEx=grid_points.find(vEx);
        auto itNx=grid_points.find(vNx);
        auto itWx=grid_points.find(vWx);
        auto itSx=grid_points.find(vSx);
        if (itEx == grid_points.end()||itNx == grid_points.end()||itWx == grid_points.end()||itSx == grid_points.end()){
            std::vector<double> L;
            L.assign(times_per_year,-1.0);  //border points get value -1.0 to indicate we have coputed no value for them
            grid_points[v]=L;
            continue;
        }

        //v = *it_v;

        vector3d vN(v.x+height_map_resolution,v.y,0);            //get points to the north, west, south and east
        vector3d vW(v.x,v.y+height_map_resolution,0);
        vector3d vS(v.x-height_map_resolution,v.y,0);
        vector3d vE(v.x,v.y-height_map_resolution,0);

        auto itE=grid_points.find(vE);
        auto itN=grid_points.find(vN);
        auto itW=grid_points.find(vW);
        auto itS=grid_points.find(vS);

        if (itE == grid_points.end()||itN == grid_points.end()||itW == grid_points.end()||itS == grid_points.end()){
            exit(-1);
        }

        vE=itE->first;
        vN=itN->first;
        vW=itW->first;
        vS=itS->first;

        vector3d Normal = ((((vE-v)^(vN-v))+((vW-v)^(vS-v)))/2).norm();       //normal is the crossproduct of two prependicular differences. Avgd.

        std::vector<double> horizon_elevation_angles;   //vector to hold elevation angles at theta
        horizon_elevation_angles.assign(sun_angles, 0);
        //    std::vector<vector3d> horizon(sun_angles);
        std::vector<double> dists(sun_angles);

        size_t edge_points = 0;
        size_t interior_points = 0;

        for(int theta = 0; theta<sun_angles;theta++){        //compute for each angle theta
            double th = theta*M_PI/180;
            {
                double sin_theta = sin(th);   //calculate sin of the angle in radians
                double yend = EQ_DBL(copysign(1,sin_theta),1)?east_y_max:0;     //if the sinus is positive endvalue is east_y_max, if negative 0
                for(double y=v.y+copysign(height_map_resolution,sin_theta);(y >= 0 && y <= east_y_max);y+=copysign(height_map_resolution,sin_theta)){//increase/decrease if theta +/-
                    double x = v.x+(y-v.y)/tan(th);                             //find the x that goes with y for this theta
                    double x1 = floor(x-((int)x%(int)height_map_resolution));       //find the nearest lower gridpoint by subtracting remainder according to height_map_resolution
                    double x2 = x1 + height_map_resolution;                  //Add height_map_resolution to get nearest higher gridpoint
                    double phi = horizon_elevation_angles[theta];
                    double dist = (v-vector3d(x,y,v.z)).length();
                    double phiMaxTheta = atan(5000.0/dist);      //maximal phi at this theta (with height 5000 m)

                    if(x1>=north_x_max||x1<0||x2>=north_x_max||x2<0||phi>phiMaxTheta){
                        break;
                    }
                    if((int)x%(int)height_map_resolution){                       //if x is not a grid point

                        auto it_vec1 = grid_points.find(vector3d(x1,y,0));      //get the two gridpoints from the set
                        auto it_vec2 = grid_points.find(vector3d(x2,y,0));
                        if (it_vec1 == grid_points.end()||it_vec2 == grid_points.end()){
                            edge_points++;
                            continue;
                        }
                        vector3d vec1 = it_vec1->first;
                        vector3d vec2 = it_vec2->first;
                        double height = vec1.z*(x2-x)/height_map_resolution + vec2.z*(x-x1)/height_map_resolution-v.z;       //compute height at x via linear interpolation
                        dist=(v-vector3d(x,y,v.z)).length();
                        phi = atan(height/dist);

                        if(phi>horizon_elevation_angles[theta]){//see if larger
                            horizon_elevation_angles[theta]=phi;
                            dists[theta] = dist/1000;
                        }

                    }
                    else{//if x is a gridpoint
                        auto it_vec = grid_points.find(vector3d(x,y,0));  //get vector
                        if (it_vec == grid_points.end()){
                            //exit(-1);
                            edge_points++;
                            continue;
                        }
                        vector3d vec = it_vec->first;
                        double height = vec.z-v.z;                          //get height
                        double dist=(v-vector3d(x,y,v.z)).length();
                        double phi = atan(height/dist);
                        if(phi>horizon_elevation_angles[theta]){//see if larger
                            horizon_elevation_angles[theta]=phi;
                            dists[theta] = dist/1000;
                        }

                    }
                    interior_points++;
                }
            }
            double cos_theta = cos(th);
            double xend = EQ_DBL(copysign(1,cos_theta),1)?north_x_max:0;     //if the cosinus is positive endvalue is north_x_max, if negative 0

            for(double x=v.x+copysign(height_map_resolution,cos_theta);(x >= 0 && x <= north_x_max);x+=copysign(height_map_resolution,cos_theta)){//increase/decrease if theta +/-
                double y = v.y+(x-v.x)*tan(th);                             //find the y that goes with x for this theta
                double y1 = floor(y-((int)y%(int)height_map_resolution));       //find the nearest lower gridpoint by subtracting remainder according to height_map_resolution
                double y2 = y1 + height_map_resolution;                  //Add height_map_resolution to get nearest higher gridpoint
                double phi = horizon_elevation_angles[theta];
                double dist = sqrt((v.x-x)*(v.x-x) + (v.y-y)*(v.y-y));
                double phiMaxTheta = atan(5000.0/dist);      //maximal phi at this theta (with height 5000 m)

                if(y1>=north_x_max||y1<0||y2>=north_x_max||y2<0||phi>phiMaxTheta){
                    break;
                }
                if((int)y%(int)height_map_resolution){                       //if y is not a grid point
                    auto it_vec1 = grid_points.find(vector3d(x,y1,0));      //get the two gridpoints from the set
                    auto it_vec2 = grid_points.find(vector3d(x,y2,0));
                    if (it_vec1 == grid_points.end()||it_vec2 == grid_points.end()){
                        //exit(-1);
                        edge_points++;
                        continue;
                    }
                    vector3d vec1 = it_vec1->first;
                    vector3d vec2 = it_vec2->first;
                    double height = vec1.z*(y2-y)/height_map_resolution + vec2.z*(y-y1)/height_map_resolution-v.z;       //compute height at y via linear interpolation
                    double dist=(v-vector3d(x,y,v.z)).length();
                    double phi = atan(height/dist);
                    if(phi>horizon_elevation_angles[theta]){//see if larger
                        horizon_elevation_angles[theta]=phi;
                        dists[theta] = dist/1000;
                    }

                }
                else{//if y is a gridpoint
                    auto it_vec = grid_points.find(vector3d(x,y,0));  //get vector
                    if (it_vec == grid_points.end()){
                        //exit(-1);
                        edge_points++;
                        continue;
                    }
                    vector3d vec = it_vec->first;
                    double height = vec.z-v.z;                          //get height
                    double dist=(v-vector3d(x,y,v.z)).length();
                    double phi = atan(height/dist);
                    if(phi>horizon_elevation_angles[theta]){//see if larger
                        horizon_elevation_angles[theta]=phi;
                        dists[theta] = dist/1000;
                    }

                }
                interior_points++;
            }

        }

        int k = 0;

        std::vector<double> average_sun_intensity;
        average_sun_intensity.assign(times_per_year, 0.0);
        double sun_intensity = 0;

        for(int j=0;j<times_per_year;j++){
            for (k=0;k<sun_angles;k++) {
                if(horizon_elevation_angles[k]>sun_elevation_angle[j][k]){
                    sun_intensity=0;
                }
                else{
                    double height = 0;
                    if(elevation_dependant_sun_intensity){
                        height = v.z;
                    }
                    //spherical shell approximation for air mass attenuation, see wikipedia
                    // http://en.wikipedia.org/wiki/Air_mass_%28solar_energy%29
                    double phi = sun_elevation_angle[j][k];
                    double R_E = 6371000.0;      //Earth radius, meters
                    double y_atm = 9000.0;       //Athmospheric thickness, meters
                    double r = R_E/y_atm;
                    double c = height/y_atm;
                    double air_mass_coefficient = sqrt((r+c)*(r+c)*cos(phi)*cos(phi) + (2*r+1+c)*(1-c))-(r+c)*cos(phi);
                    double I_0 = 1.353; // kW/m^2
                    double I = 1.1 * I_0 * pow(0.7, pow(air_mass_coefficient, 0.678));
                    sun_intensity = I * (sun_vec_day_angle[j][k] * Normal);

                    average_sun_intensity[j]+=sun_intensity;

                }
                average_sun_intensity[j] = average_sun_intensity[j] / sun_angles;
                grid_point.second[j]=average_sun_intensity[j];
                max_sun_intensity[j] = (average_sun_intensity[j] > max_sun_intensity[j])?average_sun_intensity[j]:max_sun_intensity[j];
                min_sun_intensity[j] = (average_sun_intensity[j] < min_sun_intensity[j])?average_sun_intensity[j]:min_sun_intensity[j];
            }
            assert(average_sun_intensity.size() == times_per_year);
            grid_points[v]=average_sun_intensity;

            assert(grid_point.second.size() == times_per_year);

        }
    }
        std::cout << grid_points.size() << std::endl;

        for(int k = 0; k < times_per_year; ++k){
            std::ostringstream oss("");
            oss << k;
            std::ofstream ofs(output_file + oss.str() + ".xyz");
            if (!ofs.is_open()){
                exit(-2);
            }
            for(auto grid_point : grid_points){
                vector3d v = grid_point.first;
                if(grid_point.second.size()>k){
                    ofs << v.x << " " << v.y << " " << v.z << " ";
                    ofs << grid_points[v][k] << std::endl;
                }
            }
        }
        return 0;
    }


/**
 *  filename : main.cpp
 *  author   : Do Won Cha
 *  content  : Ray trace 3 spheres and a plane with reflection
 */

#include <Eigen/Core>
#include <cstdio>
#include <cstring>
#include <string>
#include <vector>
#include <queue>
#include <fstream>
#include <memory>

#include "scene.hpp"
#include "ray_tracer.h"
#include "primitives/material.hpp"
#include "primitives/surface_plane.hpp"
#include "primitives/surface_sphere.hpp"
#include "primitives/light.hpp"

using namespace Eigen;
using namespace raytracer;

std::unique_ptr<Scene> scene;
std::unique_ptr<RayTracer> ray;

void Idle()
{
	ray->Render();
}

void Display()
{
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );

  glDrawPixels(camera_->screen_width(),
               camera_->screen_height(),
               GL_RGBA,
               GL_FLOAT,
               ray->frame_buffer_.data());

  glutSwapBuffers();
  glutPostRedisplay();
}

int main(int argc, char* argv[])
{
	// Glut initialization
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGBA);
	glutInitWindowPosition(100, 100);
	glutInitWindowSize(512, 512);
	glutCreateWindow("Ray Tracer");

	// Glew initialization
	glewExperimental = GL_TRUE;
	GLenum err = glewInit();
	if (err != GLEW_OK)
	{
		/* Problem: glewInit failed, something is seriously wrong. */
		fprintf(stderr, "Error: %s\n", glewGetErrorString(err));
		exit(EXIT_FAILURE);
	}
	printf("Status: GLEW %s\n", glewGetString(GLEW_VERSION));

	// glut funcs
	glutIdleFunc(Idle);
	glutDisplayFunc(Display);

	scene = std::make_unique<Scene>();

	scene->add_material(std::make_unique<Material>(
		Vector4f(0.2f, 0.0f, 0.0f, 1.0f),			// ambient
		Vector4f(1.0f, 0.0f, 0.0f, 1.0f),			// diffuse
		Vector4f(0.0f, 0.0f, 0.0f, 1.0f)				// specular
	), "red");

	scene->add_material(std::make_unique<Material>(
		Vector4f(0.0f, 0.2f, 0.0f, 1.0f),
		Vector4f(0.0f, 0.5f, 0.0f, 1.0f),
		Vector4f(0.5f, 0.5f, 0.5f, 1.0f),
		32.0f																		// specular power
	), "green");

	scene->add_material(std::make_unique<Material>(
		Vector4f(0.0f, 0.0f, 0.2f, 1.0f),
		Vector4f(0.0f, 0.0f, 1.0f, 1.0f),
		Vector4f(0.0f, 0.0f, 0.0f, 1.0f),
		0.0f,																		// specular power
		32.0f																		// reflectivity
	), "blue");

	scene->add_material(std::make_unique<Material>(
		Vector4f(0.2f, 0.2f, 0.2f, 1.0f),
		Vector4f(1.0f, 1.0f, 1.0f, 1.0f),
		Vector4f(0.0f, 0.0f, 0.0f, 1.0f),
		0.0f,
		0.5f					// Reflectivity
	), "white");

	// add 3 spheres(position, radius, material name) to the scene
	scene->add_surface(std::make_unique<Sphere>(Vector3f(-4.0f, 0.0f, -7.0f), 1.0f, "red"));
	scene->add_surface(std::make_unique<Sphere>(Vector3f(0.0f, 0.0f, -7.0f), 2.0f, "green"));
	scene->add_surface(std::make_unique<Sphere>(Vector3f(4.0f, 0.0f, -7.0f), 1.0f, "blue"));

	// Add flat white plane to the scene.
	scene->add_surface(std::make_unique<Plane>(Vector3f(0.0f, -2.0f, 0.0f), Vector3f(0.0f, 1.0f, 0.0f), "white"));

	// Add a light to the scene, default intensity = 1.0f
	scene->add_light(std::make_unique<Light>(
		Vector3f(-4.0f, 4.0f, -3.0f),   // position
    Vector3f(1.0f, 1.0f, 1.0f),			// ambient
    Vector3f(1.0f, 1.0f, 1.0f)  		// diffuse
	));

  ray = std::make_unique<RayTracer>(&argc, argv);
	ray->initialize(scene);

	glutMainLoop();

  exit(EXIT_SUCCESS);
}


/*
 * cuCTProjectionOperator.cpp
 *
 *  Created on: Feb 18, 2015
 *      Author: u051747
 */

#include "CTProjectionOperator.h"
#include <boost/math/constants/constants.hpp>
#include "ct_projection.h"
#include "hoNDArray_math.h"

using namespace boost::math::float_constants;
namespace Gadgetron {

    template<template<class> class ARRAY>
    CTProjectionOperator<ARRAY>::CTProjectionOperator() {
        // TODO Auto-generated constructor stub

        samples_per_pixel_ = 1.5f;

    }

    template<template<class> class ARRAY>
    CTProjectionOperator<ARRAY>::~CTProjectionOperator() {
        // TODO Auto-generated destructor stub
    }

    template<template<class> class ARRAY>
    void CTProjectionOperator<ARRAY>::mult_M(ARRAY<float> *input,
                                             ARRAY<float> *output, bool accumulate) {

        auto dims = *input->get_dimensions();
        std::vector<size_t> dims3d(dims);

        if (dims3d.size() == 4) dims3d.pop_back();
        auto outdims = *output->get_dimensions();
        std::vector<size_t> outbindims(outdims);
        float *input_ptr = input->get_data_ptr();
        ARRAY<float> *tmp_out = output;
        if (accumulate)
            tmp_out = new ARRAY<float>(output->get_dimensions());

        float *output_ptr = tmp_out->get_data_ptr();
        for (int bin = 0; bin < binning->get_number_of_bins(); bin++) {
            //Check for empty bins
            if (binning->get_bin(bin).size() == 0)
                continue;
            ARRAY<float> input_view(dims3d, input_ptr);
            outbindims.back() = detector_focal_cyls[bin].size();
            auto output_view = ARRAY<float>(outbindims, output_ptr);

            ct_forwards_projection(&output_view, &input_view, detector_focal_cyls[bin], focal_offset_cyls[bin],
                                   central_elements[bin], is_dims_in_mm, ps_spacing, ADD, samples_per_pixel_, false);
            //conebeam_forwards_projection(output_view2.get(),&input_view,angles[bin],offsets[bin],samples_per_pixel_,is_dims_in_mm_,acquisition_->get_geometry()->get_FOV(),acquisition_->get_geometry()->get_SDD(),acquisition_->get_geometry()->get_SAD(),accumulate);

            input_ptr += input_view.get_number_of_elements();
            output_ptr += output_view.get_number_of_elements();

        }
        if (weights)
            *tmp_out *= *weights;

        if (accumulate) {
            *output += *tmp_out;
            delete tmp_out;
        }

    }

    template<template<class> class ARRAY>
    void CTProjectionOperator<ARRAY>::mult_MH(ARRAY<float> *input,
                                              ARRAY<float> *output, bool accumulate) {

        ARRAY<float> *tmp_in = input;
        if (weights) {
            tmp_in = new ARRAY<float>(input);
            *tmp_in *= *weights;
        }
        auto dims = *output->get_dimensions();
        std::vector<size_t> dims3d = dims;
        if (dims3d.size() == 4) dims3d.pop_back();

        auto indims = *input->get_dimensions();
        std::vector<size_t> inbindims(indims);
        float *input_ptr = tmp_in->get_data_ptr();
        float *output_ptr = output->get_data_ptr();
        if (!accumulate)
            clear(output);
        for (int bin = 0; bin < binning->get_number_of_bins(); bin++) {
            //Check for empty bins
            if (binning->get_bin(bin).size() == 0)
                continue;

            ARRAY<float> output_view(dims3d, output_ptr);
            inbindims.back() = detector_focal_cyls[bin].size();
            auto input_view = ARRAY<float>(inbindims, input_ptr);

            vector_td<int, 3> is_dims_in_pixels{dims3d[0], dims3d[1], dims3d[2]};

            ct_backwards_projection(&input_view, &output_view, detector_focal_cyls[bin], focal_offset_cyls[bin],
                                    central_elements[bin], proj_indices[bin], is_dims_in_mm, ps_spacing, ADD,
                                    accumulate);
            input_ptr += input_view.get_number_of_elements();
            output_ptr += output_view.get_number_of_elements();
        }

        if (weights) {
            delete tmp_in;
        }

    }

    template<template<class> class ARRAY>
    void Gadgetron::CTProjectionOperator<ARRAY>::setup(boost::shared_ptr<CT_acquisition> acquisition,
                                                       floatd3 is_dims_in_mm) {
        std::vector<unsigned int> bins(acquisition->geometry.detectorFocalCenterAngularPosition.size());
        std::iota(bins.begin(), bins.end(), 0);
        auto tmp_binning = boost::make_shared<CBCT_binning>(std::vector<std::vector<unsigned int>>(1, bins));
        this->setup(acquisition, tmp_binning, is_dims_in_mm);
    }

    template<template<class> class ARRAY>
    std::vector<intd2> Gadgetron::CTProjectionOperator<ARRAY>::calculate_slice_indices(CT_acquisition &acquisition) {

        floatd2 detectorSize = acquisition.geometry.detectorSize;
        auto &centralElements = acquisition.geometry.detectorCentralElement;
        auto &axialPosition = acquisition.geometry.detectorFocalCenterAxialPosition;

        auto image_dims = *this->get_domain_dimensions();
        auto proj_dims = *this->get_codomain_dimensions();

        std::vector<intd2> slice_indices(image_dims[2]);
        std::fill(slice_indices.begin(), slice_indices.end(), intd2(0, 0));
        std::vector<float> start_point(centralElements.size() + 1);

        for (int i = 0; i < centralElements.size(); i++) {
            start_point[i] = axialPosition[i] + (centralElements[i][1] - float(proj_dims[1]) / 2) * detectorSize[1] -
                             detectorSize[1] * proj_dims[1] / 2;
            //std::cout << "Start point " << start_point[i] << std::endl;
        }
        start_point.back() = 2 * start_point[centralElements.size() - 1] - start_point[centralElements.size() - 2];

        int projection_start = 0;
        int projection_stop = 0;
        for (int i = 0; i < slice_indices.size(); i++) {
            float slice_start = is_dims_in_mm[2] / image_dims[2] * (i - 0.5f) - is_dims_in_mm[2] / 2;
            float slice_stop = is_dims_in_mm[2] / image_dims[2] * (i + 0.5f) - is_dims_in_mm[2] / 2;
            //std::cout << "Slice start " << slice_start << " slice end " << slice_stop << std::endl;
            while (slice_start > (start_point[projection_start] + detectorSize[1] * proj_dims[1])) {
                projection_start++;
                if (projection_start >= centralElements.size()) {
                    projection_start = centralElements.size();
                    break;
                }
            }
            while (slice_stop > start_point[projection_stop]) {

                projection_stop++;
                if (projection_stop >= centralElements.size()) {
                    projection_stop = centralElements.size();
                    break;
                }
            }

            slice_indices[i][0] = projection_start;
            slice_indices[i][1] = projection_stop;
            //std::cout << "Projection start stop " << projection_start << " " << projection_stop << " start point " << start_point[projection_start] << " " << start_point[projection_stop] << " " << detectorSize[1] << std::endl;
        }

        std::cout << "Indices size " << slice_indices.size() << std::endl;
        return slice_indices;

    }

    template<template<class> class ARRAY>
    void Gadgetron::CTProjectionOperator<ARRAY>::setup(boost::shared_ptr<CT_acquisition> acquisition,
                                                       boost::shared_ptr<ARRAY<float>> weights, floatd3 is_dims_in_mm) {
        this->weights = weights;
        setup(acquisition, is_dims_in_mm);
    }

    template<template<class> class ARRAY>
    void Gadgetron::CTProjectionOperator<ARRAY>::setup(boost::shared_ptr<CT_acquisition> acquisition,
                                                       boost::shared_ptr<CBCT_binning> binning, floatd3 is_dims_in_mm) {
        this->binning = binning;
        auto bins = binning->get_bins();
        this->is_dims_in_mm = is_dims_in_mm;

        //Variables needed for differewnt
        detector_focal_cyls = std::vector<std::vector<floatd3>>(bins.size());
        focal_offset_cyls = std::vector<std::vector<floatd3>>(bins.size());
        central_elements = std::vector<std::vector<floatd2>>(bins.size());
        proj_indices = std::vector<std::vector<intd2>>(bins.size());

        proj_indices[0] = calculate_slice_indices(*acquisition);
        CT_geometry &geometry = acquisition->geometry;
        ps_spacing = acquisition->geometry.detectorSize;
        ADD = acquisition->geometry.constantRadialDistance[0];

        if (bins.size() != 1) throw std::runtime_error("CT reconstruction does not fully support 4D data yet");
        for (size_t b = 0; b < bins.size(); b++) {
            for (auto i : bins[b]) {
                detector_focal_cyls[b].emplace_back(geometry.detectorFocalCenterAngularPosition[i],
                                                    geometry.detectorFocalRadialDistance[i],
                                                    geometry.detectorFocalCenterAxialPosition[i]);

                focal_offset_cyls[b].emplace_back(geometry.sourceAngularPositionShift[i],
                                                  geometry.sourceRadialDistanceShift[i],
                                                  geometry.sourceAxialPositionShift[i]);

                central_elements[b].push_back(geometry.detectorCentralElement[i]);
                //proj_indices[b].push_back(all_proj_indices[i]);
            }

        }
    }

    template
    class CTProjectionOperator<cuNDArray>;

    template
    class CTProjectionOperator<hoCuNDArray>;

} /* namespace Gadgetron */


#ifndef INCLUDED_objective_storage_RotamerScores_HH
#define INCLUDED_objective_storage_RotamerScores_HH

#include "scheme/util/SimpleArray.hh"
#include "scheme/util/assert.hh"
#include <boost/lexical_cast.hpp>

#include <vector>

namespace scheme { namespace objective { namespace storage {

struct Empty {};

template<
	class _Data = uint16_t,
	int _RotamerBits = 9,
	int _Divisor = -13
>
struct RotamerScore {
 	BOOST_STATIC_ASSERT( _Divisor < 0 );
 	typedef RotamerScore< _Data, _RotamerBits, _Divisor > THIS;
	typedef _Data Data;
	static const int RotamerBits = _RotamerBits;
	static const int Divisor = _Divisor;
	static const int ScoreBits = sizeof(_Data)*8 - _RotamerBits;
	static const Data one = 1;
	static const Data RotamerMask = (( one << RotamerBits ) - one );
	static const bool UseSat = false;
	static const int NSat = 0;
	static const int sizeof_sat = 0;

	Data data_;
	RotamerScore(): data_( RotamerMask ) {}
	RotamerScore( Data data ): data_(data) {}
	RotamerScore( Data rot, float score ){
		Data sdat = score * _Divisor;
		assert( sdat < ( one<<ScoreBits ) );
		assert( rot < (one<<RotamerBits) );
		data_ = rot | (sdat<<RotamerBits);
	}
	float score() const { return data2float( get_score_data() ); }
	Data  rotamer() const { return data_ & RotamerMask; }
	void  set_score( float score ){ set_score_data( float2data( score ) ); }
	void  set_rotamer( Data rot ){ assert(rot < one<<RotamerBits); data_ = (data_& (~RotamerMask)) | rot; }

	void  set_score_data( Data sd ){ assert(sd < (one<<ScoreBits)); data_ = rotamer() | ( sd << RotamerBits ); }
	Data  get_score_data() const { return data_ >> RotamerBits; }
	static float divisor() { return _Divisor; }

	static float data2float( Data data ){ return float(data)/_Divisor; }
	static Data  float2data( float f ){ return Data( f*_Divisor ); }

	bool do_i_satisfy_anything() const {
		return false;
	}

	bool operator < ( THIS const & other ) const { return data_ > other.data_; } // reverse so low score is low
	bool operator== ( THIS const & other ) const { return data_ == other.data_; }
	bool operator!= ( THIS const & other ) const { return data_ != other.data_; }
	bool operator== ( Data const & other ) const { return data_ == other; }

	bool empty() const { return data_ == RotamerMask; }

 	void set_or_merge( THIS const & other, bool force ){
 		if( other < *this || force ){
 			*this = other;
 		}
 	}

	static std::string name() {
		static std::string const name = "RotamerScore< "
			 + std::string("data_size ") + boost::lexical_cast<std::string>(sizeof(Data)) + ", "
			 + boost::lexical_cast<std::string>(THIS::RotamerBits) + ", "
			 + boost::lexical_cast<std::string>(THIS::Divisor)	 +" >";
		return name;
	}

} __attribute__((packed));
template< class Data, int RBits, int Div > int  const RotamerScore<Data,RBits,Div>::RotamerBits;
template< class Data, int RBits, int Div > int  const RotamerScore<Data,RBits,Div>::Divisor;
template< class Data, int RBits, int Div > int  const RotamerScore<Data,RBits,Div>::ScoreBits;
template< class Data, int RBits, int Div > bool const RotamerScore<Data,RBits,Div>::UseSat;
template< class Data, int RBits, int Div >
std::ostream & operator << ( std::ostream & out, RotamerScore<Data,RBits,Div> const & val ){
	out << val.rotamer() << "<" << val.score() << ">";
	return out;
}

template<class _Dat=uint8_t >
struct SatisfactionDatum {
	using Dat = _Dat;
	Dat data_;
	static int const MAXVAL = std::numeric_limits<Dat>::max();
	SatisfactionDatum() : data_(MAXVAL) {}
	SatisfactionDatum( Dat d ) : data_(d) {}
	bool empty() const { return data_ == MAXVAL; }
	bool not_empty() const { return data_ != MAXVAL; }
	int target_sat_num() const { return (int)data_; }
	int rotamer_sat_num() const { return 0; }
	void clear() { data_ = MAXVAL; }
	bool operator==(SatisfactionDatum const & o) const { return data_==o.data_; }
} __attribute__((packed));
template<class D >
std::ostream & operator << ( std::ostream & out, SatisfactionDatum<D> const & val ){
	out << (int)val.data_;
	return out;
}

template<
	class _Data = uint16_t,
	int _RotamerBits = 9,
	int _Divisor = -13,
	class _SatDatum=SatisfactionDatum<uint8_t>,
	int _NSat=2
>
struct RotamerScoreSat : public RotamerScore<_Data,_RotamerBits,_Divisor> {
	typedef RotamerScoreSat<_Data,_RotamerBits,_Divisor,_SatDatum,_NSat> THIS;
	typedef RotamerScore<_Data,_RotamerBits,_Divisor> BASE;
	typedef _Data Data;
	typedef _SatDatum SatDatum;
	static const bool UseSat = true;
	static const int RotamerBits = _RotamerBits;
	static const int Divisor = _Divisor;
	static const int NSat = _NSat;
	static const int sizeof_sat = sizeof(SatDatum);
	util::SimpleArray<NSat,SatDatum> sat_data_;
	RotamerScoreSat() : BASE() {}
	RotamerScoreSat( Data data ) : BASE( data ) {}
	RotamerScoreSat( Data rot, float score, int sat1=-1, int sat2=-1 ) : BASE(rot,score)
	{
		if( sat1 < 0 || NSat < 1 ) return;
		ALWAYS_ASSERT( sat1 < SatDatum::MAXVAL );
		sat_data_[0].data_ = (typename SatDatum::Dat)sat1;
		if( sat2 >= 0 && NSat >= 2 ){;
			ALWAYS_ASSERT( sat2 < SatDatum::MAXVAL );
			sat_data_[1].data_ = (typename SatDatum::Dat)sat2;
		}
		// std::cout << __FILE__ << ":" << __LINE__ << " " << __FUNCTION__ << " " << sat1 << "/" << sat_data_[0] << " " << sat2 << "/" << sat_data_[1] << std::endl;
	}
	static std::string name()
	{
		static std::string const name = std::string("RotamerScoreSat< dsize=")
		     + boost::lexical_cast<std::string>(sizeof(Data)) + ", "
			 + "rbit=" + boost::lexical_cast<std::string>(BASE::RotamerBits) + ", "
			 + "div=" + boost::lexical_cast<std::string>(BASE::Divisor)	 + ", "
			 + std::string("nsat=") + boost::lexical_cast<std::string>(sizeof(SatDatum)*NSat)	  +" >";
		return name;
	}
	bool do_i_satisfy_anything() const {
		for( int isat = 0; isat < NSat; ++isat ){
			if (sat_data_[isat].not_empty()){
				//return sat_data_[isat].target_sat_num();
				return true;
			}
		}
		return false;
	}
	template< class Array >
	void get_sat_groups_raw( Array & sat_groups_out ) const {
		for( int isat = 0; isat < NSat; ++isat ){
			sat_groups_out[isat] = sat_data_[isat].target_sat_num();
		}
	}
	void get_sat_groups( std::vector<int> & sat_groups_out ) const
	{
		for( int isat = 0; isat < NSat; ++isat ){
			if( sat_data_[isat].not_empty() ){
				sat_groups_out.push_back( sat_data_[isat].target_sat_num() );
				// std::cout << "Sat " << isat << " " << sat_data_[isat].target_sat_num() << std::endl;
			} else {
				// std::cout << "Sat " << isat << " Empty!!" << std::endl;
			}
		}
	}

    int get_requirement_num( ) const {
        if( NSat >= 1 && sat_data_[0].not_empty() ){
            return sat_data_[0].target_sat_num();
        } else {
            return -1;
        }
    }

	void mark_sat_groups( std::vector<bool> & sat_groups_mask ) const {
		for( int isat = 0; isat < NSat; ++isat ){
			if( sat_data_[isat].not_empty() ){
				int val = sat_data_[isat].target_sat_num();
				if (val < 0 || val >= sat_groups_mask.size()) {
					//std::cout << "Sat" << val << std::endl;
                    return;
				}
				sat_groups_mask[ sat_data_[isat].target_sat_num() ] = true;
			}
		}
	}
	void clear_sat_groups( ) {
		for( int isat = 0; isat < NSat; ++isat ){
			sat_data_[isat].clear();
		}
	}
	bool is_new_sat( SatDatum const & sd ) const
	{
		if( sd.empty() ) return false;
		for( int i = 0; i < NSat; ++i ){
			if( sat_data_[i] == sd ) return false;
		}
		return true;
	}
	void set_or_merge( THIS const & other, bool force )
	{
		if( this->empty() || force ){
			*this = other;
		} else {
			// merge sat data iff same rotamer, mostly for bounding grids
			if( other.rotamer() == this->rotamer() ){
				int osat = 0;
				for( int isat = 0; isat < NSat; ++isat ){
					if( sat_data_[isat].empty() ){
						while( osat < NSat && ! is_new_sat(other.sat_data_[osat]) ) ++osat;
						if( osat >= NSat ) break;
						sat_data_[isat] = other.sat_data_[osat];
						++osat;
					}
				}
			}
			BASE::set_or_merge( other, force );
		}
	}

	bool operator==(THIS const & o) const { return this->data_==o.data_ && this->sat_data_==o.sat_data_; }
	bool operator!=(THIS const & o) const { return this->data_!=o.data_ || this->sat_data_!=o.sat_data_; }
	bool operator < ( THIS const & other ) const {
		int nsat=0,onsat=0;
		for( int i = 0; i < NSat; ++i ){
			 nsat += this->sat_data_[i].not_empty();
			onsat += other.sat_data_[i].not_empty();
		}
		if( nsat == onsat ) return this->data_ > other.data_;
		return nsat > onsat;
	} // reverse so low score is low

} __attribute__((packed));
template< class Data, int RBits, int Div, class Sat, int N > int  const RotamerScoreSat<Data,RBits,Div,Sat,N>::RotamerBits;
template< class Data, int RBits, int Div, class Sat, int N > int  const RotamerScoreSat<Data,RBits,Div,Sat,N>::Divisor;
template< class Data, int RBits, int Div, class Sat, int N > bool const RotamerScoreSat<Data,RBits,Div,Sat,N>::UseSat;
template< class Data, int RBits, int Div, class Sat, int N > int  const RotamerScoreSat<Data,RBits,Div,Sat,N>::NSat;
template< class Data, int RBits, int Div, class Sat, int N >
std::ostream & operator << ( std::ostream & out, RotamerScoreSat<Data,RBits,Div,Sat,N> const & val ){
	out << val.rotamer() << "<" << val.score() << ">";
	std::vector<int> sat;
	val.get_sat_groups(sat);
	for( int j = 0; j < sat.size(); ++j ){
		out << "," << sat[j];
	}
	out << "  ";

	return out;
}

template<
	int _N,
	class _RotamerScore = RotamerScore<>
>
struct RotamerScores {
	BOOST_STATIC_ASSERT(( _N > 0   ));
	BOOST_STATIC_ASSERT(( _N < 256 )); // arbitrary

	typedef _RotamerScore RotScore;
	typedef typename RotScore::Data Data;
	typedef RotamerScores< _N, RotScore > THIS;

	static int const N = _N;
	util::SimpleArray<N,RotScore> rotscores_;

	RotamerScores(){
		rotscores_.fill( RotScore::RotamerMask );
	}

	// void super_print( std::ostream & out, shared_ptr< RotamerIndex > rot_index_p ) const {
	// 	for( int i = 0; i < N; ++i ){
	// 		if ( rotscores_[i].empty()) break;

	// 		uint64_t irot = rotscores_[i].rotamer();

	// 		out << rot_index_p->resname( irot ) << " " << rotscores_[i].score() << std::endl;
	// 		// rotscores_[i].super_print( out, rot_index_p );
	// 	}
	// }
	// void add_rotamer( Data rot, float score ){
		// add_rotamer( RotScore(rot,score) );
	// }
	void add_rotamer( Data rot, float score, int sat1=-1, int sat2=-1, bool force=false )	{
		add_rotamer_impl< RotScore::UseSat >( rot, score, sat1, sat2, force );
	}
	void rotamer_sat_groups( int irot, std::vector<int> & sat_groups_out ) const	{
		rotamer_sat_groups_impl< RotScore::UseSat >( irot, sat_groups_out );
	}
	void mark_sat_groups( int irot, std::vector<bool> & sat_groups_mask ) const	{
		mark_sat_groups_impl< RotScore::UseSat >( irot, sat_groups_mask );
	}
	template<class Array>
	void get_sat_groups_raw( int irot, Array & a ) const	{
		get_sat_groups_raw_impl< RotScore::UseSat, Array >( irot, a );
	}
    int get_requirement_num( int irot ) const {
        return get_requirement_num_impl< RotScore::UseSat >( irot );
    }
	void add_rotamer( RotScore to_insert, bool force )
	{
		Data irot = to_insert.rotamer();
		int insert_pos = 0;
		RotScore worst( std::numeric_limits<Data>::max() );
		for( int i = 0; i < N; ++i ){
			// std::cout << " iter " << i << " " << rotscores_[i].score() << " " << rotscores_[i].rotamer() << " "
			          // << "cur " << rotscores_[i].data_ << " low " << worst.data_ << std::endl;
			// if rot already stored, this is the position we check
			if( rotscores_[i].rotamer() == irot ){
				insert_pos = i;
				// std::cout << "rotamer equal at " << i << std::endl;
				break;
			}
			// else we take the worst position
			if( worst < rotscores_[i] ){
				// std::cout << "worst is " << i << std::endl;
				worst = rotscores_[i];
				insert_pos = i;
			}
		}
		// std::cout << "insert_pos " << insert_pos << std::endl;
		// now insert if new val is better than worst stored val
		rotscores_[insert_pos].set_or_merge( to_insert, force );
	}
	template<int N2>
	void merge( RotamerScores<N2,RotScore> const & other, bool force=false )
	{
		for( int i = 0; i < N2; ++i ){
			if( other.empty(i) ) break;
			add_rotamer( other.rotscores_[i], force );
		}
	}
	float score_of_rotamer( int irot ) const
	{
		for( int i = 0; i < N; ++i ){
			if( rotscores_[i].rotamer() == irot ){
				return rotscores_[i].score();
			}
		}
		return 0.0f;
	}

	float score( int i ) const { assert(i<N); return rotscores_[i].score(); }
	Data rotamer( int i ) const { assert(i<N); return rotscores_[i].rotamer(); }
	bool do_i_satisfy_anything(int i) const { assert(i<N); return rotscores_[i].do_i_satisfy_anything(); }
	void clear_sats() {
		for ( int i = 0; i < N; ++i ) {
			clear_sat_groups_impl<RotScore::UseSat>( i );
		}
	}

	bool empty( int i ) const { return rotscores_[i].empty(); }

	static int maxsize(){ return _N; }

	int size() const { int i; for(i=0;i<_N;++i) if( rotscores_[i].empty() ) break; return i; }

	void sort_rotamers(){
		std::sort( rotscores_.begin(), rotscores_.end() );
	}

	bool is_sorted() const {
		for( int i = 1; i < _N; ++i )
			if( rotscores_[i] < rotscores_[i-1] )
				return false;
		return true;
	}

	int count_these_irots( int irot_low, int irot_high ) {
		int count = 0;
		for( int i = 0; i < N; ++i ){
			int rotamer = rotscores_[i].rotamer();
			if (rotamer < irot_low) continue;
			if (rotamer > irot_high) continue;
			count++;
		}
		return count;
	}

	static std::string name() {
		static std::string const name = std::string("RotamerScores< N=" )
		     + boost::lexical_cast<std::string>(_N) + ", "
			 + RotScore::name()	 +" >";
		return name;
	}

	bool operator==(THIS const & o) const { return rotscores_ == o.rotscores_; }
	bool operator!=(THIS const & o) const { return rotscores_ != o.rotscores_; }

	template< bool UseSat >	typename boost::enable_if_c< UseSat, void >::type
	add_rotamer_impl( Data rot, float score, int sat1, int sat2, bool force ){ add_rotamer( RotScore(rot,score,sat1,sat2), force ); }
	template< bool UseSat >	typename boost::disable_if_c< UseSat, void >::type
	add_rotamer_impl( Data rot, float score, int sat1, int sat2, bool force ){ add_rotamer( RotScore(rot,score), force ); }

	template< bool UseSat >	typename boost::enable_if_c< UseSat, void >::type
	rotamer_sat_groups_impl( int irot, std::vector<int> & sat_groups_out ) const { rotscores_[irot].get_sat_groups( sat_groups_out ); }
	template< bool UseSat >	typename boost::disable_if_c< UseSat, void >::type
	rotamer_sat_groups_impl( int irot, std::vector<int> & sat_groups_out ) const { return; }

	template< bool UseSat >	typename boost::enable_if_c< UseSat, void >::type
	mark_sat_groups_impl( int irot, std::vector<bool> & sat_groups_mask ) const { rotscores_[irot].mark_sat_groups( sat_groups_mask ); }
	template< bool UseSat >	typename boost::disable_if_c< UseSat, void >::type
	mark_sat_groups_impl( int irot, std::vector<bool> & sat_groups_mask ) const { return; }

	template< bool UseSat >	typename boost::enable_if_c< UseSat, void >::type
	clear_sat_groups_impl( int irot ) { rotscores_[irot].clear_sat_groups(); }
	template< bool UseSat >	typename boost::disable_if_c< UseSat, void >::type
	clear_sat_groups_impl( int irot ) { return; }

	template< bool UseSat, class Array > typename boost::enable_if_c< UseSat, void >::type
	get_sat_groups_raw_impl( int irot, Array & a ) const { rotscores_[irot].get_sat_groups_raw( a ); }
	template< bool UseSat, class Array > typename boost::disable_if_c< UseSat, void >::type
	get_sat_groups_raw_impl( int irot, Array &   ) const { return; }

    template< bool UseSat > typename boost::enable_if_c< UseSat, int >::type
    get_requirement_num_impl( int irot ) const { return rotscores_[irot].get_requirement_num( ); }
    template< bool UseSat > typename boost::disable_if_c< UseSat, int >::type
    get_requirement_num_impl( int irot ) const { return -1; }

};

template< int N, class R >
std::ostream & operator << ( std::ostream & out, RotamerScores<N,R> const & val ){
	out << val.name() << "( ";
	for(int i = 0; i < val.size(); ++i){
		out << val.rotscores_[i] << " ";
	}
	out << ")";
	return out;
}

}}}

#endif


#pragma once
#include <Eigen/Core>

namespace Legolas{

// template <class BLOCK>
// struct DynamicArrayTraits{
// };

// template <class SCALAR_TYPE, int PACK_SIZE>
// struct DynamicArrayTraits<Eigen::Array<

// template <class SCALAR_TYPE, int PACK_SIZE>

template <class BLOCK, int modulo>
struct EigenBlockMapTraits{
  typedef Eigen::Map<BLOCK> Map;
  typedef typename BLOCK::Scalar Scalar;
};

template <class BLOCK>
struct EigenBlockMapTraits<BLOCK,0>{
  //  typedef Eigen::Map<BLOCK,Eigen::Aligned> Map;
  typedef Eigen::Map<BLOCK,Eigen::Aligned32> Map;
};

//16 pour SSE et 32 pour AVX
//template <class BLOCK, int BYTE=16>
template <class BLOCK, int BYTE=32>
struct EigenBlockMap{
  static const int modulo=sizeof(BLOCK)%BYTE;
  typedef typename EigenBlockMapTraits<BLOCK,modulo>::Map Map;

  typedef typename BLOCK::Scalar SCALAR_TYPE;
  typedef Eigen::Array<SCALAR_TYPE,Eigen::Dynamic,1> EigenArray;
  typedef typename EigenBlockMapTraits<EigenArray,modulo>::Map EigenArrayView;
  typedef typename EigenBlockMapTraits<const EigenArray,modulo>::Map ConstEigenArrayView;

};

// template <class SCALAR_TYPE>
// struct EigenInterface{
// };

template <class SCALAR_TYPE, int PACK_SIZE>
struct EigenInterface{

  static const int packSize_=PACK_SIZE;
  static inline int shift(int i){return i*packSize_;}

  typedef Eigen::Matrix<SCALAR_TYPE,packSize_, 1> BlockVector;
  typedef typename EigenBlockMap<BlockVector>::Map BlockVectorView;
  typedef typename EigenBlockMap<const BlockVector>::Map ConstBlockVectorView;

  typedef Eigen::Array<SCALAR_TYPE,packSize_, 1> BlockArray;
  typedef typename EigenBlockMap<BlockArray>::Map BlockArrayView;
  typedef typename EigenBlockMap<const BlockArray>::Map ConstBlockArrayView;

  typedef Eigen::Matrix<SCALAR_TYPE,packSize_,packSize_> BlockMatrix;
  typedef typename EigenBlockMap<BlockMatrix>::Map BlockMatrixView;
  typedef typename EigenBlockMap<const BlockMatrix>::Map ConstBlockMatrixView;

  typedef Eigen::Array<SCALAR_TYPE,Eigen::Dynamic,1>     EigenVector;//deprecated
  typedef Eigen::Array<SCALAR_TYPE,Eigen::Dynamic,1>     EigenArray;
  typedef Eigen::Matrix<SCALAR_TYPE,Eigen::Dynamic,1>    EigenMatrix;

  typedef typename EigenBlockMap<BlockArray>::EigenArrayView EigenArrayView;
  typedef typename EigenBlockMap<BlockArray>::ConstEigenArrayView ConstEigenArrayView;

};

  // template <>
  // struct EigenInterface<double>{
  //   static const int packSize_=2;

  //   static inline int shift(int i){return i*packSize_;}

  //   typedef Eigen::Matrix<double,packSize_, 1> BlockVector;
  //   typedef typename EigenBlockMap<BlockVector>::Map BlockVectorView;
  //   typedef typename EigenBlockMap<const BlockVector>::Map ConstBlockVectorView;

  //   typedef Eigen::Array<double,packSize_, 1> BlockArray;
  //   typedef typename EigenBlockMap<BlockArray>::Map BlockArrayView;
  //   typedef typename EigenBlockMap<const BlockArray>::Map ConstBlockArrayView;

  //   typedef Eigen::Matrix<double,packSize_,packSize_> BlockMatrix;
  //   typedef typename EigenBlockMap<BlockMatrix>::Map BlockMatrixView;
  //   typedef typename EigenBlockMap<const BlockMatrix>::Map ConstBlockMatrixView;

  //   typedef Eigen::Array<double,Eigen::Dynamic,1>     EigenVector;
  //   typedef Eigen::Matrix<double,Eigen::Dynamic,1>    EigenMatrix;

  // };
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#ifndef BOOST_SIMD_FUNCTION_SCALAR_ASINPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_ASINPI_HPP_INCLUDED

#include <boost/simd/function/definition/asinpi.hpp>
#include <boost/simd/arch/common/scalar/function/asinpi.hpp>

#endif


 /*
  Copyright (c) 2016, Bart Vermeulen
  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 OWNER 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 "Point.hpp"
#include <CGAL/basic.h>
#include <CGAL/Search_traits.h>
#include <CGAL/Orthogonal_incremental_neighbor_search.h>
#include <CGAL/iterator.h>
#include "mex.h"
#include <math.h>
#include <algorithm>
#include <iterator>
#include <Eigen/Dense>
#include <thread>
#include <functional>
#include <future>
#include <chrono>

using namespace std;

extern void main_();

typedef CGAL::Search_traits<double, Point, Point::Cit, Construct_coord_iterator> Traits; // search traits
typedef CGAL::Orthogonal_incremental_neighbor_search<Traits> K_inc_neighbor_search; // incremental searcher
typedef K_inc_neighbor_search::Tree Tree; // The tree
typedef K_inc_neighbor_search::iterator Piterator; //The search iterator
typedef K_inc_neighbor_search::Point_with_transformed_distance P_with_dist; //Return type of searcher
struct Point_rw_not_zero{
	bool operator() (const Piterator& it){
		return it->first.rw()==0; //functor to exclude points with rw == 0
	}
};
typedef CGAL::Filter_iterator< Piterator, Point_rw_not_zero > P_pos_rw_it; // construct filtered iterator

void loess(vector< Point > &,const vector< Point >&, vector< double > &, mwSize, mwSize, mwSize, mwSize);

typedef std::vector< P_with_dist >::const_iterator regpoints_iit;
typedef std::back_insert_iterator< std::vector< double > > weights_oit;
void triCube(regpoints_iit, regpoints_iit, weights_oit);

void biCube(Tree const & tree, const vector< double > &);

void localFit( Tree const &, vector< Point > const & qp,mwSize q, vector< double > &, mwSize, mwSize, double &);
double median(vector< double >);

void mexFunction(int nlhs,mxArray *plhs[],int nrhs, const mxArray * prhs[]){
   	mwSize nin,nd,nout,q,niter=0, order=1, nthreads=thread::hardware_concurrency() ;
	double * span=0, *x,*v,*xi,*vi;
	if (nrhs < 4)
		mexErrMsgIdAndTxt("loess:nargin","At least 4 arguments are required");
	// Input checking: x, v, xi
	nin=mxGetM( prhs[0] );
	nd=mxGetN( prhs[0] );
	nout=mxGetM( prhs[2] );
	if ( !( mxGetNumberOfDimensions( prhs[0] )==2 && mxGetNumberOfDimensions( prhs[1] )==2 && mxGetNumberOfDimensions( prhs[2] ) ==2 ) )
		mexErrMsgIdAndTxt("loess:notArray","Inputs should not have more than two dimensions");
	if ( nin != static_cast<mwSize>( mxGetM( prhs[1] ) ) )
		mexErrMsgIdAndTxt("loess:nin","Second input (values) should have same number of rows as first input (locations)");
	if ( nd != static_cast<mwSize>( mxGetN(prhs[2] ) ) )
		mexErrMsgIdAndTxt("loess:nd","Third input (query points) should have same number of columns as first input (locations)");
	if ( mxGetN( prhs[1] ) != 1)
		mexErrMsgIdAndTxt("loess:notColumnV","Second input should be a column vector");
	if ( !( mxIsDouble( prhs[0] ) && mxIsDouble( prhs[1] ) && mxIsDouble( prhs[2] ) && mxIsDouble( prhs[3] )  ) )
		mexErrMsgIdAndTxt("loess:isdouble","All input arguments should be of type Double");
	if ( mxGetNumberOfElements( prhs[3] ) != 1 )
		mexErrMsgIdAndTxt("loess:spanNotScalar","Fourth argument should be a scalar");
	// if ( nin < min_dat )
	// 	mexErrMsgIdAndTxt("loess:smallInput","At least 3 rows required in first input");
	//Read required inputs
	span = mxGetPr( prhs[3] );
	x = mxGetPr( prhs[0] );
	v = mxGetPr( prhs[1] );
	xi = mxGetPr( prhs [2] );

	if (nrhs > 4){
		if ( mxGetNumberOfElements( prhs[4] ) != 1)
			mexErrMsgIdAndTxt("loess:niterNotScalar","Fifth argument should be a scalar");
		if ( floor( * mxGetPr( prhs[4] )) != * mxGetPr( prhs[4] ) )
			mexErrMsgIdAndTxt("loess:niterNotInteger","Fifth argument should be an integer");
		if ( * mxGetPr( prhs[4] ) < 0 )
			mexErrMsgIdAndTxt("loess:niterNegative","Fifth argument should be positive");
		niter=static_cast< mwSize >(* mxGetPr( prhs[4] ) );
	}

	if (nrhs > 5){
		if ( floor( * mxGetPr( prhs[5] )) != * mxGetPr( prhs[5] ) )
			mexErrMsgIdAndTxt("loess:orderNotInteger","Sixth argument should be an integer");
		if ( mxGetNumberOfElements( prhs[5] ) != 1 )
			mexErrMsgIdAndTxt("loess:orderNotScalar","Sixth argument should be a scalar");
		if ( * mxGetPr( prhs[5] ) !=1 &&  * mxGetPr( prhs[5] ) !=2)
			mexErrMsgIdAndTxt("loess:orderNotOneOrTwo","Sixth argument should be equal to one or two");
		order = static_cast< mwSize >(* mxGetPr( prhs[5] ) );
	}

	if (nrhs > 6){
		if ( floor( * mxGetPr( prhs[6] )) != * mxGetPr( prhs[6] ) )
			mexErrMsgIdAndTxt("loess:orderNotInteger","Seventh argument (nthreads) should be an integer");
		if ( mxGetNumberOfElements( prhs[6] ) != 1 )
			mexErrMsgIdAndTxt("loess:orderNotScalar","Seventh argument (nthreads) should be a scalar");
		if ( * mxGetPr( prhs[6] ) < 1 ){
			mexWarnMsgIdAndTxt("loess:orderNotOneOrTwo","Using one thread instead!");
			nthreads=1;
		}
		else if ( * mxGetPr( prhs[6] ) > thread::hardware_concurrency() ){
			mexWarnMsgIdAndTxt("loess:orderNotOneOrTwo","Using number of available threads instead!");
			nthreads = thread::hardware_concurrency();
		}
		else
			nthreads = static_cast< unsigned int >(* mxGetPr( prhs[6] ) );
	}

	//Make output variable
	plhs[0]=mxCreateDoubleMatrix(nout,1, mxREAL);
	vi=mxGetPr( plhs[0] );

	//Transfor vars into vectors
	vector< Point > inpoints, outpoints;
	vector< double > valsout;
	//inpoints.resize(nin,Point(nd,0));
	outpoints.resize(nout,Point(nd,0));
	valsout.resize(nout,std::numeric_limits<double>::quiet_NaN() );
	for (mwSize cin=0; cin < nin; cin++){
		if (!std::isfinite(v[cin]))
			continue;
		Point tmpPoint(nd,0);
		tmpPoint.val(v[cin]);
		bool point_is_finite=true;
		for (mwSize cd=0; (cd < nd) & point_is_finite; cd++){
			if (!std::isfinite(x[cd*nin+cin])){
				point_is_finite=false;
				continue;
			}
			tmpPoint[cd]=(x[cd*nin+cin]);
		}
		if (point_is_finite)
			inpoints.push_back(tmpPoint);
	}
	for (mwSize co=0; co<nout; co++){
		for (mwSize cd=0; cd < nd; cd++) {
			outpoints[co][cd]=xi[cd*nout+co];
		}
	}

	//Make span
	if (*span > 1)
		q=static_cast<mwSize>(round(*span));
	else
		q=static_cast<mwSize>(round(*span * static_cast<double> (nin)));

	q=max(static_cast<mwSize>(3),min(nin,q));

	//Perform computation
	loess(inpoints, outpoints, valsout, q, niter, order, nthreads);

	// Copy output
	for (mwSize co=0; co<nout; co++){
		vi[co]=valsout[co];
	}
}

void loess(vector< Point > & inpoints,const vector < Point > & outpoints, vector <double> & valsout, mwSize q, mwSize niter, mwSize order, mwSize nthreads){
	// Build search tree
	Tree tree(inpoints.begin(), inpoints.end());

	// Perform regression on input data
	double prog_lf(0);
	vector<double> vals_reg=vector<double>(inpoints.size(),0); // holds regression results at input locations
	double frac_riter = double(inpoints.size())/double(inpoints.size()*niter+outpoints.size());
	double frac_interp = double(outpoints.size())/double(inpoints.size()*niter+outpoints.size());
	double prog(0);
	std::cout << std::fixed << std::setw(10) << std::setprecision(2);
	for (mwSize citer=0; citer < niter; citer++) {// robust iterations
	    prog_lf=0;
	    std::future< void > f = std::async(std::launch::async, localFit, std::ref(tree), std::ref(inpoints),q,std::ref(vals_reg), order, nthreads, std::ref(prog_lf));
		std::future_status status;
	    do {
	    	status = f.wait_for(std::chrono::seconds(1));
	    	prog = (double(citer) + prog_lf) * frac_riter;
                //mexPrintf("\r%10.2f%%", prog*100);
	    	//std::cout << "\r" << prog*100 << "%" << std::flush;
	    } while (status != std::future_status::ready);
	    biCube(tree,vals_reg); // Compute robust weights (This is not included in computation of progress)
	}

	// Perform regression on output points
	prog_lf = 0;
	std::future< void > f = std::async(std::launch::async, localFit, std::ref(tree), std::ref(outpoints),q,std::ref(valsout),order, nthreads, std::ref(prog_lf));
	std::future_status status;
    do {
    	status = f.wait_for(std::chrono::seconds(1));
    	prog = prog_lf * frac_interp + double(niter) * frac_riter;
        //mexPrintf("\r%10.2f%%", prog*100);
    	//std::cout << "\r" << prog*100 << "%" << std::flush;
    } while (status != std::future_status::ready);
    //std::cout << "\rDone.      " << std::endl;
}

void triCube(regpoints_iit pd_begin, regpoints_iit pd_end,  weights_oit w_it) {
	// Computes regression weights
	double arg=0;
	for (auto pd_it=pd_begin; pd_it != pd_end; pd_it++){
		arg=pd_it->second / (pd_end-1)->second;
		*w_it = pd_it->first.rw() * ( (arg < 1) ? pow(1 - pow(arg,1.5),1.5) : 0); // This is tricube, but distances are given squared, and we want to use the square of the weights in weighted regression hence twice the 1.5 exponent
		++w_it;
	}
}

void biCube(Tree const & tree, const vector<double> & vals_reg ){
	// biCube function for the computation of robust weights from residuals of the fit
	double arg=0;
	// calculate residuals
	vector<double> res=vector<double>(tree.size(),0);
	for (std::size_t cp=0; cp<static_cast<std::size_t>(tree.size()); cp++)
		res[cp]=abs((tree.begin()+cp)->val()-vals_reg[cp]);
	double sixmres=6*median(res);
	auto tit=tree.begin();
	for (std::size_t cp=0; cp<static_cast<std::size_t>(tree.size()); cp++){
		arg=res[cp]/sixmres;
		const_cast<Point *> (&(*tit))->rw( (arg < 1) ? 1-pow(arg,2) : 0 ); //const_cast shouldn't do any harm here. Second square is removed since we use square root of weights in regression
		tit++;
	}
}

void parFit(const Tree & tree, vector< Point >::const_iterator qp_begin, vector< Point >::const_iterator qp_end, vector< double >::iterator val_beg, mwSize q, mwSize n, mwSize order, double & prog){
// Performs the actual local regression
// Input:
// 		Tree:      the spatial search tree (not modified)
// 		qp_begin:  begin iterator of query points (not modified)
// 		qp_end:	   end iterator of query points (not modified)
// 		val_begin: begin iterator of values (modified to hold output of regression)
// 		q:         number of points for regression (not modified)
// 		n:		   number of terms in regression (not modified)
// 		order:	   order of regression (not modified)
// 		prog:	   to keep track of progress, between 0 and 1 (modified)
	// Initialize variable
	mwSize ndims=qp_begin->dims();      // number of dimensions

	// Search for N-nearest neighbors and perform regression
	for (auto qp_it = qp_begin; qp_it != qp_end; qp_it++){ // loop over all query points
		bool point_is_finite=true;
		for (auto c_it = qp_it->begin(); c_it != qp_it->end(); c_it++)
			if (!std::isfinite(*c_it))
				point_is_finite=false;
		if (!point_is_finite){
			++val_beg;
			continue;
		}
		K_inc_neighbor_search ins (tree,*qp_it); //Create incremental searcher
		P_pos_rw_it it(ins.end(), Point_rw_not_zero(), ins.begin()), end(ins.end(), Point_rw_not_zero()); //filtered iterator to exclude non-finite values and nan-values
		vector< P_with_dist > regpoints; // holds nearest neighbor search results

		// Copy N-nearest points to current query point
		for (mwSize cc=0; cc < q && it!=end; cc++){
			regpoints.push_back(*it);// store filtered nearest neighbors
			it++;
		}

		if (regpoints.size() < n){
			++val_beg;
			continue;
		}
		// Compute weights for regression
		vector< double > w; 				// hold regression weights
		triCube(regpoints.cbegin(), regpoints.cend(), std::back_inserter(w));

		// Eigen matrices for regression
		Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A(regpoints.size(),n); // Regression matrix
		Eigen::Matrix<double, Eigen::Dynamic, 1> x(n,1);		      // Regression result vector
		Eigen::Matrix<double, Eigen::Dynamic, 1> b(regpoints.size(),1);              // Known values for regression
		// Fill EIGEN matrices and perform regression
		for (mwSize cc=0; cc < regpoints.size(); cc++) { // loop over points to regress
			A(cc,0)=w[cc]; // Term 1: Intercept, i.e. 1
			b(cc,0)=regpoints[cc].first.val()*w[cc]; // Known value
			for (mwSize cd=0; cd < ndims ; cd++)  // Regression terms (1st order)
				A(cc,cd+1)=w[cc]*(regpoints[cc].first[cd]-(*qp_it)[cd]); // Terms are centered coordinates
			if (order==2){ // Quadratic terms (2nd order), i.e. all cross products
				mwSize cpos=ndims+1; // Term number in matrix (column number)
				for (mwSize cd1=0; cd1<ndims; cd1++) // Loop over dimension
					for (mwSize cd2=cd1; cd2 < ndims; cd2++ ) // Loop from outer loop dimension to number of dimensions
						A(cc, cpos++)=w[cc]*((regpoints[cc].first[cd1]-(*qp_it)[cd1]) * (regpoints[cc].first[cd2]-(*qp_it)[cd2])); // Assign cross-products (centered and weighed)
			}
		}
		x=A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b); // Perform weighted least squares regression. Note the weights get squared in here
		* val_beg = x(0,0); // store result
		++val_beg;	// Increase pointer to output values
		prog = double(std::distance(qp_begin,qp_it))/double(std::distance(qp_begin,qp_end)); // keep track of progress
	}
	prog = 1;
}

void localFit(const Tree & tree, const vector< Point > & qp, mwSize q, vector<double> & val, mwSize order, mwSize nthreads, double & prog) {
	// This function controls the local fitting by calling the function parFit in separate computational threads.
	// Inputs are:
	// 	tree:     The Spatial search tree (not modified)
	// 	qp:       Query points (not modified)
	// 	q:	      Number of points in regression (not modified)
	// 	val:      Estimated function value at query points (modified)
	// 	order:    Order of the regression (not modified)
	// 	nthreads: Number of computational threads (not modified)
	// 	prog:	  Keeps track of progress (modified)

	// Compute number of input points and dimensions
	mwSize nin=qp.size();
	mwSize ndims=qp.begin()->dims();

	// Compute number of terms in regression
	// 	Linear terms
	mwSize n=ndims+1; // number of dimensions plus one
	//  Quadratic terms
	if (order == 2)
		for (mwSize cd=1; cd < ndims + 1; cd++)
			n += cd; // cross-products (1 + 2 + ... + ndims)

	mwSize usedthreads = nthreads>nin ? nin : nthreads; // Reduce the number of threads when there are less query points than threads
	// Compute number of points in each each computational thread
	vector< mwSize > n_in_thread(usedthreads,nin/usedthreads); // Number of threads is equal to the integral division of number of points and threads
	for (mwSize cr=0; cr < nin % usedthreads; cr++)         // Remaining point (modulo) are spread over the threads
		n_in_thread[cr]++;

	// Start computation in threads asynchronously
	vector<double> prog_th(usedthreads,0); 			 // Vector holding progress (between 0 and 1) of each computation thread
	std::vector< std::future < void > > all_futures; // Vector with one feature per thread
	mwSize total=0; 								 // Keeps track of points passed to previous threads
	for (mwSize cth = 0; cth<usedthreads; cth++){       // Loop to launch computations asynchronously
		all_futures.push_back( std::async( std::launch::async, parFit, std::ref(tree), qp.begin()+total, qp.begin()+total+n_in_thread[cth], val.begin()+total, q, n ,order, std::ref(prog_th[cth]) ) ); // Start thread
		total += n_in_thread[cth];	// Keep track of points already passed to the function
	}

	// Check status of threads every second
	std::future_status status;
	for (mwSize cth = 0; cth < usedthreads; cth++){ // Check status of each thread
		do {
			status = all_futures[cth].wait_for(std::chrono::seconds(1)); // Wait for 1 second, or for thread ending (whichever comes first)
			prog = std::accumulate(prog_th.begin(),prog_th.end(),0.0)/double(usedthreads); // Compute current progress of threads
		} while (status != std::future_status::ready); // Repeat loop until thread ended successfully
	}

} // end of localFit

double median(vector<double> v) // vector copied to avoid modifications on it
{
  if(v.empty()) {
    return 0.0;
  }
  auto n = v.size() / 2;
  nth_element(v.begin(), v.begin()+n, v.end());
  auto med = v[n];
  if(!(v.size() & 1)) { //If the set size is even
    auto max_it = max_element(v.begin(), v.begin()+n);
    med = (*max_it + med) / 2.0;
  }
  return med;
}


#define BOOST_TEST_MODULE communicator_reduce_scatter

#include <boost/test/included/unit_test.hpp>
#include <mpl/mpl.hpp>
#include "test_helper.hpp"

template<typename F, typename T>
bool reduce_scatter_test(F f, const T &val) {
  const mpl::communicator &comm_world{mpl::environment::comm_world()};
  T x{val};
  std::vector<T> v_x;
  mpl::contiguous_layouts<T> l;
  for (int i{0}; i < comm_world.size(); ++i) {
    const int block_size{i + 1};
    for (int j{0}; j < block_size; ++j)
      v_x.push_back(x);
    l.push_back(mpl::contiguous_layout<T>(block_size));
    ++x;
  }
  const int block_size{comm_world.rank() + 1};
  std::vector<T> v_y(block_size);
  comm_world.reduce_scatter(f, v_x.data(), v_y.data(), l);
  x = val;
  for (int i{0}; i < comm_world.rank(); ++i)
    ++x;
  T expected{x};
  for (int i{1}; i < comm_world.size(); ++i)
    expected = f(expected, x);
  std::vector<T> v_expected(block_size, expected);
  return v_y == v_expected;
}

template<typename F, typename T>
bool ireduce_scatter_test(F f, const T &val) {
  const mpl::communicator &comm_world{mpl::environment::comm_world()};
  T x{val};
  std::vector<T> v_x;
  mpl::contiguous_layouts<T> l;
  for (int i{0}; i < comm_world.size(); ++i) {
    const int block_size{i + 1};
    for (int j{0}; j < block_size; ++j)
      v_x.push_back(x);
    l.push_back(mpl::contiguous_layout<T>(block_size));
    ++x;
  }
  const int block_size{comm_world.rank() + 1};
  std::vector<T> v_y(block_size);
  auto r{comm_world.ireduce_scatter(f, v_x.data(), v_y.data(), l)};
  x = val;
  for (int i{0}; i < comm_world.rank(); ++i)
    ++x;
  T expected{x};
  for (int i{1}; i < comm_world.size(); ++i)
    expected = f(expected, x);
  std::vector<T> v_expected(block_size, expected);
  r.wait();
  return v_y == v_expected;
}

BOOST_AUTO_TEST_CASE(reduce_scatter) {
  BOOST_TEST(reduce_scatter_test(add<double>(), 1.0));
  BOOST_TEST(reduce_scatter_test(add<tuple>(), tuple{1, 2.0}));
  BOOST_TEST(reduce_scatter_test(mpl::plus<double>(), 1.0));
  BOOST_TEST(reduce_scatter_test(mpl::plus<tuple>(), tuple{1, 2.0}));
  BOOST_TEST(reduce_scatter_test([](auto a, auto b) { return a + b; }, 1.0));
  BOOST_TEST(reduce_scatter_test([](auto a, auto b) { return a + b; }, tuple{1, 2.0}));

  BOOST_TEST(ireduce_scatter_test(add<double>(), 1.0));
  BOOST_TEST(ireduce_scatter_test(add<tuple>(), tuple{1, 2.0}));
  BOOST_TEST(ireduce_scatter_test(mpl::plus<double>(), 1.0));
  BOOST_TEST(ireduce_scatter_test(mpl::plus<tuple>(), tuple{1, 2.0}));
  BOOST_TEST(ireduce_scatter_test([](auto a, auto b) { return a + b; }, 1.0));
  BOOST_TEST(ireduce_scatter_test([](auto a, auto b) { return a + b; }, tuple{1, 2.0}));
}


#ifndef TISSOT_CONVERTER_OB_TRAN_HPP
#define TISSOT_CONVERTER_OB_TRAN_HPP

// Tissot, converts projecton source code (Proj4) to Boost.Geometry
// (or potentially other source code)
//
// Copyright (c) 2015 Barend Gehrels, Amsterdam, the Netherlands.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include "tissot_structs.hpp"
#include "tissot_util.hpp"
#include "converter_base.hpp"
#include <boost/assert.hpp>

namespace boost { namespace geometry { namespace proj4converter
{

class converter_cpp_bg_ob_tran : public converter_cpp_bg_default
{
    public :
        converter_cpp_bg_ob_tran(projection_properties& prop)
            : m_prop(prop)
        {
        }

        void convert()
        {
            // ob_tran is a projection forwarding to another projection
            m_prop.template_struct = "<Geographic, Cartesian>";
            m_prop.forward_declarations = "template <typename Geographic, typename Cartesian, typename Parameters> class factory;";

            m_prop.setup_return_type = "double";
            m_prop.setup_extra_parameters = ", bool create = true";

            m_prop.setup_extra_code.push_back("detail::ob_tran::par_ob_tran<Geographic, Cartesian> proj_parm;");
            m_prop.setup_extra_code.push_back("Parameters p = par;");
            m_prop.setup_extra_code.push_back("double phip = setup_ob_tran(p, proj_parm, false);");
            m_prop.setup_extra_code.push_back("");

            BOOST_FOREACH(derived& der, m_prop.derived_projections)
            {
                std::vector<std::string>& lines = der.constructor_lines;
                BOOST_FOREACH(std::string& line, lines)
                {
                    boost::replace_all(line, "proj_parm.link->", "pj.");
                    boost::replace_all(line, "char *name, *s;", "Parameters pj;");
                    if (boost::contains(line, "so_proj"))
                    {
                        // Replace assignment completely
                        line = tab1 + "pj.name = pj_param(par.params, \"so_proj\").s;";
                    }
                }

                // Remove some lines comparing projection name
                functor_starts_with functor("int i;");
                functor.add("for (i = 0;")
                    .add("(s = pj_list[i].id")
                    .add("++i)")
                    .add("if (!s ||")
                    .add("freeup(P);")
                    .add("return 0;")
                    .add("par.inv =") // with a conditional expression
                ;
                lines.erase
                    (
                        std::remove_if(lines.begin(), lines.end(),functor),
                        lines.end()
                    );

                // Replace creation part
                {
                    functor_starts_with functor("if (!(proj_parm.link");
                    std::vector<std::string>::iterator it
                        = std::find_if(lines.begin(), lines.end(), functor);
                    if (it != lines.end())
                    {
                        std::vector<std::string> piece;

                        piece.push_back("");
                        piece.push_back(tab1 + "if (create)");
                        piece.push_back(tab1 + "{");
                        piece.push_back(tab2 + "factory<Geographic, Cartesian, Parameters> fac;");
                        piece.push_back(tab2 + "proj_parm.link.reset(fac.create_new(pj));");
                        piece.push_back(tab2 + "if (! proj_parm.link.get()) throw proj_exception(-26);");
                        // closing curly brace is already there

                        // Replace the original expression with this piece
                        lines.insert(it, piece.begin(), piece.end());

                        // Find expression again to erase it
                        it = std::find_if(lines.begin(), lines.end(), functor);
                        lines.erase(it);
                    }
                }

                lines.push_back(tab1 + "// return phip to choose model");
                lines.push_back(tab1 + "return phip;");

                if (der.models.size() == 2u)
                {
                    BOOST_ASSERT(der.models[0].name == "oblique");
                    der.models[0].condition = "if (fabs(phip) > detail::ob_tran::TOL)";

                    BOOST_ASSERT(der.models[1].name == "transverse");
                    der.models[1].condition = "else";
                }
            }

            BOOST_FOREACH(projection& proj, m_prop.projections)
            {
                functor_nospaces_equals functor("(void)xy;");
                proj.lines.erase
                    (
                        std::remove_if(proj.lines.begin(), proj.lines.end(), functor),
                        proj.lines.end()
                    );

                BOOST_FOREACH(std::string& line, proj.lines)
                {
                    boost::replace_all(line, ", this->m_proj_parm.link", "");

                    std::string s = boost::trim_copy(line);
                    if (boost::starts_with(s, "lp ="))
                    {
                        boost::replace_all(line, "this->", "");
                        boost::replace_all(line, "lp = ", "");
                        boost::replace_all(line, "xy", "xy_x, xy_y, lp_lon, lp_lat");
                    }
                    else if (boost::starts_with(s, "return (this->"))
                    {
                        boost::replace_all(line, "return (this->", "");
                        boost::replace_last(line, ")", "");
                        boost::replace_all(line, "lp", "lp_lon, lp_lat, xy_x, xy_y");
                    }
                }
            }

            BOOST_FOREACH(std::string& line, m_prop.proj_parameters)
            {
                boost::replace_all(line, "struct PJconsts *", "boost::shared_ptr<projection<Geographic, Cartesian> > ");
            }
        }

    private :

        projection_properties& m_prop;
};

}}} // namespace boost::geometry::proj4converter

#endif // TISSOT_CONVERTER_OB_TRAN_HPP