// Copyright (c) 2015-2018 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "assembler_active_region_generator.hpp"

#include <iterator>
#include <algorithm>
#include <cmath>
#include <cassert>

#include <boost/iterator/zip_iterator.hpp>
#include <boost/tuple/tuple.hpp>

#include "basics/genomic_region.hpp"
#include "basics/cigar_string.hpp"
#include "utils/mappable_algorithms.hpp"
#include "utils/maths.hpp"
#include "utils/append.hpp"

#include <iostream>

namespace octopus { namespace coretools {

AssemblerActiveRegionGenerator::AssemblerActiveRegionGenerator(const ReferenceGenome& reference)
: reference_ {reference}
, coverage_tracker_ {}
, interesting_read_coverages_ {}
, clipped_coverage_tracker_ {}
{}

namespace {

template <typename Container>
bool contains(const Container& values, const typename Container::value_type& value)
{
    return std::find(std::cbegin(values), std::cend(values), value) != std::cend(values);
}

} // namespace

AssemblerActiveRegionGenerator::AssemblerActiveRegionGenerator(const ReferenceGenome& reference, Options options)
: reference_ {reference}
, coverage_tracker_ {}
, interesting_read_coverages_ {}
, clipped_coverage_tracker_ {}
{
    snvs_interesting_ = contains(options.trigger_types, Options::TriggerType::snv);
    indels_interesting_ = contains(options.trigger_types, Options::TriggerType::indel);
    structual_interesting_ = contains(options.trigger_types, Options::TriggerType::structual);
    trigger_quality_ = options.trigger_quality;
    trigger_clip_size_ = options.trigger_clip_size;
}

void AssemblerActiveRegionGenerator::add(const SampleName& sample, const AlignedRead& read)
{
    coverage_tracker_[sample].add(read);
    if (is_interesting(read)) {
        interesting_read_coverages_[sample].add(read);
    }
    if (structual_interesting_) {
        if (is_soft_clipped(read)) {
            clipped_coverage_tracker_[sample].add(clipped_mapped_region(read));
        } else {
            clipped_coverage_tracker_[sample].add(read);
        }
    }
}

template <typename Container>
void log_each(Container& values)
{
    for (auto& value : values) value = std::log(value);
}

template <typename Container>
auto to_logs(const Container& values)
{
    auto result = values;
    log_each(result);
    return result;
}

struct HiddenStateParameters
{
    double mean, stdev;
};

template <typename RealType>
RealType log_normal_pdf(const RealType x, const RealType mean, const RealType stdev)
{
    constexpr RealType c {0.9189385332046727417803297364056176398613974736377834};
    return std::log(stdev) - c - std::pow(x - mean, 2) / (2 * std::pow(stdev, 2));
}

double emmision_probability(const unsigned x, const HiddenStateParameters params)
{
    return log_normal_pdf(static_cast<double>(x), params.mean, params.stdev);
}

auto compute_base_deletion_probabilities(const std::vector<unsigned>& coverages,
                                         const double mean_coverage,
                                         const double stdev_coverage)
{
    constexpr std::size_t num_states {3};
    constexpr std::array<double, num_states> begin_probabilities {1 - 2e-10, 1e-10, 1e-10};
    constexpr std::array<std::array<double, num_states + 1>, num_states> transition_probabilities {{
                                                                                {1.0 - (1e-2 + 1e-3 + 1e-20), 1e-2, 1e-3, 1e-20}, // normal
                                                                                {1.0 - (1e-7 + 1e-15 + 1e-20), 1e-7, 1e-15, 1e-20}, // insertion
                                                                                {1e-5, 1e-15, 1.0 - (1e-5 + 1e-15 + 1e-20), 1e-20}, // deletion
                                                                                }};
    constexpr std::array<double, num_states> end_probabilities {1 - 2e-10, 1e-10, 1e-10};
    std::array<HiddenStateParameters, num_states> state_params {{{mean_coverage, stdev_coverage},
                                                                {mean_coverage + 3 * stdev_coverage, stdev_coverage},
                                                                {std::max(mean_coverage - 2 * stdev_coverage, 0.0), stdev_coverage / 2}}};
    const auto num_observations = coverages.size();
    // convert to log space
    const auto a_0 = to_logs(begin_probabilities);
    const auto a_e = to_logs(end_probabilities);
    auto a = transition_probabilities;
    for (auto& c : a) log_each(c);
    using State = std::array<double, num_states>;
    using maths::log_sum_exp;
    // Forward part
    std::vector<State> fwd(num_observations);
    fwd[0] = a_0;
    for (std::size_t s {0}; s < num_states; ++s) {
        fwd[0][s] += emmision_probability(coverages[0], state_params[s]);
    }
    for (std::size_t i {1}; i < num_observations; ++i) {
        for (std::size_t s {0}; s < num_states; ++s) {
            State tmp {};
            for (std::size_t t {0}; t < num_states; ++t) {
                tmp[t] = fwd[i - 1][t] + a[t][s];
            }
            fwd[i][s] = log_sum_exp(tmp) + emmision_probability(coverages[i], state_params[s]);
        }
    }
    auto end = fwd.back();
    std::transform(std::cbegin(end), std::cend(end), std::cbegin(a_e), std::begin(end),
                   [] (auto p, auto l) { return p + l; });
    const auto p_fwd = log_sum_exp(end);
    // Backward part
    std::vector<State> bkw(num_observations);
    bkw.back() = a_e;
    for (int i = num_observations - 2; i >= 0; --i) {
        for (std::size_t s {0}; s < num_states; ++s) {
            State tmp {};
            for (std::size_t t {0}; t < num_states; ++t) {
                tmp[t] = bkw[i + 1][t] + emmision_probability(coverages[i + 1], state_params[t]) + a[s][t];
            }
            bkw[i][s] = log_sum_exp(tmp);
        }
    }
    // Merge
    std::vector<State> posteriors(num_observations);
    std::transform(std::cbegin(fwd), std::cend(fwd), std::cbegin(bkw), std::begin(posteriors),
                   [=] (const auto f, const auto b) {
                       State result {};
                       for (std::size_t s {0}; s < num_states; ++s) {
                           result[s] = std::exp(f[s] + b[s] - p_fwd);
                       }
                       return result;
                   });
    std::vector<double> result(num_observations);
    std::transform(std::cbegin(posteriors), std::cend(posteriors), std::begin(result),
                   [] (const auto& s) { return s[2]; });
    return result;
}

template <typename Container>
auto expand_each(const Container& regions, const GenomicRegion::Distance n)
{
    std::vector<GenomicRegion> result {};
    result.reserve(regions.size());
    std::transform(std::cbegin(regions), std::cend(regions), std::back_inserter(result),
                   [n] (const auto& region) { return expand(region, n); });
    return result;
}

auto get_deletion_hotspots(const GenomicRegion& region, const CoverageTracker<GenomicRegion>& tracker)
{
    const auto coverages = tracker.get(region);
    const auto mean_coverage = tracker.mean(region);
    const auto stdev_coverage = tracker.stdev(region);
    const auto deletion_base_probs = compute_base_deletion_probabilities(coverages, mean_coverage, stdev_coverage);
    std::vector<bool> deletion_bases(deletion_base_probs.size());
    std::transform(std::cbegin(deletion_base_probs), std::cend(deletion_base_probs), std::begin(deletion_bases),
                   [] (const auto p) {  return p > 0.5; });
    return extract_covered_regions(expand_each(select_regions(region, deletion_bases), 50));
}

auto get_interesting_hotspots(const GenomicRegion& region,
                              const std::vector<unsigned>& interesting_coverages,
                              const std::vector<unsigned>& coverages)
{
    std::vector<bool> interesting_bases(interesting_coverages.size());
    std::transform(std::cbegin(coverages), std::cend(coverages), std::cbegin(interesting_coverages),
                   std::begin(interesting_bases),
                   [] (const auto coverage, const auto interesting_coverage) {
                       if (coverage < 10) {
                           return interesting_coverage > 0;
                       } else if (coverage <= 30) {
                           return interesting_coverage > 1;
                       } else {
                           return 10 * interesting_coverage >= coverage;
                       }
                   });
    return select_regions(region, interesting_bases);
}

auto get_interesting_hotspots(const GenomicRegion& region,
                              const CoverageTracker<GenomicRegion>& interesting_read_tracker,
                              const CoverageTracker<GenomicRegion>& tracker)
{
    const auto interesting_coverages = interesting_read_tracker.get(region);
    const auto coverages = tracker.get(region);
    return get_interesting_hotspots(region, interesting_coverages, coverages);
}

void merge(std::vector<GenomicRegion>&& src, std::vector<GenomicRegion>& dst)
{
    const auto itr = utils::append(std::move(src), dst);
    std::inplace_merge(std::begin(dst), itr, std::end(dst));
}

std::vector<GenomicRegion>
get_interesting_hotspots(const GenomicRegion& region,
                         const std::unordered_map<SampleName, CoverageTracker<GenomicRegion>>& interesting_read_tracker,
                         const std::unordered_map<SampleName, CoverageTracker<GenomicRegion>>& tracker)
{
    if (interesting_read_tracker.empty()) {
        return {};
    } else if (interesting_read_tracker.size() == 1) {
        assert(!tracker.empty());
        const auto itr = std::cbegin(interesting_read_tracker);
        assert(tracker.count(itr->first) == 1);
        return get_interesting_hotspots(region, itr->second, tracker.at(itr->first));
    } else {
        const auto n = size(region);
        std::vector<unsigned> pooled_interesting_coverage(n), pooled_coverage(n);
        std::vector<unsigned> best_sample_interesting_coverage(n), best_sample_coverage(n);
        for (const auto& p : interesting_read_tracker) {
            assert(tracker.count(p.first) == 1);
            const auto sample_coverage = tracker.at(p.first).get(region);
            const auto sample_interesting_coverage = p.second.get(region);
            assert(sample_coverage.size() == n);
            assert(sample_interesting_coverage.size() == n);
            for (std::size_t i {0}; i < sample_coverage.size(); ++i) {
                if (sample_interesting_coverage[i] > 0) {
                    pooled_interesting_coverage[i] += sample_interesting_coverage[i];
                    pooled_coverage[i] += sample_coverage[i];
                    const auto coverage_diff = sample_coverage[i] - sample_interesting_coverage[i];
                    if (coverage_diff > (best_sample_coverage[i] - best_sample_interesting_coverage[i])) {
                        best_sample_interesting_coverage[i] = sample_interesting_coverage[i];
                        best_sample_coverage[i] = sample_coverage[i];
                    }
                }
            }
        }
        auto pooled_hotspots = get_interesting_hotspots(region, pooled_interesting_coverage, pooled_coverage);
        auto best_sample_hotspots = get_interesting_hotspots(region, best_sample_interesting_coverage, best_sample_coverage);
        if (!best_sample_hotspots.empty() && best_sample_hotspots != pooled_hotspots) {
            merge(std::move(best_sample_hotspots), pooled_hotspots);
            return extract_covered_regions(pooled_hotspots);
        } else {
            return pooled_hotspots;
        }
    }
}

std::vector<GenomicRegion> AssemblerActiveRegionGenerator::generate(const GenomicRegion& region) const
{
    auto interesting_regions = get_interesting_hotspots(region, interesting_read_coverages_, coverage_tracker_);
    if (structual_interesting_) {
        for (const auto& p : clipped_coverage_tracker_) {
            auto deletion_regions = get_deletion_hotspots(region, p.second);
            merge(std::move(deletion_regions), interesting_regions);
        }
    }
    return join(extract_covered_regions(interesting_regions), 30);
}

void AssemblerActiveRegionGenerator::clear() noexcept
{
    coverage_tracker_.clear();
    interesting_read_coverages_.clear();
    clipped_coverage_tracker_.clear();
}

// private methods

namespace {

using NucleotideSequenceIterator = AlignedRead::NucleotideSequence::const_iterator;
using BaseQualityVectorIterator = AlignedRead::BaseQualityVector::const_iterator;

bool has_snv_in_match_range(const NucleotideSequenceIterator first_ref, const NucleotideSequenceIterator last_ref,
                            const NucleotideSequenceIterator first_base, const BaseQualityVectorIterator first_quality,
                            const AlignedRead::BaseQuality trigger)
{
    using boost::make_zip_iterator;
    using Tuple = boost::tuple<char, char, AlignedRead::BaseQuality>;
    const auto num_bases = std::distance(first_ref, last_ref);
    const auto last_base = std::next(first_base, num_bases);
    const auto last_quality = std::next(first_quality, num_bases);
    return std::any_of(make_zip_iterator(boost::make_tuple(first_ref, first_base, first_quality)),
                       make_zip_iterator(boost::make_tuple(last_ref, last_base, last_quality)),
                       [trigger] (const Tuple& t) {
                           const char ref_base  {t.get<0>()}, read_base {t.get<1>()};
                           return ref_base != read_base && ref_base != 'N' && read_base != 'N' && t.get<2>() >= trigger;
                       });
}

bool is_good_clip(const NucleotideSequenceIterator first_base, const NucleotideSequenceIterator last_base,
                  const BaseQualityVectorIterator first_quality, const AlignedRead::BaseQuality good_base_trigger,
                  const std::size_t min_good_bases)
{
    using boost::make_zip_iterator;
    using Tuple = boost::tuple<char, AlignedRead::BaseQuality>;
    const auto last_quality = std::next(first_quality, std::distance(first_base, last_base));
    return static_cast<std::size_t>(std::count_if(make_zip_iterator(boost::make_tuple(first_base, first_quality)),
                                                  make_zip_iterator(boost::make_tuple(last_base, last_quality)),
                                                  [=] (const Tuple& t) {
                                                      return t.get<0>() != 'N' && t.get<1>() >= good_base_trigger;
                                                  })) >= min_good_bases;
}

} // namespace

bool AssemblerActiveRegionGenerator::is_interesting(const AlignedRead& read) const
{
    using std::cbegin; using std::next; using std::move;
    using Flag = CigarOperation::Flag;
    const auto& read_sequence = read.sequence();
    auto sequence_itr = cbegin(read_sequence);
    auto base_quality_itr = cbegin(read.base_qualities());
    auto ref_index = mapped_begin(read);
    std::size_t read_index {0};
    for (const auto& cigar_operation : read.cigar()) {
        const auto op_size = cigar_operation.size();
        switch (cigar_operation.flag()) {
            case Flag::alignmentMatch:
            {
                if (snvs_interesting_) {
                    const GenomicRegion region{contig_name(read), ref_index, ref_index + op_size};
                    const auto ref_segment = reference_.get().fetch_sequence(region);
                    if (has_snv_in_match_range(std::cbegin(ref_segment), std::cend(ref_segment),
                                               next(sequence_itr, read_index),
                                               next(base_quality_itr, read_index),
                                               trigger_quality_)) {
                        return true;
                    }
                }
                read_index += op_size;
                ref_index += op_size;
                break;
            }
            case Flag::sequenceMatch:
                read_index += op_size;
                ref_index += op_size;
                break;
            case Flag::substitution:
            {
                if (snvs_interesting_) {
                    GenomicRegion {contig_name(read), ref_index, ref_index + op_size};
                    if (std::any_of(next(base_quality_itr, read_index), next(base_quality_itr, read_index + op_size),
                                    [this] (const AlignedRead::BaseQuality quality) { return quality >= trigger_quality_; })) {
                        return true;
                    }
                }
                read_index += op_size;
                ref_index += op_size;
                break;
            }
            case Flag::insertion: return true;
            case Flag::deletion: return true;
            case Flag::softClipped:
            {
                if (indels_interesting_ || structual_interesting_) {
                    if (is_good_clip(next(sequence_itr, read_index), next(sequence_itr, read_index + op_size),
                                     next(base_quality_itr, read_index), trigger_quality_, trigger_clip_size_)) {
                        return true;
                    }
                }
                read_index += op_size;
                ref_index += op_size;
                break;
            }
            case Flag::hardClipped: break;
            case Flag::padding:
                ref_index += op_size;
                break;
            case Flag::skipped:
                ref_index += op_size;
                break;
        }
    }
    return false;
}

} // namespace coretools
} // namespace octopus


#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <signal.h>
using namespace std;

#define PI 3.1415926

bool getRobotPose(tf::Stamped<tf::Pose>& robot_pose_odom, tf::TransformListener& tfListener);
bool tramformToBaseFootprint(tf::Stamped<tf::Pose>& orign_pose, tf::Stamped<tf::Pose>& output_pose_odom, tf::TransformListener& tfListener);

tf::Stamped<tf::Pose> g_goal_map;
string g_name_space;
ros::Publisher g_twist_pub;
bool start = false;

//map based
void getGoal(const geometry_msgs::PoseStamped& goal_pose)
{
  ROS_INFO("Received goal.");
  tf::poseStampedMsgToTF(goal_pose, g_goal_map);
  start = true;
}

void shutdown(int sig)
{
  ros::Duration(1).sleep(); // sleep for one second
  ROS_INFO("location test node ended!");
  geometry_msgs::Twist twist;
  twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0;
  twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0;
  g_twist_pub.publish(twist);
  ros::shutdown();
}

//need to realize use lider Lyapunov tf location
//costmap
int main(int argc, char **argv)
{
  ros::init(argc, argv, "location_test");
  ros::NodeHandle nh;
  g_name_space = nh.getNamespace();
  signal(SIGINT, shutdown);
  tf::TransformListener tfListener(ros::Duration(10));
  tf::TransformBroadcaster transformBR;
  tf::Transform transform;
  double k1 = 0.2, k2 = 1;

  ros::Publisher twist_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
  g_twist_pub = twist_pub;
  ros::Subscriber get_goal_sub = nh.subscribe(g_name_space + "/goal", 10, &getGoal);
  geometry_msgs::Twist twist;
  tf::Stamped<tf::Pose> robot_pose_odom;
  tf::Stamped<tf::Pose> robot_pose_goal;
  double control_linear_vel, control_angular_vel;
  while(!start)
  {
    ros::spinOnce();
  }

  ros::Rate loop_rate(1);
  while (ros::ok())
  {
    transform.setOrigin( g_goal_map.getOrigin() );
    transform.setRotation( g_goal_map.getRotation() );
    // send the transform of the frame "map" respect to "goal"(child_frame)
    transformBR.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
                                                   "/map",
                                                   g_name_space.substr(1, g_name_space.length() - 1) + "_goal"));

    getRobotPose(robot_pose_odom, tfListener);
    tramformToBaseFootprint(robot_pose_odom, robot_pose_goal, tfListener);
    geometry_msgs::PoseStamped msg;
    tf::poseStampedTFToMsg(robot_pose_goal, msg);
    double yaw = tf::getYaw(msg.pose.orientation);
    tf::Vector3 robot_v = robot_pose_goal.getOrigin();
    double rx = robot_v.getX();
    double ry = robot_v.getY();
    double ux = cos(yaw);
    double uy = sin(yaw);
    Eigen::Matrix<double, 2,1> P;
    Eigen::Matrix<double, 2,1> U;
    Eigen::Matrix<double, 2,2> J;
    P << rx, ry;
    U << ux, uy;
    J << cos(PI/2), -sin(PI/2), sin(PI/2), cos(PI/2);
    ROS_INFO("rx:%f, ry:%f", rx, ry);
    ROS_INFO("robot_yaw: %f, ux:%f, uy:%f", yaw, ux, uy);
    control_linear_vel = - k1 * P.transpose() * U;
    control_angular_vel = - k2 * P.transpose() * J * U;
    ROS_INFO("control_linear_vel:%f", control_linear_vel);
    ROS_INFO("control_angular_vel:%f", control_angular_vel);
    twist.linear.x = control_linear_vel; twist.linear.y = 0; twist.linear.z = 0;
    twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_angular_vel;
    twist_pub.publish(twist);

    ros::spinOnce();
    loop_rate.sleep();
  }

  return 0;
}

bool getRobotPose(tf::Stamped<tf::Pose>& robot_pose_odom, tf::TransformListener& tfListener)
{
  std::string odom_frame = g_name_space.substr(1, g_name_space.length() - 1) + "_odom";
  std::string base_frame;
  base_frame = g_name_space.substr(1, g_name_space.length() - 1) + "_base_footprint";
  robot_pose_odom.setIdentity();
  tf::Stamped < tf::Pose > robot_pose;
  robot_pose.setIdentity();
  robot_pose.frame_id_ = base_frame;
  robot_pose.stamp_ = ros::Time();
  ros::Time current_time = ros::Time::now();  // save time for checking tf delay later

  // get the global pose of the robot
  try
  {
    tfListener.transformPose(odom_frame, robot_pose, robot_pose_odom);
  }
  catch (tf::LookupException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf::ConnectivityException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf::ExtrapolationException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf2::InvalidArgumentException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "InvalidArgumentException: %s\n", ex.what());
    return false;
  }

  double transform_tolerance = 0.1;
  // check robot_pose_odom timeout
  if (current_time.toSec() - robot_pose_odom.stamp_.toSec() > transform_tolerance)
  {
    ROS_WARN_THROTTLE(1.0,
                      "Transform timeout. Current time: %.4f, robot_pose_odom stamp: %.4f, tolerance: %.4f",
                      current_time.toSec(), robot_pose_odom.stamp_.toSec(), transform_tolerance);
    return false;
  }
//  ROS_INFO("robot_pose_odom.frame_id_: %s", robot_pose_odom.frame_id_.c_str());
  return true;
}

bool tramformToBaseFootprint(tf::Stamped<tf::Pose>& orign_pose, tf::Stamped<tf::Pose>& output_pose_odom, tf::TransformListener& tfListener)
{
  std::string output_frame = g_name_space.substr(1, g_name_space.length() - 1) + "_goal";
//  orign_pose.frame_id_ = g_name_space.substr(1, g_name_space.length() - 1) + "_odom";
  orign_pose.stamp_ = ros::Time();
  output_pose_odom.setIdentity();
  ros::Time current_time = ros::Time::now();  // save time for checking tf delay later

  try
  {
    tfListener.transformPose(output_frame, orign_pose, output_pose_odom);
  }
  catch (tf::LookupException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf::ConnectivityException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf::ExtrapolationException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf2::InvalidArgumentException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "InvalidArgumentException: %s\n", ex.what());
    return false;
  }

  double transform_tolerance = 0.1;
  // check global_pose timeout
  if (current_time.toSec() - output_pose_odom.stamp_.toSec() > transform_tolerance)
  {
    ROS_WARN_THROTTLE(1.0,
                      "Transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
                      current_time.toSec(), output_pose_odom.stamp_.toSec(), transform_tolerance);
    return false;
  }
  return true;
}


#include <steemit/chain/protocol/asset.hpp>
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>

namespace steemit { namespace chain {
      typedef boost::multiprecision::int128_t  int128_t;

      uint8_t asset::decimals()const {
         auto a = (const char*)&symbol;
         return a[0];
      }
      void asset::set_decimals(uint8_t d){
         auto a = (char*)&symbol;
         a[0] = d;
      }

      std::string asset::symbol_name()const {
         auto a = (const char*)&symbol;
         assert( a[7] == 0 );
         return &a[1];
      }

       int64_t asset::precision()const {

         static int64_t table[] = {
                           1, 10, 100, 1000, 10000,
                           100000, 1000000, 10000000, 100000000ll,
                           1000000000ll, 10000000000ll,
                           100000000000ll, 1000000000000ll,
                           10000000000000ll, 100000000000000ll
                         };
         return table[ decimals() ];
      }

      string asset::to_string()const {
         string result = fc::to_string(amount.value / precision());
         if( decimals() )
         {
            auto fract = amount.value % precision();
            result += "." + fc::to_string(precision() + fract).erase(0,1);
         }
         return result + " " + symbol_name();
      }

      asset asset::from_string( const string& from )
      {
         try
         {
            string s = fc::trim( from );
            auto space_pos = s.find( " " );
            auto dot_pos = s.find( "." );

            asset result;
            result.symbol = uint64_t(0);
            auto sy = (char*)&result.symbol;
            *sy = (char) dot_pos; // Mask due to undefined architecture behavior

            auto intpart = s.substr( 0, dot_pos );
            result.amount = fc::to_int64(intpart);
            std::string fractpart;
            if( dot_pos != std::string::npos )
            {
               auto fractpart = "1" + s.substr( dot_pos + 1, space_pos - dot_pos - 1 );
               result.set_decimals( fractpart.size() - 1 );

               result.amount.value *= result.precision();
               result.amount.value += fc::to_int64(fractpart);
               result.amount.value -= result.precision();
            }
            auto symbol = s.substr( space_pos + 1 );

            if( symbol.size() )
               memcpy( sy+1, symbol.c_str(), std::min(symbol.size(),size_t(6)) );

            return result;
         }
         FC_CAPTURE_LOG_AND_RETHROW( (from) )
      }

      bool operator == ( const price& a, const price& b )
      {
         if( std::tie( a.base.symbol, a.quote.symbol ) != std::tie( b.base.symbol, b.quote.symbol ) )
             return false;

         const auto amult = uint128_t( b.quote.amount.value ) * a.base.amount.value;
         const auto bmult = uint128_t( a.quote.amount.value ) * b.base.amount.value;

         return amult == bmult;
      }

      bool operator < ( const price& a, const price& b )
      {
         if( a.base.symbol < b.base.symbol ) return true;
         if( a.base.symbol > b.base.symbol ) return false;
         if( a.quote.symbol < b.quote.symbol ) return true;
         if( a.quote.symbol > b.quote.symbol ) return false;

         const auto amult = uint128_t( b.quote.amount.value ) * a.base.amount.value;
         const auto bmult = uint128_t( a.quote.amount.value ) * b.base.amount.value;

         return amult < bmult;
      }

      bool operator <= ( const price& a, const price& b )
      {
         return (a == b) || (a < b);
      }

      bool operator != ( const price& a, const price& b )
      {
         return !(a == b);
      }

      bool operator > ( const price& a, const price& b )
      {
         return !(a <= b);
      }

      bool operator >= ( const price& a, const price& b )
      {
         return !(a < b);
      }

      asset operator * ( const asset& a, const price& b )
      {
         if( a.symbol_name() == b.base.symbol_name() )
         {
            FC_ASSERT( b.base.amount.value > 0 );
            uint128_t result = (uint128_t(a.amount.value) * b.quote.amount.value)/b.base.amount.value;
            FC_ASSERT( result.hi == 0 );
            return asset( result.to_uint64(), b.quote.symbol );
         }
         else if( a.symbol_name() == b.quote.symbol_name() )
         {
            FC_ASSERT( b.quote.amount.value > 0 );
            uint128_t result = (uint128_t(a.amount.value) * b.base.amount.value)/b.quote.amount.value;
            FC_ASSERT( result.hi == 0 );
            return asset( result.to_uint64(), b.base.symbol );
         }
         FC_THROW_EXCEPTION( fc::assert_exception, "invalid asset * price", ("asset",a)("price",b) );
      }

      price operator / ( const asset& base, const asset& quote )
      { try {
         FC_ASSERT( base.symbol_name() != quote.symbol_name() );
         return price{ base, quote };
      } FC_CAPTURE_AND_RETHROW( (base)(quote) ) }

      price price::max( asset_symbol_type base, asset_symbol_type quote ) { return asset( share_type(STEEMIT_MAX_SHARE_SUPPLY), base ) / asset( share_type(1), quote); }
      price price::min( asset_symbol_type base, asset_symbol_type quote ) { return asset( 1, base ) / asset( STEEMIT_MAX_SHARE_SUPPLY, quote); }

      bool price::is_null() const { return *this == price(); }

      void price::validate() const
      { try {
         FC_ASSERT( base.amount > share_type(0) );
         FC_ASSERT( quote.amount > share_type(0) );
         FC_ASSERT( base.symbol_name() != quote.symbol_name() );
      } FC_CAPTURE_AND_RETHROW( (base)(quote) ) }

} } // steemit::chain


#if HAVE_CONFIG_H
# include <config.h>
#endif

#include "RegPT.h"
#include "SPT.h"
#include "PowerSpectrum.h"
#include "LinearPS.h"
#include "Quadrature.h"
#include "SpecialFunctions.h"
#include "array.h"
#include "Spline.h"
#include <gsl/gsl_sf_bessel.h>

#include <cassert>
#include <cmath>
#include <cstdio>
#include <cerrno>
#include <cstdlib>
#include <stdlib.h>
#include <sys/stat.h>
#include <math.h>
#include <vector>
#include <boost/bind.hpp>
using boost::cref;

// Load Linear PS
RegPT::RegPT(const Cosmology& C, const PowerSpectrum& P_l, real epsrel_)
: C(C), P_l(P_l)
{
    epsrel = epsrel_;
}

//Error in exponential index integral
const double error1 = 1e-3;
//Atsushi limit on k-angular integration
inline double ATS(real k, real r){
 real KMAX = QMAXp/k;
 real KMIN = QMINp/k;
 if(r>=0.5) {
 return 1./(2.*r);
 }
 else{
 return  Min(XMAX, (1.+r*r-KMIN*KMIN)/2./r);
 }
}

inline int num_selec_mag(double kmin, double kmax, double y){
    	int mag_int;
    //	 mag_int = (n2*1.-1)*(y-QMINp/0.5)*0.005/QMAXp + 1./n2; // linear
    //	 mag_int = sqrt((y-QMINp/kmax)*kmin/QMAXp)*(n2-1); //quadratic
    mag_int = (int)round((n2-1)*log((y*kmax)/QMINp)/log(QMAXp*kmax/(kmin*QMINp))); //exponential
    		return mag_int;
}

// spline linear growth for sigma_d^2 integral
Spline F1_reg;
// Regularized PT exponential
static real regpt_exp(const PowerSpectrum& P_l, double q){
  return  P_l(q)/(6.*pow2(M_PI));
}
// Initialization of sigma_d damping term
Spline damp_term;
void RegPT::sigmad_init() const{
vector<double> kval_table, sigd_table;
double k,sigd;
int n3 = 200;
  for(int i = 0; i<n3; i++ ){
    k = QMINp*exp(i*log(2.*QMAXp/(QMINp))/(n3-1.));
    sigd = pow2(k)*Integrate<ExpSub>(bind(regpt_exp, cref(P_l), _1), QMINp, k/2., error1);
    kval_table.push_back(k);
    sigd_table.push_back(sigd);
      }
    damp_term = LinearSpline(kval_table,sigd_table);
  }

  // used to treat f as free parameter in GR template
double rempfr;
void RegPT::rempreg(real f) const{
rempfr = f/fl_spt;
}

// normalization of linear growth
double sig_8r;
// function to set normalization of linear growth to fiducial for data comparisons
void RegPT::Greg(real dfid) const {
  sig_8r = dfid/D_spt;
       }

/* P_{ab}(k) 1-loop in regpt: analytical LCDM and nDGP */
// 1:  P_dd, 2: P_dt , 3: P_tt ;
real RegPT::PLOOPr(int a, real k) const {
  SPT spt(C,P_l,1e-3);
  double D0,D1,D2,D3;
  if(pow2(D_spt/dnorm_spt)*damp_term(k) >= 80. ){
    return 0;
  }
  else{
  switch (a) {
    case 1:
      D0= pow2(sig_8r)*pow2(D_spt/dnorm_spt);
      D1 = pow4(sig_8r)*spt.P13D(k,1);
      D2 = pow2(D1/P_l(k)/2.)/D0;
      D3 = pow4(sig_8r);
      break;
    case 2:
      D0= -rempfr*fdgp_spt*pow2(sig_8r)*pow2(D_spt/dnorm_spt);
      D1= rempfr*pow4(sig_8r)*spt.P13D(k,2);
      D2 =  pow6(sig_8r)*spt.P13D(k,1)*spt.P13D(k,3)*pow2(dnorm_spt/(D_spt*P_l(k)*2.))/(-rempfr*fdgp_spt);
      D3 =  pow4(sig_8r)*rempfr;
      break;
    case 3:
      D0=  pow2(sig_8r)*pow2(rempfr*fdgp_spt*D_spt/dnorm_spt);
      D1 = rempfr*rempfr* pow4(sig_8r)*spt.P13D(k,3);
      D2 = pow2(D1/P_l(k)/2.)/D0;
      D3 =  pow4(sig_8r)*pow2(rempfr);
      break;
  }
  return exp(-pow2(D_spt/dnorm_spt)*damp_term(k))*(D0*P_l(k)+D1+ D3*spt.P22D(k,a)+pow2(D_spt/dnorm_spt)*damp_term(k)/2.*(2.*D0*P_l(k)+ D1) + pow2(pow2(D_spt/dnorm_spt)*damp_term(k))/4.*D0*P_l(k) + P_l(k)*D2);
}
}

/* P_{ab}(k) 1-loop regpt :  numerical for arbitrary model of gravity - kernel dependent */

// kmin and kmax are only used if the gravity model is scale independant and kernels only need be initialized once
//kmin and kmax then refer to the limits of the desired output range of scales

real RegPT::PLOOPnr(real kmin, real kmax, int a, real k) const {
  SPT spt(C,P_l,1e-3);
  double D0;
  double D1;
  double D2;
  if(pow2(F1_nk/dnorm_spt)*damp_term(k) >= 80. ){
    return 0;
  }
  else{
  switch (a) {
    case 1:
      D0= pow2(F1_nk/dnorm_spt);
      D1 = spt.P13n(kmin,kmax,1,k);
      D2 = pow2(D1/P_l(k)/2.)/D0;
      break;
    case 2:
      D0= F1_nk*G1_nk/pow2(dnorm_spt);
      D1 =  spt.P13n(kmin,kmax,2,k);
      D2 = spt.P13n(kmin,kmax,1,k)*spt.P13n(kmin,kmax,3,k)*pow2(1./(P_l(k)*2.))/D0;
      break;
    case 3:
      D0=  pow2(G1_nk/dnorm_spt);
      D1 =  spt.P13n(kmin,kmax,3,k);
      D2 = pow2(D1/P_l(k)/2.)/D0;
      break;
  }
  return exp(-pow2(F1_nk/dnorm_spt)*damp_term(k))*(D0*P_l(k) + D1 + spt.P22n(kmin,kmax,a,k) + pow2(F1_nk/dnorm_spt)*damp_term(k)/2.*(2.*D0*P_l(k)+ D1) + pow2(pow2(F1_nk/dnorm_spt)*damp_term(k))/4.*D0*P_l(k) + P_l(k)*D2);
  }
}

/* TNS  Multipoles  */
//bl is linear galaxy bias
// a = 1 : Monopole
// a = 2 : Quadrupole
// a = 3 : Hexdecapole
real RegPT::PTNSMnDGPr(real k, real bl, real sigma_v, int a) const {
  if(pow2(D_spt/dnorm_spt)*damp_term(k) >= 80. ){
    return 0;
  }
  else{
	return  (pow2(bl)*factL(k,sigma_v,fl_spt,1.,0,a,6)*PLOOPr(1, k) - 2*factL(k,sigma_v,fl_spt,1.,1,a,6)*bl*PLOOPr(2, k) + factL(k,sigma_v,fl_spt,1.,2,a,6)* PLOOPr(3, k)+  pow4(sig_8r)*ABr(k, bl, sigma_v,a));
}
}

/*  Arbitrary model TNS  Multipoles with DFoG term  */
//bl is linear galaxy bias
// a = 1 : Monopole
// a = 2 : Quadrupole
// a = 3 : Hexdecapole
real RegPT::PTNSMmgr(real k, double kmin, double kmax, int a,real bl, real sigma_v1 ) const {
  if(pow2(F1_nk/dnorm_spt)*damp_term(k) >= 80. ){
    return 0;
  }
  else{
	return  (pow2(bl)*factL(k,sigma_v1,fl_spt,1.,0,a,6)*PLOOPnr(kmin,kmax,1, k)  - 2*factL(k,sigma_v1,fl_spt,1.,1,a,6)*bl*PLOOPnr(kmin,kmax,2, k)  + factL(k,sigma_v1,fl_spt,1.,2,a,6)* PLOOPnr(kmin,kmax,3, k)+  ABnr(kmin,kmax, bl, k, sigma_v1, a));
}
}

//REDSHIFT SPACE MODEL-TNS TERMS with damping

/* Non vanishing A, B and C terms */

// Notes for analytic ABC terms : DEFINITION OF THETA= - DEL V / aHf (and is accounted for in definition of Cross Bispectrum)
// See http://arxiv.org/pdf/1006.0699v1.pdf for derivation

// NOTE ON PARAMETERS: x =k1.k/(k^2*r)

static real ABC(int a, const PowerSpectrum& P_L, real k, real r, real x) {
    real d = 1 + r*r - 2*r*x;
    double dampexpa = exp(-pow2(D_spt/dnorm_spt)*damp_term(k*r)/2.-pow2(D_spt/dnorm_spt)*damp_term(k*sqrt(d))/2.-pow2(D_spt/dnorm_spt)*damp_term(k)/2.);
    double dampexpb = exp(-pow2(D_spt/dnorm_spt)*damp_term(k*sqrt(d))-pow2(D_spt/dnorm_spt)*damp_term(k*r));
    if(d < 1e-5)
        return 0;
    else
		switch(a) {
		// A terms
				//A11
			case 1:
				return dampexpa*(-r*r*r/7.)*(x+6*x*x*x+r*r*x*(-3+10*x*x)+r*(-3+x*x-12*x*x*x*x))*P_L(k) * P_L(k*sqrt(d))/pow2(d);
      	//A12
			case 2:
				return  dampexpa*(r*r*r*r/14.)*(x*x-1)*(-1+7*r*x-6*x*x) * P_L(k) * P_L(k*sqrt(d))/pow2(d);
      	//A22
			case 3:
				return  dampexpa*(r*r*r/14.)*(r*r*x*(13-41*x*x)-4*(x+6*x*x*x)+r*(5+9*x*x+42*x*x*x*x))*P_L(k)*P_L(k*sqrt(d))/pow2(d);
      	//A33
			case 4:
				return dampexpa*(r*r*r/14.)*(1-7*r*x+6*x*x)*(-2*x+r*(-1+3*x*x))*P_L(k)*P_L(k*sqrt(d))/pow2(d);
      	//A11t
			case 5:
				return  dampexpa*(1./7.)*(x+r-2*r*x*x)*(3*r+7*x-10*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
      	//A12t
			case 6:
				return dampexpa*(r/14.)*(x*x-1)*(3*r+7*x-10*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
      	//A22t
			case 7:
				return dampexpa*(1./14.)*(28*x*x+r*x*(25-81*x*x)+r*r*(1-27*x*x+54*x*x*x*x))*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
      	//A23t
			case 8:
				return dampexpa*(r/14.)*(-x*x+1)*(r-7*x+6*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
		//A33t
			case 9:
				return dampexpa*(1./14.)*(-2*x-r+3*r*x*x)*(r-7*x+6*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
		//a11
			case 10:
				return dampexpa*(-7*x*x+pow3(r)*x*(-3+10*x*x)+3*r*(x+6*pow3(x))+r*r*(6-19*x*x-8*pow4(x)))*P_L(k)*P_L(k*r)/(7.*d);
		//a12
			case 11:
				return dampexpa*r*(-1+x*x)*(6*r-7*(1+r*r)*x + 8*r*x*x)*P_L(k)*P_L(k*r)/(14.*d);
			//a22
			case 12:
				return dampexpa*(-28*x*x+r*r*r*x*(-13+41*x*x)+r*x*(11+73*x*x)-2*r*r*(-9+31*x*x+20*x*x*x*x))*P_L(k)*P_L(k*r)/(14.*d);
		//a33
			case 13:
				return dampexpa*(7*x + r*(-6+7*r*x-8*x*x))*(-2*x+r*(-1+3*x*x))*P_L(k)*P_L(k*r)/(14.*d);
		//B terms
				//B111
			case 14:
				return dampexpb*(r*r/2.)*(x*x-1)*P_L(k*r)*P_L(k*sqrt(d))/d;
	   //B112
			case 15:
				return dampexpb*(3*r*r/8.)*pow2(x*x-1)*P_L(k*r)*P_L(k*sqrt(d))/d;
		//B121
			case 16:
				return dampexpb*(3.*r*r*r*r/8.)*pow2(x*x-1)*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
			//B122
			case 17:
				return dampexpb*(5*r*r*r*r/16.)*pow3(x*x-1)*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
			//B211
			case 18:
				return dampexpb*(r/2.)*(r+2*x-3*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))/d;
		//B212
			case 19:
				return dampexpb*(-3*r/4.)*(x*x-1)*(-r-2*x+5*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))/d;
			//B221
			case 20:
				return dampexpb*(3*r*r/4.)*(x*x-1)*(-2+r*r+6*r*x-5*r*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
			//B222
			case 21:
				return dampexpb*(-3*r*r/16.)*pow2(x*x-1)*(6-30*r*x-5*r*r+35*r*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
		//B312
			case 22:
				return dampexpb*(r/8.)*(4*x*(3-5*x*x)+r*(3-30*x*x+35*x*x*x*x))*P_L(k*r)*P_L(k*sqrt(d))/d;
		//B321
			case 23:
				return dampexpb*(r/8.)*(-8*x+r*(-12+36*x*x+12*r*x*(3-5*x*x)+r*r*(3-30*x*x+35*x*x*x*x)))*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
		//B322
			case 24:
				return dampexpb*(3*r/16.)*(x*x-1)*(-8*x+r*(-12+60*x*x+20*r*x*(3-7*x*x)+5*r*r*(1-14*x*x+21*x*x*x*x)))*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
		//B422 		//B422 // HAVE INCLUDED THE 'TYPO' IN 3RD TERM : 6rr -> 6rrx
			case 25:
				return dampexpb*(r/16.)*(8*x*(-3+5*x*x)-6*r*(3-30*x*x+35*x*x*x*x)+6*r*r*x*(15-70*x*x+63*x*x*x*x)+r*r*r*(5-21*x*x*(5-15*x*x+11*x*x*x*x)))*P_L(k*r)*P_L(k*sqrt(d))/pow2(d);
  	// C terms
			//C11
			case 26:
				return dampexpa*2.*((r*r*r)*(x*x-1)*(Dd_spt*F_spt*r*(r*x-1)-D_spt*Fd_spt*x*d)*P_L(k)*P_L(k*sqrt(d)))/ (Dd_spt*pow2(d));
			//C12
			case 27:
				return -dampexpa*D_spt*Fd_spt*((r*r*r*r)*pow2(x*x-1)*P_L(k)*P_L(k*sqrt(d)))/(Dd_spt*pow2(d));
			//C22
			case 28:
				return dampexpa*((r*r*r)*(x*x-1)*(2*Dd_spt*F_spt*r*(r*x-1)-D_spt*Fd_spt*(r+4*x+2*r*r*x-7*r*x*x))*P_L(k)*P_L(k*sqrt(d)))/ (Dd_spt*pow2(d));
			//C23
			case 29:
				return -dampexpa*D_spt*Fd_spt*((r*r*r*r)*pow2(x*x-1)*P_L(k)*P_L(k*sqrt(d)))/ (Dd_spt*pow2(d));
      //C33
			case 30:
				return dampexpa*D_spt*Fd_spt*((r*r*r)*(x*x-1)*(-2*x+r*(-1+3*x*x))*P_L(k)*P_L(k*sqrt(d)))/ (Dd_spt*pow2(d));
				//C11t
			case 31:
				return dampexpa*2*F_spt*(r*(x*x-1)*(-x+r*(-1+2*x*x))*P_L(k*r)*P_L(k*sqrt(d)))/ pow2(d);
				//C12t
			case 32:
				return -dampexpa*F_spt*(r*r*pow2(x*x-1)*P_L(k*r)*P_L(k*sqrt(d)))/ pow2(d);
				//C22t
			case 33:
				return dampexpa*r*(x*x-1)*(2*D_spt*Fd_spt*(-x+r*(-1+2*x*x)) + Dd_spt*F_spt*(-2*x+r*(-1+3*x*x)))*P_L(k*r)*P_L(k*sqrt(d))/ (Dd_spt*pow2(d));
				//C23t
			case 34:
				return -dampexpa*D_spt*Fd_spt*(r*r*pow2(x*x-1)*P_L(k*r)*P_L(k*sqrt(d)))/ (Dd_spt*pow2(d));
				//C33t
			case 35:
				return dampexpa*D_spt*Fd_spt*(r*(x*x-1)*(-2*x+r*(-1+3*x*x))*P_L(k*r)*P_L(k*sqrt(d)))/ (Dd_spt*pow2(d));
				//c11
			case 36:
				return  -dampexpa*2*r*(1-x*x)*(D_spt*Fd_spt*r*(r*x-1)-Dd_spt*F_spt*x*d)*P_L(k*r)*P_L(k)/(Dd_spt*d);
				//c12
			case 37:
				return -dampexpa*D_spt*Fd_spt*r*r*pow2(-1+x*x)*P_L(k*r)*P_L(k)/(Dd_spt*d);
				//c22
			case 38:
				return dampexpa*r*(-1+x*x)*(-2*Dd_spt*F_spt*x*d+D_spt*Fd_spt*(-2*x+2*r*r*x+3*r*(-1+x*x)))*P_L(k*r)*P_L(k)/(Dd_spt*d);
				//c33
			case 39:
				return dampexpa*D_spt*Fd_spt*r*(-1+x*x)*(-2*x+r*(-1+3*x*x))*P_L(k*r)*P_L(k)/(Dd_spt*d);
			default:
			warning("SPT: invalid indices, a = %d\n", a);
				return 0;
      }
}

/* function to select multipole or u-dependent Redshift PS */
// it gives either u^(2n) or factL(k, u(i.e.sigma_v), n, a, b, e, f) - see factL

// e = 0:  RSD PS
// e = 1 : Monopole
// e = 2 : Quadrupole
// e = 3 : Hexdecapole

// e = 3 : Hexdecapole

static real u0(real k, real u,real F0, int n, int a){
	if (a >= 1) {
		return factL(k,u,F0,1.,n,a,6); }
		else
			return pow(u,2*n);
}

static real midintabc(real bl, int a, const PowerSpectrum& P_L, real k, real u, real r) {
  real KMAX = QMAXp/k;
  real KMIN = QMINp/k;
	real YMIN = Max(XMIN, (1.+r*r-KMAX*KMAX)/2./r);
  real YMAX = ATS(k,r);
	real F0 =rempfr*fdgp_spt;
  real D1 = D_spt;
  real X1 = 1;
  real u01 = u0(k,u,F0,1,a);
  real u02 = u0(k,u,F0,2,a);
  real u03 = u0(k,u,F0,3,a);
  real u04 = u0(k,u,F0,4,a);

	//A term
		return     pow4(D1/dnorm_spt)*2.*(F0*u01*pow2(bl)*(Integrate(bind(ABC,1, cref(P_L), k, r, _1),  YMIN, YMAX ,  1e-3*P_L(k))
										   + Integrate(bind(ABC,5, cref(P_L), k,  r, _1),  YMIN, YMAX ,  1e-3*P_L(k))
										   + Integrate(bind(ABC,10, cref(P_L), k, r, _1),  YMIN, YMAX ,  1e-3*P_L(k)))

							+ F0*F0*u01*bl*(Integrate(bind(ABC,2, cref(P_L), k,  r, _1),  YMIN, YMAX , 1e-3*P_L(k))
										   + Integrate(bind(ABC,6, cref(P_L), k,  r, _1), YMIN, YMAX ,  1e-3*P_L(k))
										   + Integrate(bind(ABC,11, cref(P_L), k,  r, _1), YMIN, YMAX ,  1e-3*P_L(k)))

							+ F0*F0*u02*bl*(Integrate(bind(ABC,3, cref(P_L), k,  r, _1),   YMIN, YMAX , 1e-3*P_L(k))
										   + Integrate(bind(ABC,7, cref(P_L), k,  r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										   + Integrate(bind(ABC,12, cref(P_L), k,  r, _1),  YMIN, YMAX ,  1e-3*P_L(k)))

					     + F0*F0*F0*u02*(Integrate(bind(ABC,2, cref(P_L), k,  r, _1),  YMIN, YMAX ,  1e-3*P_L(k))
										   + Integrate(bind(ABC,8, cref(P_L), k,  r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										   + Integrate(bind(ABC,11, cref(P_L), k, r, _1), YMIN, YMAX ,  1e-3*P_L(k)))

						   + F0*F0*F0*u03*(Integrate(bind(ABC,4, cref(P_L), k, r, _1),   YMIN, YMAX , 1e-3*P_L(k))
										   + Integrate(bind(ABC,9, cref(P_L), k,  r, _1),  YMIN, YMAX ,  1e-3*P_L(k))
										   + Integrate(bind(ABC,13, cref(P_L), k,  r, _1),YMIN, YMAX ,  1e-3*P_L(k))))

	//B term
		  +      pow4(D1/dnorm_spt)*2*(F0*F0*pow2(bl)*u01*Integrate(bind(ABC,14,  cref(P_L), k, r, _1), YMIN, YMAX , 1e-3* P_L(k))

						  -F0*F0*F0*bl*u01*(Integrate(bind(ABC,15, cref(P_L), k, r, _1), YMIN, YMAX , 1e-3*P_L(k))
											    +Integrate(bind(ABC,16, cref(P_L),  k,  r, _1),YMIN, YMAX , 1e-3*P_L(k)))

						+F0*F0*F0*F0*u01*Integrate(bind(ABC,17, cref(P_L),  k, r, _1), YMIN, YMAX , 1e-3*P_L(k))

							  +F0*F0*pow2(bl)*u02*Integrate(bind(ABC,18,  cref(P_L), k,  r, _1),YMIN, YMAX ,  1e-3*P_L(k))

						 -F0*F0*F0*bl*u02*(Integrate(bind(ABC,19, cref(P_L), k,  r, _1), YMIN, YMAX , 1e-3*P_L(k))
										      + Integrate(bind(ABC,20, cref(P_L),  k,  r, _1), YMIN, YMAX ,  1e-3*P_L(k)))

					  +F0*F0*F0*F0*u02*Integrate(bind(ABC,21, cref(P_L),  k,  r, _1),YMIN, YMAX , 1e-3*P_L(k))

					    - F0*F0*F0*bl*u03*(Integrate(bind(ABC,22,  cref(P_L), k,  r, _1), YMIN, YMAX ,  1e-3*P_L(k))
											  + Integrate(bind(ABC,23,  cref(P_L), k,  r, _1), YMIN, YMAX , 1e-3*P_L(k)))

					  + F0*F0*F0*F0*u03*Integrate(bind(ABC,24,  cref(P_L), k,  r, _1),YMIN, YMAX ,  1e-3*P_L(k))

							+F0*F0*F0*F0*u04*Integrate(bind(ABC,25,  cref(P_L), k,  r, _1), YMIN, YMAX , 1e-3*P_L(k)))

	//C - nDGP analytic

		  +       X1*D1*D1*2/pow4(dnorm_spt)*(F0*pow2(bl)*u01*(Integrate(bind(ABC,26, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										+ Integrate(bind(ABC,31, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										+ Integrate(bind(ABC,36, cref(P_L), k, r, _1), YMIN, YMAX ,  1e-3*P_L(k))) +

							F0*F0*bl*u01*(Integrate(bind(ABC,27, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										+ Integrate(bind(ABC,32, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										+ Integrate(bind(ABC,37, cref(P_L), k, r, _1),  YMIN, YMAX ,  1e-3*P_L(k))) +

						   F0*F0*bl*u02*(Integrate(bind(ABC,28, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										+ Integrate(bind(ABC,33, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										+ Integrate(bind(ABC,38, cref(P_L), k,  r, _1), YMIN, YMAX ,  1e-3*P_L(k))) +

						F0*F0*F0*u02*(Integrate(bind(ABC,29, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										+ Integrate(bind(ABC,34, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
										+ Integrate(bind(ABC,37, cref(P_L), k,  r, _1),  YMIN, YMAX ,  1e-3*P_L(k))) +

					   F0*F0*F0*u03*(Integrate(bind(ABC,30, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
									   + Integrate(bind(ABC,35, cref(P_L), k, r, _1),   YMIN, YMAX ,  1e-3*P_L(k))
									   + Integrate(bind(ABC,39, cref(P_L), k,  r, _1),  YMIN, YMAX ,  1e-3*P_L(k))));

}

/* Non vanishing A, B and C terms */
//numerical calculations
// Notes for numerical AB terms : DEFINITION OF THETA=  DEL V / aH (and is accounted for in definition of Cross Bispectrum)
// Integrating them over angle
static real ABCn(double u0x[], int y, real bl, int a, const PowerSpectrum& P_L, real k, real u, real r) {
  double myresult = 0.;
	double temp_abc;
  double abc[10];
  // Set the limits of angular integration
  double KMAX = QMAXp/k;
  double KMIN = QMINp/k;
  double YMIN = Max(XMIN, (1.+r*r-KMAX*KMAX)/2./r);
  double YMAX = ATS(k,r);
  // Multipole factors
  real u01 = u0x[0];//u0(k,u,fl_spt,1,a);
  real u02 = u0x[1];//u0(k,u,fl_spt,2,a);
  real u03 = u0x[2];//u0(k,u,fl_spt,3,a);
  real u04 = u0x[3];//u0(k,u,fl_spt,4,a);
  // Find those corresponding limits in the GL Quad abscissae (See SpecialFunctions.h)
  int x_min = searchnearest(x128, YMIN);
  int x_max = searchnearest(x128, YMAX);
  for( int i = x_min; i <= x_max; i++ )
    				{
    		 		double x = x128[i];
    		 		double d = 1+ r*r - 2*r*x;
    		 		if(d < 1e-5){
    			 	temp_abc=0.;
    		 		}
    		  	else {
              // 1st order kernels F1/G1(k-p)
              abc[0] = F1kmp_nk[i*n2 + y];
              abc[1] = G1kmp_nk[i*n2 + y]/bl;
              // 1st order kernels F1/G1(p)
              abc[2] = F1p_nk[i*n2 + y];
              abc[3] = G1p_nk[i*n2 + y]/bl;
              //symmetrized 2nd order kernels for ps F2/G2(p,k-p)
              abc[4] = F2_nk[i*n2 + y];
              abc[5] = G2_nk[i*n2 + y]/bl;
              //symmetrized 2nd order kernels F2/G2(-p,k)
              abc[6] = F2B_nk[i*n2 + y];
              abc[7] = G2B_nk[i*n2 + y]/bl;
              //symmetrized 2nd order kernels F2/G2(-k,k-p)
              abc[8] = F2C_nk[i*n2 + y];
              abc[9] = G2C_nk[i*n2 + y]/bl;

              double dampexpa = exp(-pow2(F1_nk/dnorm_spt)*damp_term(k*r)/2.-pow2(F1_nk/dnorm_spt)*damp_term(k*sqrt(d))/2.-pow2(F1_nk/dnorm_spt)*damp_term(k)/2.);
              double dampexpb = exp(-pow2(F1_nk/dnorm_spt)*damp_term(k*sqrt(d))-pow2(F1_nk/dnorm_spt)*damp_term(k*r));

// A terms
//u^2
    		  temp_abc =  pow3(bl)*dampexpa/d*(-u01*(F1_nk*r*(-2*abc[8]*abc[1]*r*(r*x-1)+abc[9]*(2*abc[0]*x*d+abc[1]*r*(1-x*x)))*P_L(k)*P_L(k*sqrt(d))
            						+ abc[4]*r*(-2*abc[2]*abc[1]*r*(-1+r*x)+abc[3]*(2*abc[0]*x*d+abc[1]*r*(1-x*x)))*P_L(k*r)*P_L(k*sqrt(d))
            						+ F1_nk*r*(-2*abc[7]*abc[2]*r*(r*x-1)+abc[3]*(2*abc[6]*x*d+abc[7]*r*(1-x*x)))*P_L(k)*P_L(k*r))
//u^4
            -u02*(r*(2*abc[8]*G1_nk/bl*abc[1]*r*(r*x-1)+abc[9]*(G1_nk/bl*(-2*abc[0]*x*d+abc[1]*r*(x*x-1))+F1_nk*abc[1]*(-2*x+r*(-1+3*x*x))))*P_L(k)*P_L(k*sqrt(d))
                           +r*(abc[4]*abc[1]*abc[3]*(-2*x+r*(-1+3*x*x))+abc[5]*(2*abc[2]*abc[1]*r*(r*x-1)+abc[3]*(-2*abc[0]*x*d+abc[1]*r*(x*x-1))))*P_L(k*r)*P_L(k*sqrt(d))
                           +r*(abc[3]*abc[7]*F1_nk*(-2*x-r+3*x*x*r)+2*abc[2]*G1_nk/bl*abc[7]*r*(r*x-1)+G1_nk/bl*abc[3]*(-2*abc[6]*x*d+abc[7]*r*(x*x-1)))*P_L(k)*P_L(k*r))
//u^6
           -u03*(G1_nk/bl*abc[1]*abc[9]*r*(r+2*x-3*r*x*x)*P_L(k)*P_L(k*sqrt(d))
                        +abc[3]*abc[1]*abc[5]*r*(r+2*x-3*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))
                        +abc[3]*G1_nk/bl*abc[7]*r*(r+2*x-3*r*x*x)*P_L(k*r)*P_L(k)))

// B terms
          +pow4(bl)*dampexpb*P_L(k*r)*P_L(k*sqrt(d))/(16.*d*d)*
//u^2
          (u01*(abc[1]*abc[3]*r*r*(x*x-1)*(2*abc[0]*d*(4*abc[2]+3*abc[3]*(x*x-1))
                         +abc[1]*r*r*(x*x-1)*(6*abc[2]+5*abc[3]*(x*x-1))))
//u^4
          +u02*(abc[1]*abc[3]*r*(-4*abc[0]*d*(3*abc[3]*(x*x-1)*(-2*x+r*(5*x*x-1))+abc[2]*(-4*x+r*(-2+6*x*x)))
                					   -3*abc[1]*r*(x*x-1)*(4*abc[2]*(2-6*r*x+r*r*(-1+5*x*x))
                						 +abc[3]*(x*x-1)*(6-30*r*x+5*r*r*(-1+7*x*x)))))
//u^6
          +u03*(abc[1]*abc[3]*r*(3*abc[3]*abc[1]*(x*x-1)*(-8*x+12*r*(5*x*x-1)-20*r*r*x*(7*x*x-3)+5*pow3(r)*(1-14*x*x+21*pow4(x)))
                              +2*abc[0]*abc[3]*(4*x*(3-5*x*x)+pow3(r)*(3-30*x*x+35*pow4(x))+r*(3-54*x*x+75*pow4(x))+r*r*(6*x+40*pow3(x)-70*pow5(x)))
                              +2*abc[2]*abc[1]*(-8*x+12*r*(3*x*x-1)+r*r*(36*x-60*pow3(x))+pow3(r)*(3-30*x*x+35*pow4(x)))))
//u^8
          +u04*(pow2(abc[1]*abc[3])*r*(8*x*(5*x*x-3)-6*r*(3-30*x*x+35*pow4(x))
                     				+6*r*r*x*(15-70*x*x+63*pow4(x))  		// HAVE INCLUDED THE 'TYPO' IN TERM : 6rr -> 6rrx
                     				+pow3(r)*(5-105*x*x+315*pow4(x)-231*pow6(x)))));
    				}
    				myresult += w128[i] * temp_abc;
    				}
          return  2*myresult;
}

/*Generalised A and B correction term */

/*Analytical A and B*/
// a = 1 : LCDM
// a = 2 : MG

// e = 0:  RSD PS
// e = 1 : Monopole
// e = 2 : Quadrupole
// e = 3 : Hexdecapole

//U = u for RSD PS
//U = sigma_v for multipoles

real RegPT::ABr(real k, real bl, real U, int a) const{
    int n3 = 300;
    real KMAX = QMAXp/k;
    real KMIN = QMINp/k;
    double y[n3];
    double integrand[n3];
    for (int i = 0; i<n3; i++){
    y[i] = KMIN * exp(i*log(KMAX/KMIN)/(n3-1.));
    integrand[i] = midintabc(bl, a,cref(P_l), k, U, y[i]);
    }
  double res = 0.;
    for( int i = 1; i < n3; ++ i ){
  res += 0.5 * (y[i] - y[i-1])*(integrand[i] + integrand[i-1]);
      }
    return  k*k*k/(4*M_PI*M_PI) * res;
    }

/*Numerical A and B */

// e = 0:  RSD PS
// e = 1 : Monopole
// e = 2 : Quadrupole
// e = 3 : Hexdecapole

//U = u for RSD PS
//U = sigma_v for multipoles

real RegPT::ABnr(double kmin, double kmax, real bl, real k, real U, int a) const{
	real KMAX = QMAXp/k;
  real KMIN = QMINp/k;
  int y1 = num_selec_mag(kmin, kmax, KMIN);
  int y2 = num_selec_mag(kmin, kmax, KMAX);
  double y[n2];
  double integrand[n2];
  double u0x[4];
  u0x[0]=u0(k,U,fl_spt,1,a);
  u0x[1]=u0(k,U,fl_spt,2,a);
  u0x[2]=u0(k,U,fl_spt,3,a);
  u0x[3]=u0(k,U,fl_spt,4,a);
  for (int i = y1; i<=y2; i++){
  y[i] = QMINp/kmax * exp(i*log(QMAXp*kmax/(QMINp*kmin))/(n2*1.-1.));
  integrand[i] = ABCn(u0x,i, bl, a, cref(P_l), k, U, y[i]);
  }
double res = 0.;
  for( int i = y1+1; i <= y2; ++ i ){
res += 0.5 * (y[i] - y[i-1])*(integrand[i] + integrand[i-1]);
}
  return  k*k*k/(4*M_PI*M_PI)/pow4(dnorm_spt) * res;
}

// Separation of numerical A B terms for interpolation

static void ABnr_selec(double abarray[], int y, const PowerSpectrum& P_L, real k, real r) {
  double myresult[14];
  for(int ii=0;ii<14;ii++){
    myresult[ii]=0.;
  }
	double temp_abs[14];
  double abc[10];
  // Set the limits of angular integration
  double KMAX = QMAXp/k;
  double KMIN = QMINp/k;
  double YMIN = Max(XMIN, (1.+r*r-KMAX*KMAX)/2./r);
  double YMAX = ATS(k,r);
  // Find those corresponding limits in the GL Quad abscissae (See SpecialFunctions.h)
  int x_min = searchnearest(x128, YMIN);
  int x_max = searchnearest(x128, YMAX);
  for( int i = x_min; i <= x_max; i++ )
    				{
    		 		double x = x128[i];
    		 		double d = 1+ r*r - 2*r*x;
    		 		if(d < 1e-5){
              for(int ii=0;ii<14;ii++){
                temp_abs[ii]=0.;
              }
    		 		}
    		  	else {
              // 1st order kernels F1/G1(k-p)
              abc[0] = F1kmp_nk[i*n2 + y];
              abc[1] = G1kmp_nk[i*n2 + y];
              // 1st order kernels F1/G1(p)
              abc[2] = F1p_nk[i*n2 + y];
              abc[3] = G1p_nk[i*n2 + y];
              //symmetrized 2nd order kernels for ps F2/G2(p,k-p)
              abc[4] = F2_nk[i*n2 + y];
              abc[5] = G2_nk[i*n2 + y];
              //symmetrized 2nd order kernels F2/G2(-p,k)
              abc[6] = F2B_nk[i*n2 + y];
              abc[7] = G2B_nk[i*n2 + y];
              //symmetrized 2nd order kernels F2/G2(-k,k-p)
              abc[8] = F2C_nk[i*n2 + y];
              abc[9] = G2C_nk[i*n2 + y];

              double dampexpa = exp(-pow2(F1_nk/dnorm_spt)*damp_term(k*r)/2.-pow2(F1_nk/dnorm_spt)*damp_term(k*sqrt(d))/2.-pow2(F1_nk/dnorm_spt)*damp_term(k)/2.);
              double dampexpb = exp(-pow2(F1_nk/dnorm_spt)*damp_term(k*sqrt(d))-pow2(F1_nk/dnorm_spt)*damp_term(k*r));
              double prefacb = dampexpb*P_L(k*r)*P_L(k*sqrt(d))/(16.*d*d);
//A terms
//u^2b^2
          temp_abs[0] =  -dampexpa/d*((F1_nk*r*(-2*abc[8]*abc[1]*r*(r*x-1)+abc[9]*2*abc[0]*x*d))*P_L(k)*P_L(k*sqrt(d))
                                    + abc[4]*r*(-2*abc[2]*abc[1]*r*(-1+r*x)+abc[3]*2*abc[0]*x*d)*P_L(k*r)*P_L(k*sqrt(d))
                                    +F1_nk*r*(-2*abc[7]*abc[2]*r*(r*x-1)+abc[3]*2*abc[6]*x*d)*P_L(k)*P_L(k*r));
//u^2b
          temp_abs[1] =  -dampexpa/d*((F1_nk*r*(abc[9]*abc[1]*r*(1-x*x))*P_L(k)*P_L(k*sqrt(d))
            						            + abc[4]*r*(abc[3]*abc[1]*r*(1-x*x))*P_L(k*r)*P_L(k*sqrt(d))
            						            + F1_nk*r*(abc[3]*abc[7]*r*(1-x*x))*P_L(k)*P_L(k*r)));
//u^4 b
          temp_abs[2] = -dampexpa/d*(r*(2*abc[8]*G1_nk*abc[1]*r*(r*x-1)+abc[9]*(G1_nk*(-2*abc[0]*x*d)+F1_nk*abc[1]*(-2*x+r*(-1+3*x*x))))*P_L(k)*P_L(k*sqrt(d))
                                     +r*(abc[4]*abc[1]*abc[3]*(-2*x+r*(-1+3*x*x))+abc[5]*(2*abc[2]*abc[1]*r*(r*x-1)+abc[3]*(-2*abc[0]*x*d)))*P_L(k*r)*P_L(k*sqrt(d))
                                     +r*(abc[3]*abc[7]*F1_nk*(-2*x-r+3*x*x*r)+2*abc[2]*G1_nk*abc[7]*r*(r*x-1)+G1_nk*abc[3]*(-2*abc[6]*x*d))*P_L(k)*P_L(k*r));
// u^4
          temp_abs[3] = -dampexpa/d*(r*(abc[9]*(G1_nk*(abc[1]*r*(x*x-1))))*P_L(k)*P_L(k*sqrt(d))
                                     +r*(abc[5]*(abc[3]*(abc[1]*r*(x*x-1))))*P_L(k*r)*P_L(k*sqrt(d))
                                     +r*(G1_nk*abc[3]*(abc[7]*r*(x*x-1)))*P_L(k)*P_L(k*r));

//u^6
          temp_abs[4] = -dampexpa/d*(G1_nk*abc[1]*abc[9]*r*(r+2*x-3*r*x*x)*P_L(k)*P_L(k*sqrt(d))
                                  +abc[3]*abc[1]*abc[5]*r*(r+2*x-3*r*x*x)*P_L(k*r)*P_L(k*sqrt(d))
                                  +abc[3]*G1_nk*abc[7]*r*(r+2*x-3*r*x*x)*P_L(k*r)*P_L(k));

//B terms
//u^2 b^2
          temp_abs[5]= prefacb*(abc[1]*abc[3]*r*r*(x*x-1)*(2*abc[0]*d*(4*abc[2])));
//u^2 b
          temp_abs[6]= prefacb*(abc[1]*abc[3]*r*r*(x*x-1)*(2*abc[0]*d*(3*abc[3]*(x*x-1))
                         +abc[1]*r*r*(x*x-1)*(6*abc[2])));
//u^2
          temp_abs[7]= prefacb*(abc[1]*abc[3]*r*r*(x*x-1)*(abc[1]*r*r*(x*x-1)*(5*abc[3]*(x*x-1))));
//u^4b^2
          temp_abs[8]= prefacb*(abc[1]*abc[3]*r*(-4*abc[0]*d*(abc[2]*(-4*x+r*(-2+6*x*x)))));
//u^4 b
          temp_abs[9]= prefacb*(abc[1]*abc[3]*r*(-4*abc[0]*d*(3*abc[3]*(x*x-1)*(-2*x+r*(5*x*x-1)))
                					   -3*abc[1]*r*(x*x-1)*(4*abc[2]*(2-6*r*x+r*r*(-1+5*x*x)))));
//u^4
          temp_abs[10]= prefacb*(abc[1]*abc[3]*r*(-3*abc[1]*r*(x*x-1)*(abc[3]*(x*x-1)*(6-30*r*x+5*r*r*(-1+7*x*x)))));

//u^6 b
          temp_abs[11] = prefacb*(abc[1]*abc[3]*r*(2*abc[0]*abc[3]*(4*x*(3-5*x*x)+pow3(r)*(3-30*x*x+35*pow4(x))+r*(3-54*x*x+75*pow4(x))+r*r*(6*x+40*pow3(x)-70*pow5(x)))
                                +2*abc[2]*abc[1]*(-8*x+12*r*(3*x*x-1)+r*r*(36*x-60*pow3(x))+pow3(r)*(3-30*x*x+35*pow4(x)))));

//u^6
          temp_abs[12]= prefacb*(abc[1]*abc[3]*r*(3*abc[3]*abc[1]*(x*x-1)*(-8*x+12*r*(5*x*x-1)-20*r*r*x*(7*x*x-3)+5*pow3(r)*(1-14*x*x+21*pow4(x)))));

//u^8
          temp_abs[13]= prefacb*(pow2(abc[1]*abc[3])*r*(8*x*(5*x*x-3)-6*r*(3-30*x*x+35*pow4(x))
                               				+6*r*r*x*(15-70*x*x+63*pow4(x))  		// HAVE INCLUDED THE 'TYPO' IN TERM : 6rr -> 6rrx
                               				+pow3(r)*(5-105*x*x+315*pow4(x)-231*pow6(x))));

    				}
            for(int ii=0;ii<14;ii++){
              myresult[ii] += w128[i]*temp_abs[ii];
            }
    				}
            for(int ii=0;ii<14;ii++){
              abarray[ii]=2*myresult[ii];
            }
}

void RegPT::ABr_selec(double myarray[], real k) const {
  	real KMAX = QMAXp/k;
    real KMIN = QMINp/k;
    int y1 = num_selec_mag(k, k, KMIN);
    int y2 = num_selec_mag(k, k, KMAX);
    double y[n2];
    double integrand[n2][14];
    double abarray[14];
    for (int i = y1; i<=y2; i++){
    y[i] = QMINp/k * exp(i*log(QMAXp/QMINp)/(n2*1.-1.));
    ABnr_selec(abarray,i, cref(P_l), k, y[i]);
    for(int ii=0;ii<14;ii++){
    integrand[i][ii] = abarray[ii];
      }
    }
  double res[14];
  for(int ii=0;ii<14;ii++){
    res[ii]=0.;
  }
    for( int i = y1+1; i <= y2; ++ i ){
      for(int ii=0;ii<14;ii++){
        res[ii] += 0.5 * (y[i] - y[i-1])*(integrand[i][ii] + integrand[i-1][ii]);
      }
  }
  for(int ii=0;ii<14;ii++){
    myarray[ii] = k*k*k/(4*M_PI*M_PI)/pow4(dnorm_spt) * res[ii];
  }
}


/*! \file demo_2d_bars.cpp
    \brief Demonstrate the bars (& columns or sticks).
    \details
    \date 11 Feb 2009
    \author Paul A. Bristow
*/

// Copyright Paul A. Bristow 2008

// Distributed under the Boost Software License, Version 1.0.
// For more information, see http://www.boost.org

#include <boost/svg_plot/svg_2d_plot.hpp>
#include <map>

using std::map;
using namespace boost::svg;

int main()
{
  map<double, double> map1; // Two sample data series.
  map<double, double> map2;

  // This is random data used purely for example purposes.
  map1[1.] = 3.2; // map[x] = y
  map1[2.] = 5.4;
  map1[7.3] = 9.1;
  map1[8.] = 7.3;
  map1[5.5] = 5.4;
  map1[9.3] = 9.9;

  map2[-3.1] = -6.1;
  map2[5.4] = 4.7;

  svg_2d_plot my_plot1;

  my_plot1.title("X Times")
         .legend_on(true)
         .x_range(-1, 11)
         .background_border_color(black);

  my_plot1.plot(map1, "Series 1").fill_color(blue).shape(none).line_on(false).bar_opt(x_stick).bar_width(5).bar_color(red);
  my_plot1.write("./demo_2d_bar_x_stick.svg");
  my_plot1.plot(map1, "Series 1").shape(none).line_on(false).bar_opt(x_block).bar_width(10);
  my_plot1.write("./demo_2d_bar_x_block.svg");

  svg_2d_plot my_plot2;

  my_plot2.title("Y Times")
         .legend_on(true)
         .x_range(-10, 10)
         .background_border_color(blue);

  my_plot2.plot(map2, "Series 2").fill_color(green).shape(none).line_on(false).bar_opt(y_stick).bar_width(5).bar_color(cyan);
  my_plot2.write("./demo_2d_bar_y_stick.svg");
  my_plot2.plot(map2, "Series 2").shape(none).line_on(false).bar_opt(y_block).bar_width(10).bar_color(green).bar_area_fill(magenta);
  my_plot2.write("./demo_2d_bar_y_block.svg");

  return 0;
} // int main()

/* Output:

demo_2d_bars.cpp
Linking...
Embedding manifest...
Autorun "j:\Cpp\SVG\debug\demo_2d_bars.exe"

*/


#include "trajopt/plot_callback.hpp"
#include "trajopt/common.hpp"
#include "osgviewer/osgviewer.hpp"
#include "utils/eigen_conversions.hpp"
#include <boost/foreach.hpp>
#include "trajopt/problem_description.hpp"
#include "trajopt/collision_checker.hpp"
#include "trajopt/collision_avoidance.hpp"
#include "sco/modeling_utils.hpp"
using namespace OpenRAVE;
using namespace util;
using namespace Eigen;
using namespace std;
namespace trajopt {

void PlotTraj(OSGViewer& viewer, BeliefRobotAndDOFPtr rad, const TrajArray& traj, vector<GraphHandlePtr>& handles) {
	const int n_dof = rad->GetDOF();
	const int x_dim = rad->GetDOF(), b_dim = rad->GetBDim(), u_dim = rad->GetUDim();
	const int n_steps = traj.rows();

	vector<double> color_params;
	for (int i=0; i<n_steps; i++) {
		color_params.push_back(((double)i)/((double)n_steps-1.0));
	}

	for (int i=n_steps-1; i>=0; i--) {
		rad->SetDOFValues(toDblVec(traj.block(i,0,1,n_dof).transpose()));
		handles.push_back(viewer.PlotKinBody(rad->GetRobot()));
		//		SetColor(handles.back(), osg::Vec4f(0,color_params[i],1.0-color_params[i],1));
	}
	rad->SetDOFValues(toDblVec(traj.block(0,0,1,n_dof).transpose()));

	//	boost::shared_ptr<CollisionChecker> cc = CollisionChecker::GetOrCreate(*rad->GetRobot()->GetEnv());
	//	cc->PlotCollisionGeometry(handles);
	//	cc->PlotDebugGeometry(handles);

	if (traj.cols() > n_dof) { // if belief space?
		OR::RobotBase::RobotStateSaver saver = const_cast<BeliefRobotAndDOF*>(rad.get())->Save();

		for (int i=0; i < n_steps; ++i) {
			VectorXd theta = traj.block(i,0,1,b_dim).transpose();
			Vector3d mean;
			Matrix3d cov;
			rad->GetEndEffectorNoiseAsGaussian(theta, mean, cov);

			if (x_dim == 2 || x_dim == 3)
				handles.push_back(viewer.PlotEllipseXYContour(gaussianAsTransform(mean,cov), OR::Vector(0,color_params[i],1.0-color_params[i],1)));
			//			else // for some reason, the color is not rendered well for ellipses
			//				handles.push_back(viewer.PlotEllipsoid(gaussianAsTransform(mean,cov), OR::Vector(0,color_params[i],1.0-color_params[i],1)));
		}

		//		for (int i=0; i<n_steps; i++) {
		//			VectorXd theta = traj.block(i,0,1,n_theta).transpose();
		//			VectorXd x;
		//			MatrixXd rt_Sigma;
		//			rad->decomposeBelief(theta, x, rt_Sigma);
		//
		//			MatrixXd sigma_pts = rad->sigmaPoints(x, rt_Sigma*rt_Sigma. transpose());
		//			for (int j=1; j<sigma_pts.cols(); j++) {
		//				rad->SetDOFValues(toDblVec(sigma_pts.col(j)));
		//				handles.push_back(viewer.PlotKinBody(rad->GetRobot()));
		//				SetColor(handles.back(), osg::Vec4f(0,color_params[i],1.0-color_params[i],0.3));
		//			}
		//		}

		VectorXd theta = traj.block(0,0,1,b_dim).transpose();
		for (int i=0; i<n_steps; i++) {
			Vector3d mean;
			Matrix3d cov;
			rad->GetEndEffectorNoiseAsGaussian(theta, mean, cov);

			if (x_dim == 2 || x_dim == 3)
				handles.push_back(viewer.PlotEllipseXYContour(gaussianAsTransform(mean,cov), OR::Vector(0,color_params[i],1.0-color_params[i],1), true));
			//			else
			//				handles.push_back(viewer.PlotEllipsoid(gaussianAsTransform(mean,cov), OR::Vector(0,color_params[i],1.0-color_params[i],1)));

			if (i != (n_steps-1)) {
				VectorXd u = traj.block(i,b_dim,1,u_dim).transpose();
				theta = rad->BeliefDynamics(theta,u);
			}
		}
	}
}

void PlotCosts(OSGViewer& viewer, vector<CostPtr>& costs, vector<ConstraintPtr>& cnts, BeliefRobotAndDOFPtr rad, const VarArray& vars, const DblVec& x) {
	vector<GraphHandlePtr> handles;
	handles.clear();
	BOOST_FOREACH(CostPtr& cost, costs) {
		if (Plotter* plotter = dynamic_cast<Plotter*>(cost.get())) {
			plotter->Plot(x, *rad->GetRobot()->GetEnv(), handles);
		}
	}
	BOOST_FOREACH(ConstraintPtr& cnt, cnts) {
		if (Plotter* plotter = dynamic_cast<Plotter*>(cnt.get())) {
			plotter->Plot(x, *rad->GetRobot()->GetEnv(), handles);
		}
	}
	TrajArray traj = getTraj(x, vars);
	PlotTraj(viewer, rad, traj, handles);
	viewer.Idle();
	rad->SetDOFValues(toDblVec(traj.row(traj.rows()-1)));
}

Optimizer::Callback PlotCallback(TrajOptProb& prob) {
	OSGViewerPtr viewer = OSGViewer::GetOrCreate(prob.GetEnv());
	vector<ConstraintPtr> cnts = prob.getConstraints();
	return boost::bind(&PlotCosts, boost::ref(*viewer),
			boost::ref(prob.getCosts()),
			cnts,
			prob.GetRAD(),
			boost::ref(prob.GetVars()),
			_2);
}

}


/// Copyright (c) 2014-2016 Andrew Hundt, Johns Hopkins University
///
/// @author Andrew Hundt <ATHundt@gmail.com>
///
/// Dual licensed under the BSD or Apache License, Version 2.0
/// (the "Licenses") licenses at users option. Contributions shall be made such
/// that users can continue to choose between the Licenses.
/// You may not use this project except in compliance with the Licenses.
///
/// BSD license:
/// ------------
///
/// Redistribution and use in source and binary forms, with or without
/// modification, are permitted provided that the following conditions are met:
///
/// 1. Redistributions of source code must retain the above copyright notice, this
///    list of conditions and the following disclaimer.
/// 2. Redistributions in binary form must reproduce the above copyright notice,
///    this list of conditions and the following disclaimer in the documentation
///    and/or other materials provided with the distribution.
///
/// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
/// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
/// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
/// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
/// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
/// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
/// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
/// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
/// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
/// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
///
/// The views and conclusions contained in the software and documentation are those
/// of the authors and should not be interpreted as representing official policies,
/// either expressed or implied, of the grl Project.
///
/// Apache v2 license:
/// ------------------
///
/// You may obtain a copy of the Apache License, Version 2.0 (the "License") at
///
///     http://www.apache.org/licenses/LICENSE-2.0
///
/// Unless required by applicable law or agreed to in writing, software
/// distributed under the License is distributed on an "AS IS" BASIS,
/// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
/// See the License for the specific language governing permissions and
/// limitations under the License.

#ifndef RTREE_PLANNER_HPP_
#define RTREE_PLANNER_HPP_

#include <vector>

#include <boost/random/mersenne_twister.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

#include <boost/pending/indirect_cmp.hpp>
#include <boost/range/irange.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/algorithm/transform.hpp>
#include <boost/range/algorithm/fill.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/geometries/adapted/boost_array.hpp>
#include <boost/geometry/geometries/register/linestring.hpp>
#include <boost/geometry/index/rtree.hpp>

#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/sign.hpp>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

BOOST_GEOMETRY_REGISTER_BOOST_ARRAY_CS(cs::cartesian)

namespace plan {

// Define properties for vertex
// source boost geometry graph example: https://github.com/boostorg/geometry/blob/bd9455207bb0012237c1853c3281ea95f146f91c/example/07_b_graph_route_example.cpp
template <typename Point>
struct bg_vertex_property
{
    bg_vertex_property()
    {
        boost::geometry::assign_zero(location);
    }
    bg_vertex_property(Point const& loc)
        : location(loc)
    {
    }

    Point location;
};

// Define properties for edge
// source boost geometry graph example: https://github.com/boostorg/geometry/blob/bd9455207bb0012237c1853c3281ea95f146f91c/example/07_b_graph_route_example.cpp
template <typename Line>
struct bg_edge_property
{
    bg_edge_property(Line const& line)
        : length(boost::geometry::length(line))
        , m_line(line)
    {
    }

    inline Line const& line() const
    {
        return m_line;
    }

    double length;
private :
    Line m_line;
};

    //typedef bg::model::point<double, 3, bg::cs::cartesian> point;
    typedef boost::array<double,6> ArmPos;
    typedef ArmPos point_type;
    typedef bg::model::box<point_type> box;
    //typedef bg::model::referring_segment<point_type> line_type; /// @todo maybe better to use refs?
    typedef bg::model::segment<point_type> line_type;
    typedef line_type edge_type;
    //typedef bg::model::polygon<point, false, false> polygon; // ccw, open polygon

    typedef boost::adjacency_list
            <
                boost::vecS, boost::vecS, boost::undirectedS
                , bg_vertex_property<point_type> // bundled
                , bg_edge_property<line_type>
            > graph_type;

    typedef boost::graph_traits<graph_type>::vertex_descriptor vertex_descriptor_type;
    typedef bg_vertex_property<point_type>                     vertex_property_type;
    typedef bg_edge_property<edge_type>                        edge_property_type;

    typedef std::pair<point_type, vertex_descriptor_type> rtree_value;
    typedef boost::geometry::index::rtree<rtree_value,bgi::rstar<16, 4> > knn_rtree_type;

} // namespace plan

BOOST_GEOMETRY_REGISTER_LINESTRING(std::vector<plan::ArmPos>)

template<typename T>
inline T normalizeRadiansPiToMinusPi(T rad)
{
  // copy the sign of the value in radians to the value of pi
  T signedPI = boost::math::copysign(boost::math::constants::pi<T>(),rad);
  // set the value of rad to the appropriate signed value between pi and -pi
  rad = std::fmod(rad+signedPI,(boost::math::constants::two_pi<T>())) - signedPI;

  return rad;
}

// functor for getting sum of previous result and square of current element
// source: http://stackoverflow.com/questions/1326118/sum-of-square-of-each-elements-in-the-vector-using-for-each
template<typename T>
struct square
{
    T operator()(const T& Left, const T& Right) const
    {
        return (Left + Right*Right);
    }
};

namespace boost { namespace geometry {

/// distance on an n-torus, so any dimensions offset by 2pi are equal and distances wrap
/// @example comparable_distance(pi-.01,-pi+.01) == .02 radians
double comparable_distance(plan::ArmPos const& p1, plan::ArmPos const& p2 ) {
    plan::ArmPos diff;
    boost::transform(p1,p2,diff.begin(),std::minus<plan::ArmPos::value_type>());
    boost::transform(diff,diff.begin(),&normalizeRadiansPiToMinusPi<plan::ArmPos::value_type>);
    return boost::accumulate(diff,0,square<plan::ArmPos::value_type>());
}

/// distance between a a point and an axis aligned "box" on the surface of an n-torus
//  so any dimensions offset by 2pi are equal and distances wrap
template<typename Box>
double comparable_distance(plan::ArmPos const& armpos, Box const& box ){
    namespace bg = boost::geometry;
    plan::ArmPos normAP = normalizeRadiansPiToMinusPi(armpos);
    plan::ArmPos mindiff;
    boost::transform(normAP,bg::get<bg::min_corner>(box),mindiff.begin(),std::minus<plan::ArmPos::value_type>());
    boost::transform(mindiff,mindiff.begin(),&normalizeRadiansPiToMinusPi<plan::ArmPos::value_type>);
    plan::ArmPos maxdiff;
    boost::transform(normAP,bg::get<bg::max_corner>(box),maxdiff.begin(),std::minus<plan::ArmPos::value_type>());
    boost::transform(maxdiff,maxdiff.begin(),&normalizeRadiansPiToMinusPi<plan::ArmPos::value_type>);

    plan::ArmPos::value_type final_distance = 0.0;
    for(int i = 0; i < armpos.size(); ++i){
        if(mindiff[i] >= 0.0 && maxdiff[i] <= 0.0) continue; // between the min and max means "in the box" for this dimension
        plan::ArmPos::value_type min_dist = std::min(std::abs(mindiff[i]),std::abs(maxdiff[i]));
        final_distance+=min_dist*min_dist;
    }

    return final_distance;
//    diff (min<D> - p<D>), (p<D> - max<D>)
}

}} // namespace boost::geometry

///
/// source boost geometry graph example: https://github.com/boostorg/geometry/blob/bd9455207bb0012237c1853c3281ea95f146f91c/example/07_b_graph_route_example.cpp
template <typename Graph, typename Route>
inline void add_edge_to_route(Graph const& graph,
            typename boost::graph_traits<Graph>::vertex_descriptor vertex1,
            typename boost::graph_traits<Graph>::vertex_descriptor vertex2,
            Route& route)
{
    std::pair
        <
            typename boost::graph_traits<Graph>::edge_descriptor,
            bool
        > opt_edge = boost::edge(vertex1, vertex2, graph);
    if (opt_edge.second)
    {
        // Get properties of edge and of vertex
        plan::edge_property_type const& edge_prop = graph[opt_edge.first];
        plan::bg_vertex_property
            <
                typename boost::geometry::point_type<plan::edge_type>::type
            > const& vertex_prop = graph[vertex2];

        // Depending on how edge connects to vertex, copy it forward or backward
        if (boost::geometry::equals(*bg::segment_view<plan::edge_type>(edge_prop.line()).begin(), vertex_prop.location))
        {
            std::copy(bg::segment_view<plan::edge_type>(edge_prop.line()).begin(), bg::segment_view<plan::edge_type>(edge_prop.line()).end(),
                std::back_inserter(route));
        }
        else
        {
            std::reverse_copy(bg::segment_view<plan::edge_type>(edge_prop.line()).begin(), bg::segment_view<plan::edge_type>(edge_prop.line()).end(),
                std::back_inserter(route));
        }
    }
}

/// After running dijkstra's algorithm build list of shortest path steps between start and end position.
/// @note there will be duplicate positions that should be removed.
/// source boost geometry graph example: https://github.com/boostorg/geometry/blob/bd9455207bb0012237c1853c3281ea95f146f91c/example/07_b_graph_route_example.cpp
template <typename Graph, typename Route>
inline void build_route(Graph const& graph,
            std::vector<typename boost::graph_traits<Graph>::vertex_descriptor> const& predecessors,
            typename boost::graph_traits<Graph>::vertex_descriptor vertex1,
            typename boost::graph_traits<Graph>::vertex_descriptor vertex2,
            Route& route)
{
    typedef typename boost::graph_traits<Graph>::vertex_descriptor vertex_type;
    vertex_type pred = predecessors[vertex2];

    add_edge_to_route(graph, pred, vertex2, route);
    while (pred != vertex1)
    {
        add_edge_to_route(graph, predecessors[pred], pred, route);
        pred = predecessors[pred];
    }
}

#endif


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2010.
// 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)

#include <mpllibs/metamonad/do_.hpp>
#include <mpllibs/metamonad/lazy_metafunction.hpp>
#include <mpllibs/metamonad/either.hpp>
#include <mpllibs/metamonad/syntax.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/tag.hpp>
#include <boost/mpl/assert.hpp>

#include "common.hpp"

using boost::mpl::minus;

using mpllibs::metamonad::right;

namespace
{
  MPLLIBS_LAZY_METAFUNCTION(minus_2, (A)) ((right<minus<A, int2> >));
}

BOOST_AUTO_TEST_CASE(test_do1)
{
  using mpllibs::metamonad::do_;
  using mpllibs::metamonad::syntax;
  using mpllibs::metamonad::set;
  using mpllibs::metamonad::do_return;

  using boost::mpl::equal_to;

  typedef boost::mpl::tag<int13>::type int_tag;
  typedef mpllibs::metamonad::either_tag<int_tag, int_tag> either;

  // test_do
  BOOST_MPL_ASSERT((
    equal_to<
      right<int11>,
      do_<either,
        syntax<set<x, do_return<int13> > >,
        syntax<minus_2<x> >
      >::type
    >
  ));

  // test_do_three_steps
  BOOST_MPL_ASSERT((
    equal_to<
      right<int9>,
      do_<either,
        syntax<set<x, do_return<int13> > >,
        syntax<set<y, minus_2<x> > >,
        syntax<minus_2<y> >
      >::type
    >
  ));

  // test_do_two_calls
  BOOST_MPL_ASSERT((
    equal_to<
      right<int9>,
      do_<either,
        syntax<set<x, do_return<int13> > >,
        syntax<set<y, minus_2<x> > >,
        syntax<minus_2<x> >,
        syntax<minus_2<y> >
      >::type
    >
  ));

  // test_do_two_returns
  BOOST_MPL_ASSERT((
    equal_to<
      right<int13>,
      do_<either,
        syntax<do_return<int11> >,
        syntax<do_return<int13> >
      >::type
    >
  ));

  // test_nested_do_with_return
  BOOST_MPL_ASSERT((
    equal_to<
      right<right<int13> >,
      do_<either,
        syntax<
          do_return<
            do_<either,
              syntax<do_return<int13> >
            >
          >
        >
      >::type
    >
  ));
}


//==================================================================================================
/**
  Copyright 2016 Numscale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <bitset>

namespace boost { namespace simd { namespace ext
{
  namespace bs =  boost::simd;
  namespace bd =  boost::dispatch;

  BOOST_DISPATCH_OVERLOAD ( hmsb_
                          , (typename A0)
                          , bs::ssse3_
                          , bs::pack_<bd::ints16_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE std::bitset<8> operator()(const A0 & a0) const BOOST_NOEXCEPT
    {
      using s8type = typename A0::template retype<int8_t, 16>;
      s8type mask = { 0x01,0x03,0x05,0x07,0x09,0x0B,0x0D,0x0F
                    ,-128,-128,-128,-128,-128,-128,-128,-128
                    };

      return _mm_movemask_epi8(_mm_shuffle_epi8(bitwise_cast<s8type>(a0), mask));
    }
  };
} } }

#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) 2007-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 non_base_dimension.cpp

\details
Another example of user-defined units with conversions.

Output:
@verbatim

//[non_base_dimension_output
//]

@endverbatim
**/

#include <iostream>

#include <boost/units/io.hpp>
#include <boost/units/conversion.hpp>
#include <boost/units/unit.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/physical_dimensions.hpp>
#include <boost/units/base_unit.hpp>
#include <boost/units/make_system.hpp>

namespace boost {

namespace units {

//[non_base_dimension_snippet_1

struct imperial_gallon_tag :
    base_unit<imperial_gallon_tag, volume_dimension, 1> { };

typedef make_system<imperial_gallon_tag>::type imperial;

typedef unit<volume_dimension,imperial> imperial_gallon;

struct us_gallon_tag : base_unit<us_gallon_tag, volume_dimension, 2> { };

typedef make_system<us_gallon_tag>::type us;

typedef unit<volume_dimension,us> us_gallon;

//]

} // namespace units

} // namespace boost

BOOST_UNITS_DEFINE_CONVERSION_FACTOR(boost::units::imperial_gallon_tag,
                                     boost::units::us_gallon_tag,
                                     double, 1.2009499255);

using namespace boost::units;

int main(void)
{
    quantity<imperial_gallon>   ig1(1.0*imperial_gallon());
    quantity<us_gallon>         ug1(1.0*us_gallon());

    quantity<imperial_gallon>   ig2(ug1);
    quantity<us_gallon>         ug2(ig1);

    return 0;
}