#ifndef CTCD_HPP
#define CTCD_HPP 1

#include <Eigen/Geometry>
#include <vector>

namespace mcl {

// Source: Etienne Vouga
// https://github.com/evouga/collisiondetection

struct TimeInterval
{
    TimeInterval(double tl, double tu) : l(tl), u(tu)
    {
        if(l > u) std::swap(l, u);
        l = std::max(l, 0.0);
        u = std::min(u, 1.0);
    }

    TimeInterval() : l(0), u(0) {}

    // Returns whether or not the intersection of the intervals is nonempty
    static bool overlap(const TimeInterval &t1, const TimeInterval &t2);
    static bool overlap(const std::vector<TimeInterval> &intervals);

    // Returns the intersection of the intervals **asuming the intersection is nonempty**
    static TimeInterval intersect(const std::vector<TimeInterval> &intervals);

    double l, u;
};

class CTCD
{
public:
    // Looks for collisions between edges (q0start, p0start) and (q1start, p1start) as they move towards
    // (q0end, p0end) and (q1end, p1end). Returns true if the edges ever come closer than a distance eta to each
    // other, and stores the earliest time (in the interval [0,1]) at which they do so in t.
    // WARNING: Does not work correctly if two edges are parallel at the time of intersection -- vertexEdgeCTCD
    // should catch this case, though.
    static bool edgeEdgeCTCD(
		const Eigen::Vector3d &q0start,
		const Eigen::Vector3d &p0start,
		const Eigen::Vector3d &q1start,
		const Eigen::Vector3d &p1start,
		const Eigen::Vector3d &q0end,
		const Eigen::Vector3d &p0end,
		const Eigen::Vector3d &q1end,
		const Eigen::Vector3d &p1end,
		double eta,
		double &t,
		std::vector<double> *all_t = nullptr);

    // Looks for collisions between the vertex q0start and the face (q1start, q2start, q3start) as they move
    // towards q0end and (q1end, q2end, q3end). Returns true if the vertex and face ever come closer than a distance
    // eta to each, and stores the earliest time (in the interval [0,1]) at which they do so in t.
    static bool vertexFaceCTCD(const Eigen::Vector3d &q0start,
                               const Eigen::Vector3d &q1start,
                               const Eigen::Vector3d &q2start,
                               const Eigen::Vector3d &q3start,
                               const Eigen::Vector3d &q0end,
                               const Eigen::Vector3d &q1end,
                               const Eigen::Vector3d &q2end,
                               const Eigen::Vector3d &q3end,
                               double eta,
                               double &t,
								std::vector<double> *all_t = nullptr);

    // Looks for the degenerate case of collisions between the vertex q0start and the edge (q1start, s2start) as they
    // move towards q0end and (q1end, q2end). Returns true if the vertex and edge ever come closer than a distance
    // eta to each other, and stores the earliest time (in the interval [0,1]) at which they do so in t.
    static bool vertexEdgeCTCD(const Eigen::Vector3d &q0start,
                              const Eigen::Vector3d &q1start,
                              const Eigen::Vector3d &q2start,
                              const Eigen::Vector3d &q0end,
                              const Eigen::Vector3d &q1end,
                              const Eigen::Vector3d &q2end,
                              double eta,
                              double &t,
							std::vector<double> *all_t = nullptr
							);

    static bool vertexEdgeCTCD(const Eigen::Vector2d &q0start,
                              const Eigen::Vector2d &q1start,
                              const Eigen::Vector2d &q2start,
                              const Eigen::Vector2d &q0end,
                              const Eigen::Vector2d &q1end,
                              const Eigen::Vector2d &q2end,
                              double eta,
                              double &t,
								std::vector<double> *all_t = nullptr);
    // Looks for the degenerate case of collisions between the vertices q1start and q2start, as they move towards
    // q1end and q2end. Returns true if the vertices ever come closer than a distane of eta to each other, and stores
    // the earliest time (in the interval [0,1]) at which they do so in t.
    static bool vertexVertexCTCD(const Eigen::Vector3d &q1start,
                                const Eigen::Vector3d &q2start,
                                const Eigen::Vector3d &q1end,
                                const Eigen::Vector3d &q2end,
                                double eta, double &t,
								std::vector<double> *all_t = nullptr);
    static bool vertexVertexCTCD(const Eigen::Vector2d &q1start,
                                const Eigen::Vector2d &q2start,
                                const Eigen::Vector2d &q1end,
                                const Eigen::Vector2d &q2end,
                                double eta, double &t,
								std::vector<double> *all_t = nullptr);

private:
    // Solves the quadratic equation ax^2 + bx + c = 0, and puts the roots in t0, t1, in ascending order.
    // Returns the number of real roots found.
    static int getQuadRoots(double a, double b, double c, double &t0, double &t1);

    // Conservatively checks if the polynomial of degree degree, with coefficients op, could be positive (if pos is true)
    // or negative (if pos is negative) on the interval [0,1].
    static bool couldHaveRoots(double *op, int degree, bool pos);

    // Looks at the interval [t1, t2], on which a polynomial of degree degree and coefficients op is assumed to have
    // constant sign, and determines if the polynomial is all positive or all negative on that interval.
    // If positive, and pos is true, or if negative, and pos is false, clamps the interval to [0,1] and adds it to
    // intervals.
    static void checkInterval(double t1, double t2, double * op, int degree, std::vector<TimeInterval> &intervals, bool pos);

    // Computes the intervals of x in [0,1] where the polynomial of degree n, with coefficients in op
    // (given in "natural," descending order of power of x) is positive (when pos = true) or negative (if pos = false).
    static void findIntervals(double *op, int n, std::vector<TimeInterval> & intervals, bool pos);

    static void distancePoly3D(const Eigen::Vector3d &x10,
                               const Eigen::Vector3d &x20,
                               const Eigen::Vector3d &x30,
                               const Eigen::Vector3d &v10,
                               const Eigen::Vector3d &v20,
                               const Eigen::Vector3d &v30,
                               double minDSquared,
                               std::vector<TimeInterval> &result);

    static void barycentricPoly3D(const Eigen::Vector3d &x10,
                                  const Eigen::Vector3d &x20,
                                  const Eigen::Vector3d &x30,
                                  const Eigen::Vector3d &v10,
                                  const Eigen::Vector3d &v20,
                                  const Eigen::Vector3d &v30,
                                  std::vector<TimeInterval> &result);

    static void planePoly3D(const Eigen::Vector3d &x10,
                            const Eigen::Vector3d &x20,
                            const Eigen::Vector3d &x30,
                            const Eigen::Vector3d &v10,
                            const Eigen::Vector3d &v20,
                            const Eigen::Vector3d &v30,
                            std::vector<TimeInterval> &result);

};

} // namespace mcl

#endif


#include "subdiv_evaluator.h"
#include "mesh.h"

#include <opensubdiv/far/topologyRefiner.h>
#include <opensubdiv/far/topologyDescriptor.h>
#include <opensubdiv/far/primvarRefiner.h>
#include <opensubdiv/far/stencilTable.h>
#include <opensubdiv/far/patchTableFactory.h>
#include <opensubdiv/far/patchMap.h>

#include <Eigen/Eigen>

#include <vector>
#include <random>

using namespace OpenSubdiv;
using namespace Eigen;

std::vector<SurfacePoint> SurfacePoint::generate(const int n_points, const int n_triangles)
{
    std::vector<SurfacePoint> sps(n_points);

    std::mt19937 random_generator(1);
    std::uniform_real_distribution dist_u(0.0, 1.0);
    std::uniform_real_distribution dist_v(0.0, 1.0);
    std::uniform_int_distribution dist_face(0, n_triangles-1);
    for (int i{0}; i < n_points; ++i)
    {
        // generate random u,v
        double u = dist_u(random_generator);
        double v = dist_v(random_generator);
        if ((u+v) > 1)
        {
            u = 1 - u;
            v = 1 - v;
        }

        sps[i].u << u, v;
        sps[i].face = dist_face(random_generator);
    }

    return sps;
}

SubdivEvaluator::SubdivEvaluator(const Mesh &mesh)
{
    using Refiner_t = Far::TopologyRefinerFactory<Far::TopologyDescriptor>;

    n_vertices = mesh.n_vertices();

    // create refiner using descriptor
    Far::TopologyDescriptor desc;
    desc.numVertices = n_vertices;
    desc.numFaces = mesh.n_triangles();
    std::vector<int> num_verts_per_face(mesh.n_triangles(), 3);
    desc.numVertsPerFace = num_verts_per_face.data();
    desc.vertIndicesPerFace = &mesh.triangles()[0](0);
    // desc.vertIndicesPerFace = &topo.triangles[0](0);

    Sdc::SchemeType type = Sdc::SCHEME_LOOP;
    Sdc::Options options;
    options.SetVtxBoundaryInterpolation(Sdc::Options::VTX_BOUNDARY_NONE);
    Far::TopologyRefiner *refiner_for_patch_table = Refiner_t::Create(desc, Refiner_t::Options(type, options));

    // refine topology (adaptive takes care of irregular vertices)
    const int max_isolation = 0; // do not change this!
    refiner_for_patch_table->RefineAdaptive(Far::TopologyRefiner::AdaptiveOptions(max_isolation));

    // generate PatchTable that will be used to evaluate surface limit
    Far::PatchTableFactory::Options patch_options;
    patch_options.endCapType = Far::PatchTableFactory::Options::ENDCAP_BSPLINE_BASIS;
    patch_table = Far::PatchTableFactory::Create(*refiner_for_patch_table, patch_options);

    // compute total number of points needed to evaluate patch table
    // use local points around irregular vertices
    n_refiner_vertices = refiner_for_patch_table->GetNumVerticesTotal();
    n_local_points = patch_table->GetNumLocalPoints();

    // create a buffer to hold the position of the refined verts and local points
    evaluation_verts_buffer.resize(n_refiner_vertices + n_local_points);

    // refiner for subdividing mesh
    refiner = Refiner_t::Create(desc, Refiner_t::Options(type, options));

    // uniformly refine topology up to maxlevel
    refiner->RefineUniform(Far::TopologyRefiner::UniformOptions(maxlevel));

    delete refiner_for_patch_table;
}

std::shared_ptr<Mesh> SubdivEvaluator::generate_refined_mesh(const std::vector<Vector3d> &coarse_verts, int level)
{
    // Reference: http://graphics.pixar.com/opensubdiv/docs/far_tutorial_1_1.html

    // allocate buffer for vertex primvar data.
    // length = sum of vertices at all levels
    std::vector<OSDVertex> vbuffer(refiner->GetNumVerticesTotal());
    OSDVertex *verts{&vbuffer[0]}; // this interface is used for filling up

    // copy coarse mesh positions
    for (int i{0}; i < coarse_verts.size(); ++i)
    {
        verts[i].point = coarse_verts[i];
    }
    // use primar refiner for filling other levels using interpolation
    Far::PrimvarRefiner primvar_refiner(*refiner);
    // Note: previous level verts are used for next level interpolation
    OSDVertex *src = verts;
    for (int l{1}; l <= level; ++l)
    {
        OSDVertex *dst = src + refiner->GetLevel(l - 1).GetNumVertices();
        primvar_refiner.Interpolate(l, src, dst);
        src = dst;
    }
    // src will point to vertices at `level` interpolation after end of above loop

    std::shared_ptr<Mesh> mesh_refined = std::make_shared<Mesh>();

    // extract refined vertices
    const Far::TopologyLevel &ref_level = refiner->GetLevel(level);
    int n_verts_out = ref_level.GetNumVertices();
    mesh_refined->vertices().resize(n_verts_out);
    for (int v{0}; v < n_verts_out; ++v)
    {
        mesh_refined->vertices()[v] = src[v].point;
    }

    // extract refined faces into out topo
    int n_faces = ref_level.GetNumFaces();
    mesh_refined->triangles().resize(n_faces);
    for (int f{0}; f < n_faces; ++f)
    {
        Far::ConstIndexArray face_vert_ids = ref_level.GetFaceVertices(f);
        for (int v{0}; v < face_vert_ids.size(); ++v)
        {
            mesh_refined->triangles()[f](v) = face_vert_ids[v];
        }
    }

    mesh_refined->update_adjacencies();

    return mesh_refined;
}

void SubdivEvaluator::evaluate_subdiv_surface(const std::vector<Vector3d> &coarse_verts, const std::vector<SurfacePoint> &uvs,
                                              SurfaceFeatures &sf, const bool compute_dX) const
{
    // compute local points from coarse verts
    // 263 - 269
    for (int i{0}; i < n_vertices; ++i)
        evaluation_verts_buffer[i].point = coarse_verts[i];
    int n_stencils = patch_table->GetLocalPointStencilTable()->GetNumStencils();
    patch_table->ComputeLocalPointValues(&evaluation_verts_buffer[0], &evaluation_verts_buffer[n_refiner_vertices]);

    // get all stencils from patch table
    // (necesary to obtain the weights for the gradients)
    // 273 - 289
    const Far::StencilTable *stencil_table = patch_table->GetLocalPointStencilTable();
    std::vector<Far::Stencil> stencils(n_stencils);
    for (int i{0}; i < n_stencils; ++i)
    {
        stencils[i] = stencil_table->GetStencil(Far::Index(i));
    }

    // create PatchMap to locate patches in the table
    // 292
    Far::PatchMap patch_map(*patch_table);

    // zero output
    sf.set_zero();

    // evaluate surface at uvs
    // 326 - 412
    for (int i{0}; i < uvs.size(); ++i)
    {
        // locate patch corresponding to uv
        // 327 - 332
        int face = uvs[i].face;
        double u = uvs[i].u[0];
        double v = uvs[i].u[1];
        const Far::PatchTable::PatchHandle *patch_handle = patch_map.FindPatch(face, u, v);

        // evaluate patch weights
        // 336 - 337
        patch_table->EvaluateBasis(*patch_handle, u, v, sf.p_weights, sf.du_weights, sf.dv_weights);

        // identify control vertices corresponding to this patch
        // 339
        Far::ConstIndexArray cvs = patch_table->GetPatchVertices(*patch_handle);

        // for each control vertex
        // 340 - 411
        for (int cv1{0}; cv1 < cvs.size(); ++cv1)
        {
            // fill surface features by corresponding weighted combination of control vertices
            // 341 - 349
            sf.update(evaluation_verts_buffer[cvs[cv1]].point, i, cv1);

            // compute normals at uv on surface
            // 351 - 357
            // done at end
            // Note: derivative wrt normal is not implemented

            // compute derivative wrt control vertices
            // 362 - 410
            if (!compute_dX) continue;

            MatrixXd accumulated_weights(3, n_vertices);
            accumulated_weights.setZero();
            std::vector<bool> has_nonzero_weight(n_vertices, false);
            for (int cv2{0}; cv2 < cvs.size(); ++cv2)
            {
                if (cvs[cv2] < n_vertices)  // regular vertex
                {
                    int c{0};
                    accumulated_weights(c++, cvs[cv2]) += sf.p_weights[cv2];
                    accumulated_weights(c++, cvs[cv2]) += sf.du_weights[cv2];
                    accumulated_weights(c++, cvs[cv2]) += sf.dv_weights[cv2];
                    has_nonzero_weight[cv2] = true;
                }
                else  // local point
                {
                    int idx_offset = cvs[cv2] - n_vertices;
                    // look at the stencil associated with this local point and distribute its weight over the control vertices
                    const Far::Index *stencil_idx = stencils[idx_offset].GetVertexIndices();
                    const float *stencil_weights = stencils[idx_offset].GetWeights();
                    for (int s{0}; s < stencils[idx_offset].GetSize(); ++s)
                    {
                        int c{0};
                        accumulated_weights(c++, stencil_idx[s]) += sf.p_weights[cv2] * stencil_weights[s];
                        accumulated_weights(c++, stencil_idx[s]) += sf.du_weights[cv2] * stencil_weights[s];
                        accumulated_weights(c++, stencil_idx[s]) += sf.dv_weights[cv2] * stencil_weights[s];
                        has_nonzero_weight[stencil_idx[s]] = true;
                    }
                }
            }

            // store the weights
            float scale = 1.f / cvs.size();
            for (int cv3{0}; cv3 < n_vertices; ++cv3)
            {
                if(has_nonzero_weight[cv3])
                {
                    int c{0};
                    sf.dSdX.add(i, cv3, accumulated_weights(c++, cv3)*scale);
                    sf.dSudX.add(i, cv3, accumulated_weights(c++, cv3)*scale);
                    sf.dSvdX.add(i, cv3, accumulated_weights(c++, cv3)*scale);
                }
            }
        }
    }

    sf.compute_normal();
}


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2020, Clearpath Robotics
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
#include <fuse_constraints/normal_prior_pose_2d.h>
#include <fuse_core/util.h>

#include <Eigen/Core>
#include <glog/logging.h>

namespace fuse_constraints
{

NormalPriorPose2D::NormalPriorPose2D(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b) :
  A_(A),
  b_(b)
{
  CHECK_GT(A_.rows(), 0);
  CHECK_EQ(A_.cols(), 3);
  set_num_residuals(A_.rows());
}

bool NormalPriorPose2D::Evaluate(
  double const* const* parameters,
  double* residuals,
  double** jacobians) const
{
  fuse_core::Vector3d full_residuals_vector;
  full_residuals_vector[0] = parameters[0][0] - b_[0];  // position x
  full_residuals_vector[1] = parameters[0][1] - b_[1];  // position y
  full_residuals_vector[2] = fuse_core::wrapAngle2D(parameters[1][0] - b_[2]);  // orientation

  // Scale the residuals by the square root information matrix to account for the measurement uncertainty.
  Eigen::Map<fuse_core::VectorXd> residuals_vector(residuals, num_residuals());
  residuals_vector = A_ * full_residuals_vector;

  if (jacobians != nullptr)
  {
    // Jacobian wrt position
    if (jacobians[0] != nullptr)
    {
      Eigen::Map<fuse_core::MatrixXd>(jacobians[0], num_residuals(), 2) = A_.leftCols<2>();
    }

    // Jacobian wrt orientation
    if (jacobians[1] != nullptr)
    {
      Eigen::Map<fuse_core::VectorXd>(jacobians[1], num_residuals()) = A_.col(2);
    }
  }
  return true;
}

}  // namespace fuse_constraints


/*
 * optimize_tnc.cpp
 *
 *  Created on: Feb 9, 2010
 *      Author: smitty
 */

#include <iostream>
#include <stdio.h>
#include <nlopt.hpp>
#include <math.h>

#include "cont_models.h"

#include <armadillo>
using namespace arma;

#define LARGE 1000000000

typedef struct {
    rowvec x;
    mat ovcv;
} analysis_data;

typedef struct {
    Tree * tree;
} analysis_data_tree;

double nlopt_bm_sr(unsigned n, const double *x, double *grad, void *data) {
    if (x[1] <= 0) {
        return LARGE;
    }
    //cout << x[0] << " " << x[1] << endl;
    analysis_data * d = (analysis_data *) data;
    mat tvcv = (d->ovcv) * x[1];
    rowvec m = rowvec(d->x.n_cols); m.fill(x[0]);
    double like = norm_pdf_multivariate(d->x, m, tvcv);
    return -like;
}

double nlopt_bm_sr_log(unsigned n, const double *x, double *grad, void *data) {
    if (x[1] <= 0) {
        return LARGE;
    }
    //cout << x[0] << " " << x[1] << endl;
    analysis_data * d = (analysis_data *) data;
    mat tvcv = (d->ovcv) * x[1];
    rowvec m = rowvec(d->x.n_cols); m.fill(x[0]);
    double like = norm_log_pdf_multivariate(d->x, m, tvcv);
    return -like;
}

/*
 * single alpha ou
 */
double nlopt_ou_sr_log(unsigned n, const double *x, double *grad, void *data) {
    if (x[1] <= 0 || x[2] <= 0) {
        return LARGE;
    }
    double alpha = x[2];
    analysis_data * d = (analysis_data *) data;
    mat vcvDiag(d->ovcv.n_cols,d->ovcv.n_cols); vcvDiag.zeros();
    vcvDiag.diag() = (d->ovcv).diag();
    mat tm(vcvDiag.n_cols,vcvDiag.n_cols);
    tm.ones();
    mat diagi = trans(vcvDiag * tm);
    mat diagj = vcvDiag * tm;
    mat Tij = diagi + diagj - (2 * d->ovcv);
    mat ouvcv = (1. / (2. * alpha)) * exp(-alpha * Tij) % (1. - exp(-2. * alpha * d->ovcv));
    ouvcv = ouvcv * x[1];
    rowvec m = rowvec(d->x.n_cols); m.fill(x[0]);
    double like = norm_log_pdf_multivariate(d->x,m,ouvcv);
    return -like;
}

double nlopt_bm_bl(unsigned n, const double *x, double *grad, void *data){
    for (unsigned int i=0;i<n;i++){
        if (x[i] <= 0){
            return LARGE;
        }
    }
    double sigma =1;// x[0];//1;
    analysis_data_tree * d = (analysis_data_tree *) data;
    Tree * tr = d->tree;
    for (int i=0;i<tr->getNodeCount();i++){
        if (tr->getNode(i) != tr->getRoot()){
            tr->getNode(i)->setBL(x[i+1]);
        }
    }
    double like = calc_bm_prune(tr,sigma);
    //cout << like <<" " << sigma << endl;
    return -like;
}

vector<double> optimize_single_rate_bm_nlopt(rowvec & _x, mat & _vcv, bool log) {
    analysis_data a;
    a.x = _x;
    a.ovcv = _vcv;

    //nlopt::opt opt(nlopt::LN_NELDERMEAD, 2);
    //nlopt::opt opt(nlopt::LN_BOBYQA,2);
    //BOBYQA is better but the other finishes more
    nlopt::opt opt(nlopt::LN_SBPLX, 2);
    //nlopt::opt opt(nlopt::LN_PRAXIS,2);

    opt.set_lower_bounds(0.000000001);
    opt.set_upper_bounds(100000);
    opt.set_ftol_abs(0.000001);
    if (log) {
        opt.set_min_objective(nlopt_bm_sr_log, &a);
    } else {
        opt.set_min_objective(nlopt_bm_sr, &a);
    }
    opt.set_xtol_rel(0.000001);
    opt.set_maxeval(5000);

    double minf;
    //2 parameters, 1 anc, 2 rate
    vector<double> x(2,1);
    nlopt::result result = opt.optimize(x, minf);
    vector<double> results;
    results.push_back(x[0]);
    results.push_back(x[1]);
    results.push_back(minf);
    return results;
}

vector<double> optimize_single_rate_bm_ou_nlopt(rowvec & _x, mat & _vcv) {
    analysis_data a;
    a.x = _x;
    a.ovcv = _vcv;

    //nlopt::opt opt(nlopt::LN_NELDERMEAD, 3);
    //BOBYQA is better but the other finishes more
    //nlopt::opt opt(nlopt::LN_BOBYQA,3);
    nlopt::opt opt(nlopt::LN_SBPLX, 3);
    //nlopt::opt opt(nlopt::LN_PRAXIS,3);
    opt.set_min_objective(nlopt_ou_sr_log, &a);
    opt.set_lower_bounds(0.000000001);
    opt.set_upper_bounds(100000);
    opt.set_xtol_rel(0.000001);
    opt.set_ftol_rel(0.00001);
    opt.set_maxeval(5000);
    double minf;
    //2 parameters, 1 anc, 2 rate, 3 alpha
    vector<double> x(3, 1);
    nlopt::result result = opt.optimize(x, minf);
//    cout << result << endl;
    vector<double> results;
    results.push_back(x[0]); results.push_back(x[1]); results.push_back(x[2]);
    results.push_back(minf);
    return results;
}

vector<double> optimize_single_rate_bm_bl(Tree * tr) {
    analysis_data_tree a;
    a.tree = tr;
    int n = 1+tr->getNodeCount() - 1;
    //nlopt::opt opt(nlopt::LN_NELDERMEAD, n);
    //BOBYQA is better but the other finishes more
    //nlopt::opt opt(nlopt::LN_BOBYQA,n);
    nlopt::opt opt(nlopt::LN_SBPLX,n);
    //nlopt::opt opt(nlopt::LN_COBYLA,n);
    //nlopt::opt opt(nlopt::LN_PRAXIS,n);
    opt.set_min_objective(nlopt_bm_bl, &a);
    opt.set_lower_bounds(0.0001);
    opt.set_upper_bounds(100000);
    opt.set_xtol_rel(0.000001);
    opt.set_ftol_rel(0.000001);
    opt.set_maxeval(100000);
    double minf;
    vector<double> x(n, 1);
    nlopt::result result = opt.optimize(x, minf);
    cout << result << endl;
    vector<double> results;
    for (int i=0;i<tr->getNodeCount();i++){
        if (tr->getNode(i) != tr->getRoot()){
            tr->getNode(i)->setBL(x[i+1]);
        }
    }
    results.push_back(x[0]);
    results.push_back(minf);
    return results;
}