#include <iostream>
#include <cmath>
#include <string>
#include <vector>
#include <queue>
#include <cstdlib>
#include <cassert>
#include <limits>

#include <boost/foreach.hpp>
#include <boost/thread.hpp>
#include <boost/chrono.hpp>
#include <boost/random.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/normal_distribution.hpp>

#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Vector3.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Path.h>

#include <octomap/octomap.h>
#include <octomap_ros/conversions.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/boundary.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/radius_outlier_removal.h>

#include <pcl_conversions/pcl_conversions.h>

namespace pcl
{
    template<typename PointA, typename PointB>
    double sqdist(const PointA& a, const PointB& b)
    {
        double dx = a.x - b.x;
        double dy = a.y - b.y;
        double dz = a.z - b.z;
        return dx * dx + dy * dy + dz * dz;
    }

    template<typename PointA, typename PointB>
    double dist(const PointA& a, const PointB& b)
    {
        return sqrt(sqdist(a, b));
    }
}

class NavigationFunction
{
protected:
    ros::NodeHandle nh_;
    ros::NodeHandle pnh_;
    std::string frame_id_;
    std::string robot_frame_id_;
    ros::Subscriber octree_sub_;
    ros::Subscriber goal_point_sub_;
    ros::Subscriber goal_pose_sub_;
    ros::Publisher ground_pub_;
    ros::Publisher obstacles_pub_;
    ros::Publisher reprojected_point_goal_pub_;
    ros::Publisher reprojected_pose_goal_pub_;
    tf::TransformListener tf_listener_;
    geometry_msgs::PoseStamped robot_pose_;
    geometry_msgs::PoseStamped goal_;
    octomap::OcTree* octree_ptr_;
    pcl::PointCloud<pcl::PointXYZI> ground_pcl_;
    pcl::PointCloud<pcl::PointXYZ> obstacles_pcl_;
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZI>::Ptr ground_octree_ptr_;
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>::Ptr obstacles_octree_ptr_;
    bool treat_unknown_as_free_;
    double robot_height_;
    double robot_radius_;
    double max_superable_height_;
    double ground_voxel_connectivity_;
public:
    NavigationFunction();
    ~NavigationFunction();
    void onOctomap(const octomap_msgs::Octomap::ConstPtr& msg);
    void onGoal(const geometry_msgs::PointStamped::ConstPtr& msg);
    void onGoal(const geometry_msgs::PoseStamped::ConstPtr& msg);
    void expandOcTree();
    bool isGround(const octomap::OcTreeKey& key);
    bool isObstacle(const octomap::OcTreeKey& key);
    bool isNearObstacle(const pcl::PointXYZI& point);
    void filterInflatedRegionFromGround();
    void computeGround();
    void projectGoalPositionToGround();
    void publishGroundCloud();
    int getGoalIndex();
    void getNeighboringGroundPoints(int index, std::vector<int>& neighbors, double search_radius, bool exclude_already_labeled);
    void computeDistanceTransform();
    double getAverageIntensity(int index, double search_radius);
    void smoothIntensity(double search_radius);
    void normalizeIntensity();
};

NavigationFunction::NavigationFunction()
    : pnh_("~"),
      frame_id_("/map"),
      robot_frame_id_("/base_link"),
      octree_ptr_(0L),
      treat_unknown_as_free_(false),
      robot_height_(0.5),
      robot_radius_(0.5),
      max_superable_height_(0.2),
      ground_voxel_connectivity_(1.8)
{
    pnh_.param("frame_id", frame_id_, frame_id_);
    pnh_.param("robot_frame_id", robot_frame_id_, robot_frame_id_);
    pnh_.param("treat_unknown_as_free", treat_unknown_as_free_, treat_unknown_as_free_);
    pnh_.param("robot_height", robot_height_, robot_height_);
    pnh_.param("robot_radius", robot_radius_, robot_radius_);
    pnh_.param("max_superable_height", max_superable_height_, max_superable_height_);
    pnh_.param("ground_voxel_connectivity", ground_voxel_connectivity_, ground_voxel_connectivity_);
    octree_sub_ = nh_.subscribe<octomap_msgs::Octomap>("octree_in", 1, &NavigationFunction::onOctomap, this);
    goal_point_sub_ = nh_.subscribe<geometry_msgs::PointStamped>("goal_point_in", 1, &NavigationFunction::onGoal, this);
    goal_pose_sub_ = nh_.subscribe<geometry_msgs::PoseStamped>("goal_pose_in", 1, &NavigationFunction::onGoal, this);
    ground_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("ground_cloud_out", 1, true);
    obstacles_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("obstacles_cloud_out", 1, true);
    reprojected_point_goal_pub_ = nh_.advertise<geometry_msgs::PointStamped>("reprojected_point_goal", 1, true);
    reprojected_pose_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("reprojected_pose_goal", 1, true);
    ground_pcl_.header.frame_id = frame_id_;
    obstacles_pcl_.header.frame_id = frame_id_;
}

NavigationFunction::~NavigationFunction()
{
    if(octree_ptr_) delete octree_ptr_;
}

void NavigationFunction::onOctomap(const octomap_msgs::Octomap::ConstPtr& msg)
{
    if(octree_ptr_) delete octree_ptr_;
    octree_ptr_ = octomap_msgs::binaryMsgToMap(*msg);

    expandOcTree();
    computeGround();
    computeDistanceTransform();
}

void NavigationFunction::onGoal(const geometry_msgs::PointStamped::ConstPtr& msg)
{
    geometry_msgs::PointStamped msg2;

    try
    {
        tf_listener_.transformPoint(frame_id_, *msg, msg2);
        goal_.header.stamp = msg2.header.stamp;
        goal_.header.frame_id = msg2.header.frame_id;
        goal_.pose.position.x = msg2.point.x;
        goal_.pose.position.y = msg2.point.y;
        goal_.pose.position.z = msg2.point.z;
        goal_.pose.orientation.x = 0.0;
        goal_.pose.orientation.y = 0.0;
        goal_.pose.orientation.z = 0.0;
        goal_.pose.orientation.w = 0.0;
        projectGoalPositionToGround();
        ROS_INFO("goal set to point (%f, %f, %f)",
            goal_.pose.position.x, goal_.pose.position.y, goal_.pose.position.z);
    }
    catch(tf::TransformException& ex)
    {
        ROS_ERROR("Failed to lookup robot position: %s", ex.what());
        return;
    }

    computeDistanceTransform();

    msg2.point.x = goal_.pose.position.x;
    msg2.point.y = goal_.pose.position.y;
    msg2.point.z = goal_.pose.position.z;
    reprojected_point_goal_pub_.publish(msg2);
}

void NavigationFunction::onGoal(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    try
    {
        tf_listener_.transformPose(frame_id_, *msg, goal_);
        projectGoalPositionToGround();
        ROS_INFO("goal set to pose (%f, %f, %f), (%f, %f, %f, %f)",
                goal_.pose.position.x, goal_.pose.position.y, goal_.pose.position.z,
                goal_.pose.orientation.x, goal_.pose.orientation.y, goal_.pose.orientation.z,
                goal_.pose.orientation.w);
    }
    catch(tf::TransformException& ex)
    {
        ROS_ERROR("Failed to lookup robot position: %s", ex.what());
        return;
    }

    computeDistanceTransform();

    reprojected_pose_goal_pub_.publish(goal_);
}

void NavigationFunction::expandOcTree()
{
    if(!octree_ptr_) return;

    unsigned int maxDepth = octree_ptr_->getTreeDepth();

    // expand collapsed occupied nodes until all occupied leaves are at maximum depth
    std::vector<octomap::OcTreeNode*> collapsed_occ_nodes;

    size_t initial_size = octree_ptr_->size();
    size_t num_rounds = 0;
    size_t expanded_nodes = 0;

    do
    {
        collapsed_occ_nodes.clear();
        for(octomap::OcTree::iterator it = octree_ptr_->begin(); it != octree_ptr_->end(); ++it)
        {
            if(octree_ptr_->isNodeOccupied(*it) && it.getDepth() < maxDepth)
            {
                collapsed_occ_nodes.push_back(&(*it));
            }
        }
        for(std::vector<octomap::OcTreeNode*>::iterator it = collapsed_occ_nodes.begin(); it != collapsed_occ_nodes.end(); ++it)
        {
            (*it)->expandNode();
        }

        expanded_nodes += collapsed_occ_nodes.size();
        num_rounds++;
    } while(collapsed_occ_nodes.size() > 0);

    //ROS_INFO("received octree of %ld nodes; expanded %ld nodes in %ld rounds.", initial_size, expanded_nodes, num_rounds);
}

bool NavigationFunction::isGround(const octomap::OcTreeKey& key)
{
    octomap::OcTreeNode *node = octree_ptr_->search(key);
    if(!node) return false;
    if(!octree_ptr_->isNodeOccupied(node)) return false;

    double res = octree_ptr_->getResolution();
    int steps = ceil(robot_height_ / res);
    octomap::OcTreeKey key1(key);
    while(steps-- > 0)
    {
        key1[2]++;
        node = octree_ptr_->search(key1);
        if(!node)
        {
            if(!treat_unknown_as_free_) return false;
        }
        else
        {
            if(octree_ptr_->isNodeOccupied(node)) return false;
        }
    }
    return true;
}

bool NavigationFunction::isObstacle(const octomap::OcTreeKey& key)
{
    octomap::OcTreeNode *node;
    double res = octree_ptr_->getResolution();
    int num_voxels = 1;

    // look up...
    octomap::OcTreeKey key1(key);
    while(true)
    {
        key1[2]++;
        node = octree_ptr_->search(key1);
        if(!node) break;
        if(node && !octree_ptr_->isNodeOccupied(node)) break;
        num_voxels++;
    }

    // look down...
    octomap::OcTreeKey key2(key);
    while(true)
    {
        key2[2]--;
        node = octree_ptr_->search(key2);
        if(!node) break;
        if(node && !octree_ptr_->isNodeOccupied(node)) break;
        num_voxels++;
    }

    return res * num_voxels > max_superable_height_;
}

