//
// Created by slist on 12/12/18.
//

#include <fileIO/loadJSON.h>
#include <Pokemon/Move.h>
#include <Pokemon/Natures.h>
#include <Pokemon/Pokemon.h>
#include <Pokemon/Stats.h>
#include <Pokemon/Status.h>
#include <Pokemon/Type.h>
#include <Utils/Logging.h>

#include <boost/property_tree/ptree.hpp>
#include <cmath>
#include <string>
#include <vector>
using namespace std;
static int MAX_STAGES = 6;

Pokemon::Pokemon()
{
    Pokemon::active = benched;
    Pokemon::alive = false;
    Pokemon::status = STATUS::NO_STATUS;
    Pokemon::substitute_hp = 0;
    Pokemon::current_hp = 0;
    Pokemon::protect_active = false;
    Pokemon::protect_turns = 0;
    for(int stat = STAT::HP; stat < STAT::NUM_STATS; stat++)
        Pokemon::stat_modifiers[stat] = 0;
    for(int i = 0; i < NUM_VOLATILE_STATUS; i++)
        Pokemon::v_status_turns[i] = 0;
    Pokemon::protect_active = false;
    Pokemon::substitute_hp = 0;
    Pokemon::has_mega = false;
}

// STATE CHECKING FUNCTIONS
bool Pokemon::is_active()
{
    return Pokemon::active == in_field;
}

bool Pokemon::is_benched()
{
    return Pokemon::active == benched;
}

bool Pokemon::is_swapping()
{
    return Pokemon::active == to_be_swapped;
}

bool Pokemon::can_mega()
{
    return Pokemon::has_mega;
}

int Pokemon::get_stat_mod(STAT stat)
{
    return Pokemon::stat_modifiers[stat];
}

float Pokemon::get_stat(STAT stat)
{
    int mod = Pokemon::get_stat_mod(stat);
    float adjustment = 1;
    float equ_const;

    if(stat == STAT::ACC || stat == STAT::EVA)
        equ_const = 3.0;
    else
        equ_const = 2.0;

    if(stat == STAT::SPE && Pokemon::status == STATUS::PARALYZED)
        adjustment *= 0.5;

    if(mod < 0)
    {
        adjustment *= equ_const / (equ_const - mod);
    }
    else if(mod > 0)
    {
        adjustment *= (equ_const + mod) / equ_const;
    }

    return Pokemon::current_stats[stat] * adjustment;
}

STATUS Pokemon::get_status()
{
    return Pokemon::status;
}

bool Pokemon::is_volatile_status(VOLATILE_STATUS v_status)
{
    return Pokemon::volatile_status & v_status;
}

int Pokemon::get_volatile_status()
{
    return Pokemon::volatile_status;
}

int Pokemon::get_level()
{
    return Pokemon::level;
}

PokeTypes* Pokemon::get_type()
{
    return Pokemon::current_type;
}

string Pokemon::get_species()
{
    if(Pokemon::is_mega)
        return "Mega " + Pokemon::species;
    else
        return Pokemon::species;
}

int Pokemon::get_v_status_turns(VOLATILE_STATUS_NUMBERS v_status)
{
    return Pokemon::v_status_turns[v_status];
}

int Pokemon::get_protect_turns()
{
    return Pokemon::protect_turns;
}

bool Pokemon::is_alive()
{
    return Pokemon::alive;
}

float Pokemon::get_current_hp()
{
    return Pokemon::current_hp;
}

bool Pokemon::is_grounded()
{
    if(Pokemon::current_type[0] == PokeTypes::FLYING || Pokemon::current_type[1] == PokeTypes::FLYING)
        return false;

    return true;
}

bool Pokemon::is_protected()
{
    return Pokemon::protect_active;
}

// STATE CHANGE FUNCTIONS

bool Pokemon::use_move(int move_number)
{
    return Pokemon::moves[move_number].use();
}

bool Pokemon::setup_substitute()
{
    if(Pokemon::current_hp <= (Pokemon::current_stats[STAT::HP] / 4) || Pokemon::substitute_hp != 0)
        return false;

    Pokemon::current_hp -= (Pokemon::current_stats[STAT::HP] / 4);
    Pokemon::substitute_hp = (Pokemon::current_stats[STAT::HP] / 4);
    return true;
}

void Pokemon::mega_evolve()
{
    float temp_current_hp = Pokemon::current_hp;
    if(!Pokemon::can_mega())
        ERR_MSG("Trying to mega evolve " << Pokemon::species << ", does not have a mega evolution" << endl);

    Pokemon::current_stats = Pokemon::mega_base_stats;
    Pokemon::current_hp = temp_current_hp;
    Pokemon::is_mega = true;
}

bool Pokemon::deal_damage(float damage, bool ignore_sub)
{
    if(Pokemon::substitute_hp > 0 && !ignore_sub)
    {
        DEBUG_MSG("Substitue took damage for " << Pokemon::species << endl);
        Pokemon::substitute_hp -= damage;
        if(Pokemon::substitute_hp <= 0)
        {
            Pokemon::substitute_hp = 0;
            DEBUG_MSG("Substitue faded\n");
        }
        return true;
    }
    DEBUG_MSG("Dealt " << round((float)damage/Pokemon::current_stats[STAT::HP]*100*10)/10 << "% damage to " << Pokemon::get_species() << "\n");
    Pokemon::current_hp -=  damage;

    if(Pokemon::current_hp <= 0)
    {
        Pokemon::current_hp = 0;
        return false;
    }
    return true;
}

void Pokemon::heal_damage(int damage)
{
    DEBUG_MSG("Healed " << round((float)damage/Pokemon::current_stats[STAT::HP]*100*10)/10 << "% hitpoints to " << Pokemon::species << "\n");

    Pokemon::current_hp = Pokemon::current_hp + damage;

    if(Pokemon::current_hp >= Pokemon::current_stats[STAT::HP])
    {
        Pokemon::current_hp = Pokemon::current_stats[STAT::HP];
    }
}

bool Pokemon::set_status(STATUS new_status)
{
    if(Pokemon::status != STATUS::NO_STATUS && new_status != STATUS::NO_STATUS)
    {
        DEBUG_MSG(Pokemon::species << " is already " << status_to_string(Pokemon::status) << " and cannot be " << status_to_string(new_status) << "\n");
        return false;
    }

    switch(new_status)
    {
        case STATUS::ASLEEP:
            Pokemon::status_turns = 0;
            break;
        case STATUS::BURNED:
            if(Pokemon::type[0] == PokeTypes::FIRE || Pokemon::type[1] == PokeTypes::FIRE)
            {
                DEBUG_MSG(Pokemon::species << " cannot be " << status_to_string(new_status) << endl);
                return false;
            }
            break;
        case STATUS::FROZEN:
            if(Pokemon::type[0] == PokeTypes::ICE || Pokemon::type[1] == PokeTypes::ICE)
            {
                DEBUG_MSG(Pokemon::species << " cannot be " << status_to_string(new_status) << endl);
                return false;
            }
            break;
        case STATUS::PARALYZED:
            if(Pokemon::type[0] == PokeTypes::ELECTRIC || Pokemon::type[1] == PokeTypes::ELECTRIC)
            {
                DEBUG_MSG(Pokemon::species << " cannot be " << status_to_string(new_status) << endl);
                return false;
            }
            break;
        case STATUS::BADLY_POISONED:
            Pokemon::status_turns = 0;
        case STATUS::POISONED:
            if(Pokemon::type[0] == PokeTypes::STEEL || Pokemon::type[1] == PokeTypes::STEEL || Pokemon::type[0] == PokeTypes::POISON || Pokemon::type[1] == PokeTypes::POISON)
            {
                DEBUG_MSG(Pokemon::species << " cannot be " << status_to_string(new_status) << endl);
                return false;
            }
            break;
        case STATUS::NO_STATUS:
            break;
        default:
            ERR_MSG("Unhandled status " << new_status << "\n");
    }

    Pokemon::status = new_status;
    return true;
}

bool Pokemon::set_volatile_status(VOLATILE_STATUS v_status)
{
    if(!Pokemon::is_volatile_status(v_status))
    {
        Pokemon::volatile_status |= v_status;
        return true;
    }
    else
    {
        DEBUG_MSG(Pokemon::species << " is already " << v_status_to_string(v_status) << "\n");
        return false;
    }
}

void Pokemon::set_active()
{
    Pokemon::active = in_field;
}

void Pokemon::set_benched()
{
    Pokemon::active = benched;
}

void Pokemon::set_swapping()
{
    Pokemon::active = to_be_swapped;
}

void Pokemon::faint_poke()
{
    Pokemon::alive = false;
}

void Pokemon::increment_v_status_turns(VOLATILE_STATUS_NUMBERS v_status)
{
    Pokemon::v_status_turns[v_status]++;
}

void Pokemon::increment_protect_turns()
{
    Pokemon::protect_turns++;
    if(Pokemon::protect_turns > 6)
        Pokemon::protect_turns = 6;
}

void Pokemon::remove_type(int type_num)
{
    Pokemon::current_type[type_num] = PokeTypes::NO_TYPE;
}

void Pokemon::protect_poke()
{
    Pokemon::protect_active = true;
}

//CLEARING FUNCTIONS

void Pokemon::clear_stat_mods()
{
    for(int i = 0; i < STAT::NUM_STATS; i++)
        Pokemon::stat_modifiers[i]  = 0;
}

void Pokemon::clear_volatile_status(VOLATILE_STATUS v_status)
{
    Pokemon::volatile_status &= ~(v_status);
}

void Pokemon::clear_volatile_status_mask(int v_status_mask)
{
    Pokemon::volatile_status &= ~(v_status_mask);
}

void Pokemon::clear_volatile_statuses()
{
    Pokemon::volatile_status = 0;
    for(int i = 0; i < NUM_VOLATILE_STATUS; i++)
        Pokemon::v_status_turns[i] = 0;
}

void Pokemon::clear_v_status_turns(VOLATILE_STATUS_NUMBERS v_status)
{
    Pokemon::v_status_turns[v_status] = 0;
}

void Pokemon::clear_protect_turns()
{
    Pokemon::protect_turns = 0;
}

void Pokemon::reset_types()
{
    Pokemon::current_type[0] = Pokemon::type[0];
    Pokemon::current_type[1] = Pokemon::type[1];
}
//

void Pokemon::stat_change(STAT stat, int stages)
{
    if(stages < 0)
        DEBUG_MSG(Pokemon::species << "'s " << stat_to_string(stat) << " dropped by " << to_string(abs(stages)) << " stage(s)\n");

    if(stages > 0)
        DEBUG_MSG(Pokemon::species << "'s " << stat_to_string(stat) << " rose by " << to_string(stages) << " stage(s)\n");

    Pokemon::stat_modifiers[stat] = Pokemon::stat_modifiers[stat] + stages;

    if(Pokemon::stat_modifiers[stat] < (-1 * MAX_STAGES))
    {
        DEBUG_MSG(Pokemon::species << "'s " << stat_to_string(stat) << " can't go lower\n");
        Pokemon::stat_modifiers[stat] = -1 * MAX_STAGES;
    }

    if(Pokemon::stat_modifiers[stat] > MAX_STAGES)
    {
        DEBUG_MSG(Pokemon::species << "'s " << stat_to_string(stat) << " can't go higher\n");
        Pokemon::stat_modifiers[stat] = MAX_STAGES;
    }
}

void Pokemon::reset_protect()
{
    Pokemon::protect_active = false;
}
//

// LOADING POKEMON FUNCTIONS

void Pokemon::reset()
{
    Pokemon::active = benched;
    for(int stat = STAT::HP; stat < STAT::NUM_STATS; stat++)
    {
        Pokemon::stat_modifiers[stat] = 0;
    }
    Pokemon::alive = true;
    Pokemon::current_stats = Pokemon::base_stats;
    Pokemon::current_hp = Pokemon::current_stats[STAT::HP];
    Pokemon::clear_volatile_statuses();
    Pokemon::status = STATUS::NO_STATUS;
    for(int i = 0; i < 4; i++)
    {
        if(Pokemon::moves[i].get_name().empty())
            break;
        else
            Pokemon::moves[i].reset();
    }
    for(int i = 0; i < NUM_VOLATILE_STATUS; i++)
        Pokemon::v_status_turns[i] = 0;

    Pokemon::reset_protect();
    Pokemon::is_mega = false;
}

