/*
 * 	robot_server.hpp
 *
 *	Author(s): Tamas D. Nagy
 *	Created on: 2016-11-07
 *
 *  Abstract base class for robot servers, interfacing irob_msgs/Robot actions to the actual robots.
 *
 *  Important note: initRosCommunication must be called
 *  in child class or main function!
 *
 */

#ifndef ROBOT_SERVER_HPP_
#define ROBOT_SERVER_HPP_

#include <iostream>
#include <sstream>
#include <vector>
#include <cmath>
#include <boost/thread/thread.hpp>
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/String.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Float32.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <cmath>
#include <irob_utils/tool_pose.hpp>
#include <irob_utils/trajectory.hpp>
#include <irob_utils/utils.hpp>
#include <irob_utils/topic_name_loader.hpp>
#include <actionlib/server/simple_action_server.h>
#include <irob_msgs/ToolPoseStamped.h>
#include <irob_msgs/RobotAction.h>
#include <irob_msgs/InstrumentInfo.h>
#include <irob_msgs/InstrumentJawPart.h>

namespace saf {

class RobotServer {

public:
  // Constants
  static const bool ACTIVE = true;
  static const bool PASSIVE = false;
  static constexpr const double INFO_PUB_RATE = 10.0;

protected:

  const std::string arm_name;
  ros::NodeHandle nh;
  bool isActive;

  // Action servers
  actionlib::SimpleActionServer<irob_msgs::RobotAction> as;

  // Hand-eye registration
  Eigen::Vector3d t;
  Eigen::Matrix3d R;
  Eigen::Affine3d T_he;

  // Surgical instrument information
  irob_msgs::InstrumentInfo instrument_info;

  // Publisher
  ros::Publisher measured_cp_pub;
  ros::Publisher measured_js_pub;
  ros::Publisher instrument_info_pub;

  virtual void subscribeLowLevelTopics() = 0;
  virtual void advertiseLowLevelTopics() = 0;

  void advertiseHighLevelTopics()
  {
    // robot interface
    measured_cp_pub
        = nh.advertise<irob_msgs::ToolPoseStamped>(
          "robot/"+arm_name+"/position_cartesian_current_cf",
          1000);
    measured_js_pub
        = nh.advertise<sensor_msgs::JointState>(
          "robot/"+arm_name+"/joint_state_current",
          1000);
    instrument_info_pub
        = nh.advertise<irob_msgs::InstrumentInfo>(
          "robot/"+arm_name+"/instrument_info",
          1000);
  }

  void startActionServer()
  {
    as.start();
  }

  void publishInfo()
  {
    ros::Rate loop_rate(INFO_PUB_RATE);
    while (ros::ok())
    {
      instrument_info_pub.publish(instrument_info);
      loop_rate.sleep();
    }
  }

public:

  void initRosCommunication()
  {
    subscribeLowLevelTopics();
    if (isActive == ACTIVE)
      advertiseLowLevelTopics();

    advertiseHighLevelTopics();
    startActionServer();
    boost::thread thread(boost::bind(&RobotServer::publishInfo, this));
  }

  void loadRegistration(ros::NodeHandle priv_nh)
  {
    std::vector<double> param_t;
    priv_nh.getParam("t", param_t);

    std::vector<double> param_R;
    priv_nh.getParam("R", param_R);

    for (int i = 0; i < t.rows(); i++)
      t(i) = param_t[i];

    for (int i = 0; i < R.rows(); i++)
      for (int j = 0; j < R.cols(); j++)
        R(i, j) = param_R[(i * R.rows()) + j];

    T_he = Eigen::Translation3d(t) * Eigen::Affine3d(R) * Eigen::Scaling(0.001);

    ROS_INFO_STREAM(
          "Registration read: "<< std::endl << t << std::endl << R);
  }

  void loadInstrumentInfo(ros::NodeHandle priv_nh)
  {

    priv_nh.getParam("instrument/name", instrument_info.name);
    priv_nh.getParam("instrument/jaw_length", instrument_info.jaw_length);

    std::string basic_type;
    priv_nh.getParam("instrument/basic_type", basic_type);

    if (basic_type == "GRIPPER")
      instrument_info.basic_type = irob_msgs::InstrumentInfo::GRIPPER;
    else if (basic_type == "SCISSORS")
      instrument_info.basic_type = irob_msgs::InstrumentInfo::SCISSORS;
    else if (basic_type == "CAMERA")
      instrument_info.basic_type = irob_msgs::InstrumentInfo::CAMERA;
    else
      throw std::runtime_error(
          "Invalid basic_type read from instrument info file.");

    int i = 0;
    double probe;
    while(priv_nh.getParam("instrument/jaw_parts/p"
                           + std::to_string(i)
                           + "/start", probe))
    {
      irob_msgs::InstrumentJawPart jaw_part;
      priv_nh.getParam("instrument/jaw_parts/p"
                       + std::to_string(i)
                       + "/start",
                       jaw_part.start);
      priv_nh.getParam("instrument/jaw_parts/p"
                       + std::to_string(i)
                       + "/end",
                       jaw_part.end);
      std::string type;
      priv_nh.getParam("instrument/jaw_parts/p"
                       + std::to_string(i)
                       + "/type",
                       type);
      if (type == "JOINT")
        jaw_part.type = irob_msgs::InstrumentJawPart::JOINT;
      else if (type == "GRIPPER")
        jaw_part.type = irob_msgs::InstrumentJawPart::GRIPPER;
      else if (type == "SCISSORS")
        jaw_part.type = irob_msgs::InstrumentJawPart::SCISSORS;
      else
        throw std::runtime_error(
            "Invalid instrument part type read from instrument info file.");

      instrument_info.jaw_parts.push_back(jaw_part);
      i++;
    }

    ROS_INFO_STREAM(
          "Instrument info read: "<< std::endl << instrument_info);

  }

  virtual void resetPose(bool) = 0;
  virtual void stop() = 0;
  virtual void followTrajectory(Trajectory<ToolPose>) = 0;
  virtual void moveJointAbsolute(sensor_msgs::JointState, double) = 0;
  virtual ToolPose getPoseCurrent() = 0;

  virtual void robotActionCB(const irob_msgs::RobotGoalConstPtr& goal)
  {
    ROS_INFO_STREAM("Start robotActionCB");
    switch(goal -> action)
    {
    case irob_msgs::RobotGoal::STOP:
    {
      stop();
      break;
    }

    case  irob_msgs::RobotGoal::RESET_POSE:
    {
      resetPose(goal -> move_allowed);
      break;
    }

    case  irob_msgs::RobotGoal::FOLLOW_TRAJECTORY:
    {
      followTrajectory(goal->trajectory);
      break;
    }
    case  irob_msgs::RobotGoal::MOVE_JOINT:
    {
      moveJointAbsolute(goal->joint_state, 0.01);
      break;
    }
    default:
    {
      ROS_ERROR_STREAM(arm_name  <<
                       ": invalid robot action code received");
      irob_msgs::RobotResult result;
      result.pose = getPoseCurrent().toRosToolPose();
      result.info = "invalid robot action code";
      as.setAborted(result);
      break;
    }
    }
  }

  // initRosCommunication must be called in child class or main function
  RobotServer(ros::NodeHandle nh, ros::NodeHandle priv_nh,
              std::string arm_name, bool isActive):
    nh(nh), arm_name(arm_name), isActive(isActive),
    as(nh, "robot/"+arm_name+"/robot_action",
       boost::bind(&RobotServer::robotActionCB, this, _1), false)
  {
    loadRegistration(priv_nh);
    loadInstrumentInfo(priv_nh);
  }

  ~RobotServer() {}

};

}
#endif /* ROBOT_SERVER_HPP_ */


/**
 * \ file ToneStackFilter.cpp
 */

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

#include <ATK/Mock/FFTCheckerFilter.h>
#include <ATK/Mock/SimpleSinusGeneratorFilter.h>

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

