#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <stdexcept>
#include <functional>
#include <random>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include <arc_utilities/eigen_helpers.hpp>
#include <arc_utilities/eigen_helpers_conversions.hpp>
#include <arc_utilities/pretty_print.hpp>
#include <arc_utilities/voxel_grid.hpp>
#include <arc_utilities/simple_rrt_planner.hpp>
#include <uncertainty_planning_core/simple_pid_controller.hpp>
#include <uncertainty_planning_core/simple_uncertainty_models.hpp>
#include <uncertainty_planning_core/uncertainty_contact_planning.hpp>
#include <uncertainty_planning_examples/se2_common_config.hpp>
#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <uncertainty_planning_examples/SetSimple6dofActuationError.h>
#include <uncertainty_planning_examples/Simple6dofRobotMove.h>

using namespace uncertainty_contact_planning;

inline std::vector<Eigen::Matrix<double, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1>>> move_robot(const Eigen::Matrix<double, 3, 1>& target_config, const Eigen::Matrix<double, 3, 1>& expected_result_config, const double duration, const double execution_shortcut_distance, const bool reset, ros::ServiceClient& robot_control_service)
{
    UNUSED(expected_result_config);
    const Eigen::Isometry3d target_transform = Eigen::Translation3d(target_config(0), target_config(1), 0.0) * Eigen::Quaterniond(Eigen::AngleAxisd(target_config(2), Eigen::Vector3d::UnitZ()));
    const geometry_msgs::PoseStamped target_pose = EigenHelpersConversions::EigenIsometry3dToGeometryPoseStamped(target_transform, "world");
    // Put together service call
    uncertainty_planning_examples::Simple6dofRobotMove::Request req;
    req.time_limit = ros::Duration(duration);
    req.execution_shortcut_distance = execution_shortcut_distance;
    if (reset)
    {
        std::cout << "Resetting robot to transform: " << PrettyPrint::PrettyPrint(target_transform) << std::endl;
        req.start = target_pose;
        req.mode = uncertainty_planning_examples::Simple6dofRobotMove::Request::RESET;
    }
    else
    {
        std::cout << "Commanding robot to transform: " << PrettyPrint::PrettyPrint(target_transform) << std::endl;
        req.target = target_pose;
        req.mode = uncertainty_planning_examples::Simple6dofRobotMove::Request::EXECUTE;
    }
    uncertainty_planning_examples::Simple6dofRobotMove::Response res;
    // Call service
    try
    {
        robot_control_service.call(req, res);
    }
    catch (...)
    {
        ROS_ERROR("Move service failed");
    }
    // Unpack result
    const std::vector<geometry_msgs::PoseStamped>& poses = res.trajectory;
    std::vector<Eigen::Matrix<double, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1>>> configs(poses.size());
    for (size_t idx = 0; idx < poses.size(); idx++)
    {
        const Eigen::Isometry3d& transform = EigenHelpersConversions::GeometryPoseToEigenIsometry3d(poses[idx].pose);
        const Eigen::Vector3d position = transform.translation();
        const Eigen::AngleAxisd rotation(transform.rotation());
        const Eigen::Vector3d axis = rotation.axis();
        const double axis_dot = axis.dot(Eigen::Vector3d::UnitZ());
        const double angle = rotation.angle();
        const double zr = (axis_dot > 0.0) ? angle : -angle;
        if (((std::abs(zr) < 0.00001) || (std::abs(axis_dot) > 0.99)) == false)
        {
            std::cout << "WARNING - Dot product: " << axis_dot << ", Angle: " << zr << ", Rotation axis: " << PrettyPrint::PrettyPrint(axis) << std::endl;
        }
        const double x = position.x();
        const double y = position.y();
        if (std::abs(position.z()) >= 0.001)
        {
            std::cout << "WARNING - Z: " << position.z() << std::endl;
        }
        Eigen::Matrix<double, 3, 1> config;
        config << x, y, zr;
        configs[idx] = config;
    }
    std::cout << "Reached transform: " << PrettyPrint::PrettyPrint(configs.back()) << std::endl;
    return configs;
}

void set_uncertainty(const double max_translation_error, const double max_rotation_error, ros::ServiceClient& set_uncertainty_service)
{
    uncertainty_planning_examples::SetSimple6dofActuationErrorRequest req;
    req.translation_actuator_error = max_translation_error;
    req.rotation_actuation_error = max_rotation_error;
    uncertainty_planning_examples::SetSimple6dofActuationErrorResponse res;
    if (set_uncertainty_service.call(req, res) == false)
    {
        throw std::invalid_argument("SetActuationError failed");
    }
}

void peg_in_hole_env_se2(ros::Publisher& display_debug_publisher, ros::ServiceClient& robot_control_service, ros::ServiceClient& set_uncertainty_service)
{
    const uncertainty_planning_core::PLANNING_AND_EXECUTION_OPTIONS options = se2_common_config::GetOptions();
    const config_common::TASK_CONFIG_PARAMS extra_options = se2_common_config::GetExtraOptions();
    std::cout << PrettyPrint::PrettyPrint(options) << "\n" << PrettyPrint::PrettyPrint(extra_options) << std::endl;
    const std::pair<uncertainty_planning_core::SE2Config, uncertainty_planning_core::SE2Config> start_and_goal = se2_common_config::GetStartAndGoal();
    const uncertainty_planning_core::SE2SamplerPtr sampler = se2_common_config::GetSampler();
    const simple_robot_models::SE2_ROBOT_CONFIG robot_config = se2_common_config::GetDefaultRobotConfig(extra_options);
    const uncertainty_planning_core::SE2Robot robot = se2_common_config::GetRobot(robot_config);
    const uncertainty_planning_core::SE2SimulatorPtr simulator = se2_common_config::GetSimulator(extra_options, options.debug_level);
    // Load the policy
    try
    {
        auto policy = uncertainty_planning_core::LoadSE2Policy(options.planned_policy_file);
        policy.SetPolicyActionAttemptCount(options.policy_action_attempt_count);
        std::map<std::string, double> complete_policy_stats;
    #ifndef FORCE_DEBUG
        std::cout << "Press ENTER to simulate policy..." << std::endl;
        std::cin.get();
    #endif
        const auto policy_simulation_results = uncertainty_planning_core::SimulateSE2UncertaintyPolicy(options, robot, simulator, sampler, policy, start_and_goal.first, start_and_goal.second, display_debug_publisher);
        const std::map<std::string, double> policy_simulation_stats = policy_simulation_results.second.first;
        const std::vector<int64_t> policy_simulation_step_counts = policy_simulation_results.second.second.first;
        std::cout << "Policy simulation success: " << PrettyPrint::PrettyPrint(policy_simulation_stats) << std::endl;
        complete_policy_stats.insert(policy_simulation_stats.begin(), policy_simulation_stats.end());
    #ifndef FORCE_DEBUG
        std::cout << "Press ENTER to execute policy..." << std::endl;
        std::cin.get();
    #endif
        if (options.num_policy_executions > 0)
        {
            std::cout << "Setting actuation uncertainty..." << std::endl;
            set_uncertainty(robot_config.max_actuator_noise, robot_config.r_max_actuator_noise, set_uncertainty_service);
        }
        std::function<std::vector<Eigen::Matrix<double, 3, 1>, std::allocator<Eigen::Matrix<double, 3, 1>>>(const Eigen::Matrix<double, 3, 1>&, const Eigen::Matrix<double, 3, 1>&, const double, const double, const bool)> robot_execution_fn = [&] (const Eigen::Matrix<double, 3, 1>& target_configuration, const Eigen::Matrix<double, 3, 1>& expected_result_configuration, const double duration, const double execution_shortcut_distance, const bool reset) { return move_robot(target_configuration, expected_result_configuration, duration, execution_shortcut_distance, reset, robot_control_service); };
        const auto policy_execution_results = uncertainty_planning_core::ExecuteSE2UncertaintyPolicy(options, robot, simulator, sampler, policy, start_and_goal.first, start_and_goal.second, robot_execution_fn, display_debug_publisher);
        const std::map<std::string, double> policy_execution_stats = policy_execution_results.second.first;
        const std::vector<int64_t> policy_execution_step_counts = policy_execution_results.second.second.first;
        const std::vector<double> policy_execution_times = policy_execution_results.second.second.second;
        std::cout << "Policy execution success: " << PrettyPrint::PrettyPrint(policy_execution_stats) << std::endl;
        complete_policy_stats.insert(policy_execution_stats.begin(), policy_execution_stats.end());
        // Save the executed policy
        uncertainty_planning_core::SaveSE2Policy(policy_execution_results.first, options.executed_policy_file);
        // Print out the results & save them to the log file
        const std::string log_results = "++++++++++\n" + PrettyPrint::PrettyPrint(options) + "\n" + PrettyPrint::PrettyPrint(extra_options) + "\nRESULTS:\n" + PrettyPrint::PrettyPrint(complete_policy_stats, false, "\n") + "\nSimulation step counts: " + PrettyPrint::PrettyPrint(policy_simulation_step_counts) + "\nExecution step counts: " + PrettyPrint::PrettyPrint(policy_execution_step_counts) + "\nExecution times: " + PrettyPrint::PrettyPrint(policy_execution_times);
        std::cout << "Policy results:\n" << log_results << std::endl;
        std::ofstream log_file(options.policy_log_file, std::ios_base::out | std::ios_base::app);
        if (!log_file.is_open())
        {
            std::cerr << "\x1b[31;1m Unable to create folder/file to log to: " << options.policy_log_file << "\x1b[37m \n";
            throw std::invalid_argument( "Log filename must be write-openable" );
        }
        log_file << log_results << std::endl;
        log_file.close();
    }
    catch (...)
    {
        std::cout << "!!! Policy file does not exist, skipping !!!" << std::endl;
        const std::string log_results = "++++++++++\n" + PrettyPrint::PrettyPrint(options) + "\n" + PrettyPrint::PrettyPrint(extra_options) + "\nRESULTS:\n(Execution) Policy success: 0\n(Simulation) Policy success: 0\n(Simulation) Policy successful simulator resolves: 0\n(Simulation) Policy unsuccessful simulator environment resolves: 0\n(Simulation) Policy unsuccessful simulator resolves: 0\n(Simulation) Policy unsuccessful simulator self-collision resolves: 0\nSimulation step counts: -1\nExecution step counts: -1\nExecution times: -0.0";
        std::ofstream log_file(options.policy_log_file, std::ios_base::out | std::ios_base::app);
        if (!log_file.is_open())
        {
            std::cerr << "\x1b[31;1m Unable to create folder/file to log to: " << options.policy_log_file << "\x1b[37m \n";
            throw std::invalid_argument( "Log filename must be write-openable" );
        }
        log_file << log_results << std::endl;
        log_file.close();
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "se2_contact_execution_node");
    ros::NodeHandle nh;
    ROS_INFO("Starting SE(2) Contact Execution Node...");
    ros::Publisher display_debug_publisher = nh.advertise<visualization_msgs::MarkerArray>("uncertainty_planning_debug_display_markers", 1, true);
    ros::ServiceClient robot_control_service = nh.serviceClient<uncertainty_planning_examples::Simple6dofRobotMove>("simple_6dof_robot_move");
    ros::ServiceClient set_uncertainty_service = nh.serviceClient<uncertainty_planning_examples::SetSimple6dofActuationError>("simple6dof_robot/set_actuation_uncertainty");
    peg_in_hole_env_se2(display_debug_publisher, robot_control_service, set_uncertainty_service);
    return 0;
}