bool NavigationFunction::isNearObstacle(const pcl::PointXYZI& point)
{
    std::vector<int> pointIdx2;
    std::vector<float> pointDistSq2;
    pcl::PointXYZ p;
    p.x = point.x;
    p.y = point.y;
    p.z = point.z;
    int num_points = obstacles_octree_ptr_->nearestKSearch(p, 1, pointIdx2, pointDistSq2);
    return num_points >= 1 && pointDistSq2[0] < (robot_radius_ * robot_radius_);
}

void NavigationFunction::filterInflatedRegionFromGround()
{
    for(int i = 0; i < ground_pcl_.size(); )
    {
        if(isNearObstacle(ground_pcl_[i]))
        {
            std::swap(*(ground_pcl_.begin() + i), ground_pcl_.back());
            ground_pcl_.points.pop_back();
        }
        else i++;
    }
    ground_pcl_.width = ground_pcl_.points.size();
    ground_pcl_.height = 1;
}

void NavigationFunction::computeGround()
{
    if(!octree_ptr_) return;

    ground_pcl_.clear();
    obstacles_pcl_.clear();

    for(octomap::OcTree::leaf_iterator it = octree_ptr_->begin(); it != octree_ptr_->end(); ++it)
    {
        if(!octree_ptr_->isNodeOccupied(*it)) continue;

        if(isGround(it.getKey()))
        {
            pcl::PointXYZI point;
            point.x = it.getX();
            point.y = it.getY();
            point.z = it.getZ();
            point.intensity = std::numeric_limits<float>::infinity();
            ground_pcl_.push_back(point);
        }
        else if(isObstacle(it.getKey()))
        {
            pcl::PointXYZ point;
            point.x = it.getX();
            point.y = it.getY();
            point.z = it.getZ();
            obstacles_pcl_.push_back(point);
        }
    }

    double res = octree_ptr_->getResolution();

    obstacles_octree_ptr_ = pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>::Ptr(new pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>(res));
    obstacles_octree_ptr_->setInputCloud(obstacles_pcl_.makeShared());
    obstacles_octree_ptr_->addPointsFromInputCloud();

    filterInflatedRegionFromGround();

    ground_octree_ptr_ = pcl::octree::OctreePointCloudSearch<pcl::PointXYZI>::Ptr(new pcl::octree::OctreePointCloudSearch<pcl::PointXYZI>(res));
    ground_octree_ptr_->setInputCloud(ground_pcl_.makeShared());
    ground_octree_ptr_->addPointsFromInputCloud();
}

void NavigationFunction::projectGoalPositionToGround()
{
    pcl::PointXYZI goal;
    goal.x = goal_.pose.position.x;
    goal.y = goal_.pose.position.y;
    goal.z = goal_.pose.position.z;
    std::vector<int> pointIdx;
    std::vector<float> pointDistSq;
    if(ground_octree_ptr_->nearestKSearch(goal, 1, pointIdx, pointDistSq) < 1)
    {
        ROS_ERROR("Failed to project goal position to ground pcl");
        return;
    }
    int i = pointIdx[0];
    goal_.pose.position.x = ground_pcl_[i].x;
    goal_.pose.position.y = ground_pcl_[i].y;
    goal_.pose.position.z = ground_pcl_[i].z;
}

void NavigationFunction::publishGroundCloud()
{
    if(ground_pub_.getNumSubscribers() > 0)
    {
        sensor_msgs::PointCloud2 msg;
        pcl::toROSMsg(ground_pcl_, msg);
        ground_pub_.publish(msg);
    }

    if(obstacles_pub_.getNumSubscribers() > 0)
    {
        sensor_msgs::PointCloud2 msg;
        pcl::toROSMsg(obstacles_pcl_, msg);
        obstacles_pub_.publish(msg);
    }
}

int NavigationFunction::getGoalIndex()
{
    // find goal index in ground pcl:
    std::vector<int> pointIdx;
    std::vector<float> pointDistSq;
    pcl::PointXYZI goal;
    goal.x = goal_.pose.position.x;
    goal.y = goal_.pose.position.y;
    goal.z = goal_.pose.position.z;
    if(ground_octree_ptr_->nearestKSearch(goal, 1, pointIdx, pointDistSq) < 1)
    {
        return -1;
    }
    else
    {
        return pointIdx[0];
    }
}

void NavigationFunction::getNeighboringGroundPoints(int index, std::vector<int>& neighbors, double search_radius, bool exclude_already_labeled)
{
    neighbors.clear();
    std::vector<float> pointDistSq;
    ground_octree_ptr_->radiusSearch(ground_pcl_[index], search_radius, neighbors, pointDistSq);
}

void NavigationFunction::computeDistanceTransform()
{
    if(ground_pcl_.size() == 0)
    {
        ROS_INFO("skip computing distance transform because ground_pcl_ is empty");
        return;
    }

    // find goal index in ground pcl:
    int goal_idx = getGoalIndex();
    if(goal_idx == -1)
    {
        ROS_ERROR("unable to find goal in ground pcl");
        return;
    }

    double search_radius = ground_voxel_connectivity_ * octree_ptr_->getResolution();

    // distance to goal is zero (stored in the intensity channel):
    ground_pcl_[goal_idx].intensity = 0.0;

    std::queue<int> q;
    q.push(goal_idx);
    while(!q.empty())
    {
        int i = q.front();
        q.pop();

        std::vector<int> neighbors;
        getNeighboringGroundPoints(i, neighbors, search_radius, true);

        for(std::vector<int>::iterator it = neighbors.begin(); it != neighbors.end(); it++)
        {
            int j = *it;

            // intensity value are initially set to infinity.
            // if i is finite it means it has already been labeled.
            if(std::isfinite(ground_pcl_[j].intensity)) continue;

            // otherwise, label it:
            ground_pcl_[j].intensity = ground_pcl_[i].intensity +
                dist(ground_pcl_[j], ground_pcl_[i]);

            // continue exploring neighbours:
            q.push(j);
        }
    }

    //smoothIntensity(search_radius);
    normalizeIntensity();

    publishGroundCloud();
}

double NavigationFunction::getAverageIntensity(int index, double search_radius)
{
    std::vector<int> pointIdx;
    std::vector<float> pointDistSq;
    ground_octree_ptr_->radiusSearch(ground_pcl_[index], search_radius, pointIdx, pointDistSq);

    if(pointIdx.size() == 0) return std::numeric_limits<float>::infinity();

    double i = 0.0;
    for(std::vector<int>::iterator it = pointIdx.begin(); it != pointIdx.end(); ++it)
    {
        i += ground_pcl_[*it].intensity;
    }
    i /= (double)pointIdx.size();
    return i;
}

void NavigationFunction::smoothIntensity(double search_radius)
{
    std::vector<double> smoothed_intensity;
    smoothed_intensity.resize(ground_pcl_.size());
    for(size_t i = 0; i < ground_pcl_.size(); i++)
    {
        smoothed_intensity[i] = getAverageIntensity(i, search_radius);
    }
    for(size_t i = 0; i < ground_pcl_.size(); i++)
    {
        ground_pcl_[i] = smoothed_intensity[i];
    }
}

void NavigationFunction::normalizeIntensity()
{
    float imin = std::numeric_limits<float>::infinity();
    float imax = -std::numeric_limits<float>::infinity();
    for(pcl::PointCloud<pcl::PointXYZI>::iterator it = ground_pcl_.begin(); it != ground_pcl_.end(); ++it)
    {
        if(!std::isfinite(it->intensity)) continue;
        imin = fmin(imin, it->intensity);
        imax = fmax(imax, it->intensity);
    }
    const float eps = 0.01;
    float d = imax - imin + eps;
    for(pcl::PointCloud<pcl::PointXYZI>::iterator it = ground_pcl_.begin(); it != ground_pcl_.end(); ++it)
    {
        if(std::isfinite(it->intensity))
            it->intensity = (it->intensity - imin) / d;
        else
            it->intensity = 1.0;
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "navigation_function");

    NavigationFunction p;
    ros::spin();

    return 0;
}


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!
  @ingroup group-euler
    Function object implementing signgam capabilities

    sign of gamma function

    @par Semantic:

    For every parameter of floating type T0

    @code
    T0 r = signgam(x);
    @endcode

    is similar to

    @code
    T0 r = sign(gamma(x));
    @endcode

    signgam does not computes @ref gamma and is fast.

    @see gamma,  gammaln

  **/
  const boost::dispatch::functor<tag::signgam_> signgam = {};
} }
#endif

#include <boost/simd/function/scalar/signgam.hpp>
//#include <boost/simd/function/simd/signgam.hpp>

#endif


// json parser
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <utility>
namespace pt = boost::property_tree;

// integration routine
#include "../Calculations/Integrations.h"

#include "../Permittivity/PermittivityFactory.h"
#include "../ReflectionCoefficients/ReflectionCoefficientsFactory.h"
#include "GreensTensorPlate.h"

GreensTensorPlate::GreensTensorPlate(
    double v, double beta, double za,
    std::shared_ptr<ReflectionCoefficients> reflection_coefficients,
    double delta_cut, const vec::fixed<2> &rel_err)
    : GreensTensor(v, beta), za(za), delta_cut(delta_cut), rel_err(rel_err),
      reflection_coefficients(std::move(reflection_coefficients)) {

  // assertions
  assert(this->za >= 0);
  assert(this->delta_cut >= 0);
  assert(this->rel_err(0) >= 0 && this->rel_err(1) >= 0);
}

GreensTensorPlate::GreensTensorPlate(const std::string &input_file)
    : GreensTensor(input_file) {
  this->reflection_coefficients =
      ReflectionCoefficientsFactory::create(input_file);

  // Create a root
  pt::ptree root;

  // Load the json file in this ptree
  pt::read_json(input_file, root);

  // check if type is right
  std::string type = root.get<std::string>("GreensTensor.type");
  assert(type == "plate");

  // read parameters
  this->za = root.get<double>("GreensTensor.za");
  this->delta_cut = root.get<double>("GreensTensor.delta_cut");
  this->rel_err(0) = root.get<double>("GreensTensor.rel_err_0");
  this->rel_err(1) = root.get<double>("GreensTensor.rel_err_1");

  // assertions
  assert(this->za >= 0);
  assert(this->delta_cut >= 0);
  assert(this->rel_err(0) >= 0 && this->rel_err(1) >= 0);
}