void Pokemon::load_pokemon(boost::property_tree::ptree poke_ptree)
{
    Pokemon::load_species(poke_ptree.get<string>("species"));
    Pokemon::level = poke_ptree.get<int>("level", 0);

    boost::property_tree::ptree evs_ptree = poke_ptree.get_child("evs");
    boost::property_tree::ptree ivs_ptree = poke_ptree.get_child("ivs");

    for(int i = 0; i < STAT::NUM_STATS; i++)
    {
        Pokemon::evs[i] = evs_ptree.get(to_string(i), 0);
        Pokemon::ivs[i] = ivs_ptree.get(to_string(i), 0);
    }
    Pokemon::nature = string_to_nature(poke_ptree.get<string>("nature"));

    Pokemon::set_stats();
    if(Pokemon::can_mega())
        Pokemon::set_stats(true);

    Pokemon::current_stats = Pokemon::base_stats;

    boost::property_tree::ptree moves_ptree = poke_ptree.get_child("moves");

    string move_name;
    for(int i = 0; i < 4; i++)
    {
        move_name = moves_ptree.get<string>("MOVE" + to_string(i));
        if(!move_name.empty())
        {
            Pokemon::moves[i].load_move(move_name);
        }
    }

    Pokemon::alive = true;
    Pokemon::status = STATUS::NO_STATUS;
    Pokemon::status_turns = 0;
    Pokemon::volatile_status = 0;
    Pokemon::is_mega = false;
}

void Pokemon::load_species(string species_name)
{
    boost::property_tree::ptree root;
    boost::property_tree::ptree root_child;

    try
    {
        root = load_json_file("species/" + species_name + ".json");
    }
    catch(...)
    {
        ERR_MSG("ERROR: FAILED TO LOAD SPECIES " << species_name << "\n");
    }

    Pokemon::base_stats[STAT::HP]  = root.get<int>("HP", 0);
    Pokemon::base_stats[STAT::ATK] = root.get<int>("ATK", 0);
    Pokemon::base_stats[STAT::DEF] = root.get<int>("DEF", 0);
    Pokemon::base_stats[STAT::SPA] = root.get<int>("SPA", 0);
    Pokemon::base_stats[STAT::SPD] = root.get<int>("SPD", 0);
    Pokemon::base_stats[STAT::SPE] = root.get<int>("SPE", 0);

    Pokemon::species = species_name;

    root_child = root.get_child("TYPES");
    Pokemon::type[0] = string_to_type(root_child.get<string>("TYPE0"));
    Pokemon::type[1] = string_to_type(root_child.get<string>("TYPE1"));

    Pokemon::current_type[0] = Pokemon::type[0];
    Pokemon::current_type[1] = Pokemon::type[1];
    DEBUG_MSG("Loaded " << Pokemon::species << " species data" << endl);

    if(root.count("mega") == 1)
    {
        root_child = root.get_child("mega");
        Pokemon::mega_base_stats[STAT::HP]  = root.get<int>("HP", 0);
        Pokemon::mega_base_stats[STAT::ATK] = root.get<int>("ATK", 0);
        Pokemon::mega_base_stats[STAT::DEF] = root.get<int>("DEF", 0);
        Pokemon::mega_base_stats[STAT::SPA] = root.get<int>("SPA", 0);
        Pokemon::mega_base_stats[STAT::SPD] = root.get<int>("SPD", 0);
        Pokemon::mega_base_stats[STAT::SPE] = root.get<int>("SPE", 0);
        root_child = root_child.get_child("TYPES");
        Pokemon::mega_type[0] = string_to_type(root_child.get<string>("TYPE0"));
        Pokemon::mega_type[1] = string_to_type(root_child.get<string>("TYPE1"));
        Pokemon::has_mega = true;
        DEBUG_MSG("Loaded Mega " << Pokemon::species << " species data" <<  endl);
    }
}

float Pokemon::calculate_hp(int level, int base_hp, int ev_hp, int iv_hp)
{
    return floor(((2 * base_hp + iv_hp + floor((float)(ev_hp / 4))) * level) / 100) + level + 10;
}

float Pokemon::calculate_stat_single(int level, int base, int ev, int iv, float nature_mod)
{
    return (int)((floor(floor((float)((float)ev / 4) + iv + 2 * base) / 100 * level) + 5) * nature_mod);
}

void Pokemon::set_stats(bool use_mega)
{
    float* stats;
    if(use_mega)
        stats = Pokemon::mega_base_stats;
    else
        stats = Pokemon::base_stats;

    float nature_mod;
    for(int i = STAT::HP; i < STAT::NUM_STATS; i++)
    {
        switch(i)
        {
            case STAT::HP:
                stats[i] = calculate_hp(level, stats[i], Pokemon::evs[i], Pokemon::ivs[i]);
                Pokemon::current_hp = stats[i];
                break;
            case STAT::ATK:
            case STAT::DEF:
            case STAT::SPA:
            case STAT::SPD:
            case STAT::SPE:
                nature_mod = get_nature_mod(Pokemon::nature, (STAT)i);
                stats[i] = calculate_stat_single(level, stats[i], Pokemon::evs[i], Pokemon::ivs[i], nature_mod);
                break;
            case STAT::ACC:
            case STAT::EVA:
                stats[i] = 1;
                break;
            default:
                ERR_MSG("Unhandled stat " << i << endl);
        }
    }
}
//

void Pokemon::print_pokemon(bool detailed)
{
    if(Pokemon::species.empty())
        return;

    DEBUG_MSG(Pokemon::get_species() << "\n");
    if(detailed)
    {
        DEBUG_MSG("HP:  "  << Pokemon::current_stats[STAT::HP] << " Modifier: " << Pokemon::stat_modifiers[STAT::HP]  << "\n");
        DEBUG_MSG("ATK: " << Pokemon::get_stat(STAT::ATK) << " Modifier: " << Pokemon::stat_modifiers[STAT::ATK] << "\n");
        DEBUG_MSG("DEF: " << Pokemon::get_stat(STAT::DEF) << " Modifier: " << Pokemon::stat_modifiers[STAT::DEF] << "\n");
        DEBUG_MSG("SPA: " << Pokemon::get_stat(STAT::SPA) << " Modifier: " << Pokemon::stat_modifiers[STAT::SPA] << "\n");
        DEBUG_MSG("SPD: " << Pokemon::get_stat(STAT::SPD) << " Modifier: " << Pokemon::stat_modifiers[STAT::SPD] << "\n");
        DEBUG_MSG("SPE: " << Pokemon::get_stat(STAT::SPE) << " Modifier: " << Pokemon::stat_modifiers[STAT::SPE] << "\n");
        DEBUG_MSG("Level: " << Pokemon::level << "\n");
        DEBUG_MSG("ACTIVE: " << Pokemon::active << "\n");
        DEBUG_MSG("ALIVE: " << Pokemon::alive << "\n");
        Pokemon::moves[0].print_move();
        Pokemon::moves[1].print_move();
    }
    DEBUG_MSG("Current HP: " << round(Pokemon::current_hp / Pokemon::current_stats[STAT::HP] * 100 * 10) / 10 << "%\n");
    DEBUG_MSG("STATUS: " << status_to_string(Pokemon::status) << "\n");
}