#define PROCESSSIZE (1024*1024)
#define SAMPLINGRATE (1024*64)

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackCoefficients_throw_low_1_test )
{
  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_low(1.001), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackCoefficients_throw_low_0_test )
{
  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_low(-0.001), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackCoefficients_throw_middle_1_test )
{
  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_middle(1.001), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackCoefficients_throw_middle_0_test )
{
  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_middle(-0.001), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackCoefficients_throw_high_1_test )
{
  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_high(1.001), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackCoefficients_throw_high_0_test )
{
  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter;
  BOOST_CHECK_THROW(filter.set_high(-0.001), std::out_of_range);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_low0_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_low(0);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0.520546902788));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_low0_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_low(0);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.592829286291));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_low0_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_low(0);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0.780334219515));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_low1_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_low(1);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0.893700657602));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_low1_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_low(1);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.535008479213));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_low1_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_low(1);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0.779778536574));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_low05_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_low(.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0.883754524105));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_low05_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_low(.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.536676386406));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_low05_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_low(.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0.779785204548));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_middle0_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_middle(0);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0.893445607385));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_middle0_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_middle(0);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.3017133765430397));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_middle0_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_middle(0);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0.694818332337));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_middle1_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_middle(1);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0.880607047186));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_middle1_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_middle(1);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.645182601381));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_middle1_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_middle(1);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0.827511818981));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_middle05_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_middle(.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0.883754524105));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_middle05_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_middle(.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.536676386406));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_middle05_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_middle(.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0.779785204548));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_high0_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_high(0);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0.886723470765));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_high0_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_high(0);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.519869436363));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_high0_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_high(0);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0.496368627553));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_high1_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_high(1);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0.880785350602));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_high1_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_high(1);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.60150892675));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_high1_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_high(1);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0.98673661586));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_high05_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_high(.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0.883754524105));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_high05_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_high(.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.536676386406));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( IIRFilter_ToneStackFilter_Bassman_high05_10k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLINGRATE);
  generator.set_amplitude(1);
  generator.set_frequency(10000);

  ATK::IIRFilter<ATK::ToneStackCoefficients<double> > filter(ATK::ToneStackCoefficients<double>::buildBassmanStack());
  filter.set_input_sampling_rate(SAMPLINGRATE);
  filter.set_output_sampling_rate(SAMPLINGRATE);
  filter.set_high(.5);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(SAMPLINGRATE);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(10000, 0.779785204548));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}


#include <stdint.h>
#include <boost/random.hpp>

/// Creates a new deterministic psuedorandom number generator given the provided seed and returns a reference to it.
extern "C" void* init_rng(unsigned int seed) {
    boost::random::mt19937* gen = new boost::random::mt19937{seed};
    return (void*)gen;
}

/// Given a reference to a random number generator, returns a random integer from within the range [min, max].
extern "C" int rand_int_range(void* void_rng, int min, int max) {
    boost::random::mt19937* gen = (boost::random::mt19937*)void_rng;
    boost::random::uniform_int_distribution<> dist{min, max};
    return dist(*gen);
}


#pragma once

#include <cstdint>
#include <Eigen/Dense>

/**
 * Controller for wheel velocities
 */
class RobotController {
private:
    /**
     * Number of tracked states (number of states in state vector)
     *
     * X and Y: linear velocities of robot body (m/s)
     * W: angular velocity of robot body around z axis (rad/s)
     */
    const static int numStates = 3;

    /**
     * Number of wheels controlled
     */
    const static int numWheels = 4;

public:
    /**
     * Constructor
     *
     * @param dt_us Expected period of the controller (microseconds)
     */
    RobotController(uint32_t dt_us);

    /**
     * Updates the controller with the latest input and calculates
     * the correct wheel velocities to reach this target
     *
     * @param pv Current state (XYW vel in m/s or rad/s)
     * @param sp Current target (XYW vel in m/s or rad/s)
     * @param output Motor targets (rad/s)
     */
    void calculateBody(Eigen::Matrix<float, numStates, 1> pv,
                       Eigen::Matrix<float, numStates, 1> sp,
                       Eigen::Matrix<float, numWheels, 1>& outputs);

    /**
     * Updates the wheels such that they try to follow the target
     * Outputs the correct motor commands to do this
     *
     * @param pv Current state (W1-4 in rad/s)
     * @param sp Current target (W1-4 in rad/s)
     * @param output Motor duty cycles in % max (-1 to 1)
     */
    void calculateWheel(Eigen::Matrix<float, numWheels, 1> pv,
                        Eigen::Matrix<float, numWheels, 1> sp,
                        Eigen::Matrix<float, numWheels, 1>& outputs);

private:
    /**
     * Body velocity control gains (1/s, 1/s, 1/s)
     */
    Eigen::Matrix<float, numStates, 1> BodyKp;

    /**
     * Interval of control calculations (seconds)
     */
    float dt = 0.005;

    constexpr static float kMaxWheelAcceleration = 500.0;
    constexpr static float kMaxWheelSpeed = 110;

    Eigen::Matrix<float, numStates, 1> max_acceleration_;
    Eigen::Matrix<float, numStates, 1> last_body_ = Eigen::Matrix<float, numStates, 1>::Zero();

    Eigen::Matrix<float, numWheels, 1> last_wheels_ = Eigen::Matrix<float, numWheels, 1>::Zero();
};


#include <vector>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include <kanooth/numbers/lowlevel/generic_has_double.hpp>
#include <kanooth/numbers/lowlevel/generic_sim_double.hpp>
#include <kanooth/numbers/lowlevel/low_double_int.hpp>
#include <kanooth/number_bits.hpp>
#include <kanooth/fixed_width_ints.hpp>

#include "common/stopwatch.hpp"

boost::random::mt19937 gen;

template <typename T>
void random_array(T* begin, T* end)
{
    boost::random::uniform_int_distribution<T> rand;
    while (begin != end)
        *begin++ = rand(gen);
}

template <typename LOWLEVEL>
class tester {
public:
    tester(unsigned _length) : length(_length)
    {
        digit_type* r;
        for (unsigned k = 0; k < count; ++k) {
            r = new digit_type[length];
            random_array(r, r + length);
            a.push_back(r);
            r = new digit_type[length];
            random_array(r, r + length);
            b.push_back(r);
            c.push_back(new digit_type[length]);
        }
        random_array(&scalar, &scalar + 1);
    }

    ~tester()
    {
        for (unsigned k = 0; k < count; ++k) {
            delete[] a[k];
            delete[] b[k];
            delete[] c[k];
        }
    }

    double test_add()
    {
        stopwatch<> timer;

        unsigned reps = length > 10000 ? 1 : 10000/length;
        for (unsigned j = 0; j < reps; ++j)
            for (unsigned k = 0; k < count; ++k)
                LOWLEVEL::add(c[k], a[k], length, b[k], length);

        return 0.000000001 * count * reps * length * digit_bits / timer.seconds();
    }

    double test_sub()
    {
        stopwatch<> timer;

        unsigned reps = length > 10000 ? 1 : 10000/length;
        for (unsigned j = 0; j < reps; ++j)
            for (unsigned k = 0; k < count; ++k)
                LOWLEVEL::sub(c[k], a[k], length, b[k], length);

        return 0.000000001 * count * reps * length * digit_bits / timer.seconds();
    }

    double test_mul_1()
    {
        stopwatch<> timer;

        unsigned reps = length > 10000 ? 1 : 10000/length;
        for (unsigned j = 0; j < reps; ++j)
            for (unsigned k = 0; k < count; ++k)
                LOWLEVEL::mul_1(c[k], a[k], length, b[k][0]);

        return 0.000000001 * count * reps * length * digit_bits / timer.seconds();
    }

    void run()
    {
        std::cout.flags(std::ios::fixed);
        std::cout.precision(2);
        std::cout << "add       " << test_add() << " Gbit/s" << std::endl;
        std::cout << "sub       " << test_sub() << " Gbit/s" << std::endl;
        std::cout << "mul_1     " << test_mul_1() << " Gbit/s" << std::endl;
    }
private:
    typedef typename LOWLEVEL::digit_type digit_type;
    static const unsigned digit_bits = kanooth::number_bits<digit_type>::value;
    static const unsigned count = 10000;
    const unsigned length;
    std::vector<digit_type*> a;
    std::vector<digit_type*> b;
    std::vector<digit_type*> c;
    digit_type scalar;
};

int main()
{
    //tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint64_t, kanooth::uint128_t> >(1000).run();
    tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint32_t, kanooth::uint64_t> >(1000).run();
    //tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint16_t, kanooth::uint32_t> >(1000).run();
    //tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint64_t, kanooth::numbers::lowlevel::low_double_int<kanooth::uint64_t> > >(1000).run();
    tester<kanooth::numbers::lowlevel::generic_sim_double<kanooth::uint64_t> >(1000).run();

    /*tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint16_t, kanooth::uint32_t> >(1000).run();
    tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint16_t, kanooth::numbers::lowlevel::low_double_int<kanooth::uint16_t> > >(1000).run();
    tester<kanooth::numbers::lowlevel::generic_sim_double<kanooth::uint16_t> >(1000).run();*/

    /*tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint64_t, kanooth::uint128_t> >(1000).run();
    tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint64_t, kanooth::numbers::lowlevel::low_double_int<kanooth::uint64_t> > >(1000).run();
    tester<kanooth::numbers::lowlevel::generic_sim_double<kanooth::uint64_t> >(1000).run();*/

    return 0;
}


#pragma once
#include "math.h"
#include <boost/config.hpp>
#include <type_traits>

namespace std { template <class T> class complex; }
///

namespace coreutil {

BOOST_FORCEINLINE constexpr math_constant::math_constant(long double value)
    : value(value) {}

template <class T>
BOOST_FORCEINLINE constexpr math_constant::operator T() const {
  return value;
}

namespace math_constants_detail {

template <class T> struct is_compatible_operand;
template <class T> struct is_compatible_operand<std::complex<T>>
    : public is_compatible_operand<T> {};
template <class T> struct is_compatible_operand
    : public std::is_floating_point<T> {};

}  // namespace math_constants_detail

#define COREUTIL_MATH_TYPE_CHECK(op, t)                                 \
  static_assert(math_constants_detail::is_compatible_operand<t>(),      \
                "cannot perform '" #op "' between a math constant and non-floating point type " #t)
#define COREUTIL_MATH_DEFINE_UNOP(op)                                   \
  BOOST_FORCEINLINE constexpr math_constant operator op(math_constant a) { return math_constant(op a.value); }
#define COREUTIL_MATH_DEFINE_BINOP(op)                                  \
  BOOST_FORCEINLINE constexpr math_constant operator op(math_constant a, math_constant b) { return math_constant(a.value op b.value); } \
  template <class T> BOOST_FORCEINLINE constexpr T operator op(math_constant a, T b) { COREUTIL_MATH_TYPE_CHECK(op, T); return T(a.value) op b; } \
  template <class T> BOOST_FORCEINLINE constexpr T operator op(T a, math_constant b) { COREUTIL_MATH_TYPE_CHECK(op, T); return a op T(b.value); }
#define COREUTIL_MATH_DEFINE_OPASSIGN(op)   \
  template <class T> BOOST_FORCEINLINE constexpr T &operator op(T &a, math_constant b) { COREUTIL_MATH_TYPE_CHECK(op, T); return a op T(b.value); }
#define COREUTIL_MATH_DEFINE_COMPARISON(op)                             \
  BOOST_FORCEINLINE constexpr bool operator op(math_constant a, math_constant b) { return a.value op b.value; } \
  template <class T> BOOST_FORCEINLINE constexpr bool operator op(math_constant a, T b) { COREUTIL_MATH_TYPE_CHECK(op, T); return T(a.value) op b; } \
  template <class T> BOOST_FORCEINLINE constexpr bool operator op(T a, math_constant b) { COREUTIL_MATH_TYPE_CHECK(op, T); return a op T(b.value); }

COREUTIL_MATH_DEFINE_UNOP(+);
COREUTIL_MATH_DEFINE_UNOP(-);
COREUTIL_MATH_DEFINE_BINOP(+);
COREUTIL_MATH_DEFINE_BINOP(-);
COREUTIL_MATH_DEFINE_BINOP(*);
COREUTIL_MATH_DEFINE_BINOP(/);
COREUTIL_MATH_DEFINE_OPASSIGN(+=);
COREUTIL_MATH_DEFINE_OPASSIGN(-=);
COREUTIL_MATH_DEFINE_OPASSIGN(*=);
COREUTIL_MATH_DEFINE_OPASSIGN(/=);
COREUTIL_MATH_DEFINE_COMPARISON(==);
COREUTIL_MATH_DEFINE_COMPARISON(!=);
COREUTIL_MATH_DEFINE_COMPARISON(<);
COREUTIL_MATH_DEFINE_COMPARISON(>);
COREUTIL_MATH_DEFINE_COMPARISON(<=);
COREUTIL_MATH_DEFINE_COMPARISON(>=);

#undef COREUTIL_MATH_TYPE_CHECK
#undef COREUTIL_MATH_DEFINE_UNOP
#undef COREUTIL_MATH_DEFINE_BINOP
#undef COREUTIL_MATH_DEFINE_OPASSIGN
#undef COREUTIL_MATH_DEFINE_COMPARISON

}  // namespace coreutil


/**
 * @file core/ranking-manager/ClosestPositionTermProximityMeasure.cpp
 * @author Ian Yang
 * @date Created <2009-09-24 15:11:47>
 * @date Updated <2010-03-24 15:25:57>
 */
#include "TermProximityUtil.h"
#include "ClosestPositionTermProximityMeasure.h"

#include <boost/foreach.hpp>

#include <cmath>
#include <algorithm>
#include <numeric>
#include <iostream>

namespace sf1r {

namespace detail {

void makeClosestPositionDistanceMatrix(
    const RankDocumentProperty& documentProperty,
    float w,
    std::vector<float>& resultMatrix
)
{
    std::size_t numTerms = documentProperty.size();

    typedef RankDocumentProperty::size_type size_type;
    std::size_t diagonal = 0;   // diagonal subscripts

    for (size_type outer = 0; outer != documentProperty.size(); ++outer)
    {
        std::size_t horizontal = diagonal;
        std::size_t vertical = diagonal;

        for (size_type inner = outer + 1;
             inner != documentProperty.size(); ++inner)
        {
            float distance = closestPositionDistance(
                documentProperty, outer, inner
            );

            ++horizontal;
            vertical += numTerms;
            float score= std::pow(w, -distance);
            if(distance==1)
                score*=2;
            resultMatrix[horizontal] = resultMatrix[vertical]
                                     = score;
        }

        diagonal += numTerms;
        ++diagonal;
    }
}

} // namespace detail

void AveClosestPositionTermProximityMeasure::calculate(
    const RankDocumentProperty& documentProperty,
    array_type& result
) const
{
    std::size_t numTerms = documentProperty.size();
    termProximityMatrix_.resize(numTerms * numTerms);

    detail::makeClosestPositionDistanceMatrix(
        documentProperty,
        w_,
        termProximityMatrix_
    );

    for(std::size_t i = 0, base = 0; i < numTerms; i++, base += numTerms)
    {
        termProximityMatrix_[base + i] = 0.0F; // not counting itself
        result[i] = std::accumulate(
            termProximityMatrix_.begin() + base,
            termProximityMatrix_.begin() + base + numTerms,
            0.0F
        ) / numTerms;
    }
}

AveClosestPositionTermProximityMeasure*
AveClosestPositionTermProximityMeasure::clone() const
{
    return new AveClosestPositionTermProximityMeasure(*this);
}

void MinClosestPositionTermProximityMeasure::calculate(
    const RankDocumentProperty& documentProperty,
    array_type& result
) const
{
    std::size_t numTerms = documentProperty.size();
    termProximityMatrix_.resize(numTerms * numTerms);

    detail::makeClosestPositionDistanceMatrix(
        documentProperty,
        w_,
        termProximityMatrix_
    );

    for(std::size_t i = 0, base = 0; i < numTerms; i++, base += numTerms)
    {
        termProximityMatrix_[base + i] = (std::numeric_limits<float>::max) ();
        result[i] = *std::min_element(
            termProximityMatrix_.begin() + base,
            termProximityMatrix_.begin() + base + numTerms
        );
    }
}

MinClosestPositionTermProximityMeasure*
MinClosestPositionTermProximityMeasure::clone() const
{
    return new MinClosestPositionTermProximityMeasure(*this);
}

void MaxClosestPositionTermProximityMeasure::calculate(
    const RankDocumentProperty& documentProperty,
    array_type& result
) const
{
    std::size_t numTerms = documentProperty.size();
    termProximityMatrix_.resize(numTerms * numTerms);

    detail::makeClosestPositionDistanceMatrix(
        documentProperty,
        w_,
        termProximityMatrix_
    );

    for(std::size_t i = 0, base = 0; i < numTerms; i++, base += numTerms)
    {
        termProximityMatrix_[base + i] = 0.0F;
        result[i] = *std::max_element(
            termProximityMatrix_.begin() + base,
            termProximityMatrix_.begin() + base + numTerms
        );
    }
}

MaxClosestPositionTermProximityMeasure*
MaxClosestPositionTermProximityMeasure::clone() const
{
    return new MaxClosestPositionTermProximityMeasure(*this);
}

} // namespace sf1r


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

#include <limits>

#include "caffe/common.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"

namespace caffe {

template<>
void caffe_cpu_gemm<float>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const float alpha, const float* A, const float* B, const float beta,
    float* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_sgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

template<>
void caffe_cpu_gemm<double>(const CBLAS_TRANSPOSE TransA,
    const CBLAS_TRANSPOSE TransB, const int M, const int N, const int K,
    const double alpha, const double* A, const double* B, const double beta,
    double* C) {
  int lda = (TransA == CblasNoTrans) ? K : M;
  int ldb = (TransB == CblasNoTrans) ? N : K;
  cblas_dgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
      ldb, beta, C, N);
}

template <>
void caffe_cpu_gemv<float>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const float alpha, const float* A, const float* x,
    const float beta, float* y) {
  cblas_sgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_cpu_gemv<double>(const CBLAS_TRANSPOSE TransA, const int M,
    const int N, const double alpha, const double* A, const double* x,
    const double beta, double* y) {
  cblas_dgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
}

template <>
void caffe_axpy<float>(const int N, const float alpha, const float* X,
    float* Y) { cblas_saxpy(N, alpha, X, 1, Y, 1); }

template <>
void caffe_axpy<double>(const int N, const double alpha, const double* X,
    double* Y) { cblas_daxpy(N, alpha, X, 1, Y, 1); }

template <typename Dtype>
void caffe_set(const int N, const Dtype alpha, Dtype* Y) {
  if (alpha == 0) {
    memset(Y, 0, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    return;
  }
  for (int i = 0; i < N; ++i) {
    Y[i] = alpha;
  }
}

template void caffe_set<int>(const int N, const int alpha, int* Y);
template void caffe_set<unsigned int>(const int N, const unsigned int alpha, unsigned int* Y);
template void caffe_set<float>(const int N, const float alpha, float* Y);
template void caffe_set<double>(const int N, const double alpha, double* Y);

template <>
void caffe_add_scalar(const int N, const float alpha, float* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <>
void caffe_add_scalar(const int N, const double alpha, double* Y) {
  for (int i = 0; i < N; ++i) {
    Y[i] += alpha;
  }
}

template <typename Dtype>
void caffe_copy(const int N, const Dtype* X, Dtype* Y) {
  if (X != Y) {
    if (Caffe::mode() == Caffe::GPU) {
#ifndef CPU_ONLY
      // NOLINT_NEXT_LINE(caffe/alt_fn)
      CUDA_CHECK(cudaMemcpy(Y, X, sizeof(Dtype) * N, cudaMemcpyDefault));
#else
      NO_GPU;
#endif
    } else {
      memcpy(Y, X, sizeof(Dtype) * N);  // NOLINT(caffe/alt_fn)
    }
  }
}
template void caffe_copy<int>(const int N, const int* X, int* Y);
template void caffe_copy<unsigned int>(const int N, const unsigned int* X,
    unsigned int* Y);
template void caffe_copy<float>(const int N, const float* X, float* Y);
template void caffe_copy<double>(const int N, const double* X, double* Y);

template <typename Dtype>
void caffe_copy_subarray(const Dtype* src_p, const vector<int>& src_shape,
                         Dtype*       trg_p, const vector<int>& trg_shape,
                         const vector<int>& src_offset,
                         const vector<int>& copy_shape,
                         const vector<int>& trg_offset) {
//  VLOG(1) << "copy_subarray called with \n"
//            << "src_shape=" << toString( src_shape) << "\n"
//            << "trg_shape=" << toString( trg_shape) << "\n"
//            << "src_offset=" << toString( src_offset) << "\n"
//            << "copy_shape=" << toString( copy_shape) << "\n"
//            << "trg_offset=" << toString( trg_offset) << "\n";
  int num_axes = src_shape.size();
  CHECK_LT(num_axes, 10) << "only 10 axes are supported";
  CHECK_EQ(src_shape.size(), trg_shape.size()) << "target must have same number of axes";
  CHECK_EQ(src_shape.size(), src_offset.size());
  CHECK_EQ(src_shape.size(), copy_shape.size());
  CHECK_EQ(src_shape.size(), trg_offset.size());

  int N[10];   // copy shape
  int so[10];  // src offset
  int to[10];  // trg offset
  int ss[10];  // src stride
  int ts[10];  // trg stride
  int axes_offset = 10 - num_axes;
  for (int i = 0; i < axes_offset; ++i) {
    N[i] = 1;
    so[i] = 0;
    to[i] = 0;
    ss[i] = 0;  // dummy value, will be multiplied with zero anyway
    ts[i] = 0;  // dummy value, will be multiplied with zero anyway
  }

  for (int i = 0; i < num_axes; ++i) {
    N[ axes_offset + i] = copy_shape[i];
    so[axes_offset + i] = src_offset[i];
    to[axes_offset + i] = trg_offset[i];
  }

  ss[9] = 1;
  ts[9] = 1;
  for (int i = num_axes-1; i > 0; --i) {
    ss[axes_offset + i - 1] = src_shape[i] * ss[axes_offset + i];
    ts[axes_offset + i - 1] = trg_shape[i] * ts[axes_offset + i];
  }
//  VLOG(1) << "resulting 10d vectors\n"
//            << "N=" << toString(N,10) << "\n"
//            << "so=" << toString(so,10) << "\n"
//            << "to=" << toString(to,10) << "\n"
//            << "ss=" << toString(ss,10) << "\n"
//            << "ts=" << toString(ts,10) << "\n";

  int copy_nelem = N[9];
  for (                int i0 = 0; i0 < N[0]; ++i0) { int s0  =      ss[0] * (i0 + so[0]); int t0 =      ts[0] * (i0 + to[0]);
    for (              int i1 = 0; i1 < N[1]; ++i1) { int s1  = s0 + ss[1] * (i1 + so[1]); int t1 = t0 + ts[1] * (i1 + to[1]);
      for (            int i2 = 0; i2 < N[2]; ++i2) { int s2  = s1 + ss[2] * (i2 + so[2]); int t2 = t1 + ts[2] * (i2 + to[2]);
        for (          int i3 = 0; i3 < N[3]; ++i3) { int s3  = s2 + ss[3] * (i3 + so[3]); int t3 = t2 + ts[3] * (i3 + to[3]);
          for (        int i4 = 0; i4 < N[4]; ++i4) { int s4  = s3 + ss[4] * (i4 + so[4]); int t4 = t3 + ts[4] * (i4 + to[4]);
            for (      int i5 = 0; i5 < N[5]; ++i5) { int s5  = s4 + ss[5] * (i5 + so[5]); int t5 = t4 + ts[5] * (i5 + to[5]);
              for (    int i6 = 0; i6 < N[6]; ++i6) { int s6  = s5 + ss[6] * (i6 + so[6]); int t6 = t5 + ts[6] * (i6 + to[6]);
                for (  int i7 = 0; i7 < N[7]; ++i7) { int s7  = s6 + ss[7] * (i7 + so[7]); int t7 = t6 + ts[7] * (i7 + to[7]);
                  for (int i8 = 0; i8 < N[8]; ++i8) { int s8  = s7 + ss[8] * (i8 + so[8]); int t8 = t7 + ts[8] * (i8 + to[8]);
                    caffe_copy(copy_nelem, src_p + s8 + so[9], trg_p + t8 + to[9]);
                  }
                }
              }
            }
          }
        }
      }
    }
  }
}

template void caffe_copy_subarray<int>(         const int*          src_p, const vector<int>& src_shape, int*          trg_p, const vector<int>& trg_shape, const vector<int>& src_offset, const vector<int>& copy_shape, const vector<int>& trg_offset);
template void caffe_copy_subarray<unsigned int>(const unsigned int* src_p, const vector<int>& src_shape, unsigned int* trg_p, const vector<int>& trg_shape, const vector<int>& src_offset, const vector<int>& copy_shape, const vector<int>& trg_offset);
template void caffe_copy_subarray<float>(       const float*        src_p, const vector<int>& src_shape, float*        trg_p, const vector<int>& trg_shape, const vector<int>& src_offset, const vector<int>& copy_shape, const vector<int>& trg_offset);
template void caffe_copy_subarray<double>(      const double*       src_p, const vector<int>& src_shape, double*       trg_p, const vector<int>& trg_shape, const vector<int>& src_offset, const vector<int>& copy_shape, const vector<int>& trg_offset);

template <>
void caffe_scal<float>(const int N, const float alpha, float *X) {
  cblas_sscal(N, alpha, X, 1);
}

template <>
void caffe_scal<double>(const int N, const double alpha, double *X) {
  cblas_dscal(N, alpha, X, 1);
}

template <>
void caffe_cpu_axpby<float>(const int N, const float alpha, const float* X,
                            const float beta, float* Y) {
  cblas_saxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_cpu_axpby<double>(const int N, const double alpha, const double* X,
                             const double beta, double* Y) {
  cblas_daxpby(N, alpha, X, 1, beta, Y, 1);
}

template <>
void caffe_add<float>(const int n, const float* a, const float* b,
    float* y) {
  vsAdd(n, a, b, y);
}

template <>
void caffe_add<double>(const int n, const double* a, const double* b,
    double* y) {
  vdAdd(n, a, b, y);
}

template <>
void caffe_sub<float>(const int n, const float* a, const float* b,
    float* y) {
  vsSub(n, a, b, y);
}

template <>
void caffe_sub<double>(const int n, const double* a, const double* b,
    double* y) {
  vdSub(n, a, b, y);
}

template <>
void caffe_mul<float>(const int n, const float* a, const float* b,
    float* y) {
  vsMul(n, a, b, y);
}

template <>
void caffe_mul<double>(const int n, const double* a, const double* b,
    double* y) {
  vdMul(n, a, b, y);
}

template <>
void caffe_div<float>(const int n, const float* a, const float* b,
    float* y) {
  vsDiv(n, a, b, y);
}

template <>
void caffe_div<double>(const int n, const double* a, const double* b,
    double* y) {
  vdDiv(n, a, b, y);
}

template <>
void caffe_powx<float>(const int n, const float* a, const float b,
    float* y) {
  vsPowx(n, a, b, y);
}

template <>
void caffe_powx<double>(const int n, const double* a, const double b,
    double* y) {
  vdPowx(n, a, b, y);
}

template <>
void caffe_sqr<float>(const int n, const float* a, float* y) {
  vsSqr(n, a, y);
}

template <>
void caffe_sqr<double>(const int n, const double* a, double* y) {
  vdSqr(n, a, y);
}

template <>
void caffe_sqrt<float>(const int n, const float* a, float* y) {
  vsSqrt(n, a, y);
}

template <>
void caffe_sqrt<double>(const int n, const double* a, double* y) {
  vdSqrt(n, a, y);
}

template <>
void caffe_exp<float>(const int n, const float* a, float* y) {
  vsExp(n, a, y);
}

template <>
void caffe_exp<double>(const int n, const double* a, double* y) {
  vdExp(n, a, y);
}

template <>
void caffe_log<float>(const int n, const float* a, float* y) {
  vsLn(n, a, y);
}

template <>
void caffe_log<double>(const int n, const double* a, double* y) {
  vdLn(n, a, y);
}

template <>
void caffe_abs<float>(const int n, const float* a, float* y) {
    vsAbs(n, a, y);
}

template <>
void caffe_abs<double>(const int n, const double* a, double* y) {
    vdAbs(n, a, y);
}

unsigned int caffe_rng_rand() {
  return (*caffe_rng())();
}

template <typename Dtype>
Dtype caffe_nextafter(const Dtype b) {
  return boost::math::nextafter<Dtype>(
      b, std::numeric_limits<Dtype>::max());
}

template
float caffe_nextafter(const float b);

template
double caffe_nextafter(const double b);

template <typename Dtype>
void caffe_rng_uniform(const int n, const Dtype a, const Dtype b, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_LE(a, b);
  boost::uniform_real<Dtype> random_distribution(a, caffe_nextafter<Dtype>(b));
  boost::variate_generator<caffe::rng_t*, boost::uniform_real<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_uniform<float>(const int n, const float a, const float b,
                              float* r);

template
void caffe_rng_uniform<double>(const int n, const double a, const double b,
                               double* r);

template <typename Dtype>
void caffe_rng_gaussian(const int n, const Dtype a,
                        const Dtype sigma, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  boost::normal_distribution<Dtype> random_distribution(a, sigma);
  boost::variate_generator<caffe::rng_t*, boost::normal_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_gaussian<float>(const int n, const float mu,
                               const float sigma, float* r);

template
void caffe_rng_gaussian<double>(const int n, const double mu,
                                const double sigma, double* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, int* r);

template <typename Dtype>
void caffe_rng_bernoulli(const int n, const Dtype p, unsigned int* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*, boost::bernoulli_distribution<Dtype> >
      variate_generator(caffe_rng(), random_distribution);
  for (int i = 0; i < n; ++i) {
    r[i] = static_cast<unsigned int>(variate_generator());
  }
}

template
void caffe_rng_bernoulli<double>(const int n, const double p, unsigned int* r);

template
void caffe_rng_bernoulli<float>(const int n, const float p, unsigned int* r);

template <typename Dtype>
size_t caffe_randi_arbitrary_cdf(const size_t n, const Dtype* cdf) {
  CHECK_GT(n, 0);
  CHECK(cdf);
  Dtype maxValue = cdf[n-1];
  Dtype r;
  caffe_rng_uniform( 1, Dtype(0), maxValue, &r);
  const Dtype* p = std::lower_bound( cdf, cdf + n, r);
  return p - cdf;
}

template
size_t caffe_randi_arbitrary_cdf(const size_t n, const float* cdf);

template
size_t caffe_randi_arbitrary_cdf(const size_t n, const double* cdf);

template <typename Dtype>
void caffe_rand_pos_arbitrary_cdf(const Dtype* cdf, int nz, int ny, int nx,
                                  int* z, int* y, int* x) {
  // sample a random index and map it to coordinates
  size_t idx = caffe_randi_arbitrary_cdf( nz * ny * nx, cdf);
  *z = idx / (ny * nx);
  idx = idx - *z * ny * nx;
  *y = idx / nx;
  *x = idx - *y * nx;
}

template
void caffe_rand_pos_arbitrary_cdf(const float* cdf, int nz, int ny, int nx,
                                  int* z, int* y, int* x);
template
void caffe_rand_pos_arbitrary_cdf(const double* cdf, int nz, int ny, int nx,
                                  int* z, int* y, int* x);

template <>
float caffe_cpu_strided_dot<float>(const int n, const float* x, const int incx,
    const float* y, const int incy) {
  return cblas_sdot(n, x, incx, y, incy);
}

template <>
double caffe_cpu_strided_dot<double>(const int n, const double* x,
    const int incx, const double* y, const int incy) {
  return cblas_ddot(n, x, incx, y, incy);
}

template <typename Dtype>
Dtype caffe_cpu_dot(const int n, const Dtype* x, const Dtype* y) {
  return caffe_cpu_strided_dot(n, x, 1, y, 1);
}

template
float caffe_cpu_dot<float>(const int n, const float* x, const float* y);

template
double caffe_cpu_dot<double>(const int n, const double* x, const double* y);

template <>
float caffe_cpu_asum<float>(const int n, const float* x) {
  return cblas_sasum(n, x, 1);
}

template <>
double caffe_cpu_asum<double>(const int n, const double* x) {
  return cblas_dasum(n, x, 1);
}

template <typename Dtype>
void caffe_cpu_cumsum(const size_t n, const Dtype* x, Dtype* y) {
  CHECK_GE(n, 0);
  CHECK(x);
  CHECK(y);
  if( n == 0) return;
  Dtype cumsum = 0;
  for (size_t i = 0; i < n; ++i) {
    cumsum += x[i];
    y[i] = cumsum;
  }
}

template
void caffe_cpu_cumsum(const size_t n, const float* x, float* y);

template
void caffe_cpu_cumsum(const size_t n, const double* x, double* y);

template <>
void caffe_cpu_scale<float>(const int n, const float alpha, const float *x,
                            float* y) {
  cblas_scopy(n, x, 1, y, 1);
  cblas_sscal(n, alpha, y, 1);
}

template <>
void caffe_cpu_scale<double>(const int n, const double alpha, const double *x,
                             double* y) {
  cblas_dcopy(n, x, 1, y, 1);
  cblas_dscal(n, alpha, y, 1);
}

}  // namespace caffe


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2008 Andreas Gaida
 Copyright (C) 2008 Ralph Schreyer
 Copyright (C) 2008 Klaus Spanderen
 Copyright (C) 2014 Johannes Göttker-Schnetmann

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file triplebandlinearop.hpp
    \brief general triple band linear operator
*/

#ifndef quantlib_triple_band_linear_op_hpp
#define quantlib_triple_band_linear_op_hpp

#include <ql/methods/finitedifferences/operators/fdmlinearop.hpp>
#if !defined(QL_USE_STD_UNIQUE_PTR)
#include <boost/shared_array.hpp>
#endif
#include <memory>

namespace QuantLib {

    class FdmMesher;

    class TripleBandLinearOp : public FdmLinearOp {
      public:
        TripleBandLinearOp(Size direction,
                           const ext::shared_ptr<FdmMesher>& mesher);

        TripleBandLinearOp(const TripleBandLinearOp& m);
        TripleBandLinearOp(TripleBandLinearOp&& m) QL_NOEXCEPT;
        #ifdef QL_USE_DISPOSABLE
        TripleBandLinearOp(const Disposable<TripleBandLinearOp>& m);
        #endif
        TripleBandLinearOp& operator=(const TripleBandLinearOp& m);
        TripleBandLinearOp& operator=(TripleBandLinearOp&& m) QL_NOEXCEPT;
        #ifdef QL_USE_DISPOSABLE
        TripleBandLinearOp& operator=(const Disposable<TripleBandLinearOp>& m);
        #endif

        Disposable<Array> apply(const Array& r) const override;
        Disposable<Array> solve_splitting(const Array& r, Real a,
                                          Real b = 1.0) const;

        Disposable<TripleBandLinearOp> mult(const Array& u) const;
        // interpret u as the diagonal of a diagonal matrix, multiplied on LHS
        Disposable<TripleBandLinearOp> multR(const Array& u) const;
        // interpret u as the diagonal of a diagonal matrix, multiplied on RHS
        Disposable<TripleBandLinearOp> add(const TripleBandLinearOp& m) const;
        Disposable<TripleBandLinearOp> add(const Array& u) const;

        // some very basic linear algebra routines
        void axpyb(const Array& a, const TripleBandLinearOp& x,
                   const TripleBandLinearOp& y, const Array& b);

        void swap(TripleBandLinearOp& m);

#if !defined(QL_NO_UBLAS_SUPPORT)
        Disposable<SparseMatrix> toMatrix() const override;
#endif

      protected:
        TripleBandLinearOp() = default;

        Size direction_;
        #if !defined(QL_USE_STD_UNIQUE_PTR)
        boost::shared_array<Size> i0_, i2_;
        boost::shared_array<Size> reverseIndex_;
        boost::shared_array<Real> lower_, diag_, upper_;
        #else
        std::unique_ptr<Size[]> i0_, i2_;
        std::unique_ptr<Size[]> reverseIndex_;
        std::unique_ptr<Real[]> lower_, diag_, upper_;
        #endif

        ext::shared_ptr<FdmMesher> mesher_;
    };

    inline TripleBandLinearOp::TripleBandLinearOp(TripleBandLinearOp&& m) QL_NOEXCEPT {
        swap(m);
    }

    inline TripleBandLinearOp& TripleBandLinearOp::operator=(const TripleBandLinearOp& m) {
        TripleBandLinearOp tmp(m);
        swap(tmp);
        return *this;
    }

    inline TripleBandLinearOp& TripleBandLinearOp::operator=(TripleBandLinearOp&& m) QL_NOEXCEPT {
        swap(m);
        return *this;
    }

}

#endif


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// 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.

#ifndef BOOST_GEOMETRY_PROJECTIONS_MOD_STER_HPP
#define BOOST_GEOMETRY_PROJECTIONS_MOD_STER_HPP

#include <boost/geometry/util/math.hpp>
#include <boost/math/special_functions/hypot.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/aasincos.hpp>
#include <boost/geometry/srs/projections/impl/pj_zpoly1.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct mil_os {}; // Miller Oblated Stereographic
    struct lee_os {}; // Lee Oblated Stereographic
    struct gs48 {}; // Mod. Stereographic of 48 U.S.
    struct alsk {}; // Mod. Stereographic of Alaska
    struct gs50 {}; // Mod. Stereographic of 50 U.S.

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace mod_ster
    {

            static const double epsilon = 1e-12;

            template <typename T>
            struct par_mod_ster
            {
                pj_complex<T> *zcoeff;
                T          cchio, schio;
                int        n;
            };

            /* based upon Snyder and Linck, USGS-NMD */

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_mod_ster_ellipsoid
                : public base_t_fi<base_mod_ster_ellipsoid<T, Parameters>, T, Parameters>
            {
                par_mod_ster<T> m_proj_parm;

                inline base_mod_ster_ellipsoid(const Parameters& par)
                    : base_t_fi<base_mod_ster_ellipsoid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    static const T half_pi = detail::half_pi<T>();

                    T sinlon, coslon, esphi, chi, schi, cchi, s;
                    pj_complex<T> p;

                    sinlon = sin(lp_lon);
                    coslon = cos(lp_lon);
                    esphi = this->m_par.e * sin(lp_lat);
                    chi = 2. * atan(tan((half_pi + lp_lat) * .5) *
                        pow((1. - esphi) / (1. + esphi), this->m_par.e * .5)) - half_pi;
                    schi = sin(chi);
                    cchi = cos(chi);
                    s = 2. / (1. + this->m_proj_parm.schio * schi + this->m_proj_parm.cchio * cchi * coslon);
                    p.r = s * cchi * sinlon;
                    p.i = s * (this->m_proj_parm.cchio * schi - this->m_proj_parm.schio * cchi * coslon);
                    p = pj_zpoly1(p, this->m_proj_parm.zcoeff, this->m_proj_parm.n);
                    xy_x = p.r;
                    xy_y = p.i;
                }

                // INVERSE(e_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    static const T half_pi = detail::half_pi<T>();

                    int nn;
                    pj_complex<T> p, fxy, fpxy, dp;
                    T den, rh = 0, z, sinz = 0, cosz = 0, chi, phi = 0, dphi, esphi;

                    p.r = xy_x;
                    p.i = xy_y;
                    for (nn = 20; nn ;--nn) {
                        fxy = pj_zpolyd1(p, this->m_proj_parm.zcoeff, this->m_proj_parm.n, &fpxy);
                        fxy.r -= xy_x;
                        fxy.i -= xy_y;
                        den = fpxy.r * fpxy.r + fpxy.i * fpxy.i;
                        dp.r = -(fxy.r * fpxy.r + fxy.i * fpxy.i) / den;
                        dp.i = -(fxy.i * fpxy.r - fxy.r * fpxy.i) / den;
                        p.r += dp.r;
                        p.i += dp.i;
                        if ((fabs(dp.r) + fabs(dp.i)) <= epsilon)
                            break;
                    }
                    if (nn) {
                        rh = boost::math::hypot(p.r, p.i);
                        z = 2. * atan(.5 * rh);
                        sinz = sin(z);
                        cosz = cos(z);
                        lp_lon = this->m_par.lam0;
                        if (fabs(rh) <= epsilon) {
                            /* if we end up here input coordinates were (0,0).
                             * pj_inv() adds P->lam0 to lp.lam, this way we are
                             * sure to get the correct offset */
                            lp_lon = 0.0;
                            lp_lat = this->m_par.phi0;
                            return;
                        }
                        chi = aasin(cosz * this->m_proj_parm.schio + p.i * sinz * this->m_proj_parm.cchio / rh);
                        phi = chi;
                        for (nn = 20; nn ;--nn) {
                            esphi = this->m_par.e * sin(phi);
                            dphi = 2. * atan(tan((half_pi + chi) * .5) *
                                pow((1. + esphi) / (1. - esphi), this->m_par.e * .5)) - half_pi - phi;
                            phi += dphi;
                            if (fabs(dphi) <= epsilon)
                                break;
                        }
                    }
                    if (nn) {
                        lp_lat = phi;
                        lp_lon = atan2(p.r * sinz, rh * this->m_proj_parm.cchio * cosz - p.i *
                            this->m_proj_parm.schio * sinz);
                    } else
                        lp_lon = lp_lat = HUGE_VAL;
                }

                static inline std::string get_name()
                {
                    return "mod_ster_ellipsoid";
                }

            };

            template <typename Parameters, typename T>
            inline void setup(Parameters& par, par_mod_ster<T>& proj_parm)  /* general initialization */
            {
                static T const half_pi = detail::half_pi<T>();

                T esphi, chio;

                if (par.es != 0.0) {
                    esphi = par.e * sin(par.phi0);
                    chio = 2. * atan(tan((half_pi + par.phi0) * .5) *
                        pow((1. - esphi) / (1. + esphi), par.e * .5)) - half_pi;
                } else
                    chio = par.phi0;
                proj_parm.schio = sin(chio);
                proj_parm.cchio = cos(chio);
            }

            /* Miller Oblated Stereographic */
            template <typename Parameters, typename T>
            inline void setup_mil_os(Parameters& par, par_mod_ster<T>& proj_parm)
            {
                static const T d2r = geometry::math::d2r<T>();

                static pj_complex<T> AB[] = {
                    {0.924500, 0.},
                    {0.,       0.},
                    {0.019430, 0.}
                };

                proj_parm.n = 2;
                par.lam0 = d2r * 20.;
                par.phi0 = d2r * 18.;
                proj_parm.zcoeff = AB;
                par.es = 0.;

                setup(par, proj_parm);
            }

            /* Lee Oblated Stereographic */
            template <typename Parameters, typename T>
            inline void setup_lee_os(Parameters& par, par_mod_ster<T>& proj_parm)
            {
                static const T d2r = geometry::math::d2r<T>();

                static pj_complex<T> AB[] = {
                    { 0.721316,   0.},
                    { 0.,         0.},
                    {-0.0088162, -0.00617325}
                };

                proj_parm.n = 2;
                par.lam0 = d2r * -165.;
                par.phi0 = d2r * -10.;
                proj_parm.zcoeff = AB;
                par.es = 0.;

                setup(par, proj_parm);
            }

            // Mod. Stererographics of 48 U.S.
            template <typename Parameters, typename T>
            inline void setup_gs48(Parameters& par, par_mod_ster<T>& proj_parm)
            {
                static const T d2r = geometry::math::d2r<T>();

                static pj_complex<T> AB[] = { /* 48 United States */
                    { 0.98879,  0.},
                    { 0.,       0.},
                    {-0.050909, 0.},
                    { 0.,       0.},
                    { 0.075528, 0.}
                };

                proj_parm.n = 4;
                par.lam0 = d2r * -96.;
                par.phi0 = d2r * -39.;
                proj_parm.zcoeff = AB;
                par.es = 0.;
                par.a = 6370997.;

                setup(par, proj_parm);
            }

            // Mod. Stererographics of Alaska
            template <typename Parameters, typename T>
            inline void setup_alsk(Parameters& par, par_mod_ster<T>& proj_parm)
            {
                static const T d2r = geometry::math::d2r<T>();

                static pj_complex<T> ABe[] = { /* Alaska ellipsoid */
                    { .9945303, 0.},
                    { .0052083, -.0027404},
                    { .0072721,  .0048181},
                    {-.0151089, -.1932526},
                    { .0642675, -.1381226},
                    { .3582802, -.2884586}
                };

                static pj_complex<T> ABs[] = { /* Alaska sphere */
                    { .9972523, 0.},
                    { .0052513, -.0041175},
                    { .0074606,  .0048125},
                    {-.0153783, -.1968253},
                    { .0636871, -.1408027},
                    { .3660976, -.2937382}
                };

                proj_parm.n = 5;
                par.lam0 = d2r * -152.;
                par.phi0 = d2r * 64.;
                if (par.es != 0.0) { /* fixed ellipsoid/sphere */
                    proj_parm.zcoeff = ABe;
                    par.a = 6378206.4;
                    par.e = sqrt(par.es = 0.00676866);
                } else {
                    proj_parm.zcoeff = ABs;
                    par.a = 6370997.;
                }

                setup(par, proj_parm);
            }

            // Mod. Stererographics of 50 U.S.
            template <typename Parameters, typename T>
            inline void setup_gs50(Parameters& par, par_mod_ster<T>& proj_parm)
            {
                static const T d2r = geometry::math::d2r<T>();

                static pj_complex<T> ABe[] = { /* GS50 ellipsoid */
                    { .9827497, 0.},
                    { .0210669,  .0053804},
                    {-.1031415, -.0571664},
                    {-.0323337, -.0322847},
                    { .0502303,  .1211983},
                    { .0251805,  .0895678},
                    {-.0012315, -.1416121},
                    { .0072202, -.1317091},
                    {-.0194029,  .0759677},
                    {-.0210072,  .0834037}
                };
                static pj_complex<T> ABs[] = { /* GS50 sphere */
                    { .9842990, 0.},
                    { .0211642,  .0037608},
                    {-.1036018, -.0575102},
                    {-.0329095, -.0320119},
                    { .0499471,  .1223335},
                    { .0260460,  .0899805},
                    { .0007388, -.1435792},
                    { .0075848, -.1334108},
                    {-.0216473,  .0776645},
                    {-.0225161,  .0853673}
                };

                proj_parm.n = 9;
                par.lam0 = d2r * -120.;
                par.phi0 = d2r * 45.;
                if (par.es != 0.0) { /* fixed ellipsoid/sphere */
                    proj_parm.zcoeff = ABe;
                    par.a = 6378206.4;
                    par.e = sqrt(par.es = 0.00676866);
                } else {
                    proj_parm.zcoeff = ABs;
                    par.a = 6370997.;
                }

                setup(par, proj_parm);
            }

    }} // namespace detail::mod_ster
    #endif // doxygen

    /*!
        \brief Miller Oblated Stereographic projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal (mod)
        \par Example
        \image html ex_mil_os.gif
    */
    template <typename T, typename Parameters>
    struct mil_os_ellipsoid : public detail::mod_ster::base_mod_ster_ellipsoid<T, Parameters>
    {
        inline mil_os_ellipsoid(const Parameters& par) : detail::mod_ster::base_mod_ster_ellipsoid<T, Parameters>(par)
        {
            detail::mod_ster::setup_mil_os(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Lee Oblated Stereographic projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal (mod)
        \par Example
        \image html ex_lee_os.gif
    */
    template <typename T, typename Parameters>
    struct lee_os_ellipsoid : public detail::mod_ster::base_mod_ster_ellipsoid<T, Parameters>
    {
        inline lee_os_ellipsoid(const Parameters& par) : detail::mod_ster::base_mod_ster_ellipsoid<T, Parameters>(par)
        {
            detail::mod_ster::setup_lee_os(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Mod. Stererographics of 48 U.S. projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal (mod)
        \par Example
        \image html ex_gs48.gif
    */
    template <typename T, typename Parameters>
    struct gs48_ellipsoid : public detail::mod_ster::base_mod_ster_ellipsoid<T, Parameters>
    {
        inline gs48_ellipsoid(const Parameters& par) : detail::mod_ster::base_mod_ster_ellipsoid<T, Parameters>(par)
        {
            detail::mod_ster::setup_gs48(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Mod. Stererographics of Alaska projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal (mod)
        \par Example
        \image html ex_alsk.gif
    */
    template <typename T, typename Parameters>
    struct alsk_ellipsoid : public detail::mod_ster::base_mod_ster_ellipsoid<T, Parameters>
    {
        inline alsk_ellipsoid(const Parameters& par) : detail::mod_ster::base_mod_ster_ellipsoid<T, Parameters>(par)
        {
            detail::mod_ster::setup_alsk(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Mod. Stererographics of 50 U.S. projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal (mod)
        \par Example
        \image html ex_gs50.gif
    */
    template <typename T, typename Parameters>
    struct gs50_ellipsoid : public detail::mod_ster::base_mod_ster_ellipsoid<T, Parameters>
    {
        inline gs50_ellipsoid(const Parameters& par) : detail::mod_ster::base_mod_ster_ellipsoid<T, Parameters>(par)
        {
            detail::mod_ster::setup_gs50(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::mil_os, mil_os_ellipsoid, mil_os_ellipsoid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::lee_os, lee_os_ellipsoid, lee_os_ellipsoid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::gs48, gs48_ellipsoid, gs48_ellipsoid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::alsk, alsk_ellipsoid, alsk_ellipsoid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::gs50, gs50_ellipsoid, gs50_ellipsoid)

        // Factory entry(s)
        template <typename T, typename Parameters>
        class mil_os_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<mil_os_ellipsoid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        class lee_os_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<lee_os_ellipsoid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        class gs48_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<gs48_ellipsoid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        class alsk_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<alsk_ellipsoid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        class gs50_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<gs50_ellipsoid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        inline void mod_ster_init(detail::base_factory<T, Parameters>& factory)
        {
            factory.add_to_factory("mil_os", new mil_os_entry<T, Parameters>);
            factory.add_to_factory("lee_os", new lee_os_entry<T, Parameters>);
            factory.add_to_factory("gs48", new gs48_entry<T, Parameters>);
            factory.add_to_factory("alsk", new alsk_entry<T, Parameters>);
            factory.add_to_factory("gs50", new gs50_entry<T, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_MOD_STER_HPP


#include <iostream>
#include <fstream>
#include <vector>
#include <sstream>
#include <iomanip>
#include <string>
#include <stdio.h>

#ifndef WIN64
#define EIGEN_DONT_ALIGN_STATICALLY
#endif
#include <Eigen/Dense>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
//#include <opencv2/contrib/contrib.hpp>

#include "mex.h"
#include "matrix.h"
#include "main.cpp"

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{

	if (nlhs != 1) {
			mexErrMsgIdAndTxt("MATLAB:outputmismatch",
												"Output arguments must be 1!");
	}

	if (nrhs != 2) {
			mexErrMsgIdAndTxt("MATLAB:inputmismatch",
												"Input arguments must be 2!");
	}

	if (!mxIsChar(prhs[0])){
		mexErrMsgIdAndTxt("MATLAB:inputmismatch",
												"The First Input arguments must be a string!");
	}

	if (! mxIsNumeric(prhs[1])){
		mexErrMsgIdAndTxt("MATLAB:inputmismatch",
												"The Second Input arguments must be a matrix!");
	}

	size_t n = mxGetN(prhs[1]);
	size_t m = mxGetM(prhs[1]);

	if (n != 3 || m != 3){
		mexErrMsgIdAndTxt("MATLAB:inputmismatch",
												"wrong matrix dimension!");
	}

	double *A = mxGetPr(prhs[1]);
	Eigen::Matrix3f K;
	for (size_t i=0; i < n*m; i++){
		K(i%n, i/m) = A[i];
	}

	//size_t buflen = mxGetN(prhs[0])*sizeof(mxChar)+1;
	char *dataFolder = mxArrayToString(prhs[0]);

   if (dataFolder == NULL)
     mexErrMsgTxt("Not enough heap space to hold converted string.");

 	//mxArray *assocFile;
 	//assocFile = mxCreateString("assocFile.txt");
 	//colorFile = mxCreateString("rgb.txt");

	//char *assocfile = mxArrayToString(assocFile);
	//char *colorfile = mxArrayToString(colorFile);

	std::vector<Eigen::Matrix4f> poses = ImageAlign(dataFolder, K);
	size_t numFrame = poses.size();
	size_t len = poses[0].size();

	std::cout << numFrame << std::endl;
	std::cout << len << std::endl;

	double *D;
	plhs[0] = mxCreateDoubleMatrix((mwSize)len, (mwSize)numFrame, mxREAL);
	D = mxGetPr(plhs[0]);

	for (int num =0 ; num< numFrame; num++){

		for (size_t j=0; j< poses[num].rows(); j++){
			for (size_t i=0; i<poses[num].cols();i++){
				*D++ = poses[num](j,i);
			}
		}

	}

}


#include "NormalMapBuilder.h"

#include <cmath>
#include <algorithm>

#include <cglib/vec.h>

#include <boost/math/constants/constants.hpp>

namespace carto { namespace vt {
    NormalMapBuilder::NormalMapBuilder(const std::array<float, 4>& rgbaHeightScale, std::uint8_t alpha) : _rgbaHeightScale(rgbaHeightScale), _alpha(alpha) {
    }

    std::shared_ptr<const Bitmap> NormalMapBuilder::buildNormalMapFromHeightMap(const TileId& tileId, const std::shared_ptr<const Bitmap>& bitmap) const {
        if (!bitmap) {
            return bitmap;
        }

        int width = bitmap->width;
        int height = bitmap->height;
        std::vector<std::uint32_t> data(width * height, 0);

        auto getInterpolatedHeight = [&](int x, int y) {
            int x0 = x, x1 = x;
            int y0 = y, y1 = y;
            if (x < 0) {
                x0 = 0;
                x1 = 1;
            }
            if (x >= width) {
                x0 = width - 1;
                x1 = width - 2;
            }
            if (y < 0) {
                y0 = 0;
                y1 = 1;
            }
            if (y >= height) {
                y0 = height - 1;
                y1 = height - 2;
            }

            if (x0 == x1 && y0 == y1) {
                return unpackHeight(bitmap->data[(y0 * width) + x0]);
            }
            return 2 * unpackHeight(bitmap->data[(y0 * width) + x0]) - unpackHeight(bitmap->data[(y1 * width) + x1]);
        };

        if (width >= 2 && height >= 2) {
            float heights[3][3];
            for (int y = 0; y < height; y++) {
                double y1 = boost::math::constants::pi<double>() * ((tileId.y + (height - y - 0.5) / height) / (1 << tileId.zoom) - 0.5);
                double rz = std::tanh(y1);
                double ss = std::sqrt(std::max(0.0, 1.0 - rz * rz));

                for (int dy = 0; dy < 3; dy++) {
                    heights[dy][1] = getInterpolatedHeight(-1, y + dy - 1);
                    heights[dy][2] = getInterpolatedHeight( 0, y + dy - 1);
                }
                for (int x = 0; x < width; x++) {
                    for (int dy = 0; dy < 3; dy++) {
                        heights[dy][0] = heights[dy][1];
                        heights[dy][1] = heights[dy][2];
                        heights[dy][2] = getInterpolatedHeight(x + 1, y + dy - 1);
                    }

                    float dx = (heights[0][2] + 2 * heights[1][2] + heights[2][2]) - (heights[0][0] + 2 * heights[1][0] + heights[2][0]);
                    float dy = (heights[2][0] + 2 * heights[2][1] + heights[2][2]) - (heights[0][0] + 2 * heights[0][1] + heights[0][2]);
                    float dz = 8.0f * static_cast<float>(ss);

                    data[y * width + x] = packNormal(dx, dy, dz);
                }
            }
        }
        return std::make_shared<Bitmap>(width, height, std::move(data));
    }

    float NormalMapBuilder::unpackHeight(std::uint32_t color) const {
        union {
            std::uint32_t u32;
            std::uint8_t u8[sizeof(std::uint32_t)];
        } packedColor;
        packedColor.u32 = color;
        float height = packedColor.u8[0] * _rgbaHeightScale[0];
        height += packedColor.u8[1] * _rgbaHeightScale[1];
        height += packedColor.u8[2] * _rgbaHeightScale[2];
        height += packedColor.u8[3] * _rgbaHeightScale[3];
        return height;
    }

    std::uint32_t NormalMapBuilder::packNormal(float dx, float dy, float dz) const {
        union {
            std::uint32_t u32;
            std::uint8_t u8[sizeof(std::uint32_t)];
        } packedNormal;
        cglib::vec3<float> normal = cglib::unit(cglib::vec3<float>(dx, dy, dz));
        packedNormal.u8[0] = static_cast<std::uint8_t>((normal(0) + 1.0f) * 127.5f);
        packedNormal.u8[1] = static_cast<std::uint8_t>((normal(1) + 1.0f) * 127.5f);
        packedNormal.u8[2] = static_cast<std::uint8_t>((normal(2) + 1.0f) * 127.5f);
        packedNormal.u8[3] = _alpha;
        return packedNormal.u32;
    }
} }