void GreensTensorPlate::calculate_tensor(double omega, vec::fixed<2> k,
                                         cx_mat::fixed<3, 3> &GT) const {
  // imaginary unit
  std::complex<double> I(0.0, 1.0);

  // load wavevectors from the struct into the corresponding variables
  double kx = k(0);
  double ky = k(1);
  double k_quad = kx * kx + ky * ky;

  // squared and absolute value of the frequency
  // The tensor is calculated for a positive frequency omega
  // and afterwards transformed with respect to the sign of omega
  double omega_abs = std::abs(omega);
  double omega_quad = omega_abs * omega_abs;

  // propagation in free space and within the surface material
  // kappa is defined to have either a purely
  // positive real part or purely negatively imaginary part
  std::complex<double> kappa =
      sqrt(std::complex<double>(k_quad - omega_quad, 0.));
  kappa = std::complex<double>(std::abs(kappa.real()), -std::abs(kappa.imag()));

  // produce the reflection coefficients in s- and p-polarization
  std::complex<double> r_p, r_s;
  reflection_coefficients->calculate(omega_abs, kappa, r_p, r_s);

  // For an better overview and a efficient calculation, the
  // pre-factors of the p and s polarization are collected separately
  std::complex<double> pre = (2 * M_PI) * exp(-(2 * za) * kappa);
  std::complex<double> prefactor_s =
      pre * r_s * omega_quad / (k_quad - omega_quad);
  std::complex<double> prefactor_p = pre * r_p;

  // In the following, odd orders in ky are already omitted
  GT.zeros();
  GT(0, 0) = (prefactor_p * kx * kx + prefactor_s * ky * ky) * kappa / k_quad;
  GT(1, 1) = (prefactor_p * ky * ky + prefactor_s * kx * kx) * kappa / k_quad;
  GT(2, 2) = prefactor_p * k_quad / kappa;
  GT(2, 0) = I * prefactor_p * kx;
  GT(0, 2) = -GT(2, 0);

  // In case of negative frequencies, the tensor has to be hermitian transposed
  if (omega < 0) {
    GT = trans(GT);
  }
}

void GreensTensorPlate::integrate_k(double omega, cx_mat::fixed<3, 3> &GT,
                                    Tensor_Options fancy_complex,
                                    Weight_Options weight_function) const {

  // imaginary unit
  std::complex<double> I(0.0, 1.0);

  // intialize Green's tensor
  GT.zeros();

  // calculate the five non-zero elements of the Green's tensor. Here, the
  // symmetry in y direction was already applied. Thus, the integration only
  // consideres twice the domain from 0 to pi.

  // the xx element
  auto F_xx = [=](double x) -> double {
    return this->integrand_1d_k(x, omega, {0, 0}, fancy_complex,
                                weight_function);
  };
  GT(0, 0) = cquad(F_xx, 0, M_PI, rel_err(1), 0) / M_PI;

  // the yy element
  auto F_yy = [=](double x) -> double {
    return this->integrand_1d_k(x, omega, {1, 1}, fancy_complex,
                                weight_function);
  };
  GT(1, 1) = cquad(F_yy, 0, M_PI, rel_err(1), 0) / M_PI;

  // the zz element
  auto F_zz = [=](double x) -> double {
    return this->integrand_1d_k(x, omega, {2, 2}, fancy_complex,
                                weight_function);
  };
  GT(2, 2) = cquad(F_zz, 0, M_PI, rel_err(1), 0) / M_PI;

  // the zx element
  auto F_zx = [=](double x) -> double {
    return this->integrand_1d_k(x, omega, {2, 0}, fancy_complex,
                                weight_function);
  };
  GT(2, 0) = I * cquad(F_zx, 0, M_PI, rel_err(1), 0) / M_PI;

  // the xz element
  GT(0, 2) = -GT(2, 0);
}

double GreensTensorPlate::integrand_1d_k(double phi, double omega,
                                         const uvec::fixed<2> &indices,
                                         Tensor_Options fancy_complex,
                                         Weight_Options weight_function) const {

  double result;

  // The cut-off parameters acts as upper bound of the kappa integration.
  double kappa_cut = delta_cut / (2 * za);

  // read integration variable phi
  double cos_phi = std::cos(phi);

  // define integrand
  auto F = [=](double x) -> double {
    return this->integrand_2d_k(x, omega, phi, indices, fancy_complex,
                                weight_function);
  };

  // Calculate low-temperature edge
  double edge = std::abs(omega / (v * cos_phi));

  // Calculate the integrand corresponding to the given options. To resolve the
  // probably sharp edge of the Bose-Einstein distribution, the integration is
  // split at the edge, if the edged lies below the cut-off kappa_cut.
  if ( (kappa_cut > edge) && (2*za/v < beta) ) {
    result = cquad(F, 0, std::abs(omega / (v * cos_phi)), rel_err(0), 0);
    result += cquad(F, edge, kappa_cut, rel_err(0), std::abs(result)*rel_err(0));
  } else {
    result = cquad(F, 0, kappa_cut, rel_err(0), 0);
  }
    result += cquad(F, -std::abs(omega), 0, rel_err(0), std::abs(result)*rel_err(0));

  return result;
}

double GreensTensorPlate::integrand_2d_k(double kappa_double, double omega,
                                         double phi,
                                         const uvec::fixed<2> &indices,
                                         Tensor_Options fancy_complex,
                                         Weight_Options weight_function) const {

  double v_quad = v * v;
  double omega_quad = omega * omega;
  double cos_phi = cos(phi);
  double cos_phi_quad = cos_phi * cos_phi;
  double sin_phi_quad = 1.0 - cos_phi_quad;

  // Before the real or imaginary part of the chosen matrix element can be
  // calculated, the complex result is stored in result_complex.
  std::complex<double> result_complex;
  double result = 0.;

  // imaginary unit
  std::complex<double> I(0.0, 1.0);

  // permittivity and propagation through vacuum (kappa) and surface material
  std::complex<double> kappa_complex;
  double kappa_quad;
  // Transfer kappa to the correct complex value
  if (kappa_double < 0.0) {
    kappa_complex = std::complex<double>(0.0, kappa_double);
    kappa_quad = -kappa_double * kappa_double;
  } else {
    kappa_complex = std::complex<double>(kappa_double, 0.0);
    kappa_quad = kappa_double * kappa_double;
  }

  // Express kappa via frequency and kappa.
  // In order to achieve the desired accuracy, we subtract first
  // (kappa^2 + omega^2), since this might be equal zero.
  double k = (sqrt((kappa_quad + omega_quad)- kappa_quad * v_quad * cos_phi_quad) +
              v * omega * cos_phi) / (1.E0 - v_quad * cos_phi_quad);
  double k_quad = k * k;

  // Define the Doppler-shifted frequency
  double omega_pl = (omega + k * cos_phi * v);
  double omega_pl_quad = omega_pl * omega_pl;

  // In order to obey reality in time, a positive omega_pl is used for the
  // actual calculation. Afterwards, the corresponding symmetry operation is
  // performed if the sign of omega_pl is negative.
  double omega_pl_abs = std::abs(omega_pl);

  // producing the reflection coefficients in p- and s-polarization
  // reflection coefficients and pre-factors of the corresponding polarization
  std::complex<double> r_p, r_s;
  reflection_coefficients->calculate(omega_pl_abs, kappa_complex, r_p, r_s);

  // Impose reality in time
  if (omega_pl < 0) {
    r_s = conj(r_s);
    r_p = conj(r_p);
    kappa_complex = conj(kappa_complex);
  }

  // helpful prefactors
  // general prefactor with volume element and exponential
  std::complex<double> prefactor = std::abs(kappa_complex) *
                                   exp(-2 * za * kappa_complex) /
                                   (1. - cos_phi * v * omega_pl / k);
  // For an better overview and a efficient calculation, we collect the
  // pre-factors of the p and s polarization separately
  std::complex<double> prefactor_s =
      prefactor * r_s * omega_pl_quad / kappa_complex;
  std::complex<double> prefactor_p = prefactor * r_p * kappa_complex;

  // Calculate the G_xx element
  if (indices(0) == 0 && indices(1) == 0) {
    result_complex = prefactor_p * cos_phi_quad + prefactor_s * sin_phi_quad;
  }
  // Calculate the G_yy element
  else if (indices(0) == 1 && indices(1) == 1) {
    result_complex = prefactor_p * sin_phi_quad + prefactor_s * cos_phi_quad;
  }
  // Calculate the G_zz element
  else if (indices(0) == 2 && indices(1) == 2) {
    result_complex = prefactor_p * k_quad / kappa_quad;
  }
  // Calculate the G_zx element
  else if (indices(0) == 2 && indices(1) == 0) {
    result_complex = prefactor_p * I * cos_phi * k / kappa_complex;
  }
  // Calculate the G_xz element
  else if (indices(0) == 0 && indices(1) == 2) {
    result_complex = -prefactor_p * I * cos_phi * k / kappa_complex;
  } else {
    result_complex = 0.;
  }

  // Add weighting function if demanded
  if (weight_function == KV) {
    result_complex *= k * cos_phi;
  } else if (weight_function == TEMP) {
    result_complex /= (1.0 - exp(-beta * omega_pl));
  } else if (weight_function == NON_LTE) {
    result_complex *=
        1. / (1.0 - exp(-beta * omega_pl)) - 1. / (1.0 - exp(-beta * omega));
  } else if (weight_function == KV_TEMP) {
    result_complex *= k * cos_phi / (1.0 - exp(-beta * omega_pl));
  } else if (weight_function == KV_NON_LTE) {
    result_complex *=
        k * cos_phi *
        (1. / (1.0 - exp(-beta * omega_pl)) - 1. / (1.0 - exp(-beta * omega)));
  }

  // Calculate fancy real part of the given matrix element
  if (fancy_complex == RE) {
    if ((indices(0) == 2 && indices(1) == 0) ||
        (indices(0) == 0 && indices(1) == 2)) {
      // Mind the missing leading I! This must be added after the double
      // integration!
      result = result_complex.imag();
    } else {
      result = result_complex.real();
    }
  }
  // Calculate fancy imaginary part of the given matrix element
  else if (fancy_complex == IM) {
    if ((indices(0) == 2 && indices(1) == 0) ||
        (indices(0) == 0 && indices(1) == 2)) {
      // Mind the missing leading I! This must be added after the double
      // integration!
      result = -result_complex.real();
    } else {
      result = result_complex.imag();
    }
  }

  return result;
}