// Copyright 2006. Peter Gottschling, Matthias Troyer, Rolf Bonderer
// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MATH_IS_INVERTIBLE_INCLUDE
#define MATH_IS_INVERTIBLE_INCLUDE

#include <boost/numeric/linear_algebra/operators.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>

namespace math {

template <typename Operation, typename Element>
struct is_invertible_t
{
    // bool operator()(const Operation&, const Element&) const;
};

// By default all elements are invertible w.r.t. addition
// If only part of the elements are invertible it shall be handled by specialization of the type
// Whether invertibility is relevant at all shall be concrolled by the user with concept maps
template <typename Element>
struct is_invertible_t< add<Element>, Element >
  : public std::binary_function<add<Element>, Element, Element>
{
    bool operator() (const add<Element>&, const Element&) const
    {
	return true;
    }
};

// By default all non-zero elements are invertible w.r.t. multiplication
// If another part of the elements or all elements are invertible it shall be handled by specialization of the type
// Whether invertibility is relevant at all shall be concrolled by the user with concept maps
template <typename Element>
struct is_invertible_t< mult<Element>, Element >
  : public std::binary_function<mult<Element>, Element, Element>
{
    bool operator() (const mult<Element>&, const Element& v) const
    {
	return v != zero(v);
    }
};

// Function is shorter than typetrait-like functor
template <typename Operation, typename Element>
inline bool is_invertible(const Operation& op, const Element& v)
{
    return is_invertible_t<Operation, Element>() (op, v);
}

namespace detail {

    // Helper type whose operator returns true if v is not 0
    // 0 must be convertible into Element and Element must be EqualityComparable
    template <typename Operation, typename Element>
    struct non_zero_is_invertible_t
    {
	bool operator() (const Operation&, const Element& v)
	{
	    return v != Element(0);
	}
    };

} // namespace detail

} // namespace math

#endif // MATH_IS_INVERTIBLE_INCLUDE


/**
 * \ file SimpleOverdriveFilter.cpp
 */

#include <array>
#include <fstream>

#include <ATK/config.h>

#include <ATK/EQ/ButterworthFilter.h>
#include <ATK/EQ/IIRFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>

#include <ATK/Distortion/SimpleOverdriveFilter.h>

#include <ATK/Mock/TriangleCheckerFilter.h>

#include <ATK/Tools/DecimationFilter.h>
#include <ATK/Tools/OversamplingFilter.h>
#include <ATK/Tools/SumFilter.h>
#include <ATK/Tools/VolumeFilter.h>

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

#define PROCESSSIZE (1000)