// UNIT TEST HELPERS
void Pokemon::create_test_pokemon(PokeTypes t1, PokeTypes t2, Natures n, float pcent_hp, string species)
{
    Pokemon::level = 50;
    Pokemon::nature = n;
    for(int i = 0; i < STAT::NUM_STATS; i++)
    {
        Pokemon::evs[i] = 0;
        Pokemon::ivs[i] = 0;
    }

    Pokemon::alive = true;
    Pokemon::active = in_field;
    Pokemon::status = STATUS::NO_STATUS;
    Pokemon::status_turns = 0;
    Pokemon::volatile_status = 0;

    Pokemon::load_species(species);

    Pokemon::set_stats();
    Pokemon::current_stats = Pokemon::base_stats;

    Pokemon::name = "TESTEMON";

    Pokemon::type[0] = t1;
    Pokemon::type[1] = t2;

    Pokemon::current_type[0] = Pokemon::type[0];
    Pokemon::current_type[1] = Pokemon::type[1];

    Pokemon::current_hp = Pokemon::current_stats[STAT::HP] * pcent_hp / 100;
    Pokemon::protect_active = false;
    Pokemon::protect_turns = 0;
    Pokemon::is_mega = false;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "test_model_io"

#include <boost/test/unit_test.hpp>
#include "nanocv/nanocv.h"
#include "nanocv/tester.h"
#include "nanocv/logger.h"
#include "nanocv/tasks/task_synthetic_shapes.h"
#include <cstdio>

BOOST_AUTO_TEST_CASE(test_model_io)
{
        ncv::init();

        using namespace ncv;

        synthetic_shapes_task_t task(28, 28, 10, color_mode::rgba, 1000);
        BOOST_CHECK_EQUAL(task.load(""), true);

        const size_t cmd_outputs = task.osize();

        const size_t n_tests = 8;

        const string_t lmodel0;
        const string_t lmodel1 = lmodel0 + "linear:dims=100;act-snorm;";
        const string_t lmodel2 = lmodel1 + "linear:dims=100;act-snorm;";
        const string_t lmodel3 = lmodel2 + "linear:dims=100;act-snorm;";
        const string_t lmodel4 = lmodel3 + "linear:dims=100;act-snorm;";
        const string_t lmodel5 = lmodel4 + "linear:dims=100;act-snorm;";

        string_t cmodel;
        cmodel = cmodel + "conv:dims=16,rows=9,cols=9;pool-max;act-snorm;";
        cmodel = cmodel + "conv:dims=32,rows=5,cols=5;pool-max;act-snorm;";
        cmodel = cmodel + "conv:dims=64,rows=3,cols=3;act-snorm;";

        const string_t outlayer = "linear:dims=" + text::to_string(cmd_outputs) + ";";

        strings_t cmd_networks =
        {
                lmodel0 + outlayer,
                lmodel1 + outlayer,
                lmodel2 + outlayer,
                lmodel3 + outlayer,
                lmodel4 + outlayer,
                lmodel5 + outlayer,

                cmodel + outlayer
        };

        const rloss_t loss = ncv::get_losses().get("logistic");
        BOOST_CHECK_EQUAL(loss.operator bool(), true);

        for (const string_t& cmd_network : cmd_networks)
        {
                log_info() << "<<< running network [" << cmd_network << "] ...";

                // create feed-forward network
                const rmodel_t model = ncv::get_models().get("forward-network", cmd_network);
                BOOST_CHECK_EQUAL(model.operator bool(), true);
                BOOST_CHECK_EQUAL(model->resize(task, false), true);
                BOOST_CHECK_EQUAL(model->irows(), task.irows());
                BOOST_CHECK_EQUAL(model->icols(), task.icols());
                BOOST_CHECK_EQUAL(model->osize(), task.osize());
                BOOST_CHECK_EQUAL(static_cast<int>(model->color()), static_cast<int>(task.color()));

                // test random networks
                for (size_t t = 0; t < n_tests; t ++)
                {
                        model->random_params();

                        const fold_t fold = {0, protocol::test};

                        const string_t header = "test [" + text::to_string(t + 1) + "/" + text::to_string(n_tests) + "] ";
                        const string_t path = "./test_model_io.test";

                        // test error & parameters before saving
                        scalar_t lvalue_before, lerror_before;
                        const size_t lcount_before = ncv::test(task, fold, *loss, *model, lvalue_before, lerror_before);

                        vector_t params(model->psize());
                        BOOST_CHECK(model->save_params(params));

                        //
                        BOOST_CHECK_EQUAL(model->save(path), true);
                        BOOST_CHECK_EQUAL(model->load(path), true);
                        //

                        // test error & parameters after loading
                        scalar_t lvalue_after, lerror_after;
                        const size_t lcount_after = ncv::test(task, fold, *loss, *model, lvalue_after, lerror_after);

                        vector_t xparams(model->psize());
                        BOOST_CHECK(model->save_params(xparams));

                        // check
                        BOOST_CHECK_EQUAL(lcount_before, lcount_after);
                        BOOST_CHECK_EQUAL(lvalue_before, lvalue_after);
                        BOOST_CHECK_EQUAL(lerror_before, lerror_before);

                        BOOST_CHECK_EQUAL(params.size(), xparams.size());
                        BOOST_CHECK_LE((params - xparams).lpNorm<Eigen::Infinity>(), 1e-16);

                        // cleanup
                        std::remove(path.c_str());
                }
        }
}


/**
 * \file
 * \brief Base driver for a generic n-joint BLMC robot.
 * \copyright Copyright (c) 2019, New York University and Max Planck
 *            Gesellschaft.
 */

#pragma once

#include <array>
#include <cmath>
#include <iterator>
#include <string>

#include <yaml-cpp/yaml.h>
#include <Eigen/Eigen>

#include <robot_interfaces/monitored_robot_driver.hpp>
#include <robot_interfaces/n_joint_robot_types.hpp>
#include <robot_interfaces/robot_driver.hpp>
#include <yaml_utils/yaml_eigen.hpp>

#include <blmc_drivers/blmc_joint_module.hpp>
#include <robot_fingers/clamp.hpp>

namespace robot_fingers
{
/**
 * @brief Parameters related to the motor.
 */
struct MotorParameters
{
    //! @brief Torque constant K_t of the motor [Nm/A].
    double torque_constant_NmpA;

    /**
     * @brief Gear ratio between motor and joint.
     *
     * For a `n:1` ratio (i.e. one joint revolution requires n motor
     * revolutions) set this value to `n`.
     */
    double gear_ratio;
};

/**
 * @brief Base class for n-joint BLMC robots.
 *
 * This is a generic base class to easily implement drivers for simple BLMC
 * robots that consist of N_JOINTS joints.
 *
 * @tparam N_JOINTS Number of joints.
 * @tparam N_MOTOR_BOARDS Number of motor control boards that are used.
 */
template <typename Observation, size_t N_JOINTS, size_t N_MOTOR_BOARDS>
class NJointBlmcRobotDriver
    : public robot_interfaces::RobotDriver<
          typename robot_interfaces::NJointAction<N_JOINTS>,
          Observation>
{
public:
    static constexpr size_t num_joints = N_JOINTS;
    static constexpr size_t num_motor_boards = N_MOTOR_BOARDS;

    typedef typename robot_interfaces::NJointAction<N_JOINTS> Action;
    typedef typename robot_interfaces::RobotInterfaceTypes<Action, Observation>
        Types;

    typedef typename Action::Vector Vector;
    typedef std::array<std::shared_ptr<blmc_drivers::MotorInterface>, N_JOINTS>
        Motors;
    typedef std::array<std::shared_ptr<blmc_drivers::CanBusMotorBoard>,
                       N_MOTOR_BOARDS>
        MotorBoards;

    struct Config;  // actual declaration see below

    /**
     * @brief True if the joints have mechanical end stops, false if not.
     *
     * If set to true, it is assumed that all joints of the robot have
     * end stops that mechanically prevent them from moving out of the valid
     * range.
     *
     * If present, the end stops are used for a fully automated homing procedure
     * in which the joints first move until they hit the end stop before
     * starting the encoder index search.  This way it is ensured that the
     * correct index is used for homing without any need for manual set up.
     */
    const bool has_endstop_;

    NJointBlmcRobotDriver(const MotorBoards &motor_boards,
                          const Motors &motors,
                          const MotorParameters &motor_parameters,
                          const Config &config)
        : robot_interfaces::RobotDriver<Action, Observation>(),
          has_endstop_(config.has_endstop),
          joint_modules_(motors,
                         motor_parameters.torque_constant_NmpA * Vector::Ones(),
                         motor_parameters.gear_ratio * Vector::Ones(),
                         Vector::Zero(),
                         config.max_current_A * Vector::Ones()),
          motor_boards_(motor_boards),
          motor_parameters_(motor_parameters),
          max_torque_Nm_(config.max_current_A *
                         motor_parameters.torque_constant_NmpA *
                         motor_parameters.gear_ratio),
          config_(config)
    {
        pause_motors();
    }

    static MotorBoards create_motor_boards(
        const std::array<std::string, N_MOTOR_BOARDS> &can_ports);

    Vector get_max_torques() const
    {
        return max_torque_Nm_ * Vector::Ones();
    }

    void pause_motors();

    Vector get_measured_index_angles() const;

    /**
     * @brief Find home position of all joints and move to start position.
     *
     * Homes all joints using home_on_index_after_negative_end_stop.  When
     * finished, move the joint to the starting position (defined in
     * `config_.initial_position_rad`).
     *
     */
    void initialize() override;

    virtual Observation get_latest_observation() override = 0;
    Action apply_action(const Action &desired_action) override;
    std::string get_error() override;
    void shutdown() override;

    /**
     * @brief Process the desired action provided by the user.
     *
     * Takes the desired action from the user and does the following processing:
     *
     * ## 1. Check position limits
     *
     *   If the observed position of a joint exceeds the limits, actions for
     *   that joint that do not point back towards the allowed range are
     *   replaced with a position command to the limit value.  Further custom
     *   PD-gains are ignored in this case.
     *
     * ## 2. Run the position controller in case a target position is set.
     *
     *   If the target position is set to a value unequal to NaN for any joint,
     *   a PD position controller is executed for this joint and the resulting
     *   torque command is added to the torque command in the action.
     *
     *   If the P- and/or D-gains are set to a non-NaN value in the action, they
     *   are used for the control.  NaN-values are replaced with the default
     *   gains.
     *
     * ## 3. Apply safety checks.
     *
     *   - Limit the torque to the allowed maximum value.
     *   - Dampen velocity using the given safety_kd gains.  Damping us done
     *     joint-wise using this equation:
     *
     *         torque_damped = torque_desired - safety_kd * current_velocity
     *
     * The resulting action with modifications of all steps is returned.
     *
     * @param desired_action  Desired action given by the user.
     * @param latest_observation  Latest observation from the robot.
     * @param max_torque_Nm  Maximum allowed absolute torque.
     * @param safety_kd  D-gain for velocity damping.
     * @param default_position_control_kp  Default P-gain for position control.
     * @param default_position_control_kd  Default D-gain for position control.
     * @param lower_position_limits  Lower limits for joint positions.
     * @param upper_position_limits  Upper limits for joint positions.
     *
     * @return Resulting action after applying all the processing.
     */
    static Action process_desired_action(
        const Action &desired_action,
        const Observation &latest_observation,
        const double max_torque_Nm,
        const Vector &safety_kd,
        const Vector &default_position_control_kp,
        const Vector &default_position_control_kd,
        const Vector &lower_position_limits =
            Vector::Constant(-std::numeric_limits<double>::infinity()),
        const Vector &upper_position_limits =
            Vector::Constant(std::numeric_limits<double>::infinity()));

    /**
     * @brief Check if the joint position is within the hard limits.
     *
     * @see Config::is_within_hard_position_limits
     */
    bool is_within_hard_position_limits(const Observation &observation) const;

protected:
    blmc_drivers::BlmcJointModules<N_JOINTS> joint_modules_;
    MotorBoards motor_boards_;

    //! \brief Fixed motor parameters (assuming all joints use same setup).
    MotorParameters motor_parameters_;

    //! \brief Maximum torque allowed on each joint.
    double max_torque_Nm_;

    /**
     * \brief User-defined configuration of the driver
     *
     * Contains all configuration values that can be modified by the user (via
     * the configuration file).
     */
    Config config_;

    bool is_initialized_ = false;

    Action apply_action_uninitialized(const Action &desired_action);

    //! \brief Actual initialization that is called in a real-time thread in
    //!        initialize().
    void _initialize();

    /**
     * @brief Homing using end stops (optional) and encoder indices.
     *
     * Procedure for finding an absolute zero position (or "home" position) when
     * using relative encoders.
     *
     * If the robot has end stops (according to configuration), all joints first
     * move in negative direction until they hit the end stop.
     * Then an encoder index search is started where each joint moves slowly in
     * positive direction until the next encoder index.  The position of this
     * encoder index is the "home position".
     *
     * By default, the zero position is the same as the home position.  The
     * optional argument home_offset_rad provides a means to move the zero
     * position
     * relative to the home position.  The zero position is computed as
     *
     *     zero position = encoder index position + home offset
     *
     *
     * @param endstop_search_torques_Nm Torques that are used to move the joints
     *     while searching the end stop.
     * @param home_offset_rad Offset between the home position and the desired
     *     zero position.
     */
    bool homing(Vector endstop_search_torques_Nm,
                Vector home_offset_rad = Vector::Zero());

    /**
     * @brief Move to given goal position with a minimum jerk trajectory.
     *
     * Use a series of position actions to move to the given goal position on a
     * minimum jerk trajectory.
     *
     * @param goal_pos Angular goal position for each joint.
     * @param tolerance Allowed position error for reaching the goal.  This is
     *     checked per joint, that is the maximal possible error is +/-tolerance
     *     on each joint.
     * @param time_steps Number of control loop cycles for reaching the goal.
     *     The lower the number of steps, the faster the robot will move.
     * @return True if goal position is reached, false if timeout is exceeded.
     */
    bool move_to_position(const Vector &goal_pos,
                          const double tolerance,
                          const uint32_t time_steps);
};

/**
 * @brief Configuration of the robot that can be changed by the user.
 */
template <typename Observation, size_t N_JOINTS, size_t N_MOTOR_BOARDS>
struct NJointBlmcRobotDriver<Observation, N_JOINTS, N_MOTOR_BOARDS>::Config
{
    typedef std::array<std::string, N_MOTOR_BOARDS> CanPortArray;

    //! @brief A sub-goal of a trajectory.
    struct TrajectoryStep
    {
        //! @brief Target position to which the joints should move.
        Vector target_position_rad = Vector::Zero();

        //! @brief Number of time steps for reaching the target position.
        uint32_t move_steps = 0;
    };

    // All parameters should have default values that should not result in
    // dangerous behaviour in case someone forgets to specify them.

    /**
     * @brief List of CAN port names used by the robot.
     *
     * For each motor control board used by the robot, this specifies the
     * CAN port through which it is connected.
     *
     * Example: `{"can0", "can1"}`
     */
    CanPortArray can_ports;

    //! @brief Maximum current that can be sent to the motor [A].
    double max_current_A = 0.0;

    /**
     * @brief Whether the joints have physical end stops or not.
     *
     * This is for example relevant for homing, where (in case this value
     * is set to true) all joints move until they hit the end stop to
     * determine their absolute position.
     *
     * Note that not having end stops does not mean that the joint can
     * rotate freely in general.
     */
    bool has_endstop = false;

    //! @brief Parameters related to calibration.
    struct CalibrationParameters
    {
        //! @brief Torque that is used to find the end stop.
        Vector endstop_search_torques_Nm = Vector::Zero();
        //! @brief Number of time steps for reaching the initial position.
        uint32_t move_steps = 0;
    } calibration;

    //! @brief Tolerance for reaching the target with
    //!        @ref NJointBlmcRobotDriver::move_to_position.
    double move_to_position_tolerance_rad = 0.0;

    //! @brief D-gain to dampen velocity.  Set to zero to disable damping.
    // set some rather high damping by default
    Vector safety_kd = Vector::Constant(0.1);

    //! @brief Default control gains for the position PD controller.
    struct PositionControlGains
    {
        Vector kp = Vector::Zero();
        Vector kd = Vector::Zero();
    } position_control_gains;

    /**
     * @brief Hard lower limits for joint position.
     *
     * Exceeding this limit results in an error and robot shutdown.
     */
    Vector hard_position_limits_lower = Vector::Zero();
    /**
     * @brief Hard upper limits for joint position.
     *
     * Exceeding this limit results in an error and robot shutdown.
     */
    Vector hard_position_limits_upper = Vector::Zero();

    /**
     * @brief Soft lower limits for joint position.
     *
     * Exceeding this limit results in the action being adjusted to move the
     * joint back inside the limits.
     */
    Vector soft_position_limits_lower =
        Vector::Constant(-std::numeric_limits<double>::infinity());
    /**
     * @brief Soft upper limits for joint position.
     *
     * Exceeding this limit results in the action being adjusted to move the
     * joint back inside the limits.
     */
    Vector soft_position_limits_upper =
        Vector::Constant(std::numeric_limits<double>::infinity());

    //! @brief Offset between home position and zero.
    Vector home_offset_rad = Vector::Zero();

    /**
     * @brief Initial position to which the robot moves after
     *        initialization.
     */
    Vector initial_position_rad = Vector::Zero();

    /**
     * @brief Trajectory which is executed in the shutdown method.
     *
     * Use this to move the robot to a "rest position" during shutdown of the
     * robot driver.  It can consist of arbitrarily many steps.  Leave it empty
     * to not move during shutdown.
     */
    std::vector<TrajectoryStep> shutdown_trajectory;

    /**
     * @brief Check if the given position is within the hard limits.
     *
     * @param position Joint positions.
     *
     * @return True if `hard_position_limits_lower <= position <=
     *     hard_position_limits_upper`.
     */
    bool is_within_hard_position_limits(const Vector &position) const;

    /**
     * @brief Print the given configuration in a human-readable way.
     */
    void print() const;

    /**
     * @brief Load driver configuration from file.
     *
     * Load the configuration from the specified YAML file.  The file is
     * expected to have the same structure/key naming as the Config struct.
     * If a value can not be read from the file, the application exists
     * with an error message.
     *
     * @param config_file_name  Path/name of the configuration YAML file.
     *
     * @return Configuration
     */
    static Config load_config(const std::string &config_file_name);

private:
    /**
     * \brief Set value from user configuration to var if specified.
     *
     * Checks if a field `name` exists in `user_config`.  If yes, its value
     * is written to `var`, otherwise `var` is unchanged.
     *
     * \param[in] user_config  YAML node containing the user configuration.
     * \param[in] name  Name of the configuration entry.
     * \param[out] var  Variable to which configuration is written.  Value
     * is unchanged if the specified field name does not exist in
     * user_config, i.e.  it can be initialized with a default value.
     */
    template <typename T>
    static void set_config_value(const YAML::Node &user_config,
                                 const std::string &name,
                                 T *var);
};

/**
 * @brief Simple n-joint robot driver that uses NJointObservation.
 *
 * @tparam N_JOINTS  Number of joints
 * @tparam N_MOTOR_BOARDS  Number of motor boards.
 */
template <size_t N_JOINTS, size_t N_MOTOR_BOARDS = (N_JOINTS + 1) / 2>
class SimpleNJointBlmcRobotDriver
    : public NJointBlmcRobotDriver<
          robot_interfaces::NJointObservation<N_JOINTS>,
          N_JOINTS,
          N_MOTOR_BOARDS>
{
public:
    typedef robot_interfaces::NJointObservation<N_JOINTS> Observation;

    using NJointBlmcRobotDriver<robot_interfaces::NJointObservation<N_JOINTS>,
                                N_JOINTS,
                                N_MOTOR_BOARDS>::NJointBlmcRobotDriver;

    Observation get_latest_observation() override;
};

/**
 * @brief Create backend using the specified driver.
 *
 * @tparam Driver  Type of the driver.  Expected to inherit from
 *     NJointBlmcRobotDriver.
 *
 * @param robot_data  Instance of RobotData used for communication.
 * @param config  Driver configuration.
 * @param first_action_timeout  Duration for which the backend waits for the
 *     first action to arrive.  If exceeded, the backend shuts down.
 * @param max_number_of_actions  Number of actions after which the backend
 *     automatically shuts down.
 *
 * @return A RobotBackend instances with a driver of the specified type.
 */
template <typename Driver>
typename Driver::Types::BackendPtr create_backend(
    typename Driver::Types::BaseDataPtr robot_data,
    const typename Driver::Config &config,
    const double first_action_timeout = std::numeric_limits<double>::infinity(),
    const uint32_t max_number_of_actions = 0)
{
    constexpr double MAX_ACTION_DURATION_S = 0.003;
    constexpr double MAX_INTER_ACTION_DURATION_S = 0.005;

    config.print();

    // wrap the actual robot driver directly in a MonitoredRobotDriver
    auto monitored_driver =
        std::make_shared<robot_interfaces::MonitoredRobotDriver<Driver>>(
            std::make_shared<Driver>(config),
            MAX_ACTION_DURATION_S,
            MAX_INTER_ACTION_DURATION_S);

    constexpr bool real_time_mode = true;
    auto backend = std::make_shared<typename Driver::Types::Backend>(
        monitored_driver,
        robot_data,
        real_time_mode,
        first_action_timeout,
        max_number_of_actions);
    backend->set_max_action_repetitions(std::numeric_limits<uint32_t>::max());

    return backend;
}

/**
 * @brief Create backend using the specified driver.
 *
 * Overloaded version that takes a path to a configuration YAML file instead of
 * a Config instance.
 */
template <typename Driver>
typename Driver::Types::BackendPtr create_backend(
    typename Driver::Types::BaseDataPtr robot_data,
    const std::string &config_file_path,
    const double first_action_timeout = std::numeric_limits<double>::infinity(),
    const uint32_t max_number_of_actions = 0)
{
    std::cout << "Load robot driver configuration from file '"
              << config_file_path << "'" << std::endl;
    auto config = Driver::Config::load_config(config_file_path);

    return create_backend<Driver>(
        robot_data, config, first_action_timeout, max_number_of_actions);
}

}  // namespace robot_fingers

#include "n_joint_blmc_robot_driver.hxx"


#include <string>
#include <vector>
#include <sstream>
#include <iostream>
#include <boost/ref.hpp>
#include <boost/python.hpp>
#include <boost/python/numpy.hpp>
#include <opencv2/opencv.hpp>
#include <glog/logging.h>
namespace py = boost::python;
namespace np = boost::python::numpy;

namespace {
    using std::istringstream;
    using std::ostringstream;
    using std::string;
    using std::runtime_error;
    using std::cerr;
    using std::endl;
    using std::vector;

    // sanity check np::ndarray to make sure they are dense and continuous
    template <typename T=float>
    void check_dense (np::ndarray array, int nd = 0) {
        CHECK(array.get_dtype() == np::dtype::get_builtin<T>());
        if (nd > 0) CHECK(array.get_nd() == nd);
        else nd = array.get_nd();
        int stride = sizeof(T);
        for (int i = 0, off=nd-1; i < nd; ++i, --off) {
            CHECK(array.strides(off) == stride);
            stride *= array.shape(off);
        }
    }

    template <typename T>
    T sqr (T v) {
        return  v * v;
    }

    std::pair<int, int> get_range (float center, float r, int min, int max) {
        min = std::max(int(std::floor(center - r)), min);
        max = std::min(int(std::ceil(center + r)), max);
        return std::make_pair(min, max);
    }

    py::tuple encode (py::tuple shape, py::list nodule) {
        int Z = py::extract<int>(shape[0]);
        int Y = py::extract<int>(shape[1]);
        int X = py::extract<int>(shape[2]);

        float z = py::extract<float>(nodule[0]);
        float y = py::extract<float>(nodule[1]);
        float x = py::extract<float>(nodule[2]);
        float r = py::extract<float>(nodule[3]);

        np::ndarray A = np::zeros(py::make_tuple(Z, Y, X), np::dtype::get_builtin<float>());
        np::ndarray P = np::zeros(py::make_tuple(Z, Y, X, 4), np::dtype::get_builtin<float>());
        check_dense<float>(A);
        check_dense<float>(P);

        auto z_range = get_range(z, r, 0, Z-1);
        for (int i = z_range.first; i <= z_range.second; ++i) {
            float r1 = sqrt(sqrt(r) - sqr(z-i));
            auto y_range = get_range(y, r1, 0, Y-1);
            auto x_range = get_range(x, r1, 0, X-1);

            float *pa = reinterpret_cast<float *>(A.get_data() + i * A.strides(0));
            float *pp = reinterpret_cast<float *>(P.get_data() + i * P.strides(0));
            // slice
            for (int j = y_range.first; j <= y_range.second; ++j) {
                float *ppa = pa + (X * j + x_range.first);
                float *ppp = pp + (X * j + x_range.first)* 4;
                for (int k = x_range.first; k <= x_range.second; ++k) {
                    if (sqr(i - z) + sqr(j - y) + sqr(k - x) <= sqr(r)) {
                        // mask
                        ppa[0] = 1.0;
                        // parameters
                        ppp[0] = z - i;
                        ppp[1] = y - j;
                        ppp[2] = x - k;
                        ppp[3] = r;
                    }
                    ppa += 1;
                    ppp += 4;
                }
            }
        }
        return py::make_tuple(A, P);
    }

    struct Ball {
        float z, y, x, r, s;

        py::tuple to_python () const {
            return py::make_tuple(z, y, x, r, s);
        }
    };

    float overlap (Ball const &a, Ball const &b) {
        // TODO: a better way to calculate this
        float dist = sqrt(sqr(a.x-b.x) + sqr(a.y-b.y) + sqr(a.z-b.z));
        float I = a.r + b.r - dist;
        return I / std::min(a.r, b.r);
    }

    py::list decode (np::ndarray A, np::ndarray P, float anchor_th, float nms_th) {
        check_dense<float>(A);
        check_dense<float>(P);
        float const *pa = reinterpret_cast<float *>(A.get_data());
        float const *pp = reinterpret_cast<float *>(P.get_data());
        vector<Ball> balls;
        for (int i = 0; i < A.shape(0); ++i) {
            for (int j = 0; j < A.shape(1); ++j) {
                for (int k = 0; k < A.shape(2); ++k) {
                    if (pa[0] >= anchor_th) {
                        Ball b;
                        b.z = i + pp[0];
                        b.y = j + pp[1];
                        b.x = k + pp[2];
                        b.r = pp[3];
                        b.s = pa[0];
                        balls.push_back(b);
                    }
                    pa += 1;
                    pp += 4;
                }
            }
        }
        // nms
        vector<Ball> keep;
        std::sort(balls.begin(), balls.end(), [](Ball const &a, Ball const &b) { return a.s > b.s;});
        for (auto const &b: balls) {
            for (auto const &k: keep) {
                if (overlap(b, k) < nms_th) {
                    keep.push_back(b);
                }
            }
        }

        py::list list;
        for (auto const &b: keep) {
            list.append(b.to_python());
        }
        return list;
    }
}

BOOST_PYTHON_MODULE(cpp)
{
    np::initialize();
    def("encode", ::encode);
    def("decode", ::decode);
}


#include <iostream>
#include <vector>
#include <cassert>
#include <cstring>
#include <limits>
#include <mpi.h>
//#include <Eigen/Eigenvalues>

enum KernelType {CubicSpline = 0, WendlandC2 = 1, WendlandC4 = 2};

#include "particle_simulator.hpp"
#include "hdr_time.hpp"
#include "hdr_run.hpp"
#ifdef USE_INTRINSICS
#include "vector_x86.hpp"
#endif
#include "hdr_dimension.hpp"
#include "hdr_kernel.hpp"
#include "hdr_sph.hpp"
#include "hdr_hgas.hpp"
#include "hdr_bhns.hpp"

class SPHAnalysis : public HelmholtzGas {
public:
    SPHAnalysis() {
        this->id    = 0;
        this->istar = 0;
        this->mass  = 0.;
        this->pos   = 0.;
        this->vel   = 0.;
        this->uene  = 0.;
        this->alph  = 0.;
        this->alphu = 0.;
        this->ksr   = 0.;
        for(PS::S32 k = 0; k < NR::NumberOfNucleon; k++) {
            this->cmps[k] = 0.;
        }
    }

    void readAscii(FILE * fp) {
        fscanf(fp, "%lld%lld%lf", &this->id, &this->istar, &this->mass);      //  3
        fscanf(fp, "%lf%lf%lf", &this->pos[0], &this->pos[1], &this->pos[2]); //  6
        fscanf(fp, "%lf%lf%lf", &this->vel[0], &this->vel[1], &this->vel[2]); //  9
        fscanf(fp, "%lf%lf%lf", &this->acc[0], &this->acc[1], &this->acc[2]); // 12
        fscanf(fp, "%lf%lf%lf", &this->uene, &this->alph, &this->alphu);      // 15
        fscanf(fp, "%lf%lf%6d", &this->dens, &this->ksr,  &this->np);         // 18
        fscanf(fp, "%lf%lf%lf", &this->vsnd, &this->pres, &this->temp);       // 21
        fscanf(fp, "%lf%lf%lf", &this->divv, &this->rotv, &this->bswt);       // 24
        fscanf(fp, "%lf%lf%lf", &this->pot,  &this->abar, &this->zbar);       // 27
        fscanf(fp, "%lf",       &this->enuc);                                 // 28
        fscanf(fp, "%lf%lf%lf", &this->vsmx, &this->udot, &this->dnuc);       // 31
        for(PS::S32 k = 0; k < NuclearReaction::NumberOfNucleon; k++) {       // 32 -- 44
            fscanf(fp, "%lf", &this->cmps[k]);
        }
        fscanf(fp, "%lf", &this->pot3);
        fscanf(fp, "%lf%lf%lf", &this->tempmax[0], &this->tempmax[1], &this->tempmax[2]);
        fscanf(fp, "%lf", &this->entr);
    }

};

namespace SphericalHarmonics {
    PS::F64 CoefficientY00 = 0.5 * sqrt(1.0 / M_PI);
    PS::F64 CoefficientY10 = 0.5 * sqrt(3.0 / M_PI);
    PS::F64 CoefficientY11 = 0.5 * sqrt(1.5 / M_PI);
    PS::F64 CoefficientY20 = sqrt( 5. / (16. * M_PI));
    PS::F64 CoefficientY22 = sqrt(15. / (32. * M_PI));

    bool    FirstTime         = true;
    PS::F64 MinimumRadius     = 1e+08;
    PS::F64 MaximumRadius     = 1e+11;
    PS::S64 NumberOfBinPerDex = 10;

    PS::S64 NumberOfBin = 0;
    const PS::S64 MaximumNumberOfBin = 64;
    static PS::F64 m00re[MaximumNumberOfBin];
    static PS::F64 m10re[MaximumNumberOfBin];
    static PS::F64 m11re[MaximumNumberOfBin];
    static PS::F64 m11im[MaximumNumberOfBin];
    static PS::F64 m20re[MaximumNumberOfBin];
    static PS::F64 m22re[MaximumNumberOfBin];
    static PS::F64 m22im[MaximumNumberOfBin];
    static PS::S64 nptcl[MaximumNumberOfBin];
    static PS::F64 m00re_g[MaximumNumberOfBin];
    static PS::F64 m10re_g[MaximumNumberOfBin];
    static PS::F64 m11re_g[MaximumNumberOfBin];
    static PS::F64 m11im_g[MaximumNumberOfBin];
    static PS::F64 m20re_g[MaximumNumberOfBin];
    static PS::F64 m22re_g[MaximumNumberOfBin];
    static PS::F64 m22im_g[MaximumNumberOfBin];
    static PS::S64 nptcl_g[MaximumNumberOfBin];

    void initialize() {
        FirstTime = false;
        PS::F64 temp = log10(MaximumRadius / MinimumRadius) * NumberOfBinPerDex;
        NumberOfBin = (temp - (PS::S64)temp == 0.) ? (PS::S64)temp : (PS::S64)(temp + 1.);
        assert(NumberOfBin <= MaximumNumberOfBin);
        for(PS::S64 i = 0; i < NumberOfBin; i++) {
            m00re[i] = 0.;
            m10re[i] = 0.;
            m11re[i] = 0.;
            m11im[i] = 0.;
            m20re[i] = 0.;
            m22re[i] = 0.;
            m22im[i] = 0.;
            nptcl[i] = 0;
            m00re_g[i] = 0.;
            m10re_g[i] = 0.;
            m11re_g[i] = 0.;
            m11im_g[i] = 0.;
            nptcl_g[i] = 0.;
        }
    }

    void reinitialize() {
        for(PS::S64 i = 0; i < NumberOfBin; i++) {
            m00re[i] = 0.;
            m10re[i] = 0.;
            m11re[i] = 0.;
            m11im[i] = 0.;
            m20re[i] = 0.;
            m22re[i] = 0.;
            m22im[i] = 0.;
            nptcl[i] = 0;
            m00re_g[i] = 0.;
            m10re_g[i] = 0.;
            m11re_g[i] = 0.;
            m11im_g[i] = 0.;
            nptcl_g[i] = 0.;
        }
    }

    template <class Tsph>
    inline void addParticle(Tsph & isph) {
        PS::F64 r2 = isph.pos * isph.pos;
        if(r2 >= MaximumRadius * MaximumRadius) {
            return;
        }
        PS::F64 r1 = sqrt(r2);
        PS::S64 ibin = PS::S64(log10(r1 / MinimumRadius) * NumberOfBinPerDex);
        assert(ibin < NumberOfBin);

        PS::F64 cr = sqrt(isph.pos[0] * isph.pos[0] + isph.pos[1] * isph.pos[1]);
        PS::F64 prinv = 1. / r1;
        PS::F64 crinv = 1. / cr;
        PS::F64 costheta = isph.pos[2] * prinv;
        PS::F64 sintheta = cr          * prinv;
        PS::F64 cosphi   = isph.pos[0] * crinv;
        PS::F64 sinphi   = isph.pos[1] * crinv;

        PS::F64 costheta_sq   = costheta * costheta;
        PS::F64 sintheta_sq   = sintheta * sintheta;
        PS::F64 cosphi_2times = cosphi * cosphi - sinphi * sinphi;
        PS::F64 sinphi_2times = 2. * sinphi * cosphi;

        m00re[ibin] += isph.mass * CoefficientY00;
        m10re[ibin] += isph.mass * CoefficientY10 * costheta;
        m11re[ibin] += isph.mass * CoefficientY11 * sintheta * cosphi;
        m11im[ibin] += isph.mass * CoefficientY11 * sintheta * sinphi;
        m20re[ibin] += isph.mass * CoefficientY20 * (3. * costheta_sq - 1.);
        m22re[ibin] += isph.mass * CoefficientY22 * sintheta_sq * cosphi_2times;
        m22im[ibin] += isph.mass * CoefficientY22 * sintheta_sq * sinphi_2times;
        nptcl[ibin] += 1;
    }

    void reduce() {
        PS::S64 ierr = 0;
        ierr = MPI_Allreduce(m00re, m00re_g, NumberOfBin, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
        ierr = MPI_Allreduce(m10re, m10re_g, NumberOfBin, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
        ierr = MPI_Allreduce(m11re, m11re_g, NumberOfBin, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
        ierr = MPI_Allreduce(m11im, m11im_g, NumberOfBin, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
        ierr = MPI_Allreduce(m20re, m20re_g, NumberOfBin, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
        ierr = MPI_Allreduce(m22re, m22re_g, NumberOfBin, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
        ierr = MPI_Allreduce(m22im, m22im_g, NumberOfBin, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
        ierr = MPI_Allreduce(nptcl, nptcl_g, NumberOfBin, MPI_LONG_LONG, MPI_SUM, MPI_COMM_WORLD);
    }

    void output(FILE * fp) {
        for(PS::S64 i = 0; i < NumberOfBin; i++) {
            PS::F64 r = (i == 0) ? (0.5 * MinimumRadius)
                : (MinimumRadius * pow(10., (i - 0.5) / NumberOfBinPerDex));
            fprintf(fp, "%+e %+e %+e %+e %+e %+e %+e %+e %8d\n",
                    r,
                    m00re_g[i], m10re_g[i], m11re_g[i], m11im_g[i],
                    m20re_g[i], m22re_g[i], m22im_g[i],
                    nptcl_g[i]);
        }
    }

};

template <class Tsph>
void calcDensityCenter(Tsph  & sph,
                       PS::F64vec & xdens_g,
                       PS::F64vec & vdens_g,
                       PS::F64vec & adens_g) {
    PS::F64    tdens = 0.;
    PS::F64vec xdens = 0.;
    PS::F64vec vdens = 0.;
    PS::F64vec adens = 0.;

    for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
        if(sph[i].istar == 1) {
            continue;
        }
        tdens += sph[i].dens;
        xdens += sph[i].dens * sph[i].pos;
        vdens += sph[i].dens * sph[i].vel;
        adens += sph[i].dens * sph[i].acc;
    }

    PS::F64    tdens_g = PS::Comm::getSum(tdens);
    xdens_g = PS::Comm::getSum(xdens);
    vdens_g = PS::Comm::getSum(vdens);
    adens_g = PS::Comm::getSum(adens);

    xdens_g = xdens_g / tdens_g;
    vdens_g = vdens_g / tdens_g;
    adens_g = adens_g / tdens_g;

    return;
}

template <class Tsph>
void expandInSphericalHarmonics(char * ofile,
                                Tsph  & sph) {
    PS::F64vec xdens = 0.;
    PS::F64vec vdens = 0.;
    PS::F64vec adens = 0.;
    calcDensityCenter(sph, xdens, vdens, adens);
    for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
        sph[i].pos -= xdens;
        sph[i].vel -= vdens;
        sph[i].acc -= adens;
    }

    if(SphericalHarmonics::FirstTime) {
        SphericalHarmonics::initialize();
    } else {
        SphericalHarmonics::reinitialize();
    }

    for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
        SphericalHarmonics::addParticle(sph[i]);
    }

    SphericalHarmonics::reduce();

    if(PS::Comm::getRank() == 0) {
        FILE * fp = fopen(ofile, "w");
        SphericalHarmonics::output(fp);
        fclose(fp);
    }

}

int main(int argc, char ** argv) {
    PS::Initialize(argc, argv);

    PS::ParticleSystem<SPHAnalysis> sph;
    sph.initialize();
    sph.createParticle(0);
    sph.setNumberOfParticleLocal(0);

    char idir[1024], odir[1024];
    PS::S64 ibgn, iend, dsnp;
    FILE * fp = fopen(argv[1], "r");
    fscanf(fp, "%s", idir);
    fscanf(fp, "%s", odir);
    fscanf(fp, "%lld%lld%lld", &ibgn, &iend, &dsnp);
    fclose(fp);

    for(PS::S64 itime = ibgn; itime <= iend; itime += dsnp) {
        char tfile[1024];
        FILE *fp = NULL;
        PS::S64 tdir = 0;
        for(PS::S64 iidir = 0; iidir < 100; iidir++) {
            sprintf(tfile, "%s/t%02d/sph_t%04d_p%06d_i%06d.dat", idir, iidir, itime,
                    PS::Comm::getNumberOfProc(), 0);
            fp = fopen(tfile, "r");
            if(fp != NULL) {
                tdir = iidir;
                break;
            }
        }
        if(fp == NULL) {
            if(PS::Comm::getRank() == 0) {
                fprintf(stderr, "Not found %s\n", tfile);
            }
            continue;
        }
        fclose(fp);

        char sfile[1024];
        sprintf(sfile, "%s/t%02d/sph_t%04d", idir, tdir, itime);
        sph.readParticleAscii(sfile, "%s_p%06d_i%06d.dat");

        char ofile[1024];
        sprintf(ofile, "%s/harmonics_t%04d.dat", odir, itime);
        expandInSphericalHarmonics(ofile, sph);
    }

    PS::Finalize();

    return 0;
}


#include <helpers/svg_helpers.h>

#include <boost/test/auto_unit_test.hpp>

#include <interpreter/svg/reader.h>
#include <interpreter/svg/document.h>
#include <interpreter/svg/edge.h>
#include <interpreter/svg/location.h>

struct reader_fixture
{
	svg::reader r;
};

bool containsEndpoint(const std::vector<std::shared_ptr<svg::edge> >& edges, svg::location endpoint)
{
	return std::any_of(edges.begin(), edges.end(), [endpoint](const std::shared_ptr<svg::edge>& edge) {return edge->end_point()->loc() == endpoint; });
}

BOOST_FIXTURE_TEST_SUITE(svg_reader_sanity, reader_fixture)
BOOST_AUTO_TEST_CASE(empty_string_is_nullptr)
{
	BOOST_CHECK(r.parse("").get() == nullptr);
}

BOOST_AUTO_TEST_CASE(empty_document_returns_document_with_no_points)
{
	auto res = r.parse("<svg width=\"100\" height=\"200\"></svg>");
	BOOST_CHECK(res->points().empty());
	BOOST_CHECK_EQUAL(100, res->width());
	BOOST_CHECK_EQUAL(200, res->height());
}

BOOST_AUTO_TEST_CASE(document_with_single_line_has_two_points_one_segment_and_one_path)
{
	auto res = r.parse("<svg width='100' height='200'><line x1='0' y1='20' x2='100' y2='200' /></svg>");
	auto points = res->points();

	BOOST_CHECK_EQUAL(static_cast<std::size_t>(2), points.size());
	BOOST_CHECK_EQUAL(static_cast<std::size_t>(1), points.count(svg::location(0, 20)));
	BOOST_CHECK_EQUAL(static_cast<std::size_t>(1), points.count(svg::location(100, 200)));

	auto edges = points.at(svg::location(0, 20)).edges();
	BOOST_CHECK_EQUAL(static_cast<std::size_t>(1), edges.size());
	BOOST_CHECK_PREDICATE(containsEndpoint, (edges)(svg::location(100, 200)));
	BOOST_CHECK(std::all_of(edges.begin(), edges.end(), [](const std::shared_ptr<svg::edge>& edge) {return edge->start_point()->loc() == svg::location(0, 20); }));
}

BOOST_AUTO_TEST_CASE(points_from_multiple_lines_are_shared)
{
	auto res = r.parse("<svg width='100' height='200'><line x1='0' y1='20' x2='100' y2='200' /><line x1='100' y1='200' x2='500' y2='0' /></svg>");
	auto points = res->points();

	BOOST_CHECK_EQUAL(static_cast<std::size_t>(3), points.size());
	BOOST_CHECK_EQUAL(static_cast<std::size_t>(1), points.count(svg::location(0, 20)));
	BOOST_CHECK_EQUAL(static_cast<std::size_t>(1), points.count(svg::location(100, 200)));
	BOOST_CHECK_EQUAL(static_cast<std::size_t>(1), points.count(svg::location(500, 0)));

	auto edges = points.at(svg::location(100, 200)).edges();
	BOOST_REQUIRE_EQUAL(static_cast<std::size_t>(2), edges.size());

	auto first_endpoint = edges.at(0)->end_point();
	auto second_endpoint = edges.at(1)->end_point();
	BOOST_REQUIRE_NE(first_endpoint->loc(), second_endpoint->loc());
    BOOST_CHECK_PREDICATE(containsEndpoint, (edges)(svg::location(0, 20)));
	BOOST_CHECK_PREDICATE(containsEndpoint, (edges)(svg::location(500, 0)));
}

BOOST_AUTO_TEST_CASE(all_points_from_linear_path_need_to_be_obtained)
{
	auto res = r.parse("<svg width='400' height='400'><path d='M40,50 L200,100 l-30,20' /> </svg>");
	auto points = res->points();

	BOOST_CHECK_EQUAL(static_cast<std::size_t>(3), points.size());
	BOOST_CHECK_EQUAL(static_cast<std::size_t>(1), points.count(svg::location(40, 50)));
	BOOST_CHECK_EQUAL(static_cast<std::size_t>(1), points.count(svg::location(200, 100)));
	BOOST_CHECK_EQUAL(static_cast<std::size_t>(1), points.count(svg::location(170, 120)));

	auto edges = points.at(svg::location(200, 100)).edges();
	BOOST_REQUIRE_EQUAL(static_cast<std::size_t>(2), edges.size());
	BOOST_CHECK_PREDICATE(containsEndpoint, (edges)(svg::location(40, 50)));
	BOOST_CHECK_PREDICATE(containsEndpoint, (edges)(svg::location(170, 120)));
}

BOOST_AUTO_TEST_CASE(triangle_can_be_analyzed)
{
	auto res = r.parse("<svg width='400' height='200'><path d='M10 10 l10 20 l20 -10 z' /></svg>");

	BOOST_CHECK(res->contains_point_at(svg::location(10, 10)));
	BOOST_CHECK(res->contains_point_at(svg::location(20, 30)));
	BOOST_CHECK(res->contains_point_at(svg::location(40, 20)));

	auto points = res->points();
	BOOST_REQUIRE_EQUAL(static_cast<std::size_t>(3), res->points().size());

	for (auto& p : points)
	{
		BOOST_CHECK_EQUAL(static_cast<std::size_t>(2), p.second.edges().size());
	}
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/operator/include/functions/logical_or.hpp>
#include <boost/simd/sdk/simd/logical.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/three.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/valmax.hpp>

NT2_TEST_CASE_TPL ( logical_or_integer,  BOOST_SIMD_INTEGRAL_TYPES)
{
  using boost::simd::logical_or;
  using boost::simd::tag::logical_or_;
  typedef typename boost::dispatch::meta::call<logical_or_(T,T)>::type r_t;
  typedef typename boost::simd::logical<T> wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_EQUAL(logical_or(boost::simd::One<T>(), boost::simd::One<T>()), r_t(true));
  NT2_TEST_EQUAL(logical_or(boost::simd::One<T>(),boost::simd::Zero<T>()), r_t(true));
  NT2_TEST_EQUAL(logical_or(boost::simd::Zero<T>(), boost::simd::Zero<T>()), r_t(false));
} // end of test for integer_

NT2_TEST_CASE_TPL ( logical_or_real,  BOOST_SIMD_REAL_TYPES)
{
  using boost::simd::logical_or;
  using boost::simd::tag::logical_or_;
  typedef typename boost::dispatch::meta::call<logical_or_(T,T)>::type r_t;
  typedef typename boost::simd::logical<T> wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  std::cout << nt2::type_id<r_t> () << std::endl;
  std::cout << nt2::type_id<wished_r_t> () << std::endl;

  // specific values tests
  NT2_TEST_EQUAL(logical_or(boost::simd::Inf<T>(), boost::simd::Inf<T>()), r_t(true));
  NT2_TEST_EQUAL(logical_or(boost::simd::Minf<T>(), boost::simd::Minf<T>()), r_t(true));
  NT2_TEST_EQUAL(logical_or(boost::simd::Nan<T>(), boost::simd::Nan<T>()), r_t(true));
  NT2_TEST_EQUAL(logical_or(boost::simd::One<T>(),boost::simd::Zero<T>()), r_t(true));
  NT2_TEST_EQUAL(logical_or(boost::simd::Zero<T>(), boost::simd::Zero<T>()), r_t(false));
} // end of test for floating_

NT2_TEST_CASE ( logical_or_bool)
{
  using boost::simd::logical_or;
  using boost::simd::tag::logical_or_;
  typedef boost::dispatch::meta::call<logical_or_(bool, bool)>::type r_t;
  typedef bool wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_EQUAL(logical_or(true, false), true);
  NT2_TEST_EQUAL(logical_or(false, true), true);
  NT2_TEST_EQUAL(logical_or(true, true), true);
  NT2_TEST_EQUAL(logical_or(false, false), false);
}


///////////////////////////////////////////////////////////////////////////////
// distribution::survival::failure_time::meta::include.hpp            		 //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_FAILURE_TIME_META_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_FAILURE_TIME_META_INCLUDE_HPP_ER_2009

#include <boost/statistics/detail/distribution/survival/failure_time/meta/distribution.hpp>
#include <boost/statistics/detail/distribution/survival/failure_time/meta/random_distribution.hpp>
#include <boost/statistics/detail/distribution/survival/failure_time/meta/random_generator.hpp>

#endif


#ifndef _REGEVPROOFS_HPP_
#define _REGEVPROOFS_HPP_
/* regevProofs.hpp - Proofs of correct behavior for Regev encryption
 *
 * Copyright (C) 2021, LWE-PVSS
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject
 * to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
 * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 **/
#include <vector>
#include <iostream>
#include <stdexcept>
#include <cmath>

#include <NTL/mat_ZZ.h>

#include "utils.hpp"
#include "regevEnc.hpp"
#include "ternaryMatrix.hpp"
#include "merlin.hpp"
#include "pedersen.hpp"
#include "shamir.hpp"
#include "bulletproof.hpp"
#include "utils.hpp"

//#define DEBUGGING

namespace REGEVENC {
using CRV25519::Point, DLPROOFS::PedersenContext, TOOLS::SharingParams,
    DLPROOFS::MerlinBPctx, DLPROOFS::LinConstraint, DLPROOFS::QuadConstraint;
// NOTE: REGEVENC::Scalar is not the same as CRV25519::Scalar

inline constexpr int PAD_SIZE=4; // add 4 scalars to pad to a specific sum-of-squares

// We break the decryption error vector into subvectors, each with this many scalars
#if 1 //ifndef DEBUGGING
inline constexpr int JLDIM = 256;  // Target dimension of Johnson–Lindenstrauss
inline constexpr int SQRT_JL =16;
inline constexpr int LINYDIM=128;  // Target dimension in approximate l-infty proofs
#else
inline constexpr int JLDIM = 16;
inline constexpr int SQRT_JL =4;
inline constexpr int LINYDIM =8;
#endif

// More functionality for a Merlin-context wrapper
struct MerlinRegev: public MerlinBPctx {
    MerlinRegev() = default;
    explicit MerlinRegev(const merlin_transcript& m): MerlinBPctx(m) {}
    explicit MerlinRegev(const std::string& st): MerlinBPctx(st) {}

    void processVector(const ALGEBRA::SVector& v,
                       unsigned char* label=nullptr, size_t llen=0) {
        if (v.length()==0) return;
        unsigned char buf[ALGEBRA::bytesPerScalar()];
        ALGEBRA::scalarBytes(buf, v[0], sizeof(buf));
        merlin_transcript_commit_bytes(&mctx,label,llen,buf,sizeof(buf));
        for (size_t i=1; i<v.length(); i++) {
            ALGEBRA::scalarBytes(buf, v[i], sizeof(buf));
            merlin_transcript_commit_bytes(&mctx,nullptr,0,buf,sizeof(buf));
        }
    }
    void processVector(const ALGEBRA::EVector& v,
                       unsigned char* label=nullptr, size_t llen=0) {
        if (v.length()==0) return;
        unsigned char buf[ALGEBRA::bytesPerScalar()*ALGEBRA::scalarsPerElement()];
        ALGEBRA::elementBytes(buf, v[0], sizeof(buf));
        merlin_transcript_commit_bytes(&mctx,label,llen,buf,sizeof(buf));
        for (size_t i=1; i<v.length(); i++) {
            ALGEBRA::elementBytes(buf, v[i], sizeof(buf));
            merlin_transcript_commit_bytes(&mctx,nullptr,0,buf,sizeof(buf));
        }
    }
    void processMatrix(const ALGEBRA::SMatrix& M,
                       unsigned char* label=nullptr, size_t llen=0) {
        if (M.NumRows()<1)
            return;
        processVector(M[0],label,llen);
        for (size_t i=1; i<M.NumRows(); i++) {
            processVector(M[i]);
        }
    }
    void processMatrix(const ALGEBRA::EMatrix& M,
                       unsigned char* label=nullptr, size_t llen=0) {
        if (M.NumRows()<1)
            return;
        processVector(M[0],label,llen);
        for (size_t i=1; i<M.NumRows(); i++) {
            processVector(M[i]);
        }
    }
    // a challenge n-by-m trenary matrix
    void newTernaryMatrix(const std::string& label,
                          ALGEBRA::TernaryMatrix& R, size_t n=0, size_t m=0) {
        if (n<=0) n = R.NumRows();
        if (m<=0) m = R.NumCols();
        if (n==0 || m==0) return; // nothing to do
        size_t bufSize = (n*m +3)/4; // two bits per element
        unsigned char buf[bufSize];
        merlin_transcript_challenge_bytes(&mctx,
            (const unsigned char*)label.data(),label.size(),buf,sizeof(buf));
        R.setFromBytes(buf, n, m);
    }
    void newTernaryEMatrix(const std::string& label,
                          ALGEBRA::TernaryEMatrix& R, size_t n=0, size_t m=0) {
        if (n<=0) n = R.NumRows();
        if (m<=0) m = R.NumCols();
        if (n==0 || m==0) return; // nothing to do
        size_t bufSize = ALGEBRA::scalarsPerElement()*((n*m +3)/4); // two bits per element
        unsigned char buf[bufSize];
        merlin_transcript_challenge_bytes(&mctx,
            (const unsigned char*)label.data(),label.size(),buf,sizeof(buf));
        R.setFromBytes(buf, n, m);
    }
    ALGEBRA::Scalar newScalar(const std::string& label) {
        unsigned char buf[ALGEBRA::bytesPerScalar()];
        merlin_transcript_challenge_bytes(&mctx,
            (const unsigned char*)label.data(),label.size(),buf,sizeof(buf));
        ALGEBRA::Scalar s;
        ALGEBRA::scalarFromBytes(s, buf, sizeof(buf));
        return s;
    }
    ALGEBRA::Element newElement(const std::string& label) {
        int bufSize = ALGEBRA::bytesPerScalar()*ALGEBRA::scalarsPerElement();
        unsigned char buf[bufSize];
        merlin_transcript_challenge_bytes(&mctx,
            (const unsigned char*)label.data(),label.size(),buf,sizeof(buf));
        ALGEBRA::Element e;
        ALGEBRA::elementFromBytes(e, buf, bufSize);
        return e;
    }
};

// A stucture for holding all the public data that both the prover and
// the verifier knows. The prover will fill in the commitments, both
// can use them to derive the various challenges in the proof prootcols.
struct VerifierData {
    // The global public key, and utility objects for Pedersen commitments
    // and for "Merlin Transcripts" (to derive challenges in Fiat-Shamir)
    GlobalKey *gk;        // global Regev key
    PedersenContext *ped; // Pedersen commitment context
    MerlinRegev *mer;     // Merlin transcript context
    SharingParams *sp;    // parameters of Shamir sharing

    // Size bounds for the various compressed (JL) vectors
    ALGEBRA::BigInt B_dNoise; // compressed decryption noise
    ALGEBRA::BigInt B_sk;     // compressed secret-key
    ALGEBRA::BigInt B_r;      // comressed encryption randomness
    ALGEBRA::BigInt B_eNoise1;// compressed small encryption noise
    ALGEBRA::BigInt B_eNoise2;// compressed large encryption noise
    ALGEBRA::BigInt B_kNoise; // compressed keygen noise
    ALGEBRA::BigInt B_smallness;// Used in the approximate smallness protocol
    int smlnsBits;  // The bitsize of B_smallness above
    ALGEBRA::BigInt radix; // the radix used to split large integers into digits

    std::vector<Point> Gs, Hs; // lists of generators

    // For most vectors, we have the vector itself (e.g., sk2), the
    // compressed vector (e.g., sk2Comp) and the padding that the prover
    // computes to pad the compressed vector to some pre-determined size
    // (e.g., sk2Pad). The ones without compression and padding are
    // sk1,pt1,pt2,y. For the noise vectors we only need the indexes
    // of the compressed vector and padding, not the original vector
    // (since there is no Pedersen commitment to that vector).

    // indexes into the generator list
    int pt1Idx, sk1Idx, dCompHiIdx, dPadHiIdx, dCompLoIdx, dPadLoIdx, // dec
        pt2Idx, rIdx, rCompIdx, rPadIdx, eComp1HiIdx, ePad1HiIdx, eComp1LoIdx,
        ePad1LoIdx, eComp2HiIdx, ePad2HiIdx, eComp2LoIdx, ePad2LoIdx,  // enc
        sk2Idx, sk2CompIdx, sk2PadIdx,
        kCompHiIdx, kPadHiIdx, kCompLoIdx, kPadLoIdx,               // keygen
        yIdx, wIdx;                        // smallness proof and aggregation

    // Commitments to different variables. The quadratic equations require
    // a double-commitment with respect to both the G and H generators,
    // while the linear constraints only need commitment wrt the Gs.

    Point pt1Com, sk1Com, dCompHiCom, dPadHiCom, dCompLoCom, dPadLoCom,// dec
        pt2Com, rCom, rCompCom, rPadCom, eComp1HiCom, ePad1HiCom, eComp1LoCom,
        ePad1LoCom, eComp2HiCom, ePad2HiCom, eComp2LoCom, ePad2LoCom,  // enc
        sk2Com, sk2CompCom, sk2PadCom,
        kCompHiCom, kPadHiCom, kCompLoCom, kPadLoCom,               // keygen
        yCom, wCom;                        // smallness proof and aggregation

    // A list of all the linear and quadratic constraints
    std::vector<DLPROOFS::LinConstraint> linConstr;
    std::vector<DLPROOFS::QuadConstraint> normConstr;

    // pointers into the above vectors of constraints
    static constexpr int dLinIdx=0,   // dec
        eLin1Idx=GlobalKey::ell,
        eLin2Idx=2*GlobalKey::ell,
        rLinIdx=3*GlobalKey::ell,     // enc
        kLinIdx=4*GlobalKey::ell,
        sk2LinIdx=5*GlobalKey::ell,   // keygen,
        smlnsLinIdx=6*GlobalKey::ell, // smallness
        reShrLinIdx=7*GlobalKey::ell; // resharing
    static constexpr int dHiQuadIdx=0, dLoQuadIdx=1, rQuadIdx=2,
        e1HiQuadIdx=3, e1LoQuadIdx=4, e2HiQuadIdx=5, e2LoQuadIdx=6,
        sk2QuadIdx=7, kHiQuadIdx=8, kLoQuadIdx=9;

    ALGEBRA::EVector z; // the masked vector z from the proof of smallness

    // Set the indexes to their default values
    void setIndexes();
    void computeGenerators();

    // Reset when preparing for a new proof at the prover's site
    void prepareForNextProof() {
        std::swap(sk1Idx,sk2Idx);
        sk1Com = sk2Com; // copy commitment to the previous sk wrt the Gs

        // zero out all the commitments and constraints
        pt1Com= dCompHiCom= dPadHiCom= dCompLoCom= dPadLoCom= pt2Com= // dec
        rCom= rCompCom= rPadCom= eComp1HiCom= ePad1HiCom= eComp1LoCom=
        ePad1LoCom= eComp2HiCom= ePad2HiCom= eComp2LoCom= ePad2LoCom= // enc
        sk2Com= sk2CompCom= sk2PadCom=
        kCompHiCom= kPadHiCom= kCompLoCom= kPadLoCom=              // keygen
        yCom= wCom = Point::identity();

        // empty the constraints w/o invalidating the pointers to them
        DLPROOFS::LinConstraint emptyLin;
        for (auto& lc : linConstr) lc = emptyLin;
        DLPROOFS::QuadConstraint emptyQuad;
        for (auto& qc : normConstr) qc = emptyQuad;
    }

    VerifierData() = default;
    VerifierData(GlobalKey& gk, PedersenContext& ped, MerlinRegev& mer,
                 const SharingParams& sp);
};

// For each commitment in the VerifierData, the ProverData must hold
// the corresponding opening: the randomness r and the witness values
struct ProverData {
    VerifierData *vd;

    // commitment randomness
    CRV25519::Scalar pt1Rnd, sk1Rnd,
        dCompHiRnd, dPadHiRnd, dCompLoRnd, dPadLoRnd,                  // dec
        pt2Rnd, rRnd, rCompRnd, rPadRnd, eComp1HiRnd, ePad1HiRnd, eComp1LoRnd,
        ePad1LoRnd, eComp2HiRnd, ePad2HiRnd, eComp2LoRnd, ePad2LoRnd,  // enc
        sk2Rnd, sk2CompRnd, sk2PadRnd,
        kCompHiRnd, kPadHiRnd, kCompLoRnd, kPadLoRnd,               // keygen
        yRnd, wRnd;                    // smallness proof and aggregation

    // The compressed vectors, concatenated
    ALGEBRA::EVector compressed;

    // The witnesses for the linear and quadratic constraints

    DLPROOFS::PtxtVec linWitness;
    DLPROOFS::PtxtVec quadWitnessG, quadWitnessH;

    // Reset when preparing for a new proof at the prover's site
    void prepareForNextProof() {
        vd->prepareForNextProof(); // reset the public data
        sk1Rnd = sk2Rnd; // randomness of commitment to secret key wrt Gs
    }

    ProverData() = default;
    ProverData(VerifierData& verData): vd(&verData) {
        compressed.SetLength(10*(JLDIM+PAD_SIZE)/ALGEBRA::scalarsPerElement());
   }
};

// Proof of decryption. We assume that the ProverData,VerifierData are
// already initialized, and that ProverData contains sk1 and VerifierData
// contains a commitment to it.
void proveDecryption(ProverData& pd,
    const ALGEBRA::EMatrix& ctMat, const ALGEBRA::EVector& ctVec,
    const ALGEBRA::SVector& ptxt, const ALGEBRA::EVector& skey,
    const ALGEBRA::EVector& noise);

void verifyDecryption(VerifierData& vd, // vd has all the commitments
        const ALGEBRA::EMatrix& ctMat, const ALGEBRA::EVector& ctVec);

// Proof of encryption
void proveEncryption(ProverData& pd,
        const ALGEBRA::EVector& ct1, const ALGEBRA::EVector& ct2,
        const ALGEBRA::SVector& ptxt, const ALGEBRA::EVector& rnd,
        const ALGEBRA::EVector& noise1,const ALGEBRA::EVector& noise2);

void verifyEncryption(VerifierData& vd,
        const ALGEBRA::EVector& ct1, const ALGEBRA::EVector& ct2);

// Proof of key-generation. pkNum is index of the pkey in the GlobalKey
void proveKeyGen(ProverData& pd, int pkNum,
        const ALGEBRA::EVector& sk, const ALGEBRA::EVector& noise);

void verifyKeyGen(VerifierData& vd, int pkNum);

// Proof of correct re-sharing. It is assumed that pt1, pt2
// are already initialized in the ps structure
void proveReShare(ProverData& pd, const ALGEBRA::SVector& lagrange,
        const ALGEBRA::SVector& pt1, const ALGEBRA::SVector& pt2);
void verifyReShare(VerifierData& vd, const ALGEBRA::SVector& lagrange);
    // TOOLS::EvalSet from shamir.hpp describes the reconstruction set

// Proof of approximate smallness (of all except the compressed vectors)
void proveSmallness(ProverData& pd);
void verifySmallness(VerifierData& vd);

struct ReadyToVerify {
    // The linear and quadratic constraints and commitments
    DLPROOFS::LinConstraint linCnstr;
    Point linCom;
    DLPROOFS::QuadConstraint quadCnstr;
    Point quadCom;

    // The offset used for the G, H witnesses in the quadratic proof
    DLPROOFS::PtxtVec deltaG, deltaH;

    // temporary variables used in aggregating the proofs
    std::vector<CRV25519::Scalar> rVec, uVec;
    DLPROOFS::PtxtVec as, bs;

    // Flattened versions of the statements and generators
    std::vector<Point> linGs;
    std::vector<CRV25519::Scalar> linStmnt;

    std::vector<Point> quadGs;
    std::vector<Point> quadHs;
    std::vector<CRV25519::Scalar> offstG;
    std::vector<CRV25519::Scalar> offstH;

    void aggregateVerifier1(VerifierData& vd);
    void aggregateVerifier2(VerifierData& vd);

    void flattenLinVer(VerifierData& vd);
    void flattenQuadVer(VerifierData& vd);
};
struct ReadyToProve : public ReadyToVerify {
    // Randomness used for linCom and quadCom
    CRV25519::Scalar lComRnd, qComRnd;

    // Flattened versions of the witnesses
    std::vector<CRV25519::Scalar> linWtns;
    std::vector<CRV25519::Scalar> quadWtnsG, quadWtnsH;

    void aggregateProver(ProverData& pd);
    void flattenLinPrv(ProverData& pd);
    void flattenQuadPrv(ProverData& pd);
};

/****** utility functions *******/

// Compute the vector (1,x,x^2,...,x^{len-1})
void powerVector(ALGEBRA::SVector& vec, const ALGEBRA::Scalar& x, int len);
void powerVector(ALGEBRA::EVector& vec, const ALGEBRA::Element& x, int len);
void powerVector(std::vector<CRV25519::Scalar>& vec, const CRV25519::Scalar& x, int len);

// Commit to a slice of the vector
Point commit(const ALGEBRA::SVector& v, size_t genIdx,
             const std::vector<Point>& Gs, CRV25519::Scalar& r,
             int fromIdx=0, int toIdx=-1);
Point commit(const ALGEBRA::EVector& v, size_t genIdx,
             const std::vector<Point>& Gs, CRV25519::Scalar& r,
             int fromIdx=0, int toIdx=-1);
// commit to the same xes wrt both Gs and Hs
Point commit2(const ALGEBRA::EVector& v, size_t genIdx,
             const std::vector<Point>& Gs, const std::vector<Point>& Hs,
             CRV25519::Scalar& r, int fromIdx=0, int toIdx=-1);

// Add to v four integers a,b,c,d such that the result
// (v | a,b,c,d) has norm exactly equal to the bound
void pad2exactNorm(const ALGEBRA::SVector& v,
         ALGEBRA::SVector& padding, const ALGEBRA::BigInt& bound);
void pad2exactNorm(const ALGEBRA::EVector& v,
        ALGEBRA::EVector& padding, const ALGEBRA::BigInt& bound);
void pad2exactNorm(const ALGEBRA::Element* v, size_t len,
        ALGEBRA::Element* padSpace, const ALGEBRA::BigInt& bound);

// The expand functions below assume that GF(p^ell) is represented
// modulo X^ell +1.

// Expand a constraint a*x with a in GF(p^ell) to e constrints over scalars,
// namely store in 'constrs' ell constraints in ell variables, representing
// the ell-by-ell scalar matrix for multiply-by-a. The variables in these
// constraints are indexed by idx,idx+1,... For example, the constraints
// for the 1st row has coeffs: idx->a.freeTerm, idx+1 -> a.coeffOf(X),...
void expandConstraints(LinConstraint* constrs, int idx, const ALGEBRA::Element& a);

// Expand each of the constraints v[i]*x with successive indexes
void expandConstraints(LinConstraint* constrs, int idx,
                       const ALGEBRA::EVector& v, int from=0, int to=-1);

// This function is for the case where the secret variables are from Z_p,
// namely we have the constraint <x,v>=b over GF(p^ell), but x is over Z_p.
void makeConstraints(LinConstraint* constrs, int idx,
                     const ALGEBRA::EVector& v, int from=0, int to=-1);

// This function sets the equalsTo field for the ell constraints
// corresponding to the ell scalars representing the element e
void setEqsTo(LinConstraint* constrs, const ALGEBRA::Element& e);

bool checkQuadCommit(DLPROOFS::QuadConstraint& c,
    const Point& com, const Point& padCom, const CRV25519::Scalar& rnd,
    const CRV25519::Scalar& padRnd, DLPROOFS::PtxtVec& witness, PedersenContext* ped);
bool checkLinCommit(DLPROOFS::PtxtVec& pv,
    const Point& com, const CRV25519::Scalar& rnd,
    DLPROOFS::PtxtVec& witness, PedersenContext* ped);

} // end of namespace REGEVENC
#endif // ifndef _REGEVPROOFS_HPP_


/**
 * \file boost/numeric/ublasx/operation/num_columns.hpp
 *
 * \brief The \c num_columns operation.
 *
 * Copyright (c) 2009, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_NUM_COLUMNS_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_NUM_COLUMNS_HPP

/*
#include <boost/version.hpp>

#if BOOST_VERSION > 105100L

#include <boost/numeric/ublas/operation/num_columns.hpp>

namespace boost { namespace numeric { namespace ublasx {

using ::boost::numeric::ublas::num_columns;

}}} // Namespace boost::numeric::ublasx

#else // BOOST_VERSION

#include <boost/numeric/ublas/detail/config.hpp>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/traits.hpp>

namespace boost { namespace numeric { namespace ublasx {

template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename matrix_traits<MatrixExprT>::size_type num_columns(matrix_expression<MatrixExprT> const& me)
{
	return me().size2();
}

#endif // BOOST_VERSION
*/
#include <boost/numeric/ublas/operation/num_columns.hpp>

namespace boost { namespace numeric { namespace ublasx {

using ::boost::numeric::ublas::num_columns;

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_NUM_COLUMNS_HPP


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 integration toolbox - qmtc"

#include <nt2/include/functions/qmtc.hpp>
#include <nt2/integration/output.hpp>
#include <nt2/integration/options.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/rowvect.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/log.hpp>
#include <nt2/include/functions/oneminus.hpp>
#include <nt2/include/functions/expm1.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/if_one_else_zero.hpp>
#include <nt2/include/functions/globalsum.hpp>
#include <nt2/include/functions/globalmax.hpp>
#include <nt2/include/functions/mul_i.hpp>
#include <nt2/include/functions/linspace.hpp>
#include <nt2/include/functions/dist.hpp>
#include <nt2/include/functions/rec.hpp>
#include <nt2/include/functions/minusone.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/functions/cons.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/sqrteps.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/sqrteps.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/i.hpp>
#include <nt2/table.hpp>
#include <boost/dispatch/meta/strip.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>

struct f
{
  template<class Sig>
  struct result;

  template<class This, class X>
  struct result<This(X)>
  {
    typedef nt2::table< typename boost::dispatch::meta::strip<X>::type::value_type > type;
  };

  template < class X > inline
  nt2::table< typename X::value_type> operator()(const X & x ) const
  {
    typedef typename X::value_type T;
    return nt2::if_one_else_zero(nt2::sqr(x(1, nt2::_))+nt2::sqr(x(2, nt2::_)) <= nt2::One<T>());
  }
};

NT2_TEST_CASE_TPL( qmtc_real_out, NT2_REAL_TYPES )
{
  using nt2::qmtc;
  using nt2::options;
  using nt2::integration::output;
  typedef nt2::table<T> tab_t;
  tab_t x =  nt2::cons(nt2::of_size(2, 2), T(0), T(0), T(1), T(1));
  BOOST_AUTO_TPL(res, (qmtc(f(), x, options [ nt2::limits::maxfunccnt_ = 100000 ])));
  NT2_TEST_LESSER_EQUAL(nt2::dist(res.integrals(1), nt2::Pi<T>()/4), res.errors);
}

struct g
{
  template<class Sig>
  struct result;

  template<class This, class X>
  struct result<This(X)>
  {
    typedef nt2::table< std::complex<typename boost::dispatch::meta::strip<X>::type::value_type> > type;
  };

  template < class X > inline
  nt2::table< std::complex<typename X::value_type> > operator()(const X & x ) const
  {
    typedef typename std::complex<typename X::value_type> cT;
    return x(1, nt2::_)+nt2::I<cT>()*x(2, nt2::_);
  }
};

NT2_TEST_CASE_TPL( qmtc_cplx_out, NT2_REAL_TYPES )
{
  using nt2::qmtc;
  using nt2::options;
  using nt2::integration::output;
  typedef typename nt2::meta::as_complex<T>::type cT;
  typedef nt2::table<T> tab_t;
  tab_t x =  nt2::cons(nt2::of_size(2, 2), T(0), T(0), T(1), T(1));
  BOOST_AUTO_TPL(res, (qmtc(g(), x, options [ nt2::limits::maxfunccnt_ = 100000 ])));
  NT2_TEST_LESSER_EQUAL(nt2::dist(res.integrals(1), cT(0.5, 0.5)), res.errors);
}

NT2_TEST_CASE_TPL( qmtc_flt_out2, NT2_REAL_TYPES )
{
  using nt2::qmtc;
  using nt2::options;
  using nt2::integration::output;
  typedef nt2::table<T> tab_t;
  tab_t x =  nt2::cons(nt2::of_size(2, 2, 2), T(0), T(0), T(1), T(1), T(-1), T(-1), T(0), T(0));
  BOOST_AUTO_TPL(res, (qmtc(f(), x, options [ nt2::limits::maxfunccnt_ = 100000 ])));
  NT2_TEST_LESSER_EQUAL(nt2::dist(res.integrals(1), nt2::Pi<T>()/2), res.errors);
}


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#include "Math.hpp"

#include "MetricFactory.hpp"
#include "Random.hpp"

#include <rw/core/macros.hpp>

#include <boost/math/special_functions/fpclassify.hpp>    // boost::math::isnan()
#include <cmath>
#include <limits>

using namespace rw::math;

double Math::ranNormalDist (double mean, double sigma)
{
    return Random::ranNormalDist (mean, sigma);
}

double Math::ran ()
{
    return Random::ran ();
}

void Math::seed (unsigned seed)
{
    Random::seed (seed);
    srand (seed);
}

void Math::seed ()
{
    Random::seed ();
    srand ((unsigned int)time (NULL));
}

rw::math::Q Math::clampQ (const rw::math::Q& q, const rw::math::Q& min, const rw::math::Q& max)
{
    assert (q.size () == min.size ());
    assert (q.size () == max.size ());

    Q qres (q.size ());
    for (size_t i = 0; i < q.size (); i++)
        qres (i) = clamp (q (i), min (i), max (i));

    return qres;
}
double Math::ran (double from, double to)
{
    return Random::ran (from, to);
}

int Math::ranI (int from, int to)
{
    return Random::ranI (from, to);
}

Q Math::ranQ (const rw::math::Q& from, const rw::math::Q& to)
{
    RW_ASSERT (from.size () == to.size ());

    const size_t len = from.size ();
    Q result (len);
    for (size_t i = 0; i < len; i++)
        result (i) = ran (from (i), to (i));
    return result;
}

Q Math::ranQ (const std::pair< rw::math::Q, rw::math::Q >& bounds)
{
    return ranQ (bounds.first, bounds.second);
}

/**
 * @brief Returns a random direction in \b dim dimensions.
 *
 * The length of the vector is given by \b length;
 *
 * @param dim [in] Number of dimensions
 * @param length [in] Length of return vector. Default is 1;
 * @return Random direction
 */
Q Math::ranDir (size_t dim, double length)
{
    Q q (dim);
    for (size_t i = 0; i < dim; i++) {
        q (i) = Math::ranNormalDist (0, 1);
    }
    Metric< Q >::Ptr metric = MetricFactory::makeEuclidean< Q > ();
    return length * q / metric->distance (q);
}

/**
 * @brief Returns a weighted random direction in \b dim dimensions.
 *
 * The length of the vector is given by \b length;
 *
 * @param dim [in] Number of dimensions
 * @param weights [in] Weights to use
 * @param length [in] Length of return vector when weights are applied as weighted Euclidean metric.
 * Default is 1;
 * @return Random weigthed direction
 */
Q Math::ranWeightedDir (size_t dim, const rw::math::Q& weights, double length)
{
    Q q (dim);
    for (size_t i = 0; i < dim; i++) {
        q (i) = Math::ranNormalDist (0, 1);
    }
    Metric< Q >::Ptr metric = MetricFactory::makeWeightedEuclidean< Q > (weights);
    return length * q / metric->distance (q);
}

Q Math::sqr (const Q& q)
{
    const size_t dim = q.size ();
    Q result (dim);
    for (size_t i = 0; i < dim; i++) {
        result[i] = Math::sqr (q[i]);
    }
    return result;
}

Q Math::sqrt (const Q& q)
{
    const size_t dim = q.size ();
    Q result (dim);
    for (size_t i = 0; i < dim; i++) {
        result[i] = std::sqrt (q[i]);
    }
    return result;
}

int Math::ceilLog2 (const int n)
{
    RW_ASSERT (n > 0);
    int cnt = 0;
    int i   = n;
    int a   = 1;
    while (i != 1) {
        a <<= 1;
        i >>= 1;
        ++cnt;
    }
    if (a == n)
        return cnt;
    else
        return cnt + 1;
}

bool Math::isNaN (double d)
{
    return boost::math::isnan (d);
}

double Math::NaN ()
{
    return std::numeric_limits< double >::quiet_NaN ();
}


#include <iostream>
#include <string>
#include <vector>
#include <utility>
#include <algorithm>
#include <iterator>
#include <type_traits>

#include <boost/utility/enable_if.hpp>
#include <boost/iterator/iterator_adaptor.hpp>

template<typename Iterator, typename UnaryFunction
        ,typename Reference
            = std::result_of_t<const UnaryFunction(typename std::iterator_traits<Iterator>::reference)>
        ,typename Value = std::remove_cv_t<std::remove_reference_t<Reference>>
        >
class transform_iterator
    : public boost::iterator_adaptor<
                transform_iterator<Iterator,UnaryFunction,Reference,Value>
            ,   Iterator
            ,   Value
            ,   boost::use_default
            ,   Reference
            >
{
    struct enabler { };
    using self = transform_iterator;
public:
    using value_type            = Value;
    using reference             = Reference;
    using pointer               = typename std::iterator_traits<Iterator>::pointer;
    using difference_type       = typename std::iterator_traits<Iterator>::difference_type;
    using iterator_category     = typename std::iterator_traits<Iterator>::iterator_category;

    transform_iterator() = default;
    transform_iterator( const Iterator& it, UnaryFunction function )
        : self::iterator_adaptor_{it}, m_function{function} { }

    template<typename I2, typename F2, typename R2,typename V2>
    transform_iterator(
        const transform_iterator<I2,F2,R2,V2>& other
       ,typename boost::enable_if<
                boost::is_convertible<I2,Iterator>
               ,enabler
        >::type = enabler()
       ,typename boost::enable_if<
                boost::is_convertible<F2,UnaryFunction>
               ,enabler
        >::type = enabler()
       ,typename boost::enable_if<
                boost::is_convertible<Reference,R2>
               ,enabler
        >::type = enabler()
       ,typename boost::enable_if<
                boost::is_convertible<Value,V2>
               ,enabler
        >::type = enabler()
    )
        : self::iterator_adaptor_{other.base()}, m_function{other.m_function}
        { }

    UnaryFunction functor() const { return m_function; }
private:
    friend boost::iterator_core_access;

    reference dereference() const
    {
        return m_function(*(this->base_reference()));
    }
// -- member data
    UnaryFunction m_function;
};
template<typename Iterator, typename Function
        >
auto make_transform_iterator( const Iterator& it, Function function )
{
    return transform_iterator<Iterator,Function>(it,function);
}

int main()
{
    auto xs = std::vector<int>{1,2,3,4,5,6,7,8,9};
    auto ys = std::vector<int>{};

    std::cout << "xs:\n";
    for( auto x : xs )
        std::cout << x << ", ";
    std::cout << std::endl;

    auto doubler = [](int i){ return i*i; };
    std::copy( make_transform_iterator(xs.cbegin(),doubler)
             , make_transform_iterator(xs.cend(),doubler)
             , std::back_inserter(ys)
             );

    std::cout << "After first copy:\n";
    for( auto i : ys )
        std::cout << i << ", ";
    std::cout << std::endl;

    auto plus100 = [](int i){ return i+100; };
    std::copy( make_transform_iterator(ys.cbegin(),plus100)
             , make_transform_iterator(ys.cend(),plus100)
             , std::ostream_iterator<int>(std::cout, " ")
             );
}


#ifndef VDP_SIM_H
#define VDP_SIM_H

#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>

#include <boost/array.hpp>

class vdp_sim
{
public:
    using input_type = boost::array<double,1>;
    using state_type = boost::array<double,2>;
    using time = double;

    vdp_sim(time _period, time _ts) :
        period{_period},
        ts{_ts}
    {
        size_t K = std::ceil(period/ts);
        u_t = std::vector<time>(K);

        double t = 0.;
        std::generate(u_t.begin(),
                    u_t.end(),
                    [&t, _ts] () {return t+=_ts;});

        u = std::vector<input_type>(K);
    }

    void generate_data(const std::string& dir, size_t n);

private:
    time period;
    time ts;

    std::vector<time> u_t;
    std::vector<input_type> u;

    std::ofstream data_stream;

    const input_type
    interp1d(const std::vector<time>& u_t,
            const std::vector<input_type>& u,
            const time t);
};

#endif