#pragma once

#include <assert.h>

#include <algorithm>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <cmath>
#include <exception>
#include <functional>
#include <iostream>
#include <iterator>
#include <map>
#include <memory>
#include <opencv2/core/types.hpp>
#include <opencv2/opencv.hpp>
#include <regex>
#include <string>
#include <utility>
#include <vector>

#define print_line                                                  \
    {                                                               \
        std::cout << __FILE__ << "  ---  " << __func__ << "  ---  " \
                  << __LINE__ << std::endl;                         \
    }

namespace utils {

inline int round(float v) {
    return int(std::round(v));
}

inline cv::Point2f __add__(const cv::Point2f& a, const std::vector<float>& b) {
    return cv::Point2f{a.x + b.at(0), a.y + b.at(1)};
}

inline cv::Point2i pntf2pnti(const cv::Point2f& pnt) {
    return cv::Point2i{round(pnt.x), round(pnt.y)};
}

inline void setpntf(cv::Point2f& pntf, std::vector<float>& pt) {
    assert(pt.size() >= 2);
    pntf.x = pt.at(0);
    pntf.y = pt.at(1);
}

std::shared_ptr<std::vector<float>> arange(float b, float e, float t) {
    assert(b < e && t > 0);
    std::shared_ptr<std::vector<float>> res{new std::vector<float>{}};
    for (; b < e; b += t) {
        res->push_back(b);
    }
    return res;
}
std::shared_ptr<std::vector<float>> linspace(float b, float e, int cnt = 2) {
    float c = std::max(2, cnt);
    assert(b < e && c >= 2.0f);
    std::shared_ptr<std::vector<float>> res{new std::vector<float>{}};
    float it = b;
    float sp = e - b;
    res->push_back(it);
    while (c > 2) {
        float delta = sp / (c - 1.0f);
        it += delta;
        sp -= delta;
        c -= 1.0f;
        res->push_back(it);
    }
    res->push_back(e);
    return res;
}

inline float eular_distance(const cv::Point2f& a, const cv::Point2f& b) {
    cv::Point2f c = a - b;
    return std::sqrt(c.x * c.x + c.y * c.y);
}

inline cv::Point2f _get_quadratic_bezier_pnt(const cv::Point2f& b,
                                             const cv::Point2f& c1,
                                             const cv::Point2f& c2,
                                             const cv::Point2f& e,
                                             float t) {
    return std::pow((1.0f - t), 3) * b + 3 * std::pow((1.0f - t), 2) * t * c1 +
           3 * std::pow(t, 2) * (1.0f - t) * c2 + std::pow(t, 3) * e;
}
std::shared_ptr<std::vector<cv::Point2i>> quadratic_bezier(
    const cv::Point2f& b,
    const cv::Point2f& c1,
    const cv::Point2f& c2,
    const cv::Point2f& e,
    int cnt = -1) {
    std::shared_ptr<std::vector<cv::Point2i>> res{
        new std::vector<cv::Point2i>{}};

    cnt = cnt <= 0 ? int(eular_distance(b, c1) + eular_distance(c1, c2) +
                         eular_distance(c2, e))
                   : cnt;
    auto ts = linspace(0, 1, cnt);
    for (auto& t : *ts) {
        res->push_back(pntf2pnti(_get_quadratic_bezier_pnt(b, c1, c2, e, t)));
    }

    return res;
}

void _get_paths_from_g(boost::property_tree::ptree& pt,
                       std::vector<boost::property_tree::ptree>& paths) {
    for (auto& el : pt) {
        if (el.first == "g") {
            _get_paths_from_g(el.second, paths);
        } else if (el.first == "path") {
            paths.push_back(el.second);
        }
    }
}

std::pair<int, int> get_size_from_svg(boost::property_tree::ptree& svg) {
    auto res = std::make_pair(0, 0);
    try {
        res.first  = svg.get<int>("<xmlattr>.width");
        res.second = svg.get<int>("<xmlattr>.height");

    } catch (std::exception& e) {
        std::cerr << e.what() << std::endl;
    }

    return res;
}

std::pair<int, int> get_size_from_doc(boost::property_tree::ptree& doc) {
    auto svg = doc.get_child("svg");
    return get_size_from_svg(svg);
}

std::shared_ptr<std::vector<boost::property_tree::ptree>> get_paths_from_svg(
    boost::property_tree::ptree& doc) {
    std::shared_ptr<std::vector<boost::property_tree::ptree>> res{
        new std::vector<boost::property_tree::ptree>{}};
    auto root = doc.get_child("svg");
    auto g    = root.get_child("g");
    _get_paths_from_g(g, *res);
    return res;
}

std::shared_ptr<std::vector<boost::property_tree::ptree>> get_paths_from_svg(
    const std::string& svg_filepath) {
    boost::property_tree::ptree pt;
    boost::property_tree::read_xml(svg_filepath, pt);
    return get_paths_from_svg(pt);
}

std::shared_ptr<std::vector<std::string>> get_pathd_from_doc(
    boost::property_tree::ptree& doc) {
    auto paths = get_paths_from_svg(doc);
    std::shared_ptr<std::vector<std::string>> res{
        new std::vector<std::string>{}};

    for (auto& path : *paths) {
        res->push_back(path.get<std::string>("<xmlattr>.d"));
    }
    return res;
}

inline std::shared_ptr<std::vector<std::string>> split(
    const std::string& s,
    const std::string& p = "[\\s]+") {
    std::regex reg{p};
    std::shared_ptr<std::vector<std::string>> res{new std::vector<std::string>{
        std::sregex_token_iterator{s.begin(), s.end(), reg, -1},
        std::sregex_token_iterator{}}};
    return res;
}

inline std::shared_ptr<std::vector<float>> split_to_floats(
    const std::string& s,
    const std::string& p = ",") {
    print_line;
    std::cout << "split s:" << s << " ;; by:" << p << std::endl;
    std::regex reg{p};
    std::shared_ptr<std::vector<float>> res{new std::vector<float>{}};
    std::vector<std::string> strs{
        std::sregex_token_iterator{s.begin(), s.end(), reg, -1},
        std::sregex_token_iterator{}};
    std::transform(
        strs.begin(), strs.end(), std::back_inserter(*res), [](std::string& s) {
            float res = 0;
            try {
                res = std::stof(s);
            } catch (std::exception& e) {
                std::cerr << e.what() << std::endl;
            }
            return res;
        });
    return res;
}

static std::map<std::string,
                std::function<bool(cv::Point2f&,
                                   std::vector<std::vector<cv::Point2i>>&,
                                   const std::vector<std::string>&,
                                   int&)>>
    __handle_mapper__{
        {"m",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ways.push_back(std::vector<cv::Point2i>{});
             ++i;
             if (i >= pathd.size()) {
                 return false;
             }
             auto pts = split_to_floats(pathd.at(i));
             prev_pt.x += pts->at(0);
             prev_pt.y += pts->at(1);
             ways.back().push_back(prev_pt);
             ++i;
             return true;
         }},
        {"M",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ways.push_back(std::vector<cv::Point2i>{});
             ++i;

             if (i >= pathd.size()) {
                 return false;
             }
             auto pts = split_to_floats(pathd.at(i));
             setpntf(prev_pt, *pts);
             ways.back().push_back(prev_pt);
             ++i;
             return true;
         }},
        {"l",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ++i;
             if (i >= pathd.size()) {
                 return false;
             }
             auto pts = split_to_floats(pathd.at(i));
             prev_pt.x += pts->at(0);
             prev_pt.y += pts->at(1);
             ways.back().push_back(prev_pt);
             ++i;
             return true;
         }},
        {"L",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ++i;
             if (i >= pathd.size()) {
                 return false;
             }
             auto pts = split_to_floats(pathd.at(i));
             setpntf(prev_pt, *pts);
             ways.back().push_back(prev_pt);
             ++i;
             return true;
         }},
        {"h",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ++i;
             if (i >= pathd.size()) {
                 return false;
             }
             auto pts = split_to_floats(pathd.at(i));
             prev_pt.x += pts->at(0);
             ways.back().push_back(prev_pt);
             ++i;
             return true;
         }},
        {"H",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ++i;
             if (i >= pathd.size()) {
                 return false;
             }
             auto pts  = split_to_floats(pathd.at(i));
             prev_pt.x = pts->at(0);
             ways.back().push_back(prev_pt);
             ++i;
             return true;
         }},
        {"v",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ++i;
             if (i >= pathd.size()) {
                 return false;
             }
             auto pts = split_to_floats(pathd.at(i));
             prev_pt.y += pts->at(0);
             ways.back().push_back(prev_pt);
             ++i;
             return true;
         }},
        {"V",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ++i;
             if (i >= pathd.size()) {
                 return false;
             }
             auto pts  = split_to_floats(pathd.at(i));
             prev_pt.y = pts->at(0);
             ways.back().push_back(prev_pt);
             ++i;
             return true;
         }},
        {"c",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             cv::Point2f b = prev_pt;

             if (++i >= pathd.size()) {
                 return false;
             }
             cv::Point2f c1 = __add__(prev_pt, *split_to_floats(pathd.at(i)));

             if (++i >= pathd.size()) {
                 return false;
             }
             cv::Point2f c2 = __add__(prev_pt, *split_to_floats(pathd.at(i)));

             if (++i >= pathd.size()) {
                 return false;
             }
             cv::Point2f e = __add__(prev_pt, *split_to_floats(pathd.at(i)));

             auto pts = quadratic_bezier(b, c1, c2, e);
             prev_pt  = pts->back();
             std::copy(
                 pts->begin(), pts->end(), std::back_inserter(ways.back()));

             ++i;
             return true;
         }},
        {"C",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             cv::Point2f b = prev_pt;

             if (++i >= pathd.size()) {
                 return false;
             }
             cv::Point2f c1;
             setpntf(c1, *split_to_floats(pathd.at(i)));

             if (++i >= pathd.size()) {
                 return false;
             }
             cv::Point2f c2;
             setpntf(c2, *split_to_floats(pathd.at(i)));

             if (++i >= pathd.size()) {
                 return false;
             }
             cv::Point2f e;
             setpntf(e, *split_to_floats(pathd.at(i)));

             auto pts = quadratic_bezier(b, c1, c2, e);
             prev_pt  = pts->back();
             std::copy(
                 pts->begin(), pts->end(), std::back_inserter(ways.back()));

             ++i;
             return true;
         }},
        {"z",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ways.back().push_back(ways.back().front());
             prev_pt = ways.back().back();
             ++i;
             return true;
         }},
        {"Z",
         [](cv::Point2f& prev_pt,
            std::vector<std::vector<cv::Point2i>>& ways,
            const std::vector<std::string>& pathd,
            int& i) -> bool {
             ways.back().push_back(ways.back().front());
             prev_pt = ways.back().back();
             ++i;
             return true;
         }}};

void get_way_from_pathd(std::vector<std::string>& pathd,
                        std::vector<std::vector<cv::Point2i>>& ways) {
    print_line;

    auto prev_func_it = __handle_mapper__.begin();
    int i             = 0;
    cv::Point2f prev_pt{0, 0};
    while (i < pathd.size()) {
        auto key = pathd.at(i);
        auto fit = __handle_mapper__.find(key);
        if (fit == __handle_mapper__.end()) {
            --i;
            fit = prev_func_it;
        }
        std::cout << "handle in " << fit->first << std::endl;
        if (false == fit->second(prev_pt, ways, pathd, i)) {
            return;
        }
        std::cout << "this i is " << i << std::endl;
        prev_func_it = fit;
    }
}

std::shared_ptr<std::vector<std::vector<cv::Point2i>>> get_ways_form_paths(
    std::vector<boost::property_tree::ptree>& paths) {
    //
    print_line;
    std::shared_ptr<std::vector<std::vector<cv::Point2i>>> res{
        new std::vector<std::vector<cv::Point2i>>{}};
    for (auto& pt : paths) {
        auto d     = pt.get<std::string>("<xmlattr>.d");
        auto pathd = split(d);
        for (auto& s : *pathd) {
            std::cout << s << " ; ";
        }
        std::cout << "%%" << std::endl;
        get_way_from_pathd(*pathd, *res);
    }
    return res;
}

std::shared_ptr<std::vector<std::vector<cv::Point2i>>> get_ways_form_svg(
    boost::property_tree::ptree& doc) {
    auto paths = get_paths_from_svg(doc);
    return get_ways_form_paths(*paths);
}

std::shared_ptr<std::vector<std::vector<cv::Point2i>>> get_ways_form_svg(
    const std::string& svg_filepath) {
    boost::property_tree::ptree pt;
    boost::property_tree::read_xml(svg_filepath, pt);
    return get_ways_form_svg(pt);
}

}  // namespace utils


#include "trajopt/collision_avoidance.hpp"
#include "trajopt/rave_utils.hpp"
#include "trajopt/utils.hpp"
#include "sco/expr_vec_ops.hpp"
#include "sco/expr_ops.hpp"
#include "sco/sco_common.hpp"
#include <boost/foreach.hpp>
#include "utils/eigen_conversions.hpp"
#include "sco/modeling_utils.hpp"
#include "utils/stl_to_string.hpp"
using namespace OpenRAVE;
using namespace sco;
using namespace util;
using namespace std;

namespace trajopt {

void CollisionsToDistances(const vector<Collision>& collisions, const Link2Int& m_link2ind,
    DblVec& dists, DblVec& weights) {
  // Note: this checking (that the links are in the list we care about) is probably unnecessary
  // since we're using LinksVsAll
  dists.clear();
  weights.clear();
  dists.reserve(collisions.size());
  weights.reserve(collisions.size());
  BOOST_FOREACH(const Collision& col, collisions) {
    Link2Int::const_iterator itA = m_link2ind.find(col.linkA);
    Link2Int::const_iterator itB = m_link2ind.find(col.linkB);
    if (itA != m_link2ind.end() || itB != m_link2ind.end()) {
      dists.push_back(col.distance);
      weights.push_back(col.weight);
    }
  }
}

void CollisionsToDistanceExpressions(const vector<Collision>& collisions, RobotAndDOF& rad,
    const Link2Int& link2ind, const VarVector& vars, const DblVec& dofvals, vector<AffExpr>& exprs, DblVec& weights) {

  exprs.clear();
  weights.clear();
  exprs.reserve(collisions.size());
  weights.reserve(collisions.size());
  rad.SetDOFValues(dofvals); // since we'll be calculating jacobians
  BOOST_FOREACH(const Collision& col, collisions) {
    AffExpr dist(col.distance);
    Link2Int::const_iterator itA = link2ind.find(col.linkA);
    if (itA != link2ind.end()) {
      VectorXd dist_grad = toVector3d(col.normalB2A).transpose()*rad.PositionJacobian(itA->second, col.ptA);
      exprInc(dist, varDot(dist_grad, vars));
      exprInc(dist, -dist_grad.dot(toVectorXd(dofvals)));
    }
    Link2Int::const_iterator itB = link2ind.find(col.linkB);
    if (itB != link2ind.end()) {
      VectorXd dist_grad = -toVector3d(col.normalB2A).transpose()*rad.PositionJacobian(itB->second, col.ptB);
      exprInc(dist, varDot(dist_grad, vars));
      exprInc(dist, -dist_grad.dot(toVectorXd(dofvals)));
    }
    if (itA != link2ind.end() || itB != link2ind.end()) {
      exprs.push_back(dist);
      weights.push_back(col.weight);
    }
  }
  RAVELOG_DEBUG("%i distance expressions\n", exprs.size());
}

void CollisionsToDistanceExpressions(const vector<Collision>& collisions, RobotAndDOF& rad, const Link2Int& link2ind,
    const VarVector& vars0, const VarVector& vars1, const DblVec& vals0, const DblVec& vals1,
    vector<AffExpr>& exprs, DblVec& weights) {
  vector<AffExpr> exprs0, exprs1;
  DblVec weights0, weights1;
  CollisionsToDistanceExpressions(collisions, rad, link2ind, vars0, vals0, exprs0, weights0);
  CollisionsToDistanceExpressions(collisions, rad, link2ind, vars1, vals1, exprs1, weights1);

  exprs.resize(exprs0.size());
  weights.resize(exprs0.size());

  for (int i=0; i < exprs0.size(); ++i) {
    exprScale(exprs0[i], (1-collisions[i].time));
    exprScale(exprs1[i], collisions[i].time);
    exprs[i] = AffExpr(0);
    exprInc(exprs[i], exprs0[i]);
    exprInc(exprs[i], exprs1[i]);
    weights[i] = (weights0[i] + weights1[i])/2;
  }
}

void BeliefCollisionsToDistanceExpressions(const vector<Collision>& collisions, BeliefRobotAndDOF& brad,
    const Link2Int& link2ind, const VarVector& theta_vars, const DblVec& theta_vals, vector<AffExpr>& exprs, DblVec& weights) {
  exprs.clear();
  weights.clear();
  exprs.reserve(collisions.size());
  weights.reserve(collisions.size());
  brad.SetBeliefValues(theta_vals); // since we'll be calculating jacobians
  BOOST_FOREACH(const Collision& col, collisions) {
  	Link2Int::const_iterator itA = link2ind.find(col.linkA);
		Link2Int::const_iterator itB = link2ind.find(col.linkB);
		AffExpr dist;
		for (int i=0; i<col.mi.alpha.size(); i++) {
			//cout << "ALPHA " << col.mi.alpha[i] << endl;
  		AffExpr dist_a(col.distance);
			if (itA != link2ind.end()) {
				VectorXd dist_grad = toVector3d(col.normalB2A).transpose()*brad.BeliefJacobian(itA->second, col.mi.instance_ind[i], col.ptA);
				exprInc(dist_a, varDot(dist_grad, theta_vars));
				exprInc(dist_a, -dist_grad.dot(toVectorXd(theta_vals)));
			}
			if (itB != link2ind.end()) {
				VectorXd dist_grad = -toVector3d(col.normalB2A).transpose()*brad.BeliefJacobian(itB->second, col.mi.instance_ind[i], col.ptB);
				exprInc(dist_a, varDot(dist_grad, theta_vars));
				exprInc(dist_a, -dist_grad.dot(toVectorXd(theta_vals)));
			}
			if (itA != link2ind.end() || itB != link2ind.end()) {
		    exprScale(dist_a, col.mi.alpha[i]);
		    exprInc(dist, dist_a);
			}
		}
		if (dist.constant!=0 || dist.coeffs.size()!=0 || dist.vars.size()!=0) {
			exprs.push_back(dist);
			weights.push_back(col.weight);
		}
  }
  RAVELOG_DEBUG("%i distance expressions\n", exprs.size());
}

void CollisionEvaluator::GetCollisionsCached(const DblVec& x, vector<Collision>& collisions) {
  double key = vecSum(x);
  vector<Collision>* it = m_cache.get(key);
  if (it != NULL) {
    RAVELOG_DEBUG("using cached collision check\n");
    collisions = *it;
  }
  else {
    RAVELOG_DEBUG("not using cached collision check\n");
    CalcCollisions(x, collisions);
    m_cache.put(key, collisions);
  }
}

SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator(RobotAndDOFPtr rad, const VarVector& vars) :
  m_env(rad->GetRobot()->GetEnv()),
  m_cc(CollisionChecker::GetOrCreate(*m_env)),
  m_rad(rad),
  m_vars(vars),
  m_link2ind(),
  m_links() {
  RobotBasePtr robot = rad->GetRobot();
  const vector<KinBody::LinkPtr>& robot_links = robot->GetLinks();
  vector<KinBody::LinkPtr> links;
  vector<int> inds;
  rad->GetAffectedLinks(m_links, true, inds);
  for (int i=0; i < m_links.size(); ++i) {
    m_link2ind[m_links[i].get()] = inds[i];
  }
}

void SingleTimestepCollisionEvaluator::CalcCollisions(const DblVec& x, vector<Collision>& collisions) {
  DblVec dofvals = getDblVec(x, m_vars);
  m_rad->SetDOFValues(dofvals);
  m_cc->LinksVsAll(m_links, collisions);
}

void SingleTimestepCollisionEvaluator::CalcDists(const DblVec& x, DblVec& dists, DblVec& weights) {
  vector<Collision> collisions;
  GetCollisionsCached(x, collisions);
  CollisionsToDistances(collisions, m_link2ind, dists, weights);
}

void SingleTimestepCollisionEvaluator::CalcDistExpressions(const DblVec& x, vector<AffExpr>& exprs, DblVec& weights) {
  vector<Collision> collisions;
  GetCollisionsCached(x, collisions);
  DblVec dofvals = getDblVec(x, m_vars);
  CollisionsToDistanceExpressions(collisions, *m_rad, m_link2ind, m_vars, dofvals, exprs, weights);
}

////////////////////////////////////////

CastCollisionEvaluator::CastCollisionEvaluator(RobotAndDOFPtr rad, const VarVector& vars0, const VarVector& vars1) :
  m_env(rad->GetRobot()->GetEnv()),
  m_cc(CollisionChecker::GetOrCreate(*m_env)),
  m_rad(rad),
  m_vars0(vars0),
  m_vars1(vars1),
  m_link2ind(),
  m_links() {
  RobotBasePtr robot = rad->GetRobot();
  const vector<KinBody::LinkPtr>& robot_links = robot->GetLinks();
  vector<KinBody::LinkPtr> links;
  vector<int> inds;
  rad->GetAffectedLinks(m_links, true, inds);
  for (int i=0; i < m_links.size(); ++i) {
    m_link2ind[m_links[i].get()] = inds[i];
  }
}

void CastCollisionEvaluator::CalcCollisions(const DblVec& x, vector<Collision>& collisions) {
  DblVec dofvals0 = getDblVec(x, m_vars0);
  DblVec dofvals1 = getDblVec(x, m_vars1);
  m_rad->SetDOFValues(dofvals0);
  m_cc->CastVsAll(*m_rad, m_links, dofvals0, dofvals1, collisions);
}
void CastCollisionEvaluator::CalcDistExpressions(const DblVec& x, vector<AffExpr>& exprs, DblVec& weights) {
  vector<Collision> collisions;
  GetCollisionsCached(x, collisions);
  DblVec dofvals0 = getDblVec(x, m_vars0);
  DblVec dofvals1 = getDblVec(x, m_vars1);
  CollisionsToDistanceExpressions(collisions, *m_rad, m_link2ind, m_vars0, m_vars1, dofvals0, dofvals1, exprs, weights);
}
void CastCollisionEvaluator::CalcDists(const DblVec& x, DblVec& dists, DblVec& weights) {
  vector<Collision> collisions;
  GetCollisionsCached(x, collisions);
  CollisionsToDistances(collisions, m_link2ind, dists, weights);
}

//////////////////////////////////////////
SigmaPtsCollisionEvaluator::SigmaPtsCollisionEvaluator(BeliefRobotAndDOFPtr rad, const VarVector& theta_vars) :
  m_env(rad->GetRobot()->GetEnv()),
  m_cc(CollisionChecker::GetOrCreate(*m_env)),
  m_rad(rad),
  m_theta_vars(theta_vars),
  m_link2ind(),
  m_links() {
  RobotBasePtr robot = rad->GetRobot();
  const vector<KinBody::LinkPtr>& robot_links = robot->GetLinks();
  vector<KinBody::LinkPtr> links;
  vector<int> inds;
  rad->GetAffectedLinks(m_links, true, inds);
  for (int i=0; i < m_links.size(); ++i) {
    m_link2ind[m_links[i].get()] = inds[i];
  }
}
void SigmaPtsCollisionEvaluator::CalcCollisions(const DblVec& x, vector<Collision>& collisions) {
  DblVec theta = getDblVec(x, m_theta_vars);
  MatrixXd sigma_pts = m_rad->sigmaPoints(toVectorXd(theta));

  vector<DblVec> dofvals(sigma_pts.cols());
  for (int i=0; i<sigma_pts.cols(); i++) {
  	dofvals[i] = toDblVec(sigma_pts.col(i));
  }
  m_cc->MultiCastVsAll(*m_rad, m_links, dofvals, collisions);
}
void SigmaPtsCollisionEvaluator::CalcDistExpressions(const DblVec& x, vector<AffExpr>& exprs, DblVec& weights) {
  vector<Collision> collisions;
  GetCollisionsCached(x, collisions);
  DblVec theta = getDblVec(x, m_theta_vars);
  // MatrixXd sigma_pts = m_rad->sigmaPoints(toVectorXd(theta));
  // is sigma_pts not being used ??
  BeliefCollisionsToDistanceExpressions(collisions, *m_rad, m_link2ind, m_theta_vars, theta, exprs, weights);
}
void SigmaPtsCollisionEvaluator::CalcDists(const DblVec& x, DblVec& dists, DblVec& weights) {
  vector<Collision> collisions;
  GetCollisionsCached(x, collisions);
	CollisionsToDistances(collisions, m_link2ind, dists, weights);
}
// for numerical linearization (for debugging, not being used)
VectorXd SigmaPtsCollisionEvaluator::CalcDists(const VectorXd& theta, DblVec& weights) {
  MatrixXd sigma_pts = m_rad->sigmaPoints(theta);

  vector<DblVec> dofvals(sigma_pts.cols());
  for (int i=0; i<sigma_pts.cols(); i++) {
  	dofvals[i] = toDblVec(sigma_pts.col(i));
  }
	m_cc->SetContactDistance(100);
  vector<Collision> collisions;
  m_cc->MultiCastVsAll(*m_rad, m_links, dofvals, collisions);

  VectorXd distances(collisions.size());
  weights.resize(collisions.size());
  for (int i=0; i<m_links.size(); i++) {
		int j;
		for (j=0; j<collisions.size(); j++) {
			if (collisions[j].linkA == m_links[i].get() || collisions[j].linkB == m_links[i].get())
				break;
		}
		assert(j!=collisions.size());
		distances(i) = collisions[j].distance;
		weights[i] = collisions[j].weight;
  }
  return distances;
}
//// for numerical linearization
//void SigmaPtsCollisionEvaluator::CalcDistExpressions(const DblVec& x, vector<AffExpr>& exprs, DblVec& weights) {
//  VectorXd theta = toVectorXd(getDblVec(x, m_theta_vars));
//
//  MatrixXd A = calcNumJac(boost::bind(&SigmaPtsCollisionEvaluator::CalcDists, this, _1, weights), theta);
//  VectorXd c = CalcDists(theta, weights);
//  //-coeff * (A * (m_theta_vars - theta) + c);
//	assert(A.rows() == c.rows());
//	for (int i=0; i < A.rows(); ++i) {
//		AffExpr aff;
//		aff.constant = c(i) - A.row(i).dot(theta);
//		aff.coeffs = toDblVec(A.row(i));
//		aff.vars = m_theta_vars;
//		exprs.push_back(aff);
//	}
//}
void SigmaPtsCollisionEvaluator::CustomPlot(const DblVec& x, std::vector<OR::GraphHandlePtr>& handles) {
	DblVec theta = getDblVec(x, m_theta_vars);
	MatrixXd sigma_pts = m_rad->sigmaPoints(toVectorXd(theta));
	vector<DblVec> dofvals(sigma_pts.cols());
	for (int i=0; i<sigma_pts.cols(); i++) {
		dofvals[i] = toDblVec(sigma_pts.col(i));
	}
	m_cc->PlotCastHull(*m_rad, m_links, dofvals, handles);
}
//////////////////////////////////////////

typedef OpenRAVE::RaveVector<float> RaveVectorf;

void PlotCollisions(const std::vector<Collision>& collisions, OR::EnvironmentBase& env, vector<OR::GraphHandlePtr>& handles, double safe_dist) {
  BOOST_FOREACH(const Collision& col, collisions) {
    RaveVectorf color;
    if (col.distance < 0) color = RaveVectorf(1,0,0,1);
    else if (col.distance < safe_dist) color = RaveVectorf(1,1,0,1);
    else color = RaveVectorf(0,1,0,1);
    handles.push_back(env.drawarrow(col.ptA, col.ptB, .0025, color));
  }
}

CollisionCost::CollisionCost(double dist_pen, double coeff, RobotAndDOFPtr rad, const VarVector& vars) :
    Cost("collision"),
    m_dist_pen(dist_pen),
    m_coeff(coeff)
{
	if (vars.size() == rad->GetDOF()) {
		m_calc = CollisionEvaluatorPtr(new SingleTimestepCollisionEvaluator(rad, vars));
	} else {
		BeliefRobotAndDOFPtr brad = boost::static_pointer_cast<BeliefRobotAndDOF>(rad);
		m_calc = CollisionEvaluatorPtr(new SigmaPtsCollisionEvaluator(brad, vars));
	}
}

CollisionCost::CollisionCost(double dist_pen, double coeff, RobotAndDOFPtr rad, const VarVector& vars0, const VarVector& vars1) :
    Cost("cast_collision"),
    m_calc(new CastCollisionEvaluator(rad, vars0, vars1)), m_dist_pen(dist_pen), m_coeff(coeff)
{}

ConvexObjectivePtr CollisionCost::convex(const vector<double>& x, Model* model) {
  ConvexObjectivePtr out(new ConvexObjective(model));
  vector<AffExpr> exprs;
  DblVec weights;
  m_calc->CalcDistExpressions(x, exprs, weights);
  for (int i=0; i < exprs.size(); ++i) {
    AffExpr viol = exprSub(AffExpr(m_dist_pen), exprs[i]);
    out->addHinge(viol, m_coeff*weights[i]);
  }
  return out;
}
double CollisionCost::value(const vector<double>& x) {
  DblVec dists, weights;
  m_calc->CalcDists(x, dists, weights);
  double out = 0;
  for (int i=0; i < dists.size(); ++i) {
    out += pospart(m_dist_pen - dists[i]) * m_coeff * weights[i];
  }
  return out;
}

void CollisionCost::Plot(const DblVec& x, OR::EnvironmentBase& env, std::vector<OR::GraphHandlePtr>& handles) {
  vector<Collision> collisions;
  m_calc->GetCollisionsCached(x, collisions);
  PlotCollisions(collisions, env, handles, m_dist_pen);
  m_calc->CustomPlot(x, handles);
}

CollisionConstraint::CollisionConstraint(double dist_pen, double coeff, RobotAndDOFPtr rad, const VarVector& vars) :
    Constraint("collision"), m_dist_pen(dist_pen), m_coeff(coeff), type_(INEQ)
{
	if (vars.size() == rad->GetDOF()) {
		m_calc = CollisionEvaluatorPtr(new SingleTimestepCollisionEvaluator(rad, vars));
	} else {
		BeliefRobotAndDOFPtr brad = boost::static_pointer_cast<BeliefRobotAndDOF>(rad);
		m_calc = CollisionEvaluatorPtr(new SigmaPtsCollisionEvaluator(brad, vars));
	}
}
CollisionConstraint::CollisionConstraint(double dist_pen, double coeff, RobotAndDOFPtr rad, const VarVector& vars0, const VarVector& vars1) :
		Constraint("cast_collision"), m_calc(new CastCollisionEvaluator(rad, vars0, vars1)), m_dist_pen(dist_pen), m_coeff(coeff), type_(INEQ)
{}
ConvexConstraintsPtr CollisionConstraint::convex(const vector<double>& x, Model* model) {
	ConvexConstraintsPtr out(new ConvexConstraints(model));
  vector<AffExpr> exprs;
  DblVec weights;
  m_calc->CalcDistExpressions(x, exprs, weights);
  for (int i=0; i < exprs.size(); ++i) {
    AffExpr aff = exprMult(exprSub(AffExpr(m_dist_pen), exprs[i]), m_coeff * weights[i]);
		out->addIneqCnt(aff);
  }
  return out;
}
vector<double> CollisionConstraint::value(const vector<double>& x) {
  DblVec dists, weights;
  m_calc->CalcDists(x, dists, weights);
  vector<double> out(dists.size());
  for (int i=0; i < dists.size(); ++i) {
    out[i] = (m_dist_pen - dists[i]) * m_coeff * weights[i];
  }
  return out;
}
void CollisionConstraint::Plot(const DblVec& x, OR::EnvironmentBase& env, std::vector<OR::GraphHandlePtr>& handles) {
  vector<Collision> collisions;
  m_calc->GetCollisionsCached(x, collisions);
  PlotCollisions(collisions, env, handles, m_dist_pen);
  m_calc->CustomPlot(x, handles);
}

}


#ifndef STAN_MATH_REV_MAT_FUN_TRACE_GEN_INV_QUAD_FORM_LDLT_HPP
#define STAN_MATH_REV_MAT_FUN_TRACE_GEN_INV_QUAD_FORM_LDLT_HPP

#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/rev/core.hpp>
#include <stan/math/prim/scal/meta/is_var.hpp>
#include <stan/math/rev/scal/meta/is_var.hpp>
#include <boost/utility/enable_if.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#include <stan/math/rev/mat/fun/trace_inv_quad_form_ldlt.hpp>

namespace stan {
  namespace math {

    /**
     * Compute the trace of an inverse quadratic form.  I.E., this computes
     *       trace(D B^T A^-1 B)
     * where D is a square matrix and the LDLT_factor of A is provided.
     **/
    template <typename T1, int R1, int C1, typename T2, int R2, int C2,
              typename T3, int R3, int C3>
    inline typename
    boost::enable_if_c<stan::is_var<T1>::value ||
    stan::is_var<T2>::value ||
    stan::is_var<T3>::value, var>::type
      trace_gen_inv_quad_form_ldlt(const Eigen::Matrix<T1, R1, C1> &D,
                                   const LDLT_factor<T2, R2, C2> &A,
                                   const Eigen::Matrix<T3, R3, C3> &B) {
      check_square("trace_gen_inv_quad_form_ldlt", "D", D);
      check_multiplicable("trace_gen_inv_quad_form_ldlt",
                          "A", A,
                          "B", B);
      check_multiplicable("trace_gen_inv_quad_form_ldlt",
                          "B", B,
                          "D", D);

      trace_inv_quad_form_ldlt_impl<T2, R2, C2, T3, R3, C3> *_impl
        = new trace_inv_quad_form_ldlt_impl<T2, R2, C2, T3, R3, C3>(D, A, B);

      return var(new trace_inv_quad_form_ldlt_vari<T2, R2, C2, T3, R3, C3>
                 (_impl));
    }

  }
}
#endif


//
// Copyright Jesse Manning 2007
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_GELSD_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_GELSD_HPP

#include <algorithm>

#include <boost/numeric/bindings/traits/type.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>
#include <boost/numeric/bindings/traits/detail/utils.hpp>
#include <boost/numeric/bindings/lapack/ilaenv.hpp>

// included to implicitly convert a vector to an nx1 matrix
// so that it is compatible with lapack binding
#include <boost/numeric/bindings/traits/ublas_vector2.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

	namespace lapack {

		namespace detail {

			inline void gelsd(const int m, const int n, const int nrhs,
							  float *a, const int lda, float *b, const int ldb,
							  float *s, const float rcond, int *rank, float *work,
							  const int lwork, int *iwork, int *info)
			{
				LAPACK_SGELSD(&m, &n, &nrhs, a, &lda, b, &ldb, s,
							  &rcond, rank, work, &lwork, iwork, info);
			}

			inline void gelsd(const int m, const int n, const int nrhs,
							  double *a, const int lda, double *b, const int ldb,
							  double *s, const double rcond, int *rank, double *work,
							  const int lwork, int *iwork, int *info)
			{
				LAPACK_DGELSD(&m, &n, &nrhs, a, &lda, b, &ldb, s,
							  &rcond, rank, work, &lwork, iwork, info);
			}

			inline void gelsd(const int m, const int n, const int nrhs,
							  traits::complex_f *a, const int lda, traits::complex_f *b,
							  const int ldb, float *s, const float rcond, int *rank,
							  traits::complex_f *work, const int lwork, float *rwork,
							  int *iwork, int *info)
			{
				LAPACK_CGELSD(&m, &n, &nrhs, traits::complex_ptr(a),
							  &lda, traits::complex_ptr(b), &ldb, s,
							  &rcond, rank, traits::complex_ptr(work),
							  &lwork, rwork, iwork, info);
			}

			inline void gelsd(const int m, const int n, const int nrhs,
							  traits::complex_d *a, const int lda, traits::complex_d *b,
							  const int ldb, double *s, const double rcond, int *rank,
							  traits::complex_d *work, const int lwork, double *rwork,
							  int *iwork, int *info)
			{
				LAPACK_ZGELSD(&m, &n, &nrhs, traits::complex_ptr(a),
							  &lda, traits::complex_ptr(b), &ldb, s,
							  &rcond, rank, traits::complex_ptr(work),
							  &lwork, rwork, iwork, info);
			}

			// gelsd for real type
			template <typename MatrA, typename MatrB, typename VecS, typename Work>
			inline int gelsd(MatrA& A, MatrB& B, VecS& s, Work& work)
			{
				typedef typename MatrA::value_type val_t;
				typedef typename traits::type_traits<val_t>::real_type real_t;

				const int m = traits::matrix_size1(A);
				const int n = traits::matrix_size2(A);
				const int nrhs = traits::matrix_size2(B);
				const int maxmn = std::max(m, n);
				const int minmn = std::min(m, n);

				// sanity checks
				assert(m >= 0 && n >= 0);
				assert(nrhs >= 0);
				assert(traits::leading_dimension(A) >= std::max(1, m));
				assert(traits::leading_dimension(B) >= std::max(1, maxmn));
				assert(traits::vector_size(work) >= 1);
				assert(traits::vector_size(s) >= std::max(1, minmn));

				int info;
				const real_t rcond = -1;	// use machine precision
				int rank;

				// query for maximum size of subproblems
				const int smlsiz = ilaenv(9, "GELSD", "");
				const int nlvl = static_cast<int>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

				traits::detail::array<int> iwork(3*minmn*nlvl + 11*minmn);

				detail::gelsd(traits::matrix_size1(A),
							  traits::matrix_size2(A),
							  traits::matrix_size2(B),
							  traits::matrix_storage(A),
							  traits::leading_dimension(A),
							  traits::matrix_storage(B),
							  traits::leading_dimension(B),
							  traits::vector_storage(s),
							  rcond,
							  &rank,
							  traits::vector_storage(work),
							  traits::vector_size(work),
							  traits::vector_storage(iwork),
							  &info);

				return info;
			}

			// gelsd for complex type
			template <typename MatrA, typename MatrB, typename VecS,
						typename Work, typename RWork>
			inline int gelsd(MatrA& A, MatrB& B, VecS& s, Work& work, RWork& rwork)
			{
				typedef typename MatrA::value_type val_t;
				typedef typename traits::type_traits<val_t>::real_type real_t;

				const int m = traits::matrix_size1(A);
				const int n = traits::matrix_size2(A);
				const int nrhs = traits::matrix_size2(B);
				const int maxmn = std::max(m, n);
				const int minmn = std::min(m, n);

				// sanity checks
				assert(m >= 0 && n >= 0);
				assert(nrhs >= 0);
				assert(traits::leading_dimension(A) >= std::max(1, m));
				assert(traits::leading_dimension(B) >= std::max(1, maxmn));
				assert(traits::vector_size(work) >= 1);
				assert(traits::vector_size(s) >= std::max(1, minmn));

				int info;
				const real_t rcond = -1;	// use machine precision
				int rank;

				// query for maximum size of subproblems
				const int smlsiz = ilaenv(9, "GELSD", "");
				const int nlvl = static_cast<int>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

				traits::detail::array<int> iwork(3*minmn*nlvl + 11*minmn);

				detail::gelsd(traits::matrix_size1(A),
					traits::matrix_size2(A),
					traits::matrix_size2(B),
					traits::matrix_storage(A),
					traits::leading_dimension(A),
					traits::matrix_storage(B),
					traits::leading_dimension(B),
					traits::vector_storage(s),
					rcond,
					&rank,
					traits::vector_storage(work),
					traits::vector_size(work),
					traits::vector_storage(rwork),
					traits::vector_storage(iwork),
					&info);

				return info;
			}

			template <int N>
			struct Gelsd { };

			// specialization for gelsd real flavors (sgelsd, dgelsd)
			template <>
			struct Gelsd<1>
			{
				template <typename MatrA, typename MatrB, typename VecS>
				inline int operator() (MatrA& A, MatrB& B, VecS& s, minimal_workspace) const
				{
					typedef typename MatrA::value_type val_t;

					const int m = traits::matrix_size1(A);
					const int n = traits::matrix_size2(A);
					const int nrhs = traits::matrix_size2(B);

					const int minmn = std::min(m, n);			// minmn = m < n ? m : n
					const int maxmn = std::max(m, n);			// maxmn = m > n ? m : n
					const int maxmnr = std::max(maxmn, nrhs);	// maxmnr = maxmn > nrhs ? maxmn : nrhs

					// query for maximum size of subproblems
					const int smlsiz = ilaenv(9, "GELSD", "");
					const int nlvl = static_cast<int>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

					const int lwork = 12*minmn + 2*minmn*smlsiz + 8*minmn*nlvl +
							     	  minmn*nrhs + (smlsiz+1)*(smlsiz+1);

					traits::detail::array<val_t> work(lwork);

					return gelsd(A, B, s, work);
				}

				template <typename MatrA, typename MatrB, typename VecS>
				inline int operator() (MatrA& A, MatrB& B, VecS& s, optimal_workspace) const
				{
					typedef typename MatrA::value_type val_t;
					typedef typename traits::type_traits<val_t>::real_type real_t;

					const int m = traits::matrix_size1(A);
					const int n = traits::matrix_size2(A);
					const int nrhs = traits::matrix_size2(B);

					const int minmn = std::min(m, n);			// minmn = m < n ? m : n
					const int maxmn = std::max(m, n);			// maxmn = m > n ? m : n
					const int maxmnr = std::max(maxmn, nrhs);	// maxmnr = maxmn > nrhs ? maxmn : nrhs

					val_t temp_work;
					int temp_iwork;

					const real_t rcond = -1;
					int rank;
					int info;

					// query for optimal workspace size
					detail::gelsd(traits::matrix_size1(A),
								  traits::matrix_size2(A),
								  traits::matrix_size2(B),
								  traits::matrix_storage(A),
								  traits::leading_dimension(A),
								  traits::matrix_storage(B),
								  traits::leading_dimension(B),
								  traits::vector_storage(s),
								  rcond,
								  &rank,
								  &temp_work,	//traits::vector_storage(work),
								  -1,			//traits::vector_size(work),
								  &temp_iwork,
								  &info);

					assert(info == 0);

					const int lwork = traits::detail::to_int(temp_work);

					traits::detail::array<val_t> work(lwork);

					return gelsd(A, B, s, work);
				}

				template <typename MatrA, typename MatrB, typename VecS, typename Work>
				inline int operator() (MatrA& A, MatrB& B, VecS& s, detail::workspace1<Work>& workspace) const
				{
					const int m = traits::matrix_size1(A);
					const int n = traits::matrix_size2(A);
					const int nrhs = traits::matrix_size2(B);

					const int minmn = std::min(m, n);			// minmn = m < n ? m : n
					const int maxmn = std::max(m, n);			// maxmn = m > n ? m : n
					const int maxmnr = std::max(maxmn, nrhs);	// maxmnr = maxmn > nrhs ? maxmn : nrhs

					return gelsd(A, B, s, workspace.w_);
				}
			};

			// specialization for gelsd (cgelsd, zgelsd)
			template <>
			struct Gelsd<2>
			{
				template <typename MatrA, typename MatrB, typename VecS>
				inline int operator() (MatrA& A, MatrB& B, VecS& s, minimal_workspace) const
				{
					typedef typename MatrA::value_type val_t;
					typedef typename traits::type_traits<val_t>::real_type real_t;

					const int m = traits::matrix_size1(A);
					const int n = traits::matrix_size2(A);
					const int nrhs = traits::matrix_size2(B);

					const int minmn = std::min(m, n);			// minmn = m < n ? m : n
					const int maxmn = std::max(m, n);			// maxmn = m > n ? m : n
					const int maxmnr = std::max(maxmn, nrhs);	// maxmnr = maxmn > nrhs ? maxmn : nrhs

					// query for maximum size of subproblems
					const int smlsiz = ilaenv(9, "GELSD", "");
					const int nlvl = static_cast<int>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

					traits::detail::array<val_t> work(2*minmn + minmn*nrhs);

					const int rwork_size = 10*minmn + 2*minmn*smlsiz + 8*minmn*nlvl +
										   3*smlsiz*nrhs + (smlsiz+1)*(smlsiz+1);

					traits::detail::array<real_t> rwork(std::max(1, rwork_size));

					return gelsd(A, B, s, work, rwork);
				}

				template <typename MatrA, typename MatrB, typename VecS>
				inline int operator() (MatrA& A, MatrB& B, VecS& s, optimal_workspace) const
				{
					typedef typename MatrA::value_type val_t;
					typedef typename traits::type_traits<val_t>::real_type real_t;

					const int m = traits::matrix_size1(A);
					const int n = traits::matrix_size2(A);
					const int nrhs = traits::matrix_size2(B);

					const int minmn = std::min(m, n);			// minmn = m < n ? m : n
					const int maxmn = std::max(m, n);			// maxmn = m > n ? m : n
					const int maxmnr = std::max(maxmn, nrhs);	// maxmnr = maxmn > nrhs ? maxmn : nrhs

					val_t temp_work;
					real_t temp_rwork;
					int temp_iwork;

					const real_t rcond = -1;
					int rank;
					int info;

					// query for optimal workspace size
					detail::gelsd(traits::matrix_size1(A),
								  traits::matrix_size2(A),
								  traits::matrix_size2(B),
								  traits::matrix_storage(A),
								  traits::leading_dimension(A),
								  traits::matrix_storage(B),
								  traits::leading_dimension(B),
								  traits::vector_storage(s),
								  rcond,
								  &rank,
								  &temp_work,	//traits::vector_storage(work),
								  -1,			//traits::vector_size(work),
								  &temp_rwork,
								  &temp_iwork,
								  &info);

					assert(info == 0);

					const int lwork = traits::detail::to_int(temp_work);

					traits::detail::array<val_t> work(lwork);

					// query for maximum size of subproblems
					const int smlsiz = ilaenv(9, "GELSD", "");
					const int nlvl = static_cast<int>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

					const int rwork_size = 10*minmn + 2*minmn*smlsiz + 8*minmn*nlvl +
											3*smlsiz*nrhs + (smlsiz+1)*(smlsiz+1);

					traits::detail::array<real_t> rwork(std::max(1, rwork_size));

					return gelsd(A, B, s, work, rwork);
				}

				template <typename MatrA, typename MatrB, typename VecS, typename Work, typename RWork>
				inline int operator() (MatrA& A, MatrB& B, VecS& s, detail::workspace2<Work, RWork>& workspace) const
				{
					const int m = traits::matrix_size1(A);
					const int n = traits::matrix_size2(A);
					const int nrhs = traits::matrix_size2(B);

					const int minmn = std::min(m, n);			// minmn = m < n ? m : n
					const int maxmn = std::max(m, n);			// maxmn = m > n ? m : n
					const int maxmnr = std::max(maxmn, nrhs);	// maxmnr = maxmn > nrhs ? maxmn : nrhs

					return gelsd(A, B, s, workspace.w_, workspace.wr_);
				}
			};

		} // detail

		// gelsd
		// Parameters:
		//	A:			matrix of coefficients
		//	B:			matrix of solutions (stored column-wise)
		//	s:			vector to store singular values on output, length >= max(1, min(m,n))
		//  workspace:	either optimal, minimal, or user supplied
		//
		template <typename MatrA, typename MatrB, typename VecS, typename Work>
		inline int gelsd(MatrA& A, MatrB& B, VecS& s, Work& workspace)
		{
			typedef typename MatrA::value_type val_t;

			return detail::Gelsd<n_workspace_args<val_t>::value>() (A, B, s, workspace);
		}

		// gelsd, no singular values are returned
		// Parameters:
		//	A:			matrix of coefficients
		//	B:			matrix of solutions (stored column-wise)
		//	workspace:	either optimal, minimal, or user supplied
		//
		template <typename MatrA, typename MatrB, typename Work>
		inline int gelsd(MatrA& A, MatrB& B, Work& workspace)
		{
			typedef typename MatrA::value_type val_t;
			typedef typename traits::type_traits<val_t>::real_type real_t;

			const int m = traits::matrix_size1(A);
			const int n = traits::matrix_size2(A);

			const int s_size = std::max(1, std::min(m,n));
			traits::detail::array<real_t> s(s_size);

			return detail::Gelsd<n_workspace_args<val_t>::value>() (A, B, s, workspace);
		}

	} // lapack

}}}

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_INTERSECTION_HPP
#define BOOST_GEOMETRY_ALGORITHMS_INTERSECTION_HPP

#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/algorithms/detail/overlay/intersection_insert.hpp>
#include <boost/geometry/algorithms/intersects.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace intersection
{

template
<
    typename Box1, typename Box2,
    typename BoxOut,
    typename Strategy,
    std::size_t Dimension, std::size_t DimensionCount
>
struct intersection_box_box
{
    static inline bool apply(Box1 const& box1,
            Box2 const& box2, BoxOut& box_out,
            Strategy const& strategy)
    {
        typedef typename coordinate_type<BoxOut>::type ct;

        ct min1 = get<min_corner, Dimension>(box1);
        ct min2 = get<min_corner, Dimension>(box2);
        ct max1 = get<max_corner, Dimension>(box1);
        ct max2 = get<max_corner, Dimension>(box2);

        if (max1 < min2 || max2 < min1)
        {
            return false;
        }
        // Set dimensions of output coordinate
        set<min_corner, Dimension>(box_out, min1 < min2 ? min2 : min1);
        set<max_corner, Dimension>(box_out, max1 > max2 ? max2 : max1);

        return intersection_box_box
            <
                Box1, Box2, BoxOut, Strategy,
                Dimension + 1, DimensionCount
            >::apply(box1, box2, box_out, strategy);
    }
};

template
<
    typename Box1, typename Box2,
    typename BoxOut,
    typename Strategy,
    std::size_t DimensionCount
>
struct intersection_box_box<Box1, Box2, BoxOut, Strategy, DimensionCount, DimensionCount>
{
    static inline bool apply(Box1 const&, Box2 const&, BoxOut&, Strategy const&)
    {
        return true;
    }
};

}} // namespace detail::intersection
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// By default, all is forwarded to the intersection_insert-dispatcher
template
<
    typename Tag1, typename Tag2, typename TagOut,
    typename Geometry1, typename Geometry2,
    typename GeometryOut,
    typename Strategy
>
struct intersection
{
    typedef std::back_insert_iterator<GeometryOut> output_iterator;

    static inline bool apply(Geometry1 const& geometry1,
            Geometry2 const& geometry2,
            GeometryOut& geometry_out,
            Strategy const& strategy)
    {
        typedef typename boost::range_value<GeometryOut>::type OneOut;

        intersection_insert
        <
            Tag1, Tag2, typename geometry::tag<OneOut>::type,
            geometry::is_areal<Geometry1>::value,
            geometry::is_areal<Geometry2>::value,
            geometry::is_areal<OneOut>::value,
            Geometry1, Geometry2,
            detail::overlay::do_reverse<geometry::point_order<Geometry1>::value, false>::value,
            detail::overlay::do_reverse<geometry::point_order<Geometry2>::value, false>::value,
            detail::overlay::do_reverse<geometry::point_order<OneOut>::value>::value,
            output_iterator, OneOut,
            overlay_intersection,
            Strategy
        >::apply(geometry1, geometry2, std::back_inserter(geometry_out), strategy);

        return true;
    }

};

template
<
    typename Box1, typename Box2,
    typename BoxOut,
    typename Strategy
>
struct intersection
    <
        box_tag, box_tag, box_tag,
        Box1, Box2, BoxOut,
        Strategy
    > : public detail::intersection::intersection_box_box
            <
                Box1, Box2, BoxOut,
                Strategy,
                0, geometry::dimension<Box1>::value
            >
{};

template
<
    typename Tag1, typename Tag2, typename TagOut,
    typename Geometry1, typename Geometry2,
    typename GeometryOut,
    typename Strategy
>
struct intersection_reversed
{
    static inline bool apply(Geometry1 const& geometry1,
            Geometry2 const& geometry2,
            GeometryOut& geometry_out,
            Strategy const& strategy)
    {
        return intersection
            <
                Tag2, Tag1, TagOut,
                Geometry2, Geometry1,
                GeometryOut, Strategy
            >::apply(geometry2, geometry1, geometry_out, strategy);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

/*!
\brief \brief_calc2{intersection}
\ingroup intersection
\details \details_calc2{intersection, spatial set theoretic intersection}.
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam GeometryOut Collection of geometries (e.g. std::vector, std::deque, boost::geometry::multi*) of which
    the value_type fulfills a \p_l_or_c concept, or it is the output geometry (e.g. for a box)
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param geometry_out The output geometry, either a multi_point, multi_polygon,
    multi_linestring, or a box (for intersection of two boxes)

\qbk{[include reference/algorithms/intersection.qbk]}
*/
template
<
    typename Geometry1,
    typename Geometry2,
    typename GeometryOut
>
inline bool intersection(Geometry1 const& geometry1,
            Geometry2 const& geometry2,
            GeometryOut& geometry_out)
{
    concept::check<Geometry1 const>();
    concept::check<Geometry2 const>();

    typedef strategy_intersection
        <
            typename cs_tag<Geometry1>::type,
            Geometry1,
            Geometry2,
            typename geometry::point_type<Geometry1>::type
        > strategy;

    return boost::mpl::if_c
        <
            geometry::reverse_dispatch<Geometry1, Geometry2>::type::value,
            dispatch::intersection_reversed
            <
                    typename geometry::tag<Geometry1>::type,
                    typename geometry::tag<Geometry2>::type,
                    typename geometry::tag<GeometryOut>::type,
                    Geometry1, Geometry2, GeometryOut, strategy
            >,
            dispatch::intersection
            <
                    typename geometry::tag<Geometry1>::type,
                    typename geometry::tag<Geometry2>::type,
                    typename geometry::tag<GeometryOut>::type,
                    Geometry1, Geometry2, GeometryOut, strategy
            >
        >::type::apply(geometry1, geometry2, geometry_out, strategy());
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_INTERSECTION_HPP


/*
 *  carpack.cpp
 *  carpack
 *
 *  Created by Brandon Kelly on 12/19/12.
 *
 *  Method definitions of classes for MCMC sampling from continuous time
 *  autoregressive (CAR) models.
 *
 */

// Standard includes
#include <complex>
#include <iostream>

// Armadillo includes
#include <armadillo>

// Boost includes
#include <boost/math/special_functions/binomial.hpp>

// Local includes
#include "include/carpack.hpp"

// Global random number generator object, instantiated in random.cpp
extern boost::random::mt19937 rng;

// Object containing some common random number generators.
extern RandomGenerator RandGen;

/********************************************************************
						METHODS OF CAR1 CLASS
 *******************************************************************/

// Method of CAR1 class to generate the starting values of the
// parameters theta = (mu, sigma, measerr_scale, log(omega)).

arma::vec CAR1::StartingValue()
{
	double log_omega_start, car1_stdev_start, sigma;

	// Initialize the standard deviation of the CAR(1) process
	// by drawing from its prior
	car1_stdev_start = RandGen.scaled_inverse_chisqr(y_.n_elem-1, arma::var(y_));
	car1_stdev_start = sqrt(car1_stdev_start);

    // Get initial value of the time series mean
    double mu = RandGen.normal(arma::mean(y_), car1_stdev_start / y_.n_elem);

	// Initialize log(omega) to log( 1 / (a * median(dt)) ), where
	// a ~ Uniform(1,50) , under the constraint that
	// tau = 1 / omega < max(time)

    arma::vec dt = time_(arma::span(1,time_.n_elem-1)) - time_(arma::span(0,time_.n_elem-2));
	log_omega_start = -1.0 * log(arma::median(dt) * RandGen.uniform( 1.0, 50.0 ));
	log_omega_start = std::min(log_omega_start, max_freq_);

	sigma = car1_stdev_start * sqrt(2.0 * exp(log_omega_start));

	// Get initial value of the measurement error scaling parameter by
	// drawing from its prior.

	double measerr_scale = RandGen.scaled_inverse_chisqr(measerr_dof_, 1.0);
    measerr_scale = std::min(measerr_scale, 1.99);
    measerr_scale = std::max(measerr_scale, 0.51);

	arma::vec theta(4);

	theta << car1_stdev_start << measerr_scale << mu << log_omega_start << arma::endr;

	// Initialize the Kalman filter
    pKFilter_->SetOmega(exp(log_omega_start));
    pKFilter_->SetSigsqr(sigma * sigma);
    arma::vec proposed_yerr = sqrt(measerr_scale) * yerr_;
    pKFilter_->SetTimeSeriesErr(proposed_yerr);
    arma::vec ycent = y_ - mu;
    pKFilter_->SetTimeSeries(ycent);
    pKFilter_->Filter();

	return theta;
}

arma::vec CAR1::SetStartingValue(arma::vec init)
{
   if (init.n_elem != 4) {
      std::cout << "WARNING: initial guess not length 4, initializing with prior" << std::endl;
      return StartingValue();
   }

   double logpost = LogDensity(init);
   bool good_initials = arma::is_finite(logpost);
   if (good_initials == false) {
      std::cout << "WARNING: initial guess yields non-finite likelihood, initializing with prior" << std::endl;
      return StartingValue();
   }

   double mu, log_omega_start, car1_stdev_start, sigma, measerr_scale;
   car1_stdev_start = init[0];
   measerr_scale    = init[1];
   mu               = init[2];
   log_omega_start  = init[3];
   sigma            = car1_stdev_start * sqrt(2.0 * exp(log_omega_start));

   pKFilter_->SetOmega(exp(log_omega_start));
   pKFilter_->SetSigsqr(sigma * sigma);
   arma::vec proposed_yerr = sqrt(measerr_scale) * yerr_;
   pKFilter_->SetTimeSeriesErr(proposed_yerr);
   arma::vec ycent = y_ - mu;
   pKFilter_->SetTimeSeries(ycent);
   pKFilter_->Filter();

   return init;
}

bool CAR1::CheckPriorBounds(arma::vec theta)
{
    double ysigma = theta(0);
    double measerr_scale = theta(1);
    double omega = exp(theta(3));

    bool prior_satisfied = true;
    if ( (omega > max_freq_) || (omega < min_freq_) ||
        (ysigma > max_stdev_) || (ysigma < 0) ||
        (measerr_scale < 0.5) || (measerr_scale > 2.0) ) {
		// prior bounds not satisfied
		prior_satisfied = false;
	}
    return prior_satisfied;
}

/********************************************************************
                        METHODS OF CARp CLASS
 *******************************************************************/

// Calculate the roots of the AR(p) polynomial from the parameters
arma::cx_vec CARp::ARRoots(arma::vec theta)
{
    arma::cx_vec ar_roots(p_);

    // Construct the complex vector of roots of the characteristic polynomial:
    // alpha(s) = s^p + alpha_1 s^{p-1} + ... + alpha_{p-1} s + alpha_p
    for (int i=0; i<p_/2; i++) {
        // alpha(s) decomposed into its quadratic terms:
        //   alpha(s) = (quad_term1 + quad_term2 * s + s^2) * ...
        double quad_term1 = exp(theta(3+2*i));
        double quad_term2 = exp(theta(3+2*i+1));

        double discriminant = quad_term2 * quad_term2 - 4.0 * quad_term1;

        if (discriminant > 0) {
            // two real roots
            double root1 = -0.5 * (quad_term2 + sqrt(discriminant));
            double root2 = -0.5 * (quad_term2 - sqrt(discriminant));
            ar_roots(2*i) = std::complex<double> (root1, 0.0);
            ar_roots(2*i+1) = std::complex<double> (root2, 0.0);
        } else {
            double real_part = -0.5 * quad_term2;
            double imag_part = -0.5 * sqrt(-discriminant);
            ar_roots(2*i) = std::complex<double> (real_part, imag_part);
            ar_roots(2*i+1) = std::complex<double> (real_part, -imag_part);
        }
    }

    if ((p_ % 2) == 1) {
        // p is odd, so add in additional low-frequency component
        double real_root = -exp(theta(3+p_-1));
        ar_roots(p_-1) = std::complex<double> (real_root, 0.0);
    }

    return ar_roots;
}

// Return the starting value and set log_posterior_
arma::vec CARp::StartingValue()
{
    // Create the parameter vector, theta
    arma::vec theta(p_+3);

    bool good_initials = false;
    int iguess_count = 0;
    while (!good_initials) {

        arma::vec loga = StartingAR();
        for (int i=0; i<p_; i++) {
            theta(3+i) = loga(i);
        }

        // Initial guess for model standard deviation is randomly distributed
        // around measured standard deviation of the time series
        double yvar = RandGen.scaled_inverse_chisqr(y_.n_elem-1, arma::var(y_));

        // Get initial value of the time series mean
        double mu = RandGen.normal(arma::mean(y_), sqrt(yvar) / y_.n_elem);

        arma::cx_vec alpha_roots = ARRoots(theta);
        double sigsqr = yvar / Variance(alpha_roots, ma_coefs_, 1.0);

        // Get initial value of the measurement error scaling parameter by
        // drawing from its prior.
        double measerr_scale = RandGen.scaled_inverse_chisqr(measerr_dof_, 1.0);
        measerr_scale = std::min(measerr_scale, 1.99);
        measerr_scale = std::max(measerr_scale, 0.51);

        theta(0) = sqrt(yvar);
        theta(1) = measerr_scale;
        theta(2) = mu;

        // set the Kalman filter parameters
        pKFilter_->SetSigsqr(sigsqr);
        pKFilter_->SetOmega(ExtractAR(theta));
        arma::vec proposed_yerr = sqrt(measerr_scale) * yerr_;
        pKFilter_->SetTimeSeriesErr(proposed_yerr);
        arma::vec ycent = y_ - mu;
        pKFilter_->SetTimeSeries(ycent);

        // run the kalman filter
        pKFilter_->Filter();

        double logpost = LogDensity(theta);
        good_initials = arma::is_finite(logpost);

        iguess_count++;
        if (iguess_count > 200) {
            std::cout << "Tried 200 initial guesses, still trying..." << std::endl;
        }
    } // continue loop until the starting values give us a finite posterior

    return theta;
}

// Return the starting value and set log_posterior_
arma::vec CARp::SetStartingValue(arma::vec init)
{

   if (init.n_elem != (p_+3)) {
      std::cout << "WARNING: initial guess wrong length, initializing with prior" << std::endl;
      return StartingValue();
   }

   double logpost = LogDensity(init);
   bool good_initials = arma::is_finite(logpost);
   if (good_initials == false) {
      std::cout << "WARNING: initial guess yields non-finite likelihood, initializing with prior" << std::endl;
      return StartingValue();
   }

   double yvar = init(0)*init(0);
   double measerr_scale = init(1);
   double mu = init(2);

   arma::cx_vec alpha_roots = ARRoots(init);
   double sigsqr = yvar / Variance(alpha_roots, ma_coefs_, 1.0);

   // set the Kalman filter parameters
   pKFilter_->SetSigsqr(sigsqr);
   pKFilter_->SetOmega(ExtractAR(init));
   arma::vec proposed_yerr = sqrt(measerr_scale) * yerr_;
   pKFilter_->SetTimeSeriesErr(proposed_yerr);
   arma::vec ycent = y_ - mu;
   pKFilter_->SetTimeSeries(ycent);
   pKFilter_->Filter();

   return init;
}

// return the starting values for the autoregressive polynomial paramters
arma::vec CARp::StartingAR() {
    double min_freq = 1.0 / (time_.max() - time_.min());

    // Obtain initial values for Lorentzian centroids (= system frequencies) and
    // widths (= break frequencies)
    arma::vec lorentz_cent((p_+1)/2);
    lorentz_cent.randu();
    lorentz_cent = log(max_freq_ / min_freq) * lorentz_cent + log(min_freq);
    lorentz_cent = arma::exp(lorentz_cent);

    // Force system frequencies to be in descending order to make the model identifiable
    lorentz_cent = arma::sort(lorentz_cent, 1);

    arma::vec lorentz_width((p_+1)/2);
    lorentz_width.randu();
    lorentz_width = log(max_freq_ / min_freq) * lorentz_width + log(min_freq);
    lorentz_width = arma::exp(lorentz_width);

    if ((p_ % 2) == 1) {
        // p is odd, so add additional low-frequency component
        lorentz_cent(p_/2) = 0.0;
        // make initial break frequency of low-frequency component less than minimum
        // value of the system frequencies
        lorentz_width(p_/2) = exp(RandGen.uniform(log(min_freq), log(lorentz_cent(p_/2-1))));
    }

    arma::vec loga(p_);

    // convert the PSD lorentzian parameters to quadratic terms in the AR polynomial decomposition
    for (int i=0; i<p_/2; i++) {
        double real_part = -2.0 * arma::datum::pi * lorentz_width(i);
        double imag_part = 2.0 * arma::datum::pi * lorentz_cent(i);
        double quad_term1 = real_part * real_part + imag_part * imag_part;
        double quad_term2 = -2.0 * real_part;
        loga(2*i) = log(quad_term1);
        loga(1+2*i) = log(quad_term2);
    }
    if ((p_ % 2) == 1) {
        // p is odd, so add in additional value of lorentz_width
        double real_part = -2.0 * arma::datum::pi * lorentz_width(p_/2);
        loga(p_-1) = log(-real_part);
    }
    return loga;
}

// check prior bounds
bool CARp::CheckPriorBounds(arma::vec theta)
{
    if (ignore_prior_) {return true;}

    double ysigma = theta(0);
    double measerr_scale = theta(1);
    arma::cx_vec ar_roots = ExtractAR(theta);

    arma::vec lorentz_cent = arma::abs(arma::imag(ar_roots)) / 2.0 / arma::datum::pi;
    arma::vec lorentz_width = -arma::real(ar_roots) / 2.0 / arma::datum::pi;

    // Find the set of Frequencies satisfying the prior bounds
    arma::uvec valid_frequencies1 = arma::find(lorentz_cent < max_freq_);
	arma::uvec valid_frequencies2 = arma::find(lorentz_width < max_freq_);
    arma::uvec valid_frequencies3 = arma::find(lorentz_width > min_freq_);

    double tol = 1e-4;
    bool prior_satisfied = unique_roots(ar_roots, tol); // are the roots unique?

    if ( (valid_frequencies1.n_elem != lorentz_cent.n_elem) ||
        (valid_frequencies2.n_elem != lorentz_width.n_elem) ||
        (valid_frequencies3.n_elem != lorentz_width.n_elem) ||
        (ysigma > max_stdev_) || (ysigma < 0) ||
        (measerr_scale < 0.5) || (measerr_scale > 2.0) ) {
        // Value are outside of prior bounds

//        std::cout << "prior bounds violated" << std::endl;
//        std::cout << "# of valid centroids: " << valid_frequencies1.n_elem << std::endl;
//        std::cout << "# of valid widths: " << valid_frequencies2.n_elem << std::endl;
//        std::cout << "max_freq: " << max_freq_ << std::endl;
//        std::cout << "min_freq: " << min_freq_ << std::endl;
//        lorentz_cent.print("centroid");
//        lorentz_width.print("width");
//        std::cout << "ysigma: " << ysigma << ", max_stdev: " << max_stdev_ << std::endl;
//        std::cout << "measerr_scale: " << measerr_scale << std::endl;
//
        prior_satisfied = false;
    }

    if (order_lorentzians_) {
        // Make sure the Lorentzian centroids are still in decreasing order
        for (int i=1; i<lorentz_cent.n_elem; i++) {
            double lorentz_cent_difference = lorentz_cent(i) - lorentz_cent(i-1);
            if (lorentz_cent_difference > 1e-8) {
                // Lorentzians are not in decreasing order, reject this proposal
                prior_satisfied = false;
            }
        }
    }

//    // Make sure Lorentzian widths are greater than minimum frequency for those Lorentzians with centroids
//    // less than the minimum frequency.
//    for (int i=0; i<lorentz_cent.n_elem; i++) {
//        if (lorentz_cent(i) < min_freq_) {
//            if (lorentz_width(i) < min_freq_) {
//                prior_satisfied = false;
//            }
//        }
//    }
    return prior_satisfied;
}

// Calculate the variance of the CAR(p) process
double CARp::Variance(arma::cx_vec alpha_roots, arma::vec ma_coefs, double sigma, double dt)
{
    std::complex<double> car_var(0.0,0.0);
    std::complex<double> denom(0.0,0.0);
    std::complex<double> numer(0.0,0.0);

	// Calculate the variance of a CAR(p) process
	for (int k=0; k<alpha_roots.n_elem; k++) {
		std::complex<double> denom_product(1.0,0.0);

		for (int l=0; l<alpha_roots.n_elem; l++) {
			if (l != k) {
				denom_product *= (alpha_roots(l) - alpha_roots(k)) *
                (std::conj(alpha_roots(l)) + alpha_roots(k));
			}
		}
        denom = -2.0 * std::real(alpha_roots(k)) * denom_product;

        int q = ma_coefs.n_elem;
        std::complex<double> ma_sum1(0.0,0.0);
        std::complex<double> ma_sum2(0.0,0.0);
        for (int l=0; l<q; l++) {
            ma_sum1 += ma_coefs(l) * std::pow(alpha_roots(k),l);
            ma_sum2 += ma_coefs(l) * std::pow(-alpha_roots(k),l);
        }
        numer = ma_sum1 * ma_sum2 * std::exp(alpha_roots(k) * dt);

        car_var += numer / denom;
	}

	// Variance is real-valued, so only return the real part of CARMA_var.
    return sigma * sigma * car_var.real();
}

/*******************************************************************
                        METHODS OF CARMA CLASS
 ******************************************************************/

// Return the starting value and set log_posterior_
arma::vec CARMA::StartingValue()
{
    // Create the parameter vector, theta
    arma::vec theta(p_+q_+3);

    bool good_initials = false;
    while (!good_initials) {

        // Initial guess for model standard deviation is randomly distributed
        // around measured standard deviation of the time series
        arma::vec loga = StartingAR();
        for (int i=0; i<p_; i++) {
            theta(3+i) = loga(i);
        }

        arma::vec log_ma_quad = StartingMA();
        theta(arma::span(p_+3,theta.n_elem-1)) = log_ma_quad;
        arma::vec ma_coefs = ExtractMA(theta);

        // Initial guess for model standard deviation is randomly distributed
        // around measured standard deviation of the time series
        double yvar = RandGen.scaled_inverse_chisqr(y_.n_elem-1, arma::var(y_));

        // Get initial value of the time series mean
        double mu = RandGen.normal(arma::mean(y_), sqrt(yvar) / y_.n_elem);

        arma::cx_vec alpha_roots = ARRoots(theta);
        double sigsqr = yvar / Variance(alpha_roots, ma_coefs, 1.0);
        good_initials = arma::is_finite(sigsqr);
        // Don't run kalman filter if we're already bad_initials

        if (good_initials) {
            // Get initial value of the measurement error scaling parameter by
            // drawing from its prior.

            double measerr_scale = RandGen.scaled_inverse_chisqr(measerr_dof_, 1.0);
            measerr_scale = std::min(measerr_scale, 1.99);
            measerr_scale = std::max(measerr_scale, 0.51);

            theta(0) = sqrt(yvar);
            theta(1) = measerr_scale;
            theta(2) = mu;

            // set the Kalman filter parameters
            pKFilter_->SetSigsqr(sigsqr);
            pKFilter_->SetOmega(ExtractAR(theta));
            pKFilter_->SetMA(ExtractMA(theta));
            arma::vec proposed_yerr = sqrt(measerr_scale) * yerr_;
            pKFilter_->SetTimeSeriesErr(proposed_yerr);
            arma::vec ycent = y_ - mu;
            pKFilter_->SetTimeSeries(ycent);

            // run the kalman filter
            pKFilter_->Filter();

            double logpost = LogDensity(theta);
            good_initials = arma::is_finite(logpost);
        }
    } // continue loop until the starting values give us a finite posterior

    return theta;
}

arma::vec CARMA::SetStartingValue(arma::vec init)
{
   if (init.n_elem != (p_+q_+3)) {
      std::cout << "WARNING: initial guess wrong length, initializing with prior" << std::endl;
      return StartingValue();
   }

   double logpost = LogDensity(init);
   bool good_initials = arma::is_finite(logpost);
   if (good_initials == false) {
      std::cout << "WARNING: initial guess yields non-finite likelihood, initializing with prior" << std::endl;
      return StartingValue();
   }

   double yvar          = init(0)*init(0);
   double measerr_scale = init(1);
   double mu            = init(2);

   arma::cx_vec alpha_roots = ARRoots(init);
   arma::vec ma_coefs       = ExtractMA(init);
   double sigsqr            = yvar / Variance(alpha_roots, ma_coefs, 1.0);

   // set the Kalman filter parameters
   pKFilter_->SetSigsqr(sigsqr);
   pKFilter_->SetOmega(ExtractAR(init));
   pKFilter_->SetMA(ExtractMA(init));
   arma::vec proposed_yerr = sqrt(measerr_scale) * yerr_;
   pKFilter_->SetTimeSeriesErr(proposed_yerr);
   arma::vec ycent = y_ - mu;
   pKFilter_->SetTimeSeries(ycent);
   pKFilter_->Filter();

   return init;
}

// get initial guess for the moving average polynomial coefficients
arma::vec CARMA::StartingMA() {
    arma::vec ma_quad(q_);
    ma_quad.randn();
    return arma::abs(ma_quad);
}

// extract the moving-average coefficients from the CARMA parameter vector
arma::vec CARMA::ExtractMA(arma::vec theta)
{
    arma::cx_vec ma_roots(q_);

    // Construct the complex vector of roots of the characteristic polynomial:
    // alpha(s) = s^p + alpha_1 s^{p-1} + ... + alpha_{p-1} s + alpha_p
    for (int i=0; i<q_/2; i++) {
        // alpha(s) decomposed into its quadratic terms:
        //   alpha(s) = (quad_term1 + quad_term2 * s + s^2) * ...
        double quad_term1 = exp(theta(3+p_+2*i));
        double quad_term2 = exp(theta(3+p_+2*i+1));

        double discriminant = quad_term2 * quad_term2 - 4.0 * quad_term1;

        if (discriminant > 0) {
            // two real roots
            double root1 = -0.5 * (quad_term2 + sqrt(discriminant));
            double root2 = -0.5 * (quad_term2 - sqrt(discriminant));
            ma_roots(2*i) = std::complex<double> (root1, 0.0);
            ma_roots(2*i+1) = std::complex<double> (root2, 0.0);
        } else {
            double real_part = -0.5 * quad_term2;
            double imag_part = -0.5 * sqrt(-discriminant);
            ma_roots(2*i) = std::complex<double> (real_part, imag_part);
            ma_roots(2*i+1) = std::complex<double> (real_part, -imag_part);
        }
    }

    if ((q_ % 2) == 1) {
        // p is odd, so add in additional low-frequency component
        double real_root = -exp(theta(3+p_+q_-1));
        ma_roots(q_-1) = std::complex<double> (real_root, 0.0);
    }

    // calculate the coefficients of the polynomial
    //
    //    p(x) = x^q + c_1 * x^{q-1} + ... + c_{q-1} * x + c_q
    //
    // from it roots. note that poly_coefs[0] = 1.0 = c_0.
    arma::vec poly_coefs = polycoefs(ma_roots);

    // convert coefficients to MA polynomial representation:
    //
    //   beta(s) = beta_q * x^q + beta_{q-1} * x^{q-1} + ... + beta_1 x + beta_0,
    //
    // where beta_0 = 1.0.
    //
    poly_coefs = poly_coefs / poly_coefs(q_); // standardize so c_q = 1 instead of c_0;
    arma::vec ma_coefs = arma::zeros(p_);

    // poly_coefs[0]   poly_coefs[1]   ...   poly_coefs[q] = 1.0
    //    ||                ||                     ||
    // ma_coefs[q]    ma_coefs[q-1]    ...    ma_coefs[0]
    for (int i=0; i<q_+1; i++) {
        ma_coefs(i) = poly_coefs(q_-i);
    }

    return ma_coefs;
}

/*******************************************************************
                        METHODS OF ZCARMA CLASS
 *******************************************************************/

arma::vec ZCARMA::StartingValue()
{
    // Create the parameter vector, theta
    arma::vec theta(p_+4);

    bool good_initials = false;
    while (!good_initials) {

        // Initial guess for model standard deviation is randomly distributed
        // around measured standard deviation of the time series
        arma::vec loga = StartingAR();
        for (int i=0; i<p_; i++) {
            theta(3+i) = loga(i);
        }

        theta(3+p_) = logit(StartingKappa());

        // compute the coefficients of the MA polynomial
        arma::vec ma_coefs = ExtractMA(theta);

        // Initial guess for model standard deviation is randomly distributed
        // around measured standard deviation of the time series
        double yvar = RandGen.scaled_inverse_chisqr(y_.n_elem-1, arma::var(y_));

        // Get initial value of the time series mean
        double mu = RandGen.normal(arma::mean(y_), sqrt(yvar) / y_.n_elem);

        arma::cx_vec alpha_roots = ARRoots(theta);
        double sigsqr = yvar / Variance(alpha_roots, ma_coefs, 1.0);

        // Get initial value of the measurement error scaling parameter by
        // drawing from its prior.

        double measerr_scale = RandGen.scaled_inverse_chisqr(measerr_dof_, 1.0);
        measerr_scale = std::min(measerr_scale, 1.99);
        measerr_scale = std::max(measerr_scale, 0.51);

        theta(0) = sqrt(yvar);
        theta(1) = measerr_scale;
        theta(2) = mu;

        // set the Kalman filter parameters
        pKFilter_->SetSigsqr(sigsqr);
        pKFilter_->SetOmega(ExtractAR(theta));
        pKFilter_->SetMA(ma_coefs);
        arma::vec proposed_yerr = sqrt(measerr_scale) * yerr_;
        pKFilter_->SetTimeSeriesErr(proposed_yerr);
        arma::vec ycent = y_ - mu;
        pKFilter_->SetTimeSeries(ycent);

        // run the kalman filter
        pKFilter_->Filter();

        double logpost = LogDensity(theta);
        good_initials = arma::is_finite(logpost);
    } // continue loop until the starting values give us a finite posterior

    return theta;
}

arma::vec ZCARMA::SetStartingValue(arma::vec init)
{
   if (init.n_elem != (p_+4)) {
      std::cout << "WARNING: initial guess wrong length, initializing with prior" << std::endl;
      return StartingValue();
   }

   double logpost = LogDensity(init);
   bool good_initials = arma::is_finite(logpost);
   if (good_initials == false) {
      std::cout << "WARNING: initial guess yields non-finite likelihood, initializing with prior" << std::endl;
      return StartingValue();
   }

   double yvar          = init(0)*init(0);
   double measerr_scale = init(1);
   double mu            = init(2);

   arma::cx_vec alpha_roots = ARRoots(init);
   arma::vec ma_coefs = ExtractMA(init);
   double sigsqr = yvar / Variance(alpha_roots, ma_coefs, 1.0);

   pKFilter_->SetSigsqr(sigsqr);
   pKFilter_->SetOmega(ExtractAR(init));
   pKFilter_->SetMA(ma_coefs);
   arma::vec proposed_yerr = sqrt(measerr_scale) * yerr_;
   pKFilter_->SetTimeSeriesErr(proposed_yerr);
   arma::vec ycent = y_ - mu;
   pKFilter_->SetTimeSeries(ycent);
   pKFilter_->Filter();

   return init;
}

// get initial guess for the moving average polynomial coefficients, parameterized by kappa
double ZCARMA::StartingKappa() {
    double kappa_normed = RandGen.uniform();
    return kappa_normed;
}

// extract the moving average coefficients from the parameter vector
arma::vec ZCARMA::ExtractMA(arma::vec theta)
{
    double kappa_normed = inv_logit(theta(3 + p_));
    double kappa = (kappa_high_ - kappa_low_) * kappa_normed + kappa_low_;
    // Set the moving average terms
    arma::vec ma_coefs(p_);
	ma_coefs(0) = 1.0;
	for (int i=1; i<p_; i++) {
		ma_coefs(i) = boost::math::binomial_coefficient<double>(p_-1, i) / pow(kappa,i);
	}
    return ma_coefs;
}

/*********************************************************************
                                FUNCTIONS
 ********************************************************************/

double logit(double x) { return log(x / (1.0 - x)); }
double inv_logit(double x) { return exp(x) / (1.0 + exp(x)); }

// Check that all of the roots are unique to within a specified fractional
// tolerance.
bool unique_roots(arma::cx_vec roots, double tolerance)
{
    // Initialize the smallest fractional difference
    double min_frac_diff = 100.0 * tolerance;

    int p = roots.n_elem;

    for (int i=0; i<(p-1); i++) {
        for (int j=i+1; j<p; j++) {
            // Calculate fractional difference between roots(i) and roots(j)
            double frac_diff = std::abs( (roots(i) - roots(j)) /
                                        (roots(i) + roots(j)) );
            if (frac_diff < min_frac_diff) {
                // Found new minimum fractional difference, record it
                min_frac_diff = frac_diff;
            }
        }
    }

    // Test if the roots unique to within the specified tolerance
    bool unique = (min_frac_diff > tolerance);

    return unique;
}

// Return the coefficients of a polynomial given its roots. The polynomial
// is assumed to be of the form:
//
//      p(x) = x^n + c_1 * x^{n-1} + ... + c_{n-1} * x + c_n
//
// where {c_i ; i=1,...,n} are the coefficients. Note that this function
// returns a (n+1)-element column vector, where c_0 = 1.0.

arma::vec polycoefs(arma::cx_vec roots)
{
    arma::cx_vec coefs(roots.n_elem+1);
    coefs.zeros(); // Initialize all values to zero

    coefs(0) = 1.0; // Coefficient for highest order term is set to one

    for (int i=0; i<roots.n_elem; i++) {
        // Calculate the coefficients using a recursion formula
        coefs(arma::span(1,i+1)) = coefs(arma::span(1,i+1)) - roots(i) * coefs(arma::span(0,i));
    }

    // The coefficients must be real, so only return the real part
    return arma::real(coefs);
}


#ifndef ODESOLVERIMEX_HPP_INCLUDED
#define ODESOLVERIMEX_HPP_INCLUDED

#include <petscts.h>
#include <string>
#include <cmath>
#include <vector>
#include <map>
#include <algorithm>
#include <assert.h>
#include <boost/circular_buffer.hpp>
#include "integratorContextImex.hpp"
#include "genFuncs.hpp"

using namespace std;

/*
 * Provides a set of algorithms to solve a system of ODEs of
 * the form y' = f(t,y) using IMEX time stepping,
 * where y is represented as an array of one or more Vecs
 * (PETSc data type).
 *
 * Containers for integration:
 *   var          map<string,Vec> of explicitly integrated variables
 *   varIm    map<string,Vec> of implicitly integrated variables
 *
 * SOLVER TYPE        ALGORITHM
 *  RK32_WBE        explicit part Runge-Kutta (2,3), implicit controlled by user
 *  RK43_WBE        explicit part Runge-Kutta (3,4), implicit controlled by user
 *
 *
 * At minimum, the user must specify:
 *     QUANTITY               FUNCTION
 *  max number of steps      constructor
 *  solver type              constructor
 *  initial conditions       setInitialConds     Note: this array will be modified during integration
 *  initial step size        constructor
 *  step size alg            constructor (this is only used by the adaptive time-stepping algorithm)
 *  f(t,y)                   object passed to integrate must have member function d_dt(PetscScalar, PetscScalar*,PetscScalar*)
 *  final time               constructor and setTimeRange
 *  timeMonitor              object passed to integrate must have member function timeMonitor
 *
 *
 * Optional fields that can also be specified:
 *     QUANTITY               FUNCTION
 *  tolerance                setTolerance
 *  maximum step size        setStepSize
 *  minimum step size        setTimeStepBounds
 *  initial step size        setTimeStepBounds
 *
 * Once the odeSolver context is set, call integrate() to perform
 * the integration.
 *
 * y(t=final time) is stored in the initial conditions array.  Summary output
 * information is provided by viewSolver.  Users can obtain information at
 * each time step within a user-defined monitor function.
 *
 */

class OdeSolverImex
{
public:

  PetscReal               _initT,_finalT,_currT,_deltaT;
  PetscReal          _newDeltaT; // stores future deltaT for access by outside classes, primarily for checkpointing
  PetscInt                _maxNumSteps,_stepCount;
  map<string,Vec>         _varEx,_dvar; // explicit integration variable and rate
  map<string,Vec>         _varIm; // implicit integration variable, once per time step
  vector<string>          _errInds; // which inds of _var to use for error control
  vector<double>          _scale; // scale factor for entries in _errInds
  double                  _runTime;
  string                  _controlType;
  string                  _normType;

  PetscReal   _minDeltaT,_maxDeltaT;
  PetscReal   _totTol; // total tolerance, might be atol, or rtol, or a combination of both
  PetscInt    _numRejectedSteps,_numMinSteps,_numMaxSteps;

  // for PID error control
  boost::circular_buffer<double> _errA;

  // constructor and destructor
  OdeSolverImex(PetscInt maxNumSteps,PetscReal finalT,PetscReal deltaT,string controlType);
  virtual ~OdeSolverImex() {};

  // member functions
  PetscErrorCode setInitialStepCount(const PetscReal stepCount);
  PetscErrorCode setToleranceType(const string normType); // type of norm used for error control

  // virtual member functions are declared in base class and redefined in derived class
  virtual PetscErrorCode setTimeRange(const PetscReal initT,const PetscReal finalT) = 0;
  virtual PetscErrorCode setStepSize(const PetscReal deltaT) = 0;
  virtual PetscErrorCode setTolerance(const PetscReal tol) = 0;
  virtual PetscErrorCode setTimeStepBounds(const PetscReal minDeltaT, const PetscReal maxDeltaT) = 0;
  virtual PetscErrorCode setInitialConds(map<string,Vec>& varEx, map<string,Vec>& varIm) = 0;
  virtual PetscErrorCode setErrInds(vector<string>& errInds) = 0;
  virtual PetscErrorCode setErrInds(vector<string>& errInds, vector<double> scale) = 0;

  virtual PetscErrorCode view() = 0;
  virtual PetscErrorCode integrate(IntegratorContextImex *obj) = 0;
  virtual PetscReal computeStepSize(const PetscReal totErr) = 0;
  virtual PetscReal computeError() = 0;
};

// Explicit RK32 scheme from Hairer et al., with added Backward Euler implicit scheme once per time step
// derived class from OdeSolverImex
class RK32_WBE : public OdeSolverImex
{
public:

  // for P or PID error control
  PetscReal   _kappa,_ord; // safety factor in step size determinance, order of accuracy of method
  PetscReal   _totErr; // error between 3rd order solution and embedded 2nd order solution

  // intermediate values for time stepping for the explicit variable
  map<string,Vec> _k1,_f1,_k2,_f2,_y2,_y3;
  map<string,Vec> _vardTIm;

  // constructor and destructor
  RK32_WBE(PetscInt maxNumSteps,PetscReal finalT,PetscReal deltaT,string controlType);
  ~RK32_WBE();

  // member functions
  PetscErrorCode setTimeRange(const PetscReal initT,const PetscReal finalT);
  PetscErrorCode setStepSize(const PetscReal deltaT);
  PetscErrorCode setTolerance(const PetscReal tol);
  PetscErrorCode setTimeStepBounds(const PetscReal minDeltaT, const PetscReal maxDeltaT);
  PetscErrorCode setInitialConds(map<string,Vec>& varEx, map<string,Vec>& varIm);
  PetscErrorCode setErrInds(vector<string>& errInds);
  PetscErrorCode setErrInds(vector<string>& errInds, vector<double> scale);
  PetscErrorCode view();
  PetscErrorCode integrate(IntegratorContextImex *obj);
  PetscReal computeStepSize(const PetscReal totErr);
  PetscReal computeError();
};

// Runge-Kutta 4(3) scheme for explicit time integration, with added
// Backward Euler implicit scheme once per time step.
// Based on "ARK4(3)6L[2]SA-ERK" algorithm from Kennedy and Carpenter (2003):
// "Additive Runge-Kutta schemes for convection-diffusion-reaction equations"
// Note: Has matching IMEX equivalent
// derived class from OdeSolverImex
class RK43_WBE : public OdeSolverImex
{
public:

  // for P or PID error control
  // safety factor in step size determinance, order of accuracy of method
  PetscReal   _kappa,_ord;
  // error between 3rd order solution and embedded 2nd order solution
  PetscReal   _totErr;

  // intermediate values for time stepping for the explicit variable
  map<string,Vec> _k1,_k2,_k3,_k4,_k5,_k6,_y4,_y3;
  map<string,Vec> _f1,_f2,_f3,_f4,_f5,_f6;

  // intermediate value for implict variable
  map<string,Vec> _vardTIm;

  // constructor and destructor
  RK43_WBE(PetscInt maxNumSteps,PetscReal finalT,PetscReal deltaT,string controlType);
  ~RK43_WBE();

  // member functions
  PetscErrorCode setTimeRange(const PetscReal initT,const PetscReal finalT);
  PetscErrorCode setStepSize(const PetscReal deltaT);
  PetscErrorCode setTolerance(const PetscReal tol);
  PetscErrorCode setTimeStepBounds(const PetscReal minDeltaT, const PetscReal maxDeltaT);
  PetscErrorCode setInitialConds(map<string,Vec>& varEx, map<string,Vec>& varIm);
  PetscErrorCode setErrInds(vector<string>& errInds);
  PetscErrorCode setErrInds(vector<string>& errInds, vector<double> scale);
  PetscErrorCode view();
  PetscErrorCode integrate(IntegratorContextImex *obj);
  PetscReal computeStepSize(const PetscReal totErr);
  PetscReal computeError();
};

#endif


#include <iostream>
#include <vector>
#include <algorithm>
#include <cmath>
#include "GSLInterface.h"
#include <gsl/gsl_poly.h>
#include "utils.h"
#include "coordsys.h"
#include "coordtransforms.h"
#include "potential.h"
#include "orbit.h"
#include "aa.h"
#include <boost/python.hpp>
#include <boost/python/numeric.hpp>
using namespace boost::python;

namespace {
	boost::python::numeric::array Stack_Triax_Forces(boost::python::numeric::array f){
		StackelTriaxial T(3.61/500.,-30.,-20.);
		VecDoub x(3,0); for(int i=0;i<3;i++)x[i]=extract<double>(f[i]);
		VecDoub v = T.Forces(x);
    	return boost::python::numeric::array(make_tuple(v[0],v[1],v[2]));
	}
	double Stack_Triax_H(boost::python::numeric::array f){
		StackelTriaxial T(3.61/500.,-30.,-20.);
		VecDoub x(6,0);for(int i=0;i<6;i++)x[i]=extract<double>(f[i]);
		return T.H(x);
	}
	boost::python::numeric::array Stack_Triax_Actions(boost::python::numeric::array f){
		StackelTriaxial T(3.61/500.,-30.,-20.);
		VecDoub x(6,0);for(int i=0;i<6;i++)x[i]=extract<double>(f[i]);
		Actions_TriaxialStackel AA(&T);
		VecDoub v = AA.actions(x);
    	return boost::python::numeric::array(make_tuple(v[0],v[1],v[2]));
	}

	boost::python::numeric::array Stack_Triax_Freqs(boost::python::numeric::array f){
		StackelTriaxial T(3.61/500.,-30.,-20.);
		VecDoub x(6,0);for(int i=0;i<6;i++)x[i]=extract<double>(f[i]);
		Actions_TriaxialStackel AA(&T);
		VecDoub v = AA.actions(x,1);
    	return boost::python::numeric::array(make_tuple(v[3],v[4],v[5]));
	}
}

BOOST_PYTHON_MODULE(triax_py){
	boost::python::numeric::array::set_module_and_type("numpy", "ndarray");
    def("Stack_Triax_Forces", Stack_Triax_Forces);
    def("Stack_Triax_H", Stack_Triax_H);
    def("Stack_Triax_Actions", Stack_Triax_Actions);
    def("Stack_Triax_Freqs", Stack_Triax_Freqs);
}


// (C) Copyright 2007-2009 Andrew Sutton
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GRAPH_CLUSTERING_COEFFICIENT_HPP
#define BOOST_GRAPH_CLUSTERING_COEFFICIENT_HPP

#include <boost/utility.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>

namespace boost
{
namespace detail
{
    template <class Graph>
    inline typename graph_traits<Graph>::degree_size_type
    possible_edges(const Graph& g, std::size_t k, directed_tag)
    {
        function_requires< GraphConcept<Graph> >();
        typedef typename graph_traits<Graph>::degree_size_type T;
        return T(k) * (T(k) - 1);
    }

    template <class Graph>
    inline typename graph_traits<Graph>::degree_size_type
    possible_edges(const Graph& g, size_t k, undirected_tag)
    {
        // dirty little trick...
        return possible_edges(g, k, directed_tag()) / 2;
    }

    // This template matches directedS and bidirectionalS.
    template <class Graph>
    inline typename graph_traits<Graph>::degree_size_type
    count_edges(const Graph& g,
                typename Graph::vertex_descriptor u,
                typename Graph::vertex_descriptor v,
                directed_tag)

    {
        function_requires< AdjacencyMatrixConcept<Graph> >();
        return (edge(u, v, g).second ? 1 : 0) +
                (edge(v, u, g).second ? 1 : 0);
    }

    // This template matches undirectedS
    template <class Graph>
    inline typename graph_traits<Graph>::degree_size_type
    count_edges(const Graph& g,
                typename Graph::vertex_descriptor u,
                typename Graph::vertex_descriptor v,
                undirected_tag)
    {
        function_requires< AdjacencyMatrixConcept<Graph> >();
        return edge(u, v, g).second ? 1 : 0;
    }
}

template <typename Graph, typename Vertex>
inline typename graph_traits<Graph>::degree_size_type
num_paths_through_vertex(const Graph& g, Vertex v)
{
    function_requires< AdjacencyGraphConcept<Graph> >();
    typedef typename graph_traits<Graph>::directed_category Directed;
    typedef typename graph_traits<Graph>::adjacency_iterator AdjacencyIterator;

    // TODO: There should actually be a set of neighborhood functions
    // for things like this (num_neighbors() would be great).

    AdjacencyIterator i, end;
    tie(i, end) = adjacent_vertices(v, g);
    std::size_t k = std::distance(i, end);
    return detail::possible_edges(g, k, Directed());
}

template <typename Graph, typename Vertex>
inline typename graph_traits<Graph>::degree_size_type
num_triangles_on_vertex(const Graph& g, Vertex v)
{
    function_requires< IncidenceGraphConcept<Graph> >();
    function_requires< AdjacencyGraphConcept<Graph> >();
    typedef typename graph_traits<Graph>::degree_size_type Degree;
    typedef typename graph_traits<Graph>::directed_category Directed;
    typedef typename graph_traits<Graph>::adjacency_iterator AdjacencyIterator;

    // TODO: I might be able to reduce the requirement from adjacency graph
    // to incidence graph by using out edges.

    Degree count(0);
    AdjacencyIterator i, j, end;
    for(tie(i, end) = adjacent_vertices(v, g); i != end; ++i) {
        for(j = boost::next(i); j != end; ++j) {
            count += detail::count_edges(g, *i, *j, Directed());
        }
    }
    return count;
} /* namespace detail */

template <typename T, typename Graph, typename Vertex>
inline T
clustering_coefficient(const Graph& g, Vertex v)
{
    T zero(0);
    T routes = T(num_paths_through_vertex(g, v));
    return (routes > zero) ?
        T(num_triangles_on_vertex(g, v)) / routes : zero;
}

template <typename Graph, typename Vertex>
inline double
clustering_coefficient(const Graph& g, Vertex v)
{ return clustering_coefficient<double>(g, v); }

template <typename Graph, typename ClusteringMap>
inline typename property_traits<ClusteringMap>::value_type
all_clustering_coefficients(const Graph& g, ClusteringMap cm)
{
    function_requires< VertexListGraphConcept<Graph> >();
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename graph_traits<Graph>::vertex_iterator VertexIterator;
    function_requires< WritablePropertyMapConcept<ClusteringMap,Vertex> >();
    typedef typename property_traits<ClusteringMap>::value_type Coefficient;

    Coefficient sum(0);
    VertexIterator i, end;
    for(tie(i, end) = vertices(g); i != end; ++i) {
        Coefficient cc = clustering_coefficient<Coefficient>(g, *i);
        put(cm, *i, cc);
        sum += cc;
    }
    return sum / Coefficient(num_vertices(g));
}

template <typename Graph, typename ClusteringMap>
inline typename property_traits<ClusteringMap>::value_type
mean_clustering_coefficient(const Graph& g, ClusteringMap cm)
{
    function_requires< VertexListGraphConcept<Graph> >();
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename graph_traits<Graph>::vertex_iterator VertexIterator;
    function_requires< ReadablePropertyMapConcept<ClusteringMap,Vertex> >();
    typedef typename property_traits<ClusteringMap>::value_type Coefficient;

    Coefficient cc(0);
    VertexIterator i, end;
    for(tie(i, end) = vertices(g); i != end; ++i) {
        cc += get(cm, *i);
    }
    return cc / Coefficient(num_vertices(g));
}

} /* namespace boost */

#endif