BOOST_AUTO_TEST_CASE( SimpleOverdriveFilter_const_sin1k )
{
  std::array<double, PROCESSSIZE> data;
  {
    std::ifstream input(ATK_SOURCE_TREE "/tests/data/input1.dat", std::ios::binary);
    input.read(reinterpret_cast<char*>(data.data()), PROCESSSIZE * sizeof(double));
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> datacheck;
  {
    std::ifstream check(ATK_SOURCE_TREE "/tests/data/output4_so.dat", std::ios::binary);
    check.read(reinterpret_cast<char*>(datacheck.data()), PROCESSSIZE * sizeof(double));
  }

  ATK::InPointerFilter<double> check(datacheck.data(), 1, PROCESSSIZE, false);
  check.set_output_sampling_rate(48000);

  ATK::VolumeFilter<double> volume;
  volume.set_input_sampling_rate(48000);
  volume.set_volume(-1);
  volume.set_input_port(0, &check, 0);

  ATK::OversamplingFilter<double, ATK::Oversampling6points5order_4<double> > oversampling_filter;
  oversampling_filter.set_input_sampling_rate(48000);
  oversampling_filter.set_output_sampling_rate(48000 * 4);
  oversampling_filter.set_input_port(0, &generator, 0);

  ATK::SimpleOverdriveFilter<double> filter;
  filter.set_input_sampling_rate(48000 * 4);
  filter.set_input_port(0, &oversampling_filter, 0);

  ATK::IIRFilter<ATK::ButterworthLowPassCoefficients<double> > lowfilter;
  lowfilter.set_input_sampling_rate(48000 * 4);
  lowfilter.set_output_sampling_rate(48000 * 4);
  lowfilter.set_cut_frequency(48000);
  lowfilter.set_order(5);
  lowfilter.set_input_port(0, &filter, 0);

  ATK::DecimationFilter<double> decimation_filter(1);
  decimation_filter.set_input_sampling_rate(48000 * 4);
  decimation_filter.set_output_sampling_rate(48000);
  decimation_filter.set_input_port(0, &lowfilter, 0);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_input_port(0, &decimation_filter, 0);
  sum.set_input_port(1, &volume, 0);

  ATK::VolumeFilter<double> volume2;
  volume2.set_input_sampling_rate(48000);
  volume2.set_volume(.001);
  volume2.set_input_port(0, &sum, 0);

  ATK::TriangleCheckerFilter<double> checker;
  checker.set_input_sampling_rate(48000);
  checker.set_amplitude(0);
  checker.set_frequency(1000);
  checker.set_input_port(0, &volume2, 0);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( SimpleOverdriveFilter_0_const )
{
  std::array<double, PROCESSSIZE> data;
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 0;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  ATK::SimpleOverdriveFilter<double> filter;
  filter.set_input_sampling_rate(48000);
  filter.set_output_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  std::array<double, PROCESSSIZE> outdata;

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(int64_t i = 0; i < 50; ++i)
  {
    BOOST_REQUIRE_SMALL(outdata[i], 1e-10);
  }
}


#include <cstdlib>
#include <sstream>
#include <iostream>
#include <fstream>
#include <vector>

#include <boost/program_options.hpp>
#include <boost/program_options/variables_map.hpp>

#include "filelib.h"
#include "weights.h"
#include "sparse_vector.h"
#include "optimize.h"

using namespace std;
namespace po = boost::program_options;

// since this is a ranking model, there should be equal numbers of
// positive and negative examples, so the bias should be 0
static const double MAX_BIAS = 1e-10;

void InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("weights,w", po::value<string>(), "Weights from previous iteration (used as initialization and interpolation")
        ("interpolation,p",po::value<double>()->default_value(0.9), "Output weights are p*w + (1-p)*w_prev")
        ("memory_buffers,m",po::value<unsigned>()->default_value(200), "Number of memory buffers (LBFGS)")
        ("sigma_squared,s",po::value<double>()->default_value(0.1), "Sigma squared for Gaussian prior")
        ("min_reg,r",po::value<double>()->default_value(1e-8), "When tuning (-T) regularization strength, minimum regularization strenght")
        ("max_reg,R",po::value<double>()->default_value(10.0), "When tuning (-T) regularization strength, maximum regularization strenght")
        ("testset,t",po::value<string>(), "Optional held-out test set")
        ("tune_regularizer,T", "Use the held out test set (-t) to tune the regularization strength")
        ("help,h", "Help");
  po::options_description dcmdline_options;
  dcmdline_options.add(opts);
  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  if (conf->count("help")) {
    cerr << dcmdline_options << endl;
    exit(1);
  }
}

void ParseSparseVector(string& line, size_t cur, SparseVector<double>* out) {
  SparseVector<double>& x = *out;
  size_t last_start = cur;
  size_t last_comma = string::npos;
  while(cur <= line.size()) {
    if (line[cur] == ' ' || cur == line.size()) {
      if (!(cur > last_start && last_comma != string::npos && cur > last_comma)) {
        cerr << "[ERROR] " << line << endl << "  position = " << cur << endl;
        exit(1);
      }
      const int fid = FD::Convert(line.substr(last_start, last_comma - last_start));
      if (cur < line.size()) line[cur] = 0;
      const double val = strtod(&line[last_comma + 1], NULL);
      x.set_value(fid, val);

      last_comma = string::npos;
      last_start = cur+1;
    } else {
      if (line[cur] == '=')
        last_comma = cur;
    }
    ++cur;
  }
}

void ReadCorpus(istream* pin, vector<pair<bool, SparseVector<double> > >* corpus) {
  istream& in = *pin;
  corpus->clear();
  bool flag = false;
  int lc = 0;
  string line;
  SparseVector<double> x;
  while(getline(in, line)) {
    ++lc;
    if (lc % 1000 == 0) { cerr << '.'; flag = true; }
    if (lc % 40000 == 0) { cerr << " [" << lc << "]\n"; flag = false; }
    if (line.empty()) continue;
    const size_t ks = line.find("\t");
    assert(string::npos != ks);
    assert(ks == 1);
    const bool y = line[0] == '1';
    x.clear();
    ParseSparseVector(line, ks + 1, &x);
    corpus->push_back(make_pair(y, x));
  }
  if (flag) cerr << endl;
}

void GradAdd(const SparseVector<double>& v, const double scale, vector<double>* acc) {
  for (SparseVector<double>::const_iterator it = v.begin();
       it != v.end(); ++it) {
    (*acc)[it->first] += it->second * scale;
  }
}

double TrainingInference(const vector<double>& x,
                         const vector<pair<bool, SparseVector<double> > >& corpus,
                         vector<double>* g = NULL) {
  double cll = 0;
  for (int i = 0; i < corpus.size(); ++i) {
    const double dotprod = corpus[i].second.dot(x) + x[0]; // x[0] is bias
    double lp_false = dotprod;
    double lp_true = -dotprod;
    if (0 < lp_true) {
      lp_true += log1p(exp(-lp_true));
      lp_false = log1p(exp(lp_false));
    } else {
      lp_true = log1p(exp(lp_true));
      lp_false += log1p(exp(-lp_false));
    }
    lp_true*=-1;
    lp_false*=-1;
    if (corpus[i].first) {  // true label
      cll -= lp_true;
      if (g) {
        // g -= corpus[i].second * exp(lp_false);
        GradAdd(corpus[i].second, -exp(lp_false), g);
        (*g)[0] -= exp(lp_false); // bias
      }
    } else {                  // false label
      cll -= lp_false;
      if (g) {
        // g += corpus[i].second * exp(lp_true);
        GradAdd(corpus[i].second, exp(lp_true), g);
        (*g)[0] += exp(lp_true); // bias
      }
    }
  }
  return cll;
}

// return held-out log likelihood
double LearnParameters(const vector<pair<bool, SparseVector<double> > >& training,
                       const vector<pair<bool, SparseVector<double> > >& testing,
                       const double sigsq,
                       const unsigned memory_buffers,
                       vector<double>* px) {
  vector<double>& x = *px;
  vector<double> vg(FD::NumFeats(), 0.0);
  bool converged = false;
  LBFGSOptimizer opt(FD::NumFeats(), memory_buffers);
  double tppl = 0.0;
  while(!converged) {
    fill(vg.begin(), vg.end(), 0.0);
    double cll = TrainingInference(x, training, &vg);
    double ppl = cll / log(2);
    ppl /= training.size();
    ppl = pow(2.0, ppl);

    // evaluate optional held-out test set
    if (testing.size()) {
      tppl = TrainingInference(x, testing) / log(2);
      tppl /= testing.size();
      tppl = pow(2.0, tppl);
    }

    // handle regularizer
#if 1
    double norm = 0;
    for (int i = 1; i < x.size(); ++i) {
      const double mean_i = 0.0;
      const double param = (x[i] - mean_i);
      norm += param * param;
      vg[i] += param / sigsq;
    }
    const double reg = norm / (2.0 * sigsq);
#else
    double reg = 0;
#endif
    cll += reg;
    cerr << cll << " (REG=" << reg << ")\tPPL=" << ppl << "\t TEST_PPL=" << tppl << "\t";
    try {
      vector<double> old_x = x;
      do {
        opt.Optimize(cll, vg, &x);
        converged = opt.HasConverged();
      } while (!converged && x == old_x);
    } catch (...) {
      cerr << "Exception caught, assuming convergence is close enough...\n";
      converged = true;
    }
    if (fabs(x[0]) > MAX_BIAS) {
      cerr << "Biased model learned. Are your training instances wrong?\n";
      cerr << "  BIAS: " << x[0] << endl;
    }
  }
  return tppl;
}

int main(int argc, char** argv) {
  po::variables_map conf;
  InitCommandLine(argc, argv, &conf);
  string line;
  vector<pair<bool, SparseVector<double> > > training, testing;
  SparseVector<double> old_weights;
  const bool tune_regularizer = conf.count("tune_regularizer");
  if (tune_regularizer && !conf.count("testset")) {
    cerr << "--tune_regularizer requires --testset to be set\n";
    return 1;
  }
  const double min_reg = conf["min_reg"].as<double>();
  const double max_reg = conf["max_reg"].as<double>();
  double sigsq = conf["sigma_squared"].as<double>();
  assert(sigsq > 0.0);
  assert(min_reg > 0.0);
  assert(max_reg > 0.0);
  assert(max_reg > min_reg);
  const double psi = conf["interpolation"].as<double>();
  if (psi < 0.0 || psi > 1.0) { cerr << "Invalid interpolation weight: " << psi << endl; }
  if (conf.count("weights")) {
    Weights w;
    w.InitFromFile(conf["weights"].as<string>());
    w.InitSparseVector(&old_weights);
  }
  ReadCorpus(&cin, &training);
  if (conf.count("testset")) {
    ReadFile rf(conf["testset"].as<string>());
    ReadCorpus(rf.stream(), &testing);
  }
  cerr << "Number of features: " << FD::NumFeats() << endl;
  vector<double> x(FD::NumFeats(), 0.0);  // x[0] is bias
  for (SparseVector<double>::const_iterator it = old_weights.begin();
       it != old_weights.end(); ++it)
    x[it->first] = it->second;
  double tppl = 0.0;
  vector<pair<double,double> > sp;
  vector<double> smoothed;
  if (tune_regularizer) {
    cerr << "Tuning regularizer..." << endl;

    sigsq = min_reg;
    const double steps = 18;
    double sweep_factor = exp((log(max_reg) - log(min_reg)) / steps);
    cerr << "SWEEP FACTOR: " << sweep_factor << endl;
    while(sigsq < max_reg) {
      tppl = LearnParameters(training, testing, sigsq, conf["memory_buffers"].as<unsigned>(), &x);
      sp.push_back(make_pair(sigsq, tppl));
      sigsq *= sweep_factor;
    }
    smoothed.resize(sp.size(), 0);
    smoothed[0] = sp[0].second;
    smoothed.back() = sp.back().second;
    for (int i = 1; i < sp.size()-1; ++i) {
      double prev = sp[i-1].second;
      double next = sp[i+1].second;
      double cur = sp[i].second;
      smoothed[i] = (prev*0.2) + cur * 0.6 + (0.2*next);
    }
    double best_ppl = 9999999;
    unsigned best_i = 0;
    for (unsigned i = 0; i < sp.size(); ++i) {
      if (smoothed[i] < best_ppl) {
        best_ppl = smoothed[i];
        best_i = i;
      }
    }
    sigsq = sp[best_i].first;
  }
  cerr << "Learning parameters..." << endl;
  tppl = LearnParameters(training, testing, sigsq, conf["memory_buffers"].as<unsigned>(), &x);

  Weights w;
  if (conf.count("weights")) {
    cerr << "Interpolating with previous weight vectors..." << endl;
    for (int i = 1; i < x.size(); ++i)
      x[i] = (x[i] * psi) + old_weights.get(i) * (1.0 - psi);
  }
  cout.precision(15);
  cout << "# sigma^2=" << sigsq << "\theld out perplexity=";
  if (tppl) { cout << tppl << endl; } else { cout << "N/A\n"; }
  if (sp.size()) {
    cout << "# Parameter sweep:\n";
    for (int i = 0; i < sp.size(); ++i) {
      cout << "# " << sp[i].first << "\t" << sp[i].second << "\t" << smoothed[i] << endl;
    }
  }
  w.InitFromVector(x);
  w.WriteToFile("-");
  return 0;
}


#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/tuple.hpp>
#include <boost/python/with_custodian_and_ward.hpp>

#include <smtbx/refinement/least_squares.h>
#include <smtbx/refinement/weighting_schemes.h>

namespace smtbx { namespace refinement { namespace least_squares {
  namespace boost_python {

  template <typename FloatType,
            class NormalEquations,
            class OneMillerIndexLinearisation>
  struct normal_equation_building
  {
    typedef build_normal_equations<FloatType> wt;

    template <template<typename> class WeightingSchemeType>
    static void def_init(boost::python::class_<wt> &klass) {
      using namespace boost::python;
      // below the comment after each type is the argument name
      // (c.f. smtbx::refinement::build_normal_equations)
      klass.def(
        init<
          NormalEquations &, // normal_equations
          cctbx::xray::observations<FloatType> const &, // reflections
          af::const_ref<std::complex<FloatType> > const &, // f_mask
          WeightingSchemeType<FloatType> const &, // weighting_scheme
          boost::optional<FloatType>, // scale_factor
          OneMillerIndexLinearisation &, // f_calc_function
          scitbx::sparse::matrix<FloatType> const &,
            // jacobian_transpose_matching_grad_fc
          cctbx::xray::extinction_correction<FloatType> const &, // exti
          optional<bool> // objective_only=false
        >((arg("normal_equations"), arg("reflections"), arg("f_mask"),
           arg("weighting_scheme"), arg("scale_factor"),
           arg("f_calc_function"), arg("jacobian_transpose_matching_grad_fc"),
           arg("extinction"), arg("objective_only")=false)));
    }

    static void wrap(char const *name) {
      using namespace boost::python;
      class_<wt> klass(name, no_init);
        def_init<mainstream_shelx_weighting>(klass);
        def_init<unit_weighting            >(klass);
        def_init<sigma_weighting           >(klass);
      klass
        .def("observables", &wt::observables)
        .def("f_calc", &wt::f_calc)
        .def("weights", &wt::weights)
        ;
    }
  };

  template <typename FloatType,
    class NormalEquations,
    class OneMillerIndexLinearisation>
    struct design_matrix_building {
    typedef build_design_matrix<FloatType> wt;

    template <template<typename> class WeightingSchemeType>
    static void def_init(boost::python::class_<wt> &klass) {
      using namespace boost::python;
      klass.def(
        init<
        NormalEquations &, // normal_equations
        cctbx::xray::observations<FloatType> const &, // reflections
        af::const_ref<std::complex<FloatType> > const &, // f_mask
        WeightingSchemeType<FloatType> const &, // weighting_scheme
        boost::optional<FloatType>, // scale_factor
        OneMillerIndexLinearisation &, // f_calc_function
        scitbx::sparse::matrix<FloatType> const &,
        // jacobian_transpose_matching_grad_fc
        cctbx::xray::extinction_correction<FloatType> const & // exti
        >((arg("normal_equations"), arg("reflections"), arg("f_mask"),
          arg("weighting_scheme"), arg("scale_factor"),
          arg("f_calc_function"), arg("jacobian_transpose_matching_grad_fc"),
          arg("extinction"), arg("objective_only") = false)));
    }

    static void wrap(char const *name) {
      using namespace boost::python;
      class_<wt> klass(name, no_init);
      def_init<mainstream_shelx_weighting>(klass);
      def_init<unit_weighting            >(klass);
      def_init<sigma_weighting           >(klass);
      klass
        .def("observables", &wt::observables)
        .def("f_calc", &wt::f_calc)
        .def("weights", &wt::weights)
        .def("design_matrix", &wt::design_matrix)
        ;
    }
  };

  void wrap_least_squares() {
    using namespace boost::python;

    normal_equation_building<
      double,
      lstbx::normal_equations::non_linear_ls_with_separable_scale_factor<double>,
      structure_factors::direct::one_h::std_trigonometry<
        double,
        structure_factors::direct::one_h::modulus_squared
      >
    >::wrap("build_normal_equations");

    design_matrix_building<
      double,
      lstbx::normal_equations::non_linear_ls_with_separable_scale_factor<double>,
      structure_factors::direct::one_h::std_trigonometry<
        double,
        structure_factors::direct::one_h::modulus_squared
      >
    >::wrap("build_design_matrix");
  }

}}}}


#ifndef _RYSQ_KERNEL_ERI1_HPP_
#define _RYSQ_KERNEL_ERI1_HPP_

#include "kernel/quadrature.hpp"
#include <math.h>
#include <boost/mpl/int.hpp>
#include <rysq/core.hpp>
#include "vector.hpp"

#include "kernel/eri.hpp"
#include "cxx/utility/permute.hpp"

BEGIN_NAMESPACE(rysq, kernel)

template<typename T, size_t N>
struct align {
#define ALIGN_(n) ((A/sizeof(T) - (n)%(A/sizeof(T)))%(A/sizeof(T)))
    static const size_t A = 16;
    static const size_t value = ALIGN_(N);
    static size_t get(size_t M) { return ALIGN_(M); }
#undef ALIGN_
};

template<class bra_, int N>
struct Eri<bra_, boost::mpl::int_<N> > : public Eri<>
{
    typedef bra_ bra;
    typedef void ket;
    typedef kernel::Transform<bra> Transform;
    typedef Eri<>::Data Data;
    Eri(const Quartet<Shell> &quartet, Transform *transform)
	: quartet_(quartet),
	  transform_(transform)
    {
	primitives_.allocate<align>(quartet);
    }

    ~Eri() { delete transform_; }

    virtual void operator()(const Quartet<Center> &r, Data &data,
			    const Parameters &parameters) {
	double scale = rysq::SQRT_4PI5;
	double cutoff = parameters.cutoff/(scale*quartet_.K());
	quadrature::apply<bra,N,align>(quartet_, r[0], r[1], r[2], r[3],
				       scale, cutoff, primitives_,
				       (*transform_)(data));
    }

private:
    const Quartet<Shell> quartet_;
    Transform *transform_;
    quadrature::Primitives<double, double> primitives_;

};

END_NAMESPACE(rysq, kernel)

#endif // _RYSQ_KERNEL_ERI1_HPP_


#include <unordered_map>
#include <boost/test/unit_test.hpp>
#include "../../include/utility/color.hpp"
#include "../../include/utility/direction.hpp"
#include "../../include/utility/math.hpp"
#include "../../include/utility/string.hpp"

BOOST_AUTO_TEST_SUITE(utility_tests)

namespace detail {
    inline void check_color(const xd::vec4& resulting_color, const xd::vec4& expected_color) {
        float epsilon = 0.01f;
        BOOST_CHECK_CLOSE(resulting_color.r, expected_color.r, epsilon);
        BOOST_CHECK_CLOSE(resulting_color.g, expected_color.g, epsilon);
        BOOST_CHECK_CLOSE(resulting_color.b, expected_color.b, epsilon);
    }
    std::unordered_map<std::string, xd::vec4> hex_to_color_map = {
        { "000000", xd::vec4(0.0f, 0.0f, 0.0f, 1.0f) },
        { "ffffff", xd::vec4(1.0f, 1.0f, 1.0f, 1.0f) },
        { "ca50f0", xd::vec4(0.79215686f, 0.31372549f, 0.94117647f, 1.0f) },
        { "f0ca50f0", xd::vec4(0.79215686f, 0.31372549f, 0.94117647f, 0.94117647f) },
    };
}

BOOST_AUTO_TEST_CASE(color_utility_hex_to_color) {
    BOOST_CHECK_THROW(hex_to_color(""), std::exception);
    BOOST_CHECK_THROW(hex_to_color("00FF00FF0"), std::exception);
    BOOST_CHECK_THROW(hex_to_color("REDRUM"), std::exception);
    for (auto& [hex, color] : detail::hex_to_color_map) {
        detail::check_color(hex_to_color(hex), color);
        detail::check_color(hex_to_color("#" + hex), color);
        detail::check_color(hex_to_color("#" + string_utilities::capitalize(hex)), color);
    }
}

BOOST_AUTO_TEST_CASE(color_utility_color_to_hex) {
    for (auto& [hex, color] : detail::hex_to_color_map) {
        BOOST_CHECK_EQUAL(color_to_hex(color), hex);
        if (hex.size() == 6) {
            BOOST_CHECK_EQUAL(color_to_hex(color, true), "ff" + hex);
        }
    }
}

BOOST_AUTO_TEST_CASE(math_utility_check_close) {
    BOOST_CHECK(check_close(4.56f, 4.56f));
    BOOST_CHECK(check_close(-54.566780f, -54.566781f));
    BOOST_CHECK(check_close(-54.566780f, -54.566781f, 0.0001f));
}

BOOST_AUTO_TEST_CASE(direction_utility_is_diagonal) {
    BOOST_CHECK(is_diagonal(Direction::LEFT | Direction::UP));
    BOOST_CHECK(is_diagonal(Direction::DOWN | Direction::RIGHT));
    BOOST_CHECK(!is_diagonal(Direction::LEFT));
    BOOST_CHECK(!is_diagonal(Direction::UP));
}

BOOST_AUTO_TEST_CASE(direction_utility_direction_to_string) {
    BOOST_CHECK_EQUAL(direction_to_string(Direction::NONE), "");
    BOOST_CHECK_EQUAL(direction_to_string(Direction::UP), "Up");
    BOOST_CHECK_EQUAL(direction_to_string(Direction::RIGHT), "Right");
    BOOST_CHECK_EQUAL(direction_to_string(Direction::UP | Direction::RIGHT), "Up|Right");
    BOOST_CHECK_EQUAL(direction_to_string(Direction::DOWN), "Down");
    BOOST_CHECK_EQUAL(direction_to_string(Direction::DOWN | Direction::RIGHT), "Down|Right");
    BOOST_CHECK_EQUAL(direction_to_string(Direction::UP | Direction::DOWN | Direction::RIGHT), "Up|Down|Right");
    BOOST_CHECK_EQUAL(direction_to_string(Direction::LEFT), "Left");
    BOOST_CHECK_EQUAL(direction_to_string(Direction::UP | Direction::LEFT), "Up|Left");
    BOOST_CHECK_EQUAL(direction_to_string(Direction::LEFT | Direction::RIGHT), "Left|Right");
    BOOST_CHECK_EQUAL(direction_to_string(Direction::UP | Direction::LEFT | Direction::RIGHT), "Up|Left|Right");
    BOOST_CHECK_EQUAL(direction_to_string(Direction::DOWN | Direction::LEFT), "Down|Left");
    BOOST_CHECK_EQUAL(direction_to_string(Direction::UP | Direction::DOWN | Direction::LEFT), "Up|Down|Left");
    BOOST_CHECK_EQUAL(direction_to_string(Direction::DOWN | Direction::LEFT | Direction::RIGHT), "Down|Left|Right");
    BOOST_CHECK_EQUAL(direction_to_string(Direction::UP | Direction::DOWN | Direction::LEFT | Direction::RIGHT), "Up|Down|Left|Right");
}

BOOST_AUTO_TEST_CASE(direction_utility_string_to_direction) {
    BOOST_CHECK_EQUAL(string_to_direction(""), Direction::NONE);
    BOOST_CHECK_EQUAL(string_to_direction("Up"), Direction::UP);
    BOOST_CHECK_EQUAL(string_to_direction("Left"), Direction::LEFT);
    BOOST_CHECK_EQUAL(string_to_direction("Down"), Direction::DOWN);
    BOOST_CHECK_EQUAL(string_to_direction("Right"), Direction::RIGHT);
    BOOST_CHECK_EQUAL(string_to_direction("Up|Left"), Direction::UP | Direction::LEFT);
    BOOST_CHECK_EQUAL(string_to_direction("Down|Right"), Direction::DOWN | Direction::RIGHT);
    BOOST_CHECK_EQUAL(string_to_direction("Right|Right|Up"), Direction::UP | Direction::RIGHT);
    BOOST_CHECK_EQUAL(string_to_direction("Up|Down|Left|Right"), Direction::UP | Direction::DOWN | Direction::LEFT | Direction::RIGHT);
    BOOST_CHECK_EQUAL(string_to_direction("Wrong"), Direction::NONE);
    BOOST_CHECK_EQUAL(string_to_direction("Wrong|Right"), Direction::RIGHT);
}

BOOST_AUTO_TEST_CASE(direction_utility_diagonal_to_four_directions) {
    BOOST_CHECK_EQUAL(diagonal_to_four_directions(Direction::LEFT | Direction::UP), Direction::UP);
    BOOST_CHECK_EQUAL(diagonal_to_four_directions(Direction::DOWN | Direction::RIGHT), Direction::DOWN);
    BOOST_CHECK_EQUAL(diagonal_to_four_directions(Direction::UP | Direction::DOWN), Direction::UP);
    BOOST_CHECK_EQUAL(diagonal_to_four_directions(Direction::LEFT | Direction::RIGHT), Direction::LEFT);
    BOOST_CHECK_EQUAL(diagonal_to_four_directions(Direction::RIGHT), Direction::RIGHT);
    BOOST_CHECK_EQUAL(diagonal_to_four_directions(Direction::UP), Direction::UP);
    BOOST_CHECK_EQUAL(diagonal_to_four_directions(Direction::DOWN), Direction::DOWN);
    BOOST_CHECK_EQUAL(diagonal_to_four_directions(Direction::LEFT), Direction::LEFT);
    BOOST_CHECK_EQUAL(diagonal_to_four_directions(Direction::UP | Direction::DOWN | Direction::LEFT | Direction::RIGHT), Direction::UP);
}

BOOST_AUTO_TEST_SUITE_END()


//  Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_STATS_EXTREME_VALUE_HPP
#define BOOST_STATS_EXTREME_VALUE_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>

//
// This is the maximum extreme value distribution, see
// http://www.itl.nist.gov/div898/handbook/eda/section3/eda366g.htm
// and http://mathworld.wolfram.com/ExtremeValueDistribution.html
// Also known as a Fisher-Tippett distribution, a log-Weibull
// distribution or a Gumbel distribution.

#include <utility>
#include <cmath>

#ifdef _MSC_VER
# pragma warning(push)
# pragma warning(disable: 4702) // unreachable code (return after domain_error throw).
#endif

namespace boost{ namespace math{

namespace detail{
//
// Error check:
//
template <class RealType, class Policy>
inline bool verify_scale_b(const char* function, RealType b, RealType* presult, const Policy& pol)
{
   if((b <= 0) || !(boost::math::isfinite)(b))
   {
      *presult = policies::raise_domain_error<RealType>(
         function,
         "The scale parameter \"b\" must be finite and > 0, but was: %1%.", b, pol);
      return false;
   }
   return true;
}

} // namespace detail

template <class RealType = double, class Policy = policies::policy<> >
class extreme_value_distribution
{
public:
   typedef RealType value_type;
   typedef Policy policy_type;

   extreme_value_distribution(RealType a = 0, RealType b = 1)
      : m_a(a), m_b(b)
   {
      RealType err;
      detail::verify_scale_b("boost::math::extreme_value_distribution<%1%>::extreme_value_distribution", b, &err, Policy());
      detail::check_finite("boost::math::extreme_value_distribution<%1%>::extreme_value_distribution", a, &err, Policy());
   } // extreme_value_distribution

   RealType location()const { return m_a; }
   RealType scale()const { return m_b; }

private:
   RealType m_a, m_b;
};

typedef extreme_value_distribution<double> extreme_value;

#ifdef __cpp_deduction_guides
template <class RealType>
extreme_value_distribution(RealType)->extreme_value_distribution<typename boost::math::tools::promote_args<RealType>::type>;
template <class RealType>
extreme_value_distribution(RealType,RealType)->extreme_value_distribution<typename boost::math::tools::promote_args<RealType>::type>;
#endif

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const extreme_value_distribution<RealType, Policy>& /*dist*/)
{ // Range of permissible values for random variable x.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(
      std::numeric_limits<RealType>::has_infinity ? -std::numeric_limits<RealType>::infinity() : -max_value<RealType>(),
      std::numeric_limits<RealType>::has_infinity ? std::numeric_limits<RealType>::infinity() : max_value<RealType>());
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const extreme_value_distribution<RealType, Policy>& /*dist*/)
{ // Range of supported values for random variable x.
   // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(-max_value<RealType>(),  max_value<RealType>());
}

template <class RealType, class Policy>
inline RealType pdf(const extreme_value_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   static const char* function = "boost::math::pdf(const extreme_value_distribution<%1%>&, %1%)";

   RealType a = dist.location();
   RealType b = dist.scale();
   RealType result = 0;
   if(0 == detail::verify_scale_b(function, b, &result, Policy()))
      return result;
   if(0 == detail::check_finite(function, a, &result, Policy()))
      return result;
   if((boost::math::isinf)(x))
      return 0.0f;
   if(0 == detail::check_x(function, x, &result, Policy()))
      return result;
   RealType e = (a - x) / b;
   if(e < tools::log_max_value<RealType>())
      result = exp(e) * exp(-exp(e)) / b;
   // else.... result *must* be zero since exp(e) is infinite...
   return result;
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const extreme_value_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   static const char* function = "boost::math::cdf(const extreme_value_distribution<%1%>&, %1%)";

   if((boost::math::isinf)(x))
      return x < 0 ? 0.0f : 1.0f;
   RealType a = dist.location();
   RealType b = dist.scale();
   RealType result = 0;
   if(0 == detail::verify_scale_b(function, b, &result, Policy()))
      return result;
   if(0 == detail::check_finite(function, a, &result, Policy()))
      return result;
   if(0 == detail::check_finite(function, a, &result, Policy()))
      return result;
   if(0 == detail::check_x("boost::math::cdf(const extreme_value_distribution<%1%>&, %1%)", x, &result, Policy()))
      return result;

   result = exp(-exp((a-x)/b));

   return result;
} // cdf

template <class RealType, class Policy>
RealType quantile(const extreme_value_distribution<RealType, Policy>& dist, const RealType& p)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   static const char* function = "boost::math::quantile(const extreme_value_distribution<%1%>&, %1%)";

   RealType a = dist.location();
   RealType b = dist.scale();
   RealType result = 0;
   if(0 == detail::verify_scale_b(function, b, &result, Policy()))
      return result;
   if(0 == detail::check_finite(function, a, &result, Policy()))
      return result;
   if(0 == detail::check_probability(function, p, &result, Policy()))
      return result;

   if(p == 0)
      return -policies::raise_overflow_error<RealType>(function, 0, Policy());
   if(p == 1)
      return policies::raise_overflow_error<RealType>(function, 0, Policy());

   result = a - log(-log(p)) * b;

   return result;
} // quantile

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<extreme_value_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   static const char* function = "boost::math::cdf(const extreme_value_distribution<%1%>&, %1%)";

   if((boost::math::isinf)(c.param))
      return c.param < 0 ? 1.0f : 0.0f;
   RealType a = c.dist.location();
   RealType b = c.dist.scale();
   RealType result = 0;
   if(0 == detail::verify_scale_b(function, b, &result, Policy()))
      return result;
   if(0 == detail::check_finite(function, a, &result, Policy()))
      return result;
   if(0 == detail::check_x(function, c.param, &result, Policy()))
      return result;

   result = -boost::math::expm1(-exp((a-c.param)/b), Policy());

   return result;
}