std::complex<double> GreensTensorPlate::get_r_p(double omega, double k) const {
  std::complex<double> r_p, r_s;
  std::complex<double> kappa;
  if (k < omega) {
    kappa = std::complex<double>(0., -sqrt(omega * omega - k * k));
  } else {
    kappa = std::complex<double>(sqrt(k * k - omega * omega), 0.);
  }
  reflection_coefficients->calculate(omega, kappa, r_p, r_s);
  return r_p;
}

std::complex<double> GreensTensorPlate::get_r_s(double omega, double k) const {
  std::complex<double> r_s, r_p;
  std::complex<double> kappa;
  if (k < omega) {
    kappa = std::complex<double>(0., -sqrt(omega * omega - k * k));
  } else {
    kappa = std::complex<double>(sqrt(k * k - omega * omega), 0.);
  }
  reflection_coefficients->calculate(omega, kappa, r_p, r_s);
  return r_s;
}

double GreensTensorPlate::omega_ch() const {
  // Calculate omega_cut (reasonable for every plate setup)
  return this->delta_cut * this->v / this->za;
}

void GreensTensorPlate::print_info(std::ostream &stream) const {
  stream << "# GreensTensorPlate\n#\n"
         << "# v = " << v << "\n"
         << "# beta = " << beta << "\n"
         << "# za = " << za << "\n"
         << "# delta_cut = " << delta_cut << "\n"
         << "# rel_err = " << rel_err(0) << "," << rel_err(1) << "\n";
}


#define BOOST_TEST_MODULE "test_dihedral_angle_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/utility.hpp>

#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/local/DihedralAngleInteraction.hpp>
#include <mjolnir/math/constants.hpp>
#include <mjolnir/forcefield/local/ClementiDihedralPotential.hpp>
#include <mjolnir/util/make_unique.hpp>

#include <random>

BOOST_AUTO_TEST_CASE(DihedralAngleInteraction_numerical_diff)
{
    namespace test = mjolnir::test;
    mjolnir::LoggerManager::set_default_logger("test_dihedral_interaction.log");

    using traits_type         = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type           = traits_type::real_type;
    using coord_type          = traits_type::coordinate_type;
    using boundary_type       = traits_type::boundary_type;
    using system_type         = mjolnir::System<traits_type>;
    using potential_type       = mjolnir::ClementiDihedralPotential<real_type>;
    using dihedral_angle_type = mjolnir::DihedralAngleInteraction<traits_type, potential_type>;

    constexpr auto pi = mjolnir::math::constants<real_type>::pi();

    const real_type k1(1e0);
    const real_type k3(1e0);
    const real_type native(pi / 2.0);

    std::mt19937 mt(123456789);

    potential_type potential{k1, k3, native};
    dihedral_angle_type interaction("none", {{ {{0,1,2,3}}, potential}});

    for(std::size_t i=0; i<1000; ++i)
    {
        const real_type theta = i * 0.001 * (2.0 * pi);

        system_type sys(4, boundary_type{});
        test::clear_everything(sys);

        sys.position(0) = coord_type( 2.0,  std::cos(theta), std::sin(theta));
        sys.position(1) = coord_type( 1.0,  0.0, 0.0);
        sys.position(2) = coord_type( 0.0,  0.0, 0.0);
        sys.position(3) = coord_type(-1.0, -1.0, 0.0);

        test::apply_random_rotation(sys, mt);
        test::apply_random_perturbation(sys, mt, 0.01);

        constexpr real_type tol = 1e-4;
        constexpr real_type dr  = 1e-5;

        test::check_force(sys, interaction, tol, dr);
        test::check_virial(sys, interaction, tol);
        test::check_force_and_virial(sys, interaction, tol);
        test::check_force_and_energy(sys, interaction, tol);
        test::check_force_energy_virial(sys, interaction, tol);
    }
}

BOOST_AUTO_TEST_CASE(DihedralAngle_force)
{
    mjolnir::LoggerManager::set_default_logger("test_dihedral_interaction.log");

    using traits_type         = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type           = traits_type::real_type;
    using coord_type          = traits_type::coordinate_type;
    using boundary_type       = traits_type::boundary_type;
    using system_type         = mjolnir::System<traits_type>;
    using potential_type      = mjolnir::ClementiDihedralPotential<real_type>;
    using dihedral_angle_type = mjolnir::DihedralAngleInteraction<traits_type, potential_type>;

    constexpr real_type tol = 1e-7;

    const real_type k1(1e0);
    const real_type k3(1e0);
    const real_type native(mjolnir::math::constants<real_type>::pi() * 2.0 / 3.0);

    potential_type potential{k1, k3, native};
    dihedral_angle_type interaction("none", {{ {{0,1,2,3}}, potential}});

    const coord_type pos1(1e0, 0e0, 1e0);
    const coord_type pos2(0e0, 0e0, 1e0);
    const coord_type pos3(0e0, 0e0, 0e0);

    system_type sys(4, boundary_type{});

    sys.mass(0) = 1.0;
    sys.mass(1) = 1.0;
    sys.mass(2) = 1.0;
    sys.mass(3) = 1.0;
    sys.rmass(0) = 1.0;
    sys.rmass(1) = 1.0;
    sys.rmass(2) = 1.0;
    sys.rmass(3) = 1.0;

    sys.position(0) = pos1;
    sys.position(1) = pos2;
    sys.position(2) = pos3;
    sys.position(3) = coord_type(0,0,0);
    sys.velocity(0) = coord_type(0,0,0);
    sys.velocity(1) = coord_type(0,0,0);
    sys.velocity(2) = coord_type(0,0,0);
    sys.velocity(3) = coord_type(0,0,0);
    sys.force(0)    = coord_type(0,0,0);
    sys.force(1)    = coord_type(0,0,0);
    sys.force(2)    = coord_type(0,0,0);
    sys.force(3)    = coord_type(0,0,0);

    sys.name(0)  = "X";
    sys.name(1)  = "X";
    sys.name(2)  = "X";
    sys.name(3)  = "X";
    sys.group(0) = "NONE";
    sys.group(1) = "NONE";
    sys.group(2) = "NONE";
    sys.group(3) = "NONE";

    const real_type dtheta = mjolnir::math::constants<real_type>::pi() / 1800.0;
    for(int i = -1800; i < 1800; ++i)
    {
        BOOST_TEST(mjolnir::math::length(sys.position(0) - pos1) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::length(sys.position(1) - pos2) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::length(sys.position(2) - pos3) == 0.0, boost::test_tools::tolerance(tol));

        BOOST_TEST(mjolnir::math::length(sys.velocity(0)) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::length(sys.velocity(1)) == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::length(sys.velocity(2)) == 0.0, boost::test_tools::tolerance(tol));

        sys.force(0) = coord_type(0,0,0);
        sys.force(1) = coord_type(0,0,0);
        sys.force(2) = coord_type(0,0,0);
        sys.force(3) = coord_type(0,0,0);

        const real_type theta = i * dtheta;
        const coord_type pos4(std::cos(theta), -std::sin(theta), 0e0);
        sys.position(3) = pos4;

        const real_type deriv = potential.derivative(theta);
        const real_type coef = std::abs(deriv);

        interaction.calc_force(sys);

        // magnitude
        // if radius == 1e0, then force strength is equal to dV.
        BOOST_TEST(mjolnir::math::length(sys.position(1) - sys.position(0)) == 1e0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::length(sys.position(2) - sys.position(1)) == 1e0, boost::test_tools::tolerance(tol));
        BOOST_TEST(mjolnir::math::length(sys.position(3) - sys.position(2)) == 1e0, boost::test_tools::tolerance(tol));

        const real_type force_strength1 = mjolnir::math::length(sys.force(0));
        const real_type force_strength3 = mjolnir::math::length(sys.force(3));
        if(i == 1200)
        {
            BOOST_TEST(coef            == 0.0, boost::test_tools::tolerance(tol));
            BOOST_TEST(force_strength1 == 0.0, boost::test_tools::tolerance(tol));
            BOOST_TEST(force_strength3 == 0.0, boost::test_tools::tolerance(tol));
        }
        else
        {
            BOOST_TEST(coef == force_strength1, boost::test_tools::tolerance(tol));
            BOOST_TEST(coef == force_strength3, boost::test_tools::tolerance(tol));
        }

        // force applied to center particle is equal to sum of others
        const coord_type sum = sys.force(0) + sys.force(1) + sys.force(2) + sys.force(3);
        BOOST_TEST(mjolnir::math::length(sum) == 0.0, boost::test_tools::tolerance(tol));

        // direction
        if(i == 1200) // most stable point
        {
            BOOST_TEST(mjolnir::math::length(sys.force(0)) == 0.0, boost::test_tools::tolerance(tol));
            BOOST_TEST(mjolnir::math::length(sys.force(1)) == 0.0, boost::test_tools::tolerance(tol));
            BOOST_TEST(mjolnir::math::length(sys.force(2)) == 0.0, boost::test_tools::tolerance(tol));
            BOOST_TEST(mjolnir::math::length(sys.force(3)) == 0.0, boost::test_tools::tolerance(tol));
        }
        else
        {
            // perpendicular to radius vector
            const real_type normal1 = mjolnir::math::dot_product(sys.force(0), sys.position(0) - sys.position(1));
            const real_type normal4 = mjolnir::math::dot_product(sys.force(3), sys.position(2) - sys.position(3));
            BOOST_TEST(normal1 == 0.0, boost::test_tools::tolerance(tol));
            BOOST_TEST(normal4 == 0.0, boost::test_tools::tolerance(tol));
        }

        // perpendicular to z axis
        BOOST_TEST(sys.force(0)[2] == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(1)[2] == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(2)[2] == 0.0, boost::test_tools::tolerance(tol));
        BOOST_TEST(sys.force(3)[2] == 0.0, boost::test_tools::tolerance(tol));
    }
}


/*
Copyright (c) 2020 Naomasa Matsubayashi

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include <cmath>
#include <iostream>
#include <fstream>
#include <vector>
#include <algorithm>
#include <numeric>
#include <boost/program_options.hpp>
#include "ifm/fft.h"
#include "ifm/load_monoral.h"
#include "ifm/spectrum_image.h"
#include "ifm/exp_match.h"
int main( int argc, char* argv[] ) {
  boost::program_options::options_description options("オプション");
  options.add_options()
    ("help,h",    "ヘルプを表示")
    ("input,i", boost::program_options::value<std::string>(),  "入力ファイル")
    ("note,n", boost::program_options::value<int>()->default_value(60),  "音階")
    ("harmonic,H", boost::program_options::value<int>()->default_value(0),  "倍音のみ")
    ("resolution,r", boost::program_options::value<int>()->default_value(13),  "分解能")
    ("damped,d", boost::program_options::value<bool>()->default_value(false),  "減衰振動")
    ("exp,e", boost::program_options::value<bool>()->default_value(false),  "指数関数近似")
    ("linear,l", boost::program_options::value<bool>()->default_value(false),  "線形近似");
  boost::program_options::variables_map params;
  boost::program_options::store( boost::program_options::parse_command_line( argc, argv, options ), params );
  boost::program_options::notify( params );
  if( params.count("help") || !params.count("input") ) {
    std::cout << options << std::endl;
    return 0;
  }
  const std::string input_filename = params["input"].as<std::string>();
  const auto [audio,sample_rate] = ifm::load_monoral( input_filename, true );
  ifm::spectrum_image conv( params["note"].as<int>(), sample_rate, 1 << params["resolution"].as<int>() );
  auto [image,delay] = conv( audio );
  unsigned int width = conv.get_width();
  std::vector< float > envelope;
  if( params["harmonic"].as<int>() > 0 ) {
    for( unsigned int y = 0; y != image.size() / width; ++y ) {
      float sum = 0.f;
      for( unsigned int x = 1; x < width; x += 24 )
        sum += image[ x + y * width ];
      envelope.push_back( sum );
    }
  }
  else if( params["harmonic"].as<int>() < 0 ) {
    for( unsigned int y = 0; y != image.size() / width; ++y ) {
      float sum = 0.f;
      for( unsigned int x = 25; x < width; ++x ) {
        unsigned int step =  x % 24;
        if( step > 1 && step < 23 )
          sum += image[ x + y * width ];
      }
      envelope.push_back( sum );
    }
  }
  else {
    for( unsigned int y = 0; y != image.size() / width; ++y ) {
      float sum = std::accumulate( image.data() + y * width + 1, image.data() + ( y + 1 ) * width, 0.f );
      envelope.push_back( sum );
    }
  }
  bool damped = params[ "damped" ].as< bool >();
  bool exp = params[ "exp" ].as< bool >();
  bool linear = params[ "linear" ].as< bool >();
  const auto ece = ifm::get_attack_and_decay_exp( envelope.data(), envelope.size(), 0.01f, damped );
  std::cout << ece.attack_a << " " << ece.attack_b << " " << ece.decay_a << " " << ece.decay_b <<std::endl;
  const auto ecf = ifm::get_attack_and_decay( envelope.data(), envelope.size(), 0.01f, damped, damped );
  for( unsigned int y = 0; y != envelope.size(); ++y ) {
    float t = 0.01f * y;
    if( damped || y > ece.highest ) {
      std::cout << t << " " << envelope[ y ] << " ";
      if( exp ) std::cout << ifm::get_exp_envelope( ece.decay_a, ece.decay_b, t - ece.highest ) * ece.highest_level << " ";
    }
    else {
      std::cout << t << " " << envelope[ y ] << " ";
      if( exp ) std::cout << ifm::get_exp_envelope( ece.attack_a, ece.attack_b, ece.highest - t ) * ece.highest_level << " ";
    }
    if( linear ) std::cout << ifm::get_linear_envelope( ecf.first, t ) * ece.highest_level << std::endl;
    else std::cout << std::endl;
  }
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_PREDICATES_FUNCTIONS_SIMD_SSE_SSE2_IS_LEZ_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_PREDICATES_FUNCTIONS_SIMD_SSE_SSE2_IS_LEZ_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/toolbox/predicates/functions/is_lez.hpp>
#include <boost/simd/include/functions/simd/bitwise_or.hpp>
#include <boost/simd/include/functions/simd/bitofsign.hpp>
#include <boost/simd/include/functions/simd/is_less_equal.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/dispatch/meta/as_floating.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_lez_, boost::simd::tag::sse2_, (A0)
                            , ((simd_<int64_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::as_floating<A0>::type ftype;
      return bitwise_cast<result_type>(is_less_equal(b_or(One<ftype>(), bitofsign(a0)), Zero<ftype>()));
    }
  };
} } }

#endif
#endif


#include "gtest.h"

#ifdef HAS_EIGEN3
#include <Eigen/Core>
#endif

#include "../../core/GSLAM.h"
#include "../../core/Event.h"

TEST(SLAM,LoadRelease)
{
    std::string slamPlugin=svar.GetString("SLAM","");
    if(slamPlugin.empty()) return;
    GSLAM::SLAMPtr slam=GSLAM::SLAM::create(slamPlugin);
    slam->call("Stop");
    slam.reset();
}

#ifdef EIGEN_MATRIX_H

TEST(SLAM,EigenPoint)
{
    Eigen::Vector2d pt2_eigen;
    GSLAM::Point2f  pt2_gslam=pt2_eigen;
    pt2_eigen=pt2_gslam;

    Eigen::Vector3d pt3_eigen;
    GSLAM::Point3f  pt3_gslam=pt3_eigen;
    pt3_eigen=pt3_gslam;

    pi::SE3f      se3;
    Eigen::Matrix<float,6,1> ln=se3.ln();
}

#endif


/**
 * @author  Daniel Maturana
 * @year    2015
 *
 * @attention Copyright (c) 2015
 * @attention Carnegie Mellon University
 * @attention All rights reserved.
 *
 **@=*/

#ifndef TRANSFORM_NEA_PC_HPP_FIHO1SY5
#define TRANSFORM_NEA_PC_HPP_FIHO1SY5

#include <ros/console.h>

#include "nea_pc_format.hpp"

#include <Eigen/Geometry>
#include <sensor_msgs/PointCloud2.h>
#include <tf/tf.h>

namespace ca
{

// TODO should we also accept Affine3f?
template<class Scalar>
void TransformNeaPc2InPlace(const Eigen::Affine3d& cloud_to_world,
                            sensor_msgs::PointCloud2& cloud) {

  if (cloud.data.empty()) { return; }

  //ROS_ASSERT( CheckNeaPc2NumFields(cloud) );
  ROS_ASSERT( CheckNeaPc2Datatype<Scalar>(cloud) );
  ROS_ASSERT( cloud.point_step == NeaPc2Metadata<Scalar>::point_step );

  Eigen::Transform<Scalar, 3, Eigen::Affine> c2w(cloud_to_world.template cast<Scalar>());
  for (size_t i=0; i < cloud.width; ++i) {
    uint8_t *ptr = &(cloud.data[i * cloud.point_step]);
    Eigen::Map< Eigen::Matrix<Scalar, 3, 1> > p(reinterpret_cast<Scalar *>(ptr));
    p = c2w * p;

    Eigen::Map< Eigen::Matrix<Scalar, 3, 1> > vp(reinterpret_cast<Scalar *>(ptr + NeaPc2Metadata<Scalar>::x_origin_offset));
    vp = c2w * vp;
  }

}

inline
void TransformNeaPc2InPlace(const Eigen::Affine3d& cloud_to_world,
                            sensor_msgs::PointCloud2& cloud) {
  if (cloud.data.empty()) { return; }
  if (cloud.fields[0].datatype==sensor_msgs::PointField::FLOAT32) {
    TransformNeaPc2InPlace<float>(cloud_to_world, cloud);
  } else if (cloud.fields[0].datatype==sensor_msgs::PointField::FLOAT64) {
    TransformNeaPc2InPlace<double>(cloud_to_world, cloud);
  } else {
    ROS_ERROR("Unknown point cloud type");
  }
}

} /* ca */

#endif /* end of include guard: TRANSFORM_NEA_PC_HPP_FIHO1SY5 */


#include "sparse_parent_tree.h"
#include "score_cache.h"
#include <algorithm>
#include <boost/unordered_set.hpp>

bestscorecalculators::SparseParentTree::SparseParentTree(const int variable, const int variableCount) {
    this->variable = variable;
    this->variableCount = variableCount;
    this->temp = NULL;
}

bestscorecalculators::SparseParentTree::~SparseParentTree() {
    if (root) {
        delete root;
    }
}

varset getSmallestUnused(const boost::unordered_set<varset> &usedScores, FloatMap *scores, int variableCount) {
    int minCardinality = std::numeric_limits<int>::max();
    VARSET_NEW(minVarset, variableCount);
    VARSET_SET_ALL(minVarset, variableCount);

    for (auto iter = scores->begin(); iter != scores->end(); iter++) {
        varset v = (*iter).first;
        if (cardinality(v) < minCardinality) {
            if (usedScores.find(v) == usedScores.end()) {

                minCardinality = cardinality(v);
                minVarset = v;
            }
        }
    }
    return minVarset;
}

void bestscorecalculators::SparseParentTree::initialize(const scoring::ScoreCache& scoreCache) {
    FloatMap *scores = scoreCache.getCache(variable);
    boost::unordered_set<varset> usedScores;

    // first, the empty set
    VARSET_NEW(empty, variableCount);
    float score = scores->at(empty);
    root = new SparseTreeNode(empty, score, variableCount);

    //    roots.push_back(r);

    // find all the children and set bounds recursively
    root->addChildren(scores, variableCount, usedScores);

    // now, pick up all of the remaining scores
    VARSET_NEW(flag, variableCount);
    VARSET_SET_ALL(flag, variableCount);

    varset unused = getSmallestUnused(usedScores, scores, variableCount);

    while (unused != flag) {
        VARSET_NEW(set, variableCount);
        temp = root;
        // the path to get to this node
        // used to propagate bounds back up
        std::vector<SparseTreeNode*> path;
        // try to find a path from empty to this root
        for (int i = 0; i < variableCount; i++) {
            if (VARSET_GET(unused, i)) {
                path.push_back(temp);
                VARSET_SET(set, i);
                SparseTreeNode *child = temp->getChildByIndex(i);

                // check if this child was missing
                if (child == NULL) {
                    // then create a dummy node, and add it to temp
                    child = new SparseTreeNode(set, std::numeric_limits<float>::max(), variableCount);
                    child->setBound(std::numeric_limits<float>::max(), empty);
                    temp->addChild(child, i);
                }

                temp = child;
            }
        }

        // temp should now point to where the new root should go
        score = scores->at(unused);
        temp->setScore(score);

        // find all the children and set bounds recursively
        temp->addChildren(scores, variableCount, usedScores);

        for (auto prev = path.begin(); prev != path.end(); prev++) {
            if ((*prev)->getBound() > temp->getBound()) {
                (*prev)->setBound(temp->getBound(), temp->getBoundParents());
            }
        }

        // need to propagate the bounds back up...

        unused = getSmallestUnused(usedScores, scores, variableCount);
    }
}

float bestscorecalculators::SparseParentTree::getScoreBestFirst(const varset &Q) {

    //best = infinity, bestS = null
    float best = std::numeric_limits<float>::max();
    open.clear();

    // push(OPEN, {}, bound({})
    //    for (auto iter = roots.begin(); iter != roots.end(); iter++) {
    //        if (VARSET_AND((*iter)->getParents(), Q) == (*iter)->getParents()) {
    //            open.push_back((*iter));
    //            std::push_heap(open.begin(), open.end(), cstns);
    //        }
    //    }
    open.push_back(root);

    // while best > bound
    while (open.size() > 0) {
        // U = pop(OPEN)
        temp = open.front();
        std::pop_heap(open.begin(), open.end(), cstns);
        open.pop_back();

        if (temp->getScore() < best) {
            best = temp->getScore();
        }

        if (temp->getBound() >= best) {
            break;
        }

        // for each child of U
        for (auto succ = temp->childrenBegin(); succ != temp->childrenEnd(); succ++) {
            // if succ \in Q
            if (VARSET_AND((*succ)->getParents(), Q) == (*succ)->getParents()) {
                // push (OPEN, succ, bound(succ))
                open.push_back((*succ));
                std::push_heap(open.begin(), open.end(), cstns);
            }
        }
    }

    return best;
}

float bestscorecalculators::SparseParentTree::getScore(varset &Q) {

    //best = infinity, bestS = null
    float best = std::numeric_limits<float>::max();

    while (stack.size() > 0) {
        stack.pop();
    }

    int maxVariable = lastSetBit(Q);

    stack.push(root);

    // while best > bound
    while (stack.size() > 0) {
        // U = pop(OPEN)
        temp = stack.top();
        stack.pop();

        if (temp->getScore() < best) {
            best = temp->getScore();
            bestParents = temp->getParents();
        }

        if (temp->getBound() >= best) {
            continue;
        }

        if (VARSET_AND(Q, temp->getBoundParents()) == temp->getBoundParents()) {
            // the bound is definitely better than best because of the previous conditional
            best = temp->getBound();
            bestParents = temp->getBoundParents();
            continue;
        }

        for (int i = 0; i < temp->getChildCount(); i++) {
            int index = temp->getChildIndex(i);
            if (index > maxVariable) break;
            if (!VARSET_GET(Q, index)) continue;

            // check to see if I can just use the best thing from this branch
            stack.push(temp->getChildByIndex(index));

        }

        //        // for each child of U
        //        for (auto succ = temp->childrenBegin(); succ != temp->childrenEnd(); succ++) {
        //            // if succ \in Q
        //            if ((*succ)->getBound() >= best) continue;
        //            if (VARSET_AND((*succ)->getParents(), Q) == (*succ)->getParents()) {
        //                // push (OPEN, succ, bound(succ))
        //                stack.push((*succ));
        //            }
        //        }
    }

    return best;
}


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

#ifndef MTL_ROW_MAT_CVEC_INDEX_EVALUATOR_INCLUDE
#define MTL_ROW_MAT_CVEC_INDEX_EVALUATOR_INCLUDE

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/is_row_major.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/utility/static_assert.hpp>

namespace mtl { namespace vec {

/// ET class to evaluate the product of a row-major matrix and a column vector row by row
template <typename VectorOut, typename Matrix, typename VectorIn, typename Assign>
struct row_mat_cvec_index_evaluator
{
    MTL_STATIC_ASSERT((mtl::traits::is_row_major<Matrix>::value), "Only row-major matrices supported here.");
    typedef typename mtl::Collection<VectorOut>::value_type        value_type;
    typedef typename mtl::Collection<Matrix>::size_type            size_type;

    row_mat_cvec_index_evaluator(VectorOut& w, const Matrix& A, const VectorIn& v) : w(w), A(A), v(v) {}

    template <unsigned Offset>
    void at(size_type i, boost::mpl::true_)
    {
	// value_type tmp(math::zero(w[i+Offset]));
	value_type tmp(0);
	const size_type cj0= A.ref_major()[i+Offset], cj1= A.ref_major()[i+Offset+1];
	for (size_type j= cj0; j != cj1; ++j)
	    tmp+= A.data[j] * v[A.ref_minor()[j]];
	Assign::first_update(w[i+Offset], tmp);
    }

    template <unsigned Offset>
    void at(size_type i, boost::mpl::false_)
    {
	value_type tmp(math::zero(w[i+Offset]));
	for (size_type j= 0; j < num_cols(A); j++)
	    tmp+= A[i][j] * v[j];
	Assign::first_update(w[i+Offset], tmp);
    }

    template <unsigned Offset>
    void at(size_type i)
    {
	at<Offset>(i, typename mtl::traits::is_sparse<Matrix>::type());
    }

    void operator()(size_type i) { at<0>(i); }
    void operator[](size_type i) { at<0>(i); }

    VectorOut&      w;
    const Matrix&   A;
    const VectorIn& v;
};

template <typename VectorOut, typename Matrix, typename VectorIn, typename Assign>
inline std::size_t size(const row_mat_cvec_index_evaluator<VectorOut, Matrix, VectorIn, Assign>& eval)
{
    return size(eval.w);
}

}} // namespace mtl::vector

#endif // MTL_ROW_MAT_CVEC_INDEX_EVALUATOR_INCLUDE


#include "lwtnn/LightweightGraph.hh"
#include "lwtnn/InputPreprocessor.hh"
#include "lwtnn/Graph.hh"
#include <Eigen/Dense>

namespace {
  using namespace Eigen;
  using namespace lwt;

  // utility functions
  typedef LightweightGraph::NodeMap NodeMap;
  typedef std::vector<std::pair<std::string, InputPreprocessor*> > Preprocs;
  std::vector<VectorXd> get_input_vectors(const NodeMap& nodes,
                                          const Preprocs& preprocs) {
    std::vector<VectorXd> input_vectors;
    for (const auto& proc: preprocs) {
      if (!nodes.count(proc.first)) {
        throw NNEvaluationException("Can't find node " + proc.first);
      }
      const auto& preproc = *proc.second;
      input_vectors.emplace_back(preproc(nodes.at(proc.first)));
    }
    return input_vectors;
  }

}
namespace lwt {
  // ______________________________________________________________________
  // Lightweight Graph

  typedef LightweightGraph::NodeMap NodeMap;
  LightweightGraph::LightweightGraph(const GraphConfig& config,
                                     std::string default_output):
    m_graph(new Graph(config.nodes, config.layers))
  {
    for (const auto& node: config.inputs) {
      m_preprocs.emplace_back(
        node.name, new InputPreprocessor(node.variables));
    }
    size_t output_n = 0;
    for (const auto& node: config.outputs) {
      m_outputs.emplace_back(node.second.node_index, node.second.labels);
      m_output_indices.emplace(node.first, output_n);
      output_n++;
    }
    if (default_output.size() > 0) {
      if (!m_output_indices.count(default_output)) {
        throw NNConfigurationException("no output node" + default_output);
      }
      m_default_output = m_output_indices.at(default_output);
    } else if (output_n == 1) {
      m_default_output = 0;
    } else {
      throw NNConfigurationException("you must specify a default output");
    }
  }

  LightweightGraph::~LightweightGraph() {
    delete m_graph;
    for (auto& preproc: m_preprocs) {
      delete preproc.second;
      preproc.second = 0;
    }
  }

  ValueMap LightweightGraph::compute(const NodeMap& nodes) const {
    return compute(nodes, m_default_output);
  }
  ValueMap LightweightGraph::compute(const NodeMap& nodes,
                                     const std::string& output) const {
    if (!m_output_indices.count(output)) {
      throw NNEvaluationException("no output node " + output);
    }
    return compute(nodes, m_output_indices.at(output));
  }
  ValueMap LightweightGraph::compute(const NodeMap& nodes, size_t idx) const {
    VectorSource source(get_input_vectors(nodes, m_preprocs));
    VectorXd result = m_graph->compute(source, m_outputs.at(idx).first);
    const std::vector<std::string>& labels = m_outputs.at(idx).second;
    std::map<std::string, double> output;
    for (size_t iii = 0; iii < labels.size(); iii++) {
      output[labels.at(iii)] = result(iii);
    }
    return output;
  }

}


#include <string>
#include <memory>
#include <iostream>
#include <boost/python.hpp>
#include "WCECommon.hpp"
#include "WCETarcog.hpp"

using namespace Tarcog;

class GlazingSystem
{
private:
  bool set;
  bool solved;
  //general inputs
  double pressure;
  double windowWidth;
  double windowHeight;
  //Outdoor Environment
  double airTemperature;
  double airSpeed;
  double tSky;
  double solarRadiation;
  std::shared_ptr<ISO15099::CEnvironment> OutdoorEnvironment;
  //Indoor Environment
  double roomTemperature;
  std::shared_ptr<ISO15099::CEnvironment> IndoorEnvironment;
  //Layers
  int numberOfLayers;
  //Solid Layers
  std::vector<double> solidLayerThickness;
  std::vector<double> solidLayerConductance;
  std::vector<double> emissivity1;
  std::vector<double> transmittance1;
  std::vector<double> emissivity2;
  std::vector<double> transmittance2;
  std::vector<std::shared_ptr<ISO15099::CIGUSolidLayer>> SolidLayers;
  //Air Gaps
  std::vector<double> gapThickness;
  std::vector<std::string> gasType;
  std::vector<Gases::CGas> Gases;
  std::vector<std::shared_ptr<ISO15099::CIGUGapLayer>> AirGaps;
  //IGU
  ISO15099::CIGU IGU;
  //Glazing System
  std::shared_ptr<ISO15099::CSingleSystem> System;
public:
  //The constructor
  GlazingSystem(){
    this->set = false;
    this->solved = false;
    this->pressure = 101325;//[Pa] - default value
  }//GlazingSystem

  //Get Temperatures
  boost::python::list getTemperatures(){
    boost::python::list array;
    auto Temperatures = this->System->getTemperatures();
    for (unsigned int i = 0; i < Temperatures.size(); i++){
      array.append( Temperatures[i] );
    }
    return array;
  }//getTemperatures

  //Get U value
  double getUValue(){
    return this->System->getUValue();
  }//getUValue

  //Initialize
  void initialize(){
    //outdoor
    this->OutdoorEnvironment = ISO15099::Environments::outdoor(this->airTemperature,this->airSpeed,this->solarRadiation,this->tSky, ISO15099::SkyModel::AllSpecified);
    this->OutdoorEnvironment->setHCoeffModel(ISO15099::BoundaryConditionsCoeffModel::CalculateH);
    //indoor
    this->IndoorEnvironment = ISO15099::Environments::indoor(this->roomTemperature);
    //IGU
    this->IGU = ISO15099::CIGU(this->windowWidth,this->windowHeight);
    for (int i=0; i<this->numberOfLayers; i++){
      //solid layer
      this->SolidLayers[i] = ISO15099::Layers::solid(this->solidLayerThickness[i],this->solidLayerConductance[i],this->emissivity1[i],this->transmittance1[i],this->emissivity2[i],this->transmittance2[i]);
      this->SolidLayers[i]->setSolarAbsorptance(0.094189159572);
      //add to the IGU
      this->IGU.addLayer(this->SolidLayers[i]);
      //air gap
      if (i<this->numberOfLayers-1){
        //set the proper Gas
        this->Gases[i] = Gases::CGas();
        if ( this->gasType[i].compare("Air") == 0 ){
          //do nothing - default is Air
        } else if (this->gasType[i].compare("Argon") == 0){
          this->Gases[i].addGasItem(1.0,Gases::GasDef::Argon);
        } else if (this->gasType[i].compare("Krypton") == 0){
          this->Gases[i].addGasItem(1.0,Gases::GasDef::Krypton);
        } else if (this->gasType[i].compare("Xenon") == 0){
          this->Gases[i].addGasItem(1.0,Gases::GasDef::Xenon);
        }
        //create the AirGap object
        this->AirGaps[i] = ISO15099::Layers::gap(this->gapThickness[i],this->Gases[i]);
        this->IGU.addLayer(this->AirGaps[i]);
      }//if (i<this->numberOfLayers-1){
    }//for (int i=0; i<this->numberOfLayers; i++){
    //System
    this->System = std::make_shared<ISO15099::CSingleSystem>(this->IGU,this->IndoorEnvironment,this->OutdoorEnvironment);
    this->set = true;
  }//Initialize

  //setSolidLayer
  void setAirGap(int lyr,double thickness,std::string gas){
    if (lyr <= this->numberOfLayers-1){
      lyr=lyr-1;//0 based indexing!
      this->gapThickness[lyr]=thickness;
      this->gasType[lyr]=gas;
    }
  }//setSize

  //Set Indoor Environment
  void setIndoorEnvironment(double roomTemperature){
    this->roomTemperature = roomTemperature;
  }//setIndoorEnvironment

  //Set the number of Layers
  void setNumberOfLayers(int numLayers){
    this->numberOfLayers = numLayers;
    //Solid Layers
    this->solidLayerThickness.resize(numLayers);
    this->solidLayerConductance.resize(numLayers);
    this->emissivity1.resize(numLayers);
    this->transmittance1.resize(numLayers);
    this->emissivity2.resize(numLayers);
    this->transmittance2.resize(numLayers);
    this->SolidLayers.resize(numLayers);
    //Air Gaps
    this->gapThickness.resize(numLayers);
    this->gasType.resize(numLayers);
    this->Gases.resize(numLayers);
    this->AirGaps.resize(numLayers);
  }//setNumberOfLayers

  //Set pressure
  void setPressure(double p){
    this->pressure = p;
  }//setPressure

  //Set Outdoor Environment
  void setOutdoorEnvironment(double airT, double airSpd, double solarRad, double tS){
    this->airTemperature = airT;
    this->airSpeed = airSpd;
    this->solarRadiation = solarRad;
    this->tSky = tS;
  }//setOutdoorEnvironment

  void setSize(double width, double height){
    this->windowWidth = width;
    this->windowHeight = height;
  }//setSize

  //setSolidLayer
  void setSolidLayer(int lyr,double thickness,double conductance,double eps1,double tau1,double eps2, double tau2){
    if (lyr <= this->numberOfLayers){
      lyr=lyr-1;//0 based indexing!
      this->solidLayerThickness[lyr]=thickness;
      this->solidLayerConductance[lyr]=conductance;
      this->emissivity1[lyr]=eps1;
      this->transmittance1[lyr]=tau1;
      this->emissivity2[lyr]=eps2;
      this->transmittance2[lyr]=tau2;
    }
  }//setSize

  void solve(){
    if (this->set){
      this->System->solve();
      this->solved = true;
    }
  }//solve

  //A test method
  void Test(){
    //outdoor
    this->OutdoorEnvironment = ISO15099::Environments::outdoor(this->airTemperature,this->airSpeed,this->solarRadiation,this->tSky, ISO15099::SkyModel::AllSpecified);
    this->OutdoorEnvironment->setHCoeffModel(ISO15099::BoundaryConditionsCoeffModel::CalculateH);
    //indoor
    this->IndoorEnvironment = ISO15099::Environments::indoor(this->roomTemperature);
    //solid layer
    auto solidLayerThickness = 0.003048;
    auto solidLayerConductance = 1.0;
    auto SolidLayer = ISO15099::Layers::solid(solidLayerThickness,solidLayerConductance);
    SolidLayer->setSolarAbsorptance(0.094189159572);
    //IGU
    auto TestIGU = ISO15099::CIGU(this->windowWidth,this->windowHeight);
    TestIGU.addLayer(SolidLayer);
    //System
    auto TestSystem = ISO15099::CSystem(TestIGU,this->IndoorEnvironment,this->OutdoorEnvironment);
    //Run
    auto Temperatures = TestSystem.getTemperatures(ISO15099::System::Uvalue);
    std::cout << "This is a test run of BPPGlazingSystem!" << std::endl;
    for (unsigned int i = 0; i < Temperatures.size(); i++)
    {
      std::cout << Temperatures[i] << std::endl;
    }
    //std::cout << "Temperatures: " << Temperatures << std::endl;
    return;
  }//void Test(){}

};//class GlazingSystem

//BOOST.python wrapper
BOOST_PYTHON_MODULE(BPPTarcogLib)
{
  using namespace boost::python;
  class_<GlazingSystem>("GlazingSystem", init<>())
    .def("getTemperatures", &GlazingSystem::getTemperatures)
    .def("getUValue", &GlazingSystem::getUValue)
    .def("initialize", &GlazingSystem::initialize)
    .def("setAirGap", &GlazingSystem::setAirGap)
    .def("setIndoorEnvironment", &GlazingSystem::setIndoorEnvironment)
    .def("setNumberOfLayers", &GlazingSystem::setNumberOfLayers)
    .def("setOutdoorEnvironment", &GlazingSystem::setOutdoorEnvironment)
    .def("setPressure", &GlazingSystem::setPressure)
    .def("setSize", &GlazingSystem::setSize)
    .def("setSolidLayer", &GlazingSystem::setSolidLayer)
    .def("solve", &GlazingSystem::solve)
    .def("Test", &GlazingSystem::Test);
}


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

#include <nt2/linalg/functions/plinsolve.hpp>
#include <nt2/include/functions/gesvx.hpp>
#include <nt2/include/functions/sysvx.hpp>
#include <nt2/include/functions/clinsolve.hpp>
#include <nt2/include/functions/posvx.hpp>
#include <nt2/include/functions/of_size.hpp>
#include <nt2/include/functions/resize.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/linalg/options.hpp>
#include <nt2/linalg/functions/details/eval_linsolve.hpp>
#include <nt2/sdk/meta/settings_of.hpp>
#include <nt2/sdk/meta/as_real.hpp>

#include <nt2/include/functions/tie.hpp>
#include <nt2/core/container/table/table.hpp>
#include <boost/dispatch/meta/terminal_of.hpp>
#include <boost/core/ignore_unused.hpp>

namespace nt2 { namespace ext
{
  //============================================================================
  // LINSOLVE
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( plinsolve_, tag::cpu_
                            , (A0)(A1)(A2)(N2)
                            , ((ast_<A0, nt2::container::domain>))  // A
                              ((ast_<A1, nt2::container::domain>))  // B
                              ((node_<A2, nt2::tag::tie_             // X-R
                                    , N2, nt2::container::domain
                                     >
                              ))
                            )
  {
    typedef void  result_type;
    typedef typename A0::value_type ctype_t;
    typedef typename nt2::meta::as_real<ctype_t>::type   type_t;
    typedef typename meta::option<typename A0::settings_type,nt2::tag::shape_>::type shape;
    typedef nt2::memory::container<tag::table_, ctype_t, nt2::settings(nt2::_2D)> desired_semantic;
    typedef nt2::memory::container<tag::table_, ctype_t, nt2::settings(nt2::_2D,shape)> desired_semantic1;

    BOOST_FORCEINLINE result_type operator()( A0 const& a0, A1 const& a1, A2 const&  a2 ) const
    {
      eval(a0,a1,a2,N2(),shape());
    }

    //==========================================================================
    /// INTERNAL ONLY - Shape analysis

    /// INTERNAL ONLY - X = LINSOLVE(A,B) -- Rectangular shape
    BOOST_FORCEINLINE
    void eval ( A0 const& a0, A1 const& a1 , A2 const& a2 ,boost::mpl::long_<1> const
              , nt2::rectangular_ const&) const
    {
      type_t rcond;
      boost::proto::child_c<0>(a2).resize(nt2::of_size(a0.leading_size(),1));
      NT2_AS_TERMINAL_IN(desired_semantic1,a,a0);
      NT2_AS_TERMINAL_IN(desired_semantic,b,a1);
      nt2::gesvx( boost::proto::value(a), boost::proto::value(b)
              , boost::proto::value(boost::proto::child_c<0>(a2)), rcond );
    }

    //==========================================================================
    /// INTERNAL ONLY - [X,R] = LINSOLVE(A,B) -- Rectangular shape
    BOOST_FORCEINLINE
    void eval ( A0 const& a0, A1 const& a1 , A2 const& a2 ,boost::mpl::long_<2> const
              , nt2::rectangular_ const&) const
    {
      type_t rcond;
      boost::proto::child_c<0>(a2).resize(nt2::of_size(a0.leading_size(),1));
      NT2_AS_TERMINAL_IN(desired_semantic1,a,a0);
      NT2_AS_TERMINAL_IN(desired_semantic,b,a1);
      nt2::gesvx( boost::proto::value(a), boost::proto::value(b)
              , boost::proto::value(boost::proto::child_c<0>(a2))
              , rcond );
      boost::proto::child_c<1>(a2) = rcond;
    }

    //==========================================================================
    /// INTERNAL ONLY - X = LINSOLVE(A,B) -- symmetric shape
    BOOST_FORCEINLINE
    void eval ( A0 const& a0, A1 const& a1 , A2 const& a2 ,boost::mpl::long_<1> const
              , nt2::symmetric_ const&) const
    {
      type_t rcond;
      nt2::container::table<nt2_la_int> piv = nt2::zeros(a0.leading_size(), 1
                                            , nt2::meta::as_<nt2_la_int>() );
      boost::proto::child_c<0>(a2).resize(nt2::of_size(a0.leading_size(),1));
      NT2_AS_TERMINAL_IN(desired_semantic1,a,a0);
      NT2_AS_TERMINAL_IN(desired_semantic,b,a1);
      nt2_la_int iter = nt2::sysvx( boost::proto::value(a),boost::proto::value(piv)
                                 , boost::proto::value(b)
                                 , boost::proto::value(boost::proto::child_c<0>(a2))
                                 , rcond);
      boost::ignore_unused(iter);
    }

    //==========================================================================
    /// INTERNAL ONLY - [X,R] = LINSOLVE(A,B) -- symmetric shape
    BOOST_FORCEINLINE
    void eval ( A0 const& a0, A1 const& a1 , A2 const& a2 ,boost::mpl::long_<2> const
              , nt2::symmetric_ const&) const
    {
      type_t rcond;
      nt2::container::table<nt2_la_int> piv(nt2::of_size(a0.leading_size(),1));
      boost::proto::child_c<0>(a2).resize(nt2::of_size(a0.leading_size(),1));
      NT2_AS_TERMINAL_IN(desired_semantic1,a,a0);
      NT2_AS_TERMINAL_IN(desired_semantic,b,a1);
      nt2_la_int iter = nt2::sysvx( boost::proto::value(a),boost::proto::value(piv)
                                 , boost::proto::value(b)
                                 , boost::proto::value(boost::proto::child_c<0>(a2))
                                 , rcond);

      boost::ignore_unused(iter);
      boost::proto::child_c<1>(a2) = rcond;
    }

    //==========================================================================
    /// INTERNAL ONLY - X = LINSOLVE(A,B) -- positive definite shape
    BOOST_FORCEINLINE
    void eval ( A0 const& a0, A1 const& a1 , A2 const& a2 ,boost::mpl::long_<1> const
              , nt2::positive_definite_ const&) const
    {
      type_t rcond;
      boost::proto::child_c<0>(a2).resize(nt2::of_size(a0.leading_size(),1));
      NT2_AS_TERMINAL_IN(desired_semantic1,a,a0);
      NT2_AS_TERMINAL_IN(desired_semantic,b,a1);
      nt2_la_int iter = nt2::posvx( boost::proto::value(a), boost::proto::value(b)
                                  , boost::proto::value(boost::proto::child_c<0>(a2))
                                  , rcond);
      boost::ignore_unused(iter);
    }

    //==========================================================================
    /// INTERNAL ONLY - [X,R] = LINSOLVE(A,B) -- positive definite shape
    BOOST_FORCEINLINE
    void eval ( A0 const& a0, A1 const& a1 , A2 const& a2 ,boost::mpl::long_<2> const
              , nt2::positive_definite_ const&) const
    {
      type_t rcond;
      boost::proto::child_c<0>(a2).resize(nt2::of_size(a0.leading_size(),1));
      NT2_AS_TERMINAL_IN(desired_semantic1,a,a0);
      NT2_AS_TERMINAL_IN(desired_semantic,b,a1);
      nt2_la_int iter = nt2::posvx( boost::proto::value(a), boost::proto::value(b)
                                  , boost::proto::value(boost::proto::child_c<0>(a2))
                                  , rcond);

      boost::ignore_unused(iter);
      boost::proto::child_c<1>(a2) = rcond;
    }

    /// INTERNAL ONLY - No info on this shape
    template<typename N,typename sh>
    BOOST_FORCEINLINE
    void eval ( A0 const& a0, A1 const& a1, A2 const& a2, N const&, sh const&) const
    {
      nt2::clinsolve(a0,a1,a2);
    }

  };

} }

#endif


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

/**
\file

\brief test_conversion.cpp

\details
Test conversion between quantities.

Output:
@verbatim
@endverbatim
**/

#include <boost/units/quantity.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/cgs.hpp>

#include <iostream>

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#define BOOST_UNITS_CHECK_CLOSE(a, b) BOOST_CHECK_CLOSE_FRACTION(a, b, .0000001)

namespace bu = boost::units;

typedef bu::si::length  si_length;
typedef bu::si::time    si_time;
typedef bu::si::mass    si_mass;
typedef bu::si::area    si_area;

typedef bu::cgs::length cgs_length;
typedef bu::cgs::time   cgs_time;
typedef bu::cgs::mass   cgs_mass;
typedef bu::cgs::area   cgs_area;

typedef bu::multiply_typeof_helper<si_length, cgs_length>::type     mixed_length;
typedef bu::multiply_typeof_helper<si_time, cgs_time>::type         mixed_time;

typedef bu::divide_typeof_helper<bu::multiply_typeof_helper<si_mass,cgs_area>::type, mixed_time>::type  mixed_energy_1;
typedef bu::divide_typeof_helper<bu::multiply_typeof_helper<cgs_mass,mixed_length>::type,
                                 bu::multiply_typeof_helper<cgs_time,cgs_time>::type >::type            mixed_energy_2;

BOOST_AUTO_TEST_CASE(test_conversion) {
    BOOST_CHECK_EQUAL(1, 1);
    bu::quantity<mixed_length> a1(2.0 * mixed_length());
    bu::quantity<si_area> a2(a1);

    BOOST_UNITS_CHECK_CLOSE(a2.value(), .02);

    bu::quantity<mixed_length> a3(a2);

    BOOST_UNITS_CHECK_CLOSE(a3.value(), 2.0);

    bu::quantity<mixed_energy_1> e1(2.0 * mixed_energy_1());
    bu::quantity<mixed_energy_2> e2(e1);

    BOOST_UNITS_CHECK_CLOSE(e2.value(), 20.0);

    bu::quantity<bu::si::energy> e3(e1);
    BOOST_UNITS_CHECK_CLOSE(e3.value(), .0002);
    bu::quantity<mixed_energy_2> e4(e3);
    BOOST_UNITS_CHECK_CLOSE(e4.value(), 20.0);

    bu::quantity<bu::cgs::force> F0 = 20 * bu::cgs::dyne;
    BOOST_UNITS_CHECK_CLOSE(F0.value(), 20.0);

    bu::quantity<bu::si::force> F3(F0);
    BOOST_UNITS_CHECK_CLOSE(F3.value(), 2.0e-4);

    bu::quantity<bu::si::force> F5(20 * bu::cgs::dyne);
    BOOST_UNITS_CHECK_CLOSE(F5.value(), 2.0e-4);

}

BOOST_AUTO_TEST_CASE(test_dimensionless_conversions) {
    typedef bu::divide_typeof_helper<bu::cgs::force, bu::si::force>::type mixed_dimensionless;

    bu::quantity<bu::si::dimensionless> dimensionless_test1(1.0*bu::cgs::dyne/bu::si::newton);
    BOOST_CHECK(dimensionless_test1 == 1e-5);

    typedef bu::multiply_typeof_helper<bu::si::length, bu::cgs::length>::type m_cm;
    typedef bu::divide_typeof_helper<m_cm, m_cm>::type heterogeneous_dimensionless;
    bu::quantity<heterogeneous_dimensionless> dimensionless_test2(1.0*bu::cgs::dyne/bu::si::newton);
    BOOST_CHECK(dimensionless_test2.value() == 1e-5);
    bu::quantity<bu::divide_typeof_helper<bu::cgs::force, bu::si::force>::type> dimensionless_test3(dimensionless_test2);
    BOOST_UNITS_CHECK_CLOSE(dimensionless_test3.value(), 1.0);

    BOOST_UNITS_CHECK_CLOSE(boost::units::conversion_factor(mixed_dimensionless(), heterogeneous_dimensionless()), 1e-5);
    BOOST_UNITS_CHECK_CLOSE(boost::units::conversion_factor(heterogeneous_dimensionless(), mixed_dimensionless()), 1e5);

    //m/cm -> g/kg
    bu::quantity<bu::divide_typeof_helper<bu::si::length, bu::cgs::length>::type> dimensionless_test4(2.0 * bu::si::meters / bu::cgs::centimeters);
    bu::quantity<bu::divide_typeof_helper<bu::cgs::mass, bu::si::mass>::type> dimensionless_test5(dimensionless_test4);
    BOOST_UNITS_CHECK_CLOSE(dimensionless_test5.value(), 2e5);
}