template <class RealType, class Policy>
RealType quantile(const complemented2_type<extreme_value_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   static const char* function = "boost::math::quantile(const extreme_value_distribution<%1%>&, %1%)";

   RealType a = c.dist.location();
   RealType b = c.dist.scale();
   RealType q = c.param;
   RealType result = 0;
   if(0 == detail::verify_scale_b(function, b, &result, Policy()))
      return result;
   if(0 == detail::check_finite(function, a, &result, Policy()))
      return result;
   if(0 == detail::check_probability(function, q, &result, Policy()))
      return result;

   if(q == 0)
      return policies::raise_overflow_error<RealType>(function, 0, Policy());
   if(q == 1)
      return -policies::raise_overflow_error<RealType>(function, 0, Policy());

   result = a - log(-boost::math::log1p(-q, Policy())) * b;

   return result;
}

template <class RealType, class Policy>
inline RealType mean(const extreme_value_distribution<RealType, Policy>& dist)
{
   RealType a = dist.location();
   RealType b = dist.scale();
   RealType result = 0;
   if(0 == detail::verify_scale_b("boost::math::mean(const extreme_value_distribution<%1%>&)", b, &result, Policy()))
      return result;
   if (0 == detail::check_finite("boost::math::mean(const extreme_value_distribution<%1%>&)", a, &result, Policy()))
      return result;
   return a + constants::euler<RealType>() * b;
}

template <class RealType, class Policy>
inline RealType standard_deviation(const extreme_value_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING // for ADL of std functions.

   RealType b = dist.scale();
   RealType result = 0;
   if(0 == detail::verify_scale_b("boost::math::standard_deviation(const extreme_value_distribution<%1%>&)", b, &result, Policy()))
      return result;
   if(0 == detail::check_finite("boost::math::standard_deviation(const extreme_value_distribution<%1%>&)", dist.location(), &result, Policy()))
      return result;
   return constants::pi<RealType>() * b / sqrt(static_cast<RealType>(6));
}

template <class RealType, class Policy>
inline RealType mode(const extreme_value_distribution<RealType, Policy>& dist)
{
   return dist.location();
}

template <class RealType, class Policy>
inline RealType median(const extreme_value_distribution<RealType, Policy>& dist)
{
  using constants::ln_ln_two;
   return dist.location() - dist.scale() * ln_ln_two<RealType>();
}

template <class RealType, class Policy>
inline RealType skewness(const extreme_value_distribution<RealType, Policy>& /*dist*/)
{
   //
   // This is 12 * sqrt(6) * zeta(3) / pi^3:
   // See http://mathworld.wolfram.com/ExtremeValueDistribution.html
   //
   return static_cast<RealType>(1.1395470994046486574927930193898461120875997958366L);
}

template <class RealType, class Policy>
inline RealType kurtosis(const extreme_value_distribution<RealType, Policy>& /*dist*/)
{
   // See http://mathworld.wolfram.com/ExtremeValueDistribution.html
   return RealType(27) / 5;
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const extreme_value_distribution<RealType, Policy>& /*dist*/)
{
   // See http://mathworld.wolfram.com/ExtremeValueDistribution.html
   return RealType(12) / 5;
}

} // namespace math
} // namespace boost

#ifdef _MSC_VER
# pragma warning(pop)
#endif

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_EXTREME_VALUE_HPP


#include <boost/python.hpp>
#include <Box2D/Common/b2Math.h>

using namespace boost::python;

namespace pysics {
    namespace {
        struct Vec2FromTuple {
            Vec2FromTuple() {
                converter::registry::push_back(&convertible, &construct,
                                               type_id<b2Vec2>());
            }

            static void *convertible(PyObject *obj_ptr) {
                return PyTuple_Check(obj_ptr) && PyTuple_Size(obj_ptr) == 2 ? obj_ptr : 0;
            }

            static void construct(PyObject *obj_ptr,
                                  converter::rvalue_from_python_stage1_data *data)
            {
                double x = PyFloat_AsDouble(PyTuple_GetItem(obj_ptr, 0));
                double y = PyFloat_AsDouble(PyTuple_GetItem(obj_ptr, 1));
                void *storage = ((converter::rvalue_from_python_storage<b2Vec2> *) data)->storage.bytes;
                new (storage) b2Vec2(x, y);
                data->convertible = storage;
            }
        };

        std::auto_ptr<b2Vec2> construct() {
            return std::auto_ptr<b2Vec2>(new b2Vec2(0.0f, 0.0f));
        }

        int32 len(b2Vec2 *v) {
            return 2;
        }

        float32 getitem(b2Vec2 *v, int32 index) {
            if (index >= 0 && index < 2) {
                return (*v)(index);
            } else {
                throw std::out_of_range("Vec2 index out of range");
            }
        }

        void setitem(b2Vec2 *v, int32 index, float32 value) {
            if (index >= 0 && index < 2) {
                (*v)(index) = value;
            } else {
                throw std::out_of_range("Vec2 index out of range");
            }
        }

        std::string repr(const b2Vec2 &v) {
            std::ostringstream out;
            out << "Vec2(" << v.x << ", " << v.y << ")";
            return out.str();
        }
    }

    void wrap_vec_2() {
        Vec2FromTuple();

        class_<b2Vec2>("Vec2", init<float32, float32>())
            .def("__init__", make_constructor(&construct))

            .def(self += self)
            .def(self -= self)
            .def(self *= float32())

            .def(-self)
            .def(self + self)
            .def(self - self)
            .def(float32() * self)
            .def(self == self)

            .def("__len__", &len)
            .def("__getitem__", &getitem)
            .def("__setitem__", &setitem)

            .def("__abs__", &b2Vec2::Length)
            .def("__repr__", &repr)

            .def_readwrite("x", &b2Vec2::x)
            .def_readwrite("y", &b2Vec2::y)
            .add_property("valid", &b2Vec2::IsValid)

            .def("normalize", &b2Vec2::Normalize)
        ;

        float32 (*dot)(const b2Vec2 &, const b2Vec2 &) = &b2Dot;
        float32 (*cross_1)(const b2Vec2 &, const b2Vec2 &) = &b2Cross;
        b2Vec2 (*cross_2)(const b2Vec2 &, float32) = &b2Cross;
        b2Vec2 (*cross_3)(float32, const b2Vec2 &) = &b2Cross;

        def("dot", dot);
        def("cross", cross_1);
        def("cross", cross_2);
        def("cross", cross_3);
    }
}


/*
Copyright (c) 2020 Naomasa Matsubayashi

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 <fstream>
#include <iostream>
#include <chrono>
#include <boost/program_options.hpp>
#include <nlohmann/json.hpp>
#include "ifm/bessel.h"
int main( int argc, char* argv[] ) {
  boost::program_options::options_description options("オプション");
  options.add_options()
    ("help,h",    "ヘルプを表示")
    ("bessel,b", boost::program_options::value<std::string>()->default_value( "bessel.mp" ), "ベッセル関数近似係数");
  boost::program_options::variables_map params;
  boost::program_options::store( boost::program_options::parse_command_line( argc, argv, options ), params );
  boost::program_options::notify( params );
  if( params.count("help") ) {
    std::cout << options << std::endl;
    return 0;
  }
  std::vector< std::pair< float, float > > pre;
  {
    std::ifstream bessel_file( params["bessel"].as<std::string>(), std::ofstream::binary );
    nlohmann::json bessel = nlohmann::json::from_msgpack( bessel_file );
    for( const auto &v: bessel )
      pre.emplace_back( v.at( 0 ), v.at( 1 ) );
  }
  const auto t0 = std::chrono::steady_clock::now();
  for( unsigned int b_ = 0.f; b_ != 400; ++b_ ) {
    float b = b_ * 0.1f;
    for( unsigned int n = 0; n != 60; ++n ) {
      ifm::bessel_kind1( b, n );
    }
  }
  const auto t1 = std::chrono::steady_clock::now();
  for( unsigned int b_ = 0.f; b_ != 400; ++b_ ) {
    float b = b_ * 0.1f;
    float l1 = 0;
    float l2 = 0;
    for( unsigned int n = 0; n != 60; ++n ) {
      float y = ifm::bessel_kind1_approx_2019( b, n, l1, l2, pre );
      l2 = l1;
      l1 = y;
    }
  }
  const auto t2 = std::chrono::steady_clock::now();
  float e0 = float( std::chrono::duration_cast< std::chrono::microseconds >( t1 - t0 ).count() );
  float e1 = float( std::chrono::duration_cast< std::chrono::microseconds >( t2 - t1 ).count() );
  std::cout << "台形公式: " << (400*60)/e0 << "Mbps(" << 400*60 << " samples in " << e0 << "microseconds)" << std::endl;
  std::cout << "近似式: " << (400*60)/e1 << "Mbps(" << 400*60 << " samples in " << e1 << "microseconds)" << std::endl;
}


#include <GL/glew.h>
#include "RecordWindow.h"
#include "dart/external/lodepng/lodepng.h"
#include "Functions.h"
#include <algorithm>
#include <fstream>
#include <boost/filesystem.hpp>
#include <GL/glut.h>
using namespace GUI;
using namespace dart::simulation;
using namespace dart::dynamics;

RecordWindow::
RecordWindow(std::vector<std::string> motion)
	:GLUTWindow(),mTrackCamera(false),mIsRotate(false),mIsAuto(false), mTimeStep(1 / 30.0)
{
	for(int i = 0; i < motion.size(); i++) {
		mDrawRef.push_back(true);
	}
	this->mTotalFrame = 0;
	this->num = motion.size();

	std::string skel_path = std::string(CAR_DIR)+std::string("/character/") + std::string(REF_CHARACTER_TYPE) + std::string(".xml");
	for(int i = 0; i < motion.size(); i++) {
		this->mRef.push_back(new DPhy::Character(skel_path));

		std::vector<Eigen::VectorXd> record_pos;
		record_pos.clear();

		int dof = this->mRef[i]->GetSkeleton()->getPositions().rows();
		std::string record_path = motion[i];
		std::ifstream is(record_path);

		char buffer[256];
		// is >> buffer;
		// int length = atoi(buffer);
		// if(this->mTotalFrame == 0 || length < mTotalFrame) {
		// 	mTotalFrame = length;
		// }

	//	for(int k = 0; k < length; k++) {
		int length = 0;
		while(!is.eof()) {
			Eigen::VectorXd p(dof);
			for(int j = 0; j < dof; j++)
			{
				is >> buffer;
				p[j] = atof(buffer);
			}
			// is >> buffer;

			Eigen::Vector3d o;
			if(length == 0) {
				o = p.segment<3>(3);
				o[1] = 0;
			} else
				o = mMemoryObj.back();

			// for(int j = 0; j < 3; j++)
			// {
			// 	is >> buffer;
			// 	o(j) = atof(buffer);
			// }
			record_pos.push_back(p);
			mMemoryObj.push_back(o);
			length++;
		}
		record_pos.pop_back();
		mMemoryObj.pop_back();

		length -= 1;
	//	}

		is.close();
		if(this->mTotalFrame == 0 || length < mTotalFrame) {
			mTotalFrame = length;
		}
		mMemoryRef.push_back(record_pos);
		if(i == motion.size()-1)
			DPhy::SetSkeletonColor(this->mRef.back()->GetSkeleton(), Eigen::Vector4d(235./255., 87./255., 87./255., 1.0));
		else
			DPhy::SetSkeletonColor(this->mRef.back()->GetSkeleton(), Eigen::Vector4d(235./255., 235./255., 235./255., 1.0));

	}

	Eigen::Vector3d ori_otho(-1.5, 0, 0);
	for(int i = 1; i < motion.size(); i++) {
		for(int j = 0; j < mMemoryRef[i].size(); j++) {
			mMemoryRef[i][j].segment<3>(3) += ori_otho * i;
		}
	}
	this->mCurFrame = 0;
	this->mDisplayTimeout = 33;

	this->SetFrame(this->mCurFrame);

}
void
RecordWindow::
SetFrame(int n)
{
	if( n < 0 || n >= this->mTotalFrame )
	{
	 	std::cout << "Frame exceeds limits" << std::endl;
	 	return;
	}

	for(int i = 0; i < mRef.size(); i++) {
    	mRef[i]->GetSkeleton()->setPositions(mMemoryRef[i][n]);
    }
    mObj = mMemoryObj[0];

}

void
RecordWindow::
NextFrame()
{
	this->mCurFrame+=1;
	if (this->mCurFrame >= this->mTotalFrame) {
        this->mCurFrame = 0;
    }
	this->SetFrame(this->mCurFrame);
}
void
RecordWindow::
PrevFrame()
{
	this->mCurFrame-=1;
	if( this->mCurFrame < 0 ) {
        this->mCurFrame = this->mTotalFrame - 1;
    }
	this->SetFrame(this->mCurFrame);
}
void
RecordWindow::
DrawSkeletons()
{
	for(int i = 0; i < mRef.size(); i++) {
		if(this->mDrawRef[i]) {
			GUI::DrawSkeleton(this->mRef[i]->GetSkeleton(), 0);
		}
    }
}
void
RecordWindow::
DrawGround()
{
	GUI::DrawPoint(mObj, Eigen::Vector3d(1.0, 1.0, 1.0), 10);

	Eigen::Vector3d com_root;
	com_root = this->mRef[num / 2]->GetSkeleton()->getRootBodyNode()->getCOM();
	GUI::DrawGround((int)com_root[0], (int)com_root[2], 0);
}
void
RecordWindow::
Display()
{

	glClearColor(1.0, 1.0, 1.0, 1);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glEnable(GL_DEPTH_TEST);

	dart::dynamics::SkeletonPtr skel = this->mRef[num / 2]->GetSkeleton();
	Eigen::Vector3d com_root = skel->getRootBodyNode()->getCOM();
	Eigen::Vector3d com_front = skel->getRootBodyNode()->getTransform()*Eigen::Vector3d(0.0, 0.0, 2.0);

	if(this->mTrackCamera){
		Eigen::Vector3d com = skel->getRootBodyNode()->getCOM();
		Eigen::Isometry3d transform = skel->getRootBodyNode()->getTransform();
		com[1] = 0.8;

		Eigen::Vector3d camera_pos;
		camera_pos << -3, 1, 1.5;
		camera_pos = camera_pos + com;
		camera_pos[1] = 2;

		mCamera->SetCenter(com);
	}
	mCamera->Apply();

	glUseProgram(program);
	glPushMatrix();
	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    glScalef(1.0, -1.0, 1.0);
	initLights(com_root[0], com_root[2], com_front[0], com_front[2]);
	// DrawSkeletons();
	glPopMatrix();
	initLights(com_root[0], com_root[2], com_front[0], com_front[2]);
	// glColor4f(0.7, 0.0, 0.0, 0.40);  /* 40% dark red floor color */
	DrawGround();
	DrawSkeletons();
	glDisable(GL_BLEND);

	glUseProgram(0);
	glutSwapBuffers();

}
void
RecordWindow::
Reset()
{
	this->mCurFrame = 0;
	this->SetFrame(this->mCurFrame);

}
void
RecordWindow::
Keyboard(unsigned char key,int x,int y)
{
	switch(key)
	{
		case '`' :mIsRotate= !mIsRotate;break;
		case '[': mIsAuto=false;this->PrevFrame();break;
		case ']': mIsAuto=false;this->NextFrame();break;
		case 'o': this->mCurFrame-=99; this->PrevFrame();break;
		case 'p': this->mCurFrame+=99; this->NextFrame();break;
		case 's': std::cout << this->mCurFrame << std::endl;break;
		case 'r': Reset();break;
		case 't': mTrackCamera = !mTrackCamera; this->SetFrame(this->mCurFrame); break;
		case '3': mDrawRef[2] = !mDrawRef[2];break;
		case '2': mDrawRef[1] = !mDrawRef[1];break;
		case '1': mDrawRef[0] = !mDrawRef[0];break;
		case ' ':
			mIsAuto = !mIsAuto;
			break;
		case 27: exit(0);break;
		default : break;
	}
	// this->SetFrame(this->mCurFrame);

	// glutPostRedisplay();
}
void
RecordWindow::
Mouse(int button, int state, int x, int y)
{
	if(button == 3 || button == 4){
		if (button == 3)
		{
			mCamera->Pan(0,-5,0,0);
		}
		else
		{
			mCamera->Pan(0,5,0,0);
		}
	}
	else{
		if (state == GLUT_DOWN)
		{
			mIsDrag = true;
			mMouseType = button;
			mPrevX = x;
			mPrevY = y;
		}
		else
		{
			mIsDrag = false;
			mMouseType = 0;
		}
	}

	// glutPostRedisplay();
}
void
RecordWindow::
Motion(int x, int y)
{
	if (!mIsDrag)
		return;

	int mod = glutGetModifiers();
	if (mMouseType == GLUT_LEFT_BUTTON)
	{
		mCamera->Translate(x,y,mPrevX,mPrevY);
	}
	else if (mMouseType == GLUT_RIGHT_BUTTON)
	{
		mCamera->Rotate(x,y,mPrevX,mPrevY);

	}
	mPrevX = x;
	mPrevY = y;
}
void
RecordWindow::
Reshape(int w, int h)
{
	glViewport(0, 0, w, h);
	mCamera->Apply();
}

void
RecordWindow::
Step()
{
	this->mCurFrame++;
	this->SetFrame(this->mCurFrame);
}
void
RecordWindow::
Timer(int value)
{
	std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();

	if( mIsAuto && this->mCurFrame == this->mTotalFrame - 1){
         Step();
	} else if( mIsAuto && this->mCurFrame < this->mTotalFrame - 1){
        this->mCurFrame++;
        SetFrame(this->mCurFrame);

    }

	std::chrono::steady_clock::time_point end= std::chrono::steady_clock::now();
	double elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count()/1000.;

	glutTimerFunc(std::max(0.0,mDisplayTimeout-elapsed), TimerEvent,1);
	glutPostRedisplay();

}


#ifndef HOPS_MAXIMUMVOLUMEELLIPSOID_HPP
#define HOPS_MAXIMUMVOLUMEELLIPSOID_HPP

#include <cmath>
#include <Eigen/Core>
#include <hops/LinearProgram/LinearProgramFactory.hpp>

namespace hops {
    template<typename RealType>
    /**
    * @brief [Deprectated] Use PolyRound (https://gitlab.com/csb.ethz/PolyRound) for effective and efficient rounding.
    * @deprecated This class is known to be ineffective for a wide class of models.
    *
    * @details
    *  Reference:
    *  Zhang, Y., & Gao, L. (2003):
    *  On Numerical Solution of the Maximum Volume Ellipsoid Problem.
    *  SIAM Journal on Optimization, 14(1), 53–76.
    *  doi:10.1137/S1052623401397230
    *
    *  Implementation initially ported from Matlab to C++ by Samuel Leweke (2013).
    */
    class MaximumVolumeEllipsoid {
    public:
        MaximumVolumeEllipsoid(const MaximumVolumeEllipsoid &) = default;

        MaximumVolumeEllipsoid(MaximumVolumeEllipsoid &&) noexcept = default;

        MaximumVolumeEllipsoid &operator=(const MaximumVolumeEllipsoid &) = default;

        /**
         * @brief Transforms vector x to the rounded space.
         * @param x
         * @return
         */
        [[nodiscard]] Eigen::Matrix<RealType, Eigen::Dynamic, 1>
        applyRoundingTransformation(Eigen::Matrix<RealType, Eigen::Dynamic, 1> &x);

        [[nodiscard]] RealType computeVolume() const;

        [[nodiscard]] const Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> &getRoundingTransformation() const;

        [[nodiscard]] const Eigen::Matrix<RealType, Eigen::Dynamic, 1> &getCenter() const;

        [[nodiscard]] Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> getEllipsoid() const;

        [[nodiscard]] bool hasConverged() const;

        [[nodiscard]] size_t getNumberOfIterations() const;

        [[nodiscard]] RealType getCurrentError() const;

        [[nodiscard]] RealType getTolerance() const;

        static MaximumVolumeEllipsoid
        construct(const Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> &A,
                  const Eigen::Matrix<RealType, Eigen::Dynamic, 1> &b,
                  size_t maximumNumberOfIterationsToRun,
                  const Eigen::Matrix<RealType, Eigen::Dynamic, 1> &startingPoint,
                  RealType tolerance = 1e-6);

        static MaximumVolumeEllipsoid
        construct(const Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> &A,
                  const Eigen::Matrix<RealType, Eigen::Dynamic, 1> &b,
                  size_t maximumNumberOfIterationsToRun,
                  RealType tolerance = 1e-6);

        template<typename Derived>
        friend std::ostream &
        operator<<(std::ostream &out, const MaximumVolumeEllipsoid<Derived> &maximumVolumeEllipsoid);

    private:
        template<typename Derived>
        friend void swap(MaximumVolumeEllipsoid<Derived> &first, MaximumVolumeEllipsoid<Derived> &second);

        MaximumVolumeEllipsoid() = default;

        Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> roundingTransformation;
        Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic> maximumVolumeEllipsoid;
        Eigen::Matrix<RealType, Eigen::Dynamic, 1> center;

        size_t iterations = 0;
        RealType currentError = 0;
        RealType tolerance = 0;
        bool converged = false;
    };
}

#endif //HOPS_MAXIMUMVOLUMEELLIPSOID_HPP


#ifndef CAFFE_BACKEND_OPENCL_OCL_MATH_HPP_
#define CAFFE_BACKEND_OPENCL_OCL_MATH_HPP_

#include "caffe/common.hpp"
#include "caffe/backend/device.hpp"

#ifdef USE_OPENCL
#include "ocl_device.hpp"

#include <boost/math/special_functions/next.hpp>
#include <boost/random.hpp>
#include <boost/thread.hpp>

#include <cmath>
#include <cstdlib>
#include <cstring>
#include <functional>
#include <limits>
#include <random>
#include <vector>

#include "viennacl/backend/opencl.hpp"
#include "viennacl/ocl/backend.hpp"
#include "viennacl/ocl/context.hpp"
#include "viennacl/ocl/device.hpp"
#include "viennacl/ocl/platform.hpp"

#include "caffe/util/benchmark.hpp"
#include "caffe/util/math_functions.hpp"

#if defined(USE_CLBLAS)
#include <clBLAS.h>       // NOLINT
#endif  // USE_CLBLAS
#if defined(USE_CLBLAST)
#include <clblast.h>      // NOLINT
#endif  // USE_CLBLAST
#include "viennacl/linalg/inner_prod.hpp"
#include "viennacl/linalg/norm_1.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/linalg/norm_inf.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"

namespace clblast {

#if defined(USE_CLBLAST)
#if defined(USE_HALF)
#include  <clblast_half.h>
#endif  // USE_HALF
#endif  // USE_CLBLAST

}

// ViennaCL 1.5.1 compatibility fix
#ifndef VIENNACL_MINOR_VERSION
#define VIENNACL_MINOR_VERSION 5
#endif

#if VIENNACL_MINOR_VERSION > 5
#define VCL_ROW_MAJOR , true
#define VCL_COL_MAJOR , false
#else
#define VCL_ROW_MAJOR
#define VCL_COL_MAJOR
#endif

#endif  // USE_OPENCL

#endif  // CAFFE_BACKEND_OPENCL_OCL_MATH_HPP_