/* boost random/uniform_01.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: uniform_01.hpp 41369 2007-11-25 18:07:19Z bemandawes $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_UNIFORM_01_HPP
#define BOOST_RANDOM_UNIFORM_01_HPP

#include <iostream>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>

namespace boost {

// Because it is so commonly used: uniform distribution on the real [0..1)
// range.  This allows for specializations to avoid a costly int -> float
// conversion plus float multiplication
template<class UniformRandomNumberGenerator, class RealType = double>
class uniform_01
{
public:
  typedef UniformRandomNumberGenerator base_type;
  typedef RealType result_type;

  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);

#if !defined(BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS) && !(defined(BOOST_MSVC) && BOOST_MSVC <= 1300)
  BOOST_STATIC_ASSERT(!std::numeric_limits<RealType>::is_integer);
#endif

  explicit uniform_01(base_type rng)
    : _rng(rng),
      _factor(result_type(1) /
              (result_type((_rng.max)()-(_rng.min)()) +
               result_type(std::numeric_limits<base_result>::is_integer ? 1 : 0)))
  {
  }
  // compiler-generated copy ctor and copy assignment are fine

  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return result_type(0); }
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return result_type(1); }
  base_type& base() { return _rng; }
  const base_type& base() const { return _rng; }
  void reset() { }

  result_type operator()() {
    for (;;) {
      result_type result = result_type(_rng() - (_rng.min)()) * _factor;
      if (result < result_type(1))
	return result;
    }
  }

#if !defined(BOOST_NO_OPERATORS_IN_NAMESPACE) && !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS)
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const uniform_01& u)
  {
    os << u._rng;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, uniform_01& u)
  {
    is >> u._rng;
    return is;
  }
#endif

private:
  typedef typename base_type::result_type base_result;
  base_type _rng;
  result_type _factor;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class UniformRandomNumberGenerator, class RealType>
const bool uniform_01<UniformRandomNumberGenerator, RealType>::has_fixed_range;
#endif

} // namespace boost

#endif // BOOST_RANDOM_UNIFORM_01_HPP


/*
 * File:   MatrixCIM.cpp
 * Author: Intern: Bernardin HOUESSOU for LIP6
 *
 * Created on 6 juillet 2015, 17:48
 */

#include <armadillo>           // Armadillo header::armadillo library
#include "MatrixCIM.h"        // Calling created header/library
#include <string>            // String header::defines several functions to manipulate C++ strings
#include <iostream>         // Console commands ::defines the standard Input/Output stream objects
#include <cstring>         // For strcopy() and ....
#include "StringVector.h"           // Calling created header
//#include <ostream>
#include <iostream>

//Used namespaces
using namespace std;
using namespace arma;

//default  constructor defintiion
MatrixCIM::MatrixCIM(){}

//constructor definition     */

MatrixCIM::MatrixCIM(int nbrowmatcimInitial,int numbStateParInitial, int indiceParInitial,mat CIMtemp,std::string matnameInitial,std::string VarParentsnameInitial,StringVector listofconditionsInitial,StringVector transitionstatesvectorsInitial)
{
	//Initialization of the members variables
	nbrowmatcim=nbrowmatcimInitial;
	numbStatePar=numbStateParInitial;
  	indicePar=indiceParInitial;
  	CIM=CIMtemp;
  	matname=matnameInitial;
	VarParentsname=VarParentsnameInitial;
        listofconditions=listofconditionsInitial;                    //list des Parents variables or conditions
	transitionstatesvectors=transitionstatesvectorsInitial;       //Transition states vectors to show move from statei=>statei or statei to statej

	//copy nbVarCond characters value  from VarParents to VarParentstemp
	//strncpy(VarParentsname2,VarParentsname2Initial,nbrowmatcimInitial);
	//std::cout<< matname<<endl;
	//std::cout<< VarParentsname2Initial<<endl;

}

/****Destructor definition****/
MatrixCIM::~MatrixCIM()  { }

//Function to create and give CIM (template/synthax)
void MatrixCIM::CIMsynthax(int nbrow,int nSP, int IndPar,mat CIMtemp,std::string matname,std::string VPIni ,StringVector listofconditionsIni,StringVector transitionstatesvectorsIni)
{
	//Initialization of the members variables
  	nbrowmatcim=nbrow;
	numbStatePar=nSP;
	indicePar=IndPar;
	CIM=CIMtemp;
  	matname=matname;
        VarParentsname=VPIni;
        listofconditions=listofconditionsIni;                      //list des Parents variables or conditions
	transitionstatesvectors=transitionstatesvectorsIni;       //Transition states vectors to show move from statei=>statei or statei to statej

	//copy nbVarCond characters value  from VarParents to VarParentstemp
	//strncpy(VarParentsname2,VPI,size_t(VPI));
}
/*
void getvaraiableinitial(){

    StringVector variableinitial=splitvariable(test, delimiter1);
    size_t bb=variableinitial.size();//size of the vector 'sep'

             for(size_t i = 0; i <bb; i++){
              cout<<""<<variableinitial[i]<<endl;
             }
}*/

/*Vector split function to split matrix values only from text file
 */
StringVector splitmatrixvaluesonly112(string str, char delimiter) {
  StringVector internal;
  stringstream ss(str); // Turn the string into a stream.
  string tok;
  string tok2=tok;
  while(getline(ss, tok, delimiter))
  {
        tok=tok2;
        char delimiter2=';';
        if(getline(ss, tok, delimiter2)) {
                    if(getline(ss, tok, delimiter2)) {
           if(getline(ss, tok, delimiter2)) {
             // std::cout<<tok;
            internal.push_back(tok);
           }
        }
        }
  }
return internal;
}

/*Vector split function to split matrix values only from text file
 */
StringVector splitmatrixvaluesonly(string str, char delimiter) {
  StringVector internal;
  stringstream ss(str); // Turn the string into a stream.
  string tok;
  string tok2=tok;
  while(getline(ss, tok, delimiter))
  {
        tok=tok2;
        char delimiter2=';';
        if(getline(ss, tok, delimiter2)) {
           if(getline(ss, tok, delimiter2)) {
             // std::cout<<tok;
            internal.push_back(tok);
           }
        }
  }
return internal;
}

void MatrixCIM::getcimtransitionstate (){

}

void MatrixCIM::findcimwithsamevar(){}

void MatrixCIM::displayCIM (){

}

//***end of File MatrixCIM.cpp


#include "cnn/approximator.h"

#include <string>
#include <cassert>
#include <vector>
#include <iostream>
#include <boost/lexical_cast.hpp>
#include "cnn/nodes.h"
#include "cnn/expr-xtra.h"
#include <fstream>

using namespace std;
using namespace cnn::expr;
using namespace cnn;

namespace cnn {

    enum { X2C = 0, X2CB};

    void ClsBasedBuilder::display(ComputationGraph& cg) {
        display_value(i_cls, cg);
        display_value(i_cls_bias, cg);
        for (unsigned i = 0; i < ncls; ++i) {
            const std::vector<Expression>& vars = param_vars[i];
            for (size_t i = 0; i < vars.size(); i++)
                display_value(vars[i], cg);
        }
    }

    ClsBasedBuilder::ClsBasedBuilder(
        const unsigned int input_dim,
        const vector<int>& cls2nbrwords, /// #words for each class, class starts from 0
        const vector<long>& word2cls,
        const vector<long>& dict_wrd_id2within_class_id,
        Model& model,
        cnn::real iscale,
        string name) : input_dim(input_dim),
        clssize(cls2nbrwords), word2cls(word2cls), dict_wrd_id2within_class_id(dict_wrd_id2within_class_id)
    {
        unsigned int n_cls = clssize.size();
        p_cls = model.add_parameters({ n_cls, input_dim }, iscale, name + " to cls");
        p_cls_bias = model.add_parameters({ n_cls }, iscale, name + " to cls bias");
        for (size_t id = 0; id < n_cls; id++)
        {
            unsigned int  clssize = cls2nbrwords[id];
            p_R.push_back(model.add_parameters({ clssize, input_dim }, iscale, name + " to cls " + boost::lexical_cast<string>(id)));
            p_bias.push_back(model.add_parameters({ clssize }, iscale, name + " to cls bias " + boost::lexical_cast<string>(id)));
        }
    }

    void ClsBasedBuilder::new_graph_impl(ComputationGraph& cg) {
        param_vars.clear();
        i_cls = parameter(cg, p_cls);
        i_cls_bias = parameter(cg, p_cls_bias);
        for (unsigned i = 0; i < clssize.size(); ++i) {
            Parameters* p_x2h = p_R[i];
            Parameters* p_x2hb = p_bias[i];
            Expression i_x2h = parameter(cg, p_x2h);
            Expression i_x2hb = parameter(cg, p_x2hb);
            vector<Expression> vars = { i_x2h, i_x2hb };

            param_vars.push_back(vars);
        }
        errors.clear();
        set_data_in_parallel(1);
    }

    void ClsBasedBuilder::set_data_in_parallel(int n)
    {
        dparallel = n;

        errors.clear();
        errors.resize(n);
    }

    Expression ClsBasedBuilder::add_input_impl(const Expression &in, long target_wordid, unsigned uttid) {
        int cls_id = word2cls[target_wordid];
        Expression newin = reshape(in, { input_dim * dparallel });
        Expression x = pickrange(newin, uttid * input_dim, (uttid + 1)*input_dim);
        vector<Expression> param = param_vars[cls_id];
        Expression i_r_t = param[X2CB] + param[X2C] * x;
        Expression i_c_t = i_cls_bias + i_cls * x;
        Expression i_err_cls = pick(log_softmax(i_c_t), cls_id);
        Expression i_err_prb = pick(log_softmax(i_r_t), dict_wrd_id2within_class_id[target_wordid]);
        errors[uttid].push_back(-(i_err_cls + i_err_prb));

        return errors[uttid].back();
    }

    vector<cnn::real> ClsBasedBuilder::respond(const Expression &in, ComputationGraph& cg)
    {
        Expression cls_log_prob = log_softmax(i_cls * in + i_cls_bias);
        vector<Expression> within_cls_log_prob;
        for (int cli = 0; cli < clssize.size(); cli++)
        {
            vector<Expression> param = param_vars[cli];
            Expression i_r_t = param[X2CB] + param[X2C] * in;
            within_cls_log_prob.push_back(log_softmax(i_r_t));
        }
        auto cls_dist = get_value(cls_log_prob, cg);
        vector<cnn::real> dist;
        for (int cli = 0; cli < clssize.size(); cli++){
            auto within_cls_dist = get_value(within_cls_log_prob[cli], cg);
            vector<cnn::real> vtmp(clssize[cli], cls_dist[cli]);
            std::transform(within_cls_dist.begin(), within_cls_dist.end(), vtmp.begin(), within_cls_dist.begin(), std::plus<cnn::real>());
            dist.insert(dist.end(), within_cls_dist.begin(), within_cls_dist.end());
        }

        return dist;
    }

    void ClsBasedBuilder::copy(const ClsBasedBuilder& othercls) {
        const ClsBasedBuilder& clsref = (const ClsBasedBuilder&)othercls;
        assert(p_R.size() == clsref.p_R.size());
        assert(p_bias.size() == clsref.p_bias.size());
        for (size_t i = 0; i < clsref.p_R.size(); ++i) {
            p_R[i]->copy(*clsref.p_R[i]);
            p_bias[i]->copy(*clsref.p_bias[i]);
        }
        p_cls->copy(*clsref.p_cls);
        p_cls_bias->copy(*clsref.p_cls_bias);
    }

    /**
    dict_wrd_id2within_class_id : the dictionary word id to the id inside a class
    */
    void ClsBasedBuilder::load_word2cls_fn(string word2clsfn, Dict& sd, std::vector<long>& wrd2cls, std::vector<long>& dict_wrd_id2within_class_id, std::vector<int> & cls2size)
    {
        ifstream in(word2clsfn);
        string line;
        /// go through one line at a time to match dictionay
        /// it is possible that the word clustering may have different words from the dictionary,
        /// so go through one more to add missing words into the dictionary
        vector<string> missing_words;
        while (getline(in, line)) {
            std::istringstream in(line);
            std::string word;

            in >> word;
            if (sd.Contains(word) == false)
                missing_words.push_back(word);
            sd.Convert(word, true);
        }
        in.close();

        in.open(word2clsfn);
        wrd2cls.resize(sd.size());
        dict_wrd_id2within_class_id.resize(sd.size(), -1);
        map<int, int> cls2acccnt; /// the count for each class so far
        while (getline(in, line)) {

            std::istringstream in(line);
            std::string word;
            string cls;

            in >> word;
            in >> cls;

            int icls = boost::lexical_cast<int>(cls)-1;
            int wridx = -1;
            if (sd.Contains(word))
            {
                wridx = sd.Convert(word, true);

                wrd2cls[wridx] = icls;
                if (cls2acccnt.find(icls) == cls2acccnt.end())
                    cls2acccnt[icls] = 1;
                else
                    cls2acccnt[icls] += 1;
                dict_wrd_id2within_class_id[wridx] = cls2acccnt[icls] - 1;
            }
        }
        in.close();

        for (auto&p : sd.GetWordList())
        {
            long wd = sd.Convert(p);
            if (wrd2cls[wd] == -1)
                throw("check word clustering procedure as there are words in dictionary that are not clustered");
        }

        cls2size.clear();
        for (int i = 0; i < cls2acccnt.size(); i++)
        {
            cls2size.push_back(cls2acccnt[i]);
        }
    }

} // namespace cnn


#include <boost/numeric/odeint/iterator/integrate/integrate_adaptive.hpp>


/*
  Copyright (c) 2015 DataStax

  Licensed under the Apache License, Version 2.0 (the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at

  http://www.apache.org/licenses/LICENSE-2.0

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
*/

// Based on public domain source found here:
// http://openwall.info/wiki/people/solar/software/public-domain-source-code/md5

#ifndef __CASS_MD5_HPP_INCLUDED__
#define __CASS_MD5_HPP_INCLUDED__

#include "macros.hpp"

#include <boost/cstdint.hpp>

namespace cass {

class Md5 {
public:
  Md5();

  void update(const uint8_t* data, size_t size);
  void final(uint8_t* result);

private:
  const uint8_t* body(const uint8_t* data, size_t size);

private:
  // Any 32-bit or wider unsigned integer data type will do
  typedef uint32_t MD5_u32plus;

  MD5_u32plus lo_, hi_;
  MD5_u32plus a_, b_, c_, d_;
  unsigned char buffer_[64];
  MD5_u32plus block_[16];

  DISALLOW_COPY_AND_ASSIGN(Md5);
};

} // namespace cass

#endif


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "MultiIndexIterator.h"
#define BOOST_TEST_MODULE MultiIndexIterator test
#include <boost/test/unit_test.hpp>

using namespace cpputils;

typedef IdxTiny<3> Idx;

// BOOST_AUTO_TEST_SUITE( test_suite )

BOOST_AUTO_TEST_CASE( iterator )
{
  Idx lbound(-2,0,1), ubound(-1,1,3);

  MultiIndexIterator<3>
    begin(lbound,ubound,std::false_type()),
    end  (lbound,ubound,std:: true_type());

  BOOST_CHECK(all(*begin++==lbound));

  BOOST_CHECK(all(*begin++==Idx(-2,0,2)));
  BOOST_CHECK(all(*begin++==Idx(-2,0,3)));
  BOOST_CHECK(all(*begin++==Idx(-2,1,1)));
  BOOST_CHECK(all(*begin++==Idx(-2,1,2)));
  BOOST_CHECK(all(*begin++==Idx(-2,1,3)));
  BOOST_CHECK(all(*begin++==Idx(-1,0,1)));
  BOOST_CHECK(all(*begin++==Idx(-1,0,2)));
  BOOST_CHECK(all(*begin++==Idx(-1,0,3)));
  BOOST_CHECK(all(*begin++==Idx(-1,1,1)));
  BOOST_CHECK(all(*begin++==Idx(-1,1,2)));
  BOOST_CHECK(all(*begin++==Idx(-1,1,3)));

  BOOST_CHECK(begin++==end);

  BOOST_CHECK(all(*end    ==Idx(0,0,1)));

  BOOST_CHECK(all(*begin  ==Idx(0,0,2)));

}

// BOOST_AUTO_TEST_SUITE_END()


#include "ros/ros.h"
#include "sensor_msgs/Image.h"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc_c.h"
#include "opencv2/highgui/highgui.hpp"
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <dynamic_reconfigure/server.h>
#include <geometry_msgs/PointStamped.h>
#include <sensor_msgs/image_encodings.h>
#include <bits/stdc++.h>
#include <stdlib.h>
#include <string>
#include <std_msgs/Bool.h>
#include <boost/thread.hpp>

#include <vision_tasks/gateBottomRangeConfig.h>
#include <vision_tasks/gateFrontRangeConfig.h>
#include <vision_commons/filter.h>
#include <vision_commons/contour.h>
#include <vision_commons/morph.h>
#include <vision_commons/threshold.h>
#include <vision_commons/geometry.h>
#include <gate.h>

Gate::Gate(){
    this->front_clahe_clip_ = 4.0;
    this->front_clahe_grid_size_ = 8;
    this->front_clahe_bilateral_iter_ = 8;
    this->front_balanced_bilateral_iter_ = 4;
    this->front_denoise_h_ = 10.0;
    this->front_low_h_ = 0;
    this->front_high_h_ = 20;
    this->front_low_s_ = 0;
    this->front_high_s_ = 92;
    this->front_low_v_ = 46;
    this->front_high_v_ = 255;
    this->front_closing_mat_point_ = 1;
    this->front_closing_iter_ = 1;
    this->front_canny_threshold_low_ = 0;
    this->front_canny_threshold_high_ = 1000;
    this->front_canny_kernel_size_ = 3;
    this->front_hough_threshold_ = 0;
    this->front_hough_minline_ = 0;
    this->front_hough_maxgap_ = 0;
    this->front_hough_angle_tolerance_ = 0.0;
    this->front_gate_distance_tolerance_ = 50.0;
    this->front_gate_angle_tolerance_ = 0.0;

    this->bottom_clahe_clip_ = 4.0;
    this->bottom_clahe_grid_size_ = 8;
    this->bottom_clahe_bilateral_iter_ = 8;
    this->bottom_balanced_bilateral_iter_ = 4;
    this->bottom_denoise_h_ = 10.0;
    this->bottom_low_h_ = 10;
    this->bottom_low_s_ = 0;
    this->bottom_low_v_ = 0;
    this->bottom_high_h_ = 90;
    this->bottom_high_s_ = 255;
    this->bottom_high_v_ = 255;
    this->bottom_closing_mat_point_ = 1;
    this->bottom_closing_iter_ = 1;

    this->camera_frame_ = "auv-iitk";
}

cv::Point2i Gate::rotatePoint(const cv::Point2i &v1, const cv::Point2i &v2, float angle)
{
	if (v1.x > v2.x)
	{
		cv::Point2i v3 = v1 - v2;
		cv::Point2i finalVertex;
		finalVertex.x = v3.x * cos(angle) - v3.y * sin(angle);
		finalVertex.y = v3.x * sin(angle) + v3.y * cos(angle);
		finalVertex = finalVertex + v2;
		return finalVertex;
	}
	else
	{
		cv::Point2i v3 = v2 - v1;
		cv::Point2i finalVertex;
		finalVertex.x = v3.x * cos(angle) - v3.y * sin(angle);
		finalVertex.y = v3.x * sin(angle) + v3.y * cos(angle);
		finalVertex = finalVertex + v1;
		return finalVertex;
	}
};

void Gate::TaskHandling(bool status){
	if(status)
	{
		spin_thread_front = new boost::thread(&Gate::frontTaskHandling, this);
		spin_thread_bottom = new boost::thread(&Gate::bottomTaskHandling, this);
	}
	else
	{
        spin_thread_front->join();
        spin_thread_bottom->join();
	}
	std::cout << "Task Handling function over" << std::endl;
}

void Gate::frontCallback(vision_tasks::gateFrontRangeConfig &config, double level)
{
    Gate::front_clahe_clip_ = config.clahe_clip;
	Gate::front_clahe_grid_size_ = config.clahe_grid_size;
	Gate::front_clahe_bilateral_iter_ = config.clahe_bilateral_iter;
	Gate::front_balanced_bilateral_iter_ = config.balanced_bilateral_iter;
	Gate::front_denoise_h_ = config.denoise_h;
	Gate::front_low_h_ = config.low_h;
	Gate::front_high_h_ = config.high_h;
	Gate::front_low_s_ = config.low_s;
	Gate::front_high_s_ = config.high_s;
	Gate::front_low_v_ = config.low_v;
	Gate::front_high_v_ = config.high_v;
	Gate::front_closing_mat_point_ = config.closing_mat_point;
	Gate::front_closing_iter_ = config.closing_iter;
	Gate::front_canny_threshold_low_ = config.canny_threshold_low;
	Gate::front_canny_threshold_high_ = config.canny_threshold_high;
	Gate::front_canny_kernel_size_ = config.canny_kernel_size;
	Gate::front_hough_threshold_ = config.hough_threshold;
	Gate::front_hough_minline_ = config.hough_minline;
	Gate::front_hough_maxgap_ = config.hough_maxgap;
	Gate::front_hough_angle_tolerance_ = config.hough_angle_tolerance;
	Gate::front_gate_distance_tolerance_ = config.gate_distance_tolerance;
	Gate::front_gate_angle_tolerance_ = config.gate_angle_tolerance;
}

void Gate::bottomCallback(vision_tasks::gateBottomRangeConfig &config, double level)
{
    Gate::bottom_clahe_clip_ = config.clahe_clip;
    Gate::bottom_clahe_grid_size_ = config.clahe_grid_size;
    Gate::bottom_clahe_bilateral_iter_ = config.clahe_bilateral_iter;
    Gate::bottom_balanced_bilateral_iter_ = config.balanced_bilateral_iter;
    Gate::bottom_denoise_h_ = config.denoise_h;
    Gate::bottom_low_h_ = config.low_h;
    Gate::bottom_high_h_ = config.high_h;
    Gate::bottom_low_s_ = config.low_s;
    Gate::bottom_high_s_ = config.high_s;
    Gate::bottom_low_v_ = config.low_v;
    Gate::bottom_high_v_ = config.high_v;
    Gate::bottom_closing_mat_point_ = config.closing_mat_point;
    Gate::bottom_closing_iter_ = config.closing_iter;
}

void Gate::bottomTaskHandling()
{
	dynamic_reconfigure::Server<vision_tasks::gateBottomRangeConfig> server;
	dynamic_reconfigure::Server<vision_tasks::gateBottomRangeConfig>::CallbackType f_bottom;
	f_bottom = boost::bind(&Gate::bottomCallback, this, _1, _2);
	server.setCallback(f_bottom);

	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
    image_transport::ImageTransport it(nh);
	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
    image_transport::Publisher blue_filtered_pub = it.advertise("/gate_task/bottom/blue_filtered", 1);
	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
    image_transport::Publisher thresholded_pub = it.advertise("/gate_task/bottom/thresholded", 1);
	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
    image_transport::Publisher marked_pub = it.advertise("/gate_task/bottom/marked", 1);
	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
    ros::Publisher coordinates_pub = nh.advertise<geometry_msgs::PointStamped>("/gate_task/bottom/pipe_coordinates", 1000);
	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
    ros::Publisher task_done_pub = nh.advertise<std_msgs::Bool>("/gate_task/done", 1000);

	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
    image_transport::Subscriber bottom_image_sub = it.subscribe("/bottom_camera/image_raw", 1, &Gate::imageCallback, this);

	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
    cv::Scalar pipe_center_color(255, 255, 255);
	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
    cv::Scalar image_center_color(0, 0, 0);
	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
    cv::Scalar bounding_rectangle_color(255, 0, 0);

    cv::Mat blue_filtered;
    cv::Mat image_hsv;
    cv::Mat image_thresholded;
	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
    geometry_msgs::PointStamped pipe_point_message;
	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
    pipe_point_message.header.frame_id = camera_frame_.c_str();
	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
    std::vector<std::vector<cv::Point> > contours;
    cv::RotatedRect bounding_rectangle;
    std_msgs::Bool task_done_message;
    cv::Mat image_marked;
    cv::Point2f vertices2f[4];
    cv::Point vertices[4];

    while (ros::ok())
    {
        if (!image_.empty())
        {
        image_.copyTo(image_marked);
	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
        blue_filtered = vision_commons::Filter::blue_filter(image_, bottom_clahe_clip_, bottom_clahe_grid_size_, bottom_clahe_bilateral_iter_, bottom_balanced_bilateral_iter_, bottom_denoise_h_);
	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
        if (bottom_high_h_ > bottom_low_h_ && bottom_high_s_ > bottom_low_s_ && bottom_high_v_ > bottom_low_v_)
        {
	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
            cv::cvtColor(blue_filtered, image_hsv, CV_BGR2HSV);
	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
            image_thresholded = vision_commons::Threshold::threshold(image_hsv, bottom_low_h_, bottom_high_h_, bottom_low_s_, bottom_high_s_, bottom_low_v_, bottom_high_v_);
	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
            image_thresholded = vision_commons::Morph::close(image_thresholded, 2 * bottom_closing_mat_point_ + 1, bottom_closing_mat_point_, bottom_closing_mat_point_, bottom_closing_iter_);
	void imageCallback(const sensor_msgs::Image::ConstPtr &msg);
            contours = vision_commons::Contour::getBestX(image_thresholded, 1);
            if (contours.size() != 0)
            {
            bounding_rectangle = cv::minAreaRect(cv::Mat(contours[0]));
            pipe_point_message.header.stamp = ros::Time();
            pipe_point_message.point.x = (image_.size().height) / 2 - bounding_rectangle.center.y;
            pipe_point_message.point.y = bounding_rectangle.center.x - (image_.size().width) / 2;
            pipe_point_message.point.z = 0.0;
            task_done_message.data = pipe_point_message.point.x < 0;
            bounding_rectangle.points(vertices2f);
            for (int i = 0; i < 4; ++i)
            {
                vertices[i] = vertices2f[i];
            }
            cv::circle(image_marked, bounding_rectangle.center, 1, pipe_center_color, 8, 0);
            cv::circle(image_marked, cv::Point(image_.size().width / 2, image_.size().height / 2), 1, image_center_color, 8, 0);
            cv::fillConvexPoly(image_marked, vertices, 4, bounding_rectangle_color);
            }
        }
        blue_filtered_pub.publish(cv_bridge::CvImage(pipe_point_message.header, "bgr8", blue_filtered).toImageMsg());
        thresholded_pub.publish(cv_bridge::CvImage(pipe_point_message.header, "mono8", image_thresholded).toImageMsg());
        coordinates_pub.publish(pipe_point_message);
        ROS_INFO("Pipe Center (x, y) = (%.2f, %.2f)", pipe_point_message.point.x, pipe_point_message.point.y);
        task_done_pub.publish(task_done_message);
        ROS_INFO("Task done (bool) = %s", task_done_message.data ? "true" : "false");
        marked_pub.publish(cv_bridge::CvImage(pipe_point_message.header, "bgr8", image_marked).toImageMsg());
        }
        else
            ROS_INFO("Image empty");
        ros::spinOnce();
    }
}

void Gate::frontTaskHandling()
{
	dynamic_reconfigure::Server<vision_tasks::gateFrontRangeConfig> server;
	dynamic_reconfigure::Server<vision_tasks::gateFrontRangeConfig>::CallbackType f_front;
	f_front = boost::bind(&Gate::frontCallback, this, _1, _2);
	server.setCallback(f_front);

    image_transport::ImageTransport it(nh);
	image_transport::Publisher blue_filtered_pub = it.advertise("/gate_task/front/blue_filtered", 1);
	image_transport::Publisher thresholded_pub = it.advertise("/gate_task/front/thresholded", 1);
	image_transport::Publisher canny_pub = it.advertise("/gate_task/front/canny", 1);
	image_transport::Publisher lines_pub = it.advertise("/gate_task/front/lines", 1);
	image_transport::Publisher marked_pub = it.advertise("/gate_task/front/marked", 1);
	ros::Publisher coordinates_pub = nh.advertise<geometry_msgs::PointStamped>("/gate_task/front/gate_coordinates", 1000);

	image_transport::Subscriber front_image_sub = it.subscribe("/front_camera/image_raw", 1, &Gate::imageCallback, this);
	//image_transport::Subscriber front_image_sub = it.subscribe("/varun/sensors/front_camera/image_raw", 1, &Gate::imageCallback, this);

	cv::Scalar gate_center_color(255, 255, 255);
	cv::Scalar image_center_color(0, 0, 0);
	cv::Scalar hough_line_color(0, 164, 253);
	cv::Scalar horizontal_line_color(253, 0, 127);
	cv::Scalar vertical_line_color(0, 253, 0);

	cv::Mat blue_filtered;
	cv::Mat image_hsv;
	cv::Mat image_thresholded;
	cv::Mat image_gray;
	cv::Mat image_canny;
	cv::Mat image_lines;
	cv::Mat image_marked;
	std::vector<cv::Vec4i> lines;
	std::vector<cv::Vec4i> lines_filtered;
	std::vector<double> angles;
	int found = 0;
	cv::Point2i pi1;
	cv::Point2i pi2;
	cv::Point2i pj1;
	cv::Point2i pj2;
	cv::Point2i horizontal1(0, 0);
	cv::Point2i horizontal2(0, 0);
	cv::Point2i vertical1(0, 0);
	cv::Point2i vertical2(0, 0);
	cv::Point2i longest1(0, 0);
	cv::Point2i longest2(0, 0);
	geometry_msgs::PointStamped gate_point_message;
	gate_point_message.header.frame_id = camera_frame_.c_str();

	while (ros::ok())
	{
		if (!image_.empty())
		{
			image_.copyTo(image_marked);
			//blue_filtered = vision_commons::Filter::blue_filter(image_, clahe_clip_, clahe_grid_size_, clahe_bilateral_iter_, balanced_bilateral_iter_, denoise_h_);
			if (front_high_h_ > front_low_h_ && front_high_s_ > front_low_s_ && front_high_v_ > front_low_v_)
			{
				cv::cvtColor(image_, image_hsv, CV_BGR2HSV);
				image_thresholded = vision_commons::Threshold::threshold(image_hsv, front_low_h_, front_high_h_, front_low_s_, front_high_s_, front_low_v_, front_high_v_);
				image_thresholded = vision_commons::Morph::close(image_thresholded, 2 * front_closing_mat_point_ + 1, front_closing_mat_point_, front_closing_mat_point_, front_closing_iter_);
				cv::cvtColor(image_thresholded, image_gray, CV_GRAY2BGR);
				cv::Canny(image_gray, image_canny, front_canny_threshold_low_, front_canny_threshold_high_, front_canny_kernel_size_);
				image_lines = blue_filtered;
				cv::HoughLinesP(image_thresholded, lines, 1, CV_PI / 180, front_hough_threshold_, front_hough_minline_, front_hough_maxgap_);
				double angle = 0.0;
				for (int i = 0; i < lines.size(); i++)
				{
					cv::Point p1(lines[i][0], lines[i][1]);
					cv::Point p2(lines[i][2], lines[i][3]);
					angle = vision_commons::Geometry::angleWrtY(p1, p2);
					if (angle < front_hough_angle_tolerance_ || abs(angle - 90.0) < front_hough_angle_tolerance_ || abs(180.0 - angle) < front_hough_angle_tolerance_)
					{
						cv::line(image_lines, p1, p2, hough_line_color, 3, CV_AA);
						lines_filtered.push_back(lines[i]);
						angles.push_back(angle);
					}
				}
				for (int i = 0; i < lines_filtered.size(); i++)
				{
					pi1.x = lines_filtered[i][0];
					pi1.y = lines_filtered[i][1];
					pi2.x = lines_filtered[i][2];
					pi2.y = lines_filtered[i][3];
					if (vision_commons::Geometry::distance(pi1, pi2) > vision_commons::Geometry::distance(longest1, longest2))
					{
						longest1.x = pi1.x;
						longest1.y = pi1.y;
						longest2.x = pi2.x;
						longest2.y = pi2.y;
					}
					for (int j = i + 1; j < lines_filtered.size(); j++)
					{
						pj1.x = lines_filtered[j][0];
						pj1.y = lines_filtered[j][1];
						pj2.x = lines_filtered[j][2];
						pj2.y = lines_filtered[j][3];
						if (abs(angles[i] - angles[j] - 90.0) < front_gate_angle_tolerance_)
						{
							double distance1 = vision_commons::Geometry::distance(pi1, pj1);
							double distance2 = vision_commons::Geometry::distance(pi1, pj2);
							double distance3 = vision_commons::Geometry::distance(pi2, pj1);
							double distance4 = vision_commons::Geometry::distance(pi2, pj2);
							if (distance1 < front_gate_distance_tolerance_ || distance2 < front_gate_distance_tolerance_ || distance3 < front_gate_distance_tolerance_ || distance4 < front_gate_distance_tolerance_)
							{
								if (abs(angles[j] - 90.0) < abs(angles[i] - 90.0) && (vision_commons::Geometry::distance(pj1, pj2) > vision_commons::Geometry::distance(horizontal1, horizontal2) || vision_commons::Geometry::distance(pi1, pi2) > vision_commons::Geometry::distance(vertical1, vertical2)))
								{
									horizontal1.x = pj1.x;
									horizontal1.y = pj1.y;
									horizontal2.x = pj2.x;
									horizontal2.y = pj2.y;
									vertical1.x = pi1.x;
									vertical1.y = pi1.y;
									vertical2.x = pi2.x;
									vertical2.y = pi2.y;
									found = 1;
								}
								else if (abs(angles[j] - 90.0) > abs(angles[i] - 90.0) && (vision_commons::Geometry::distance(pi1, pi2) > vision_commons::Geometry::distance(horizontal1, horizontal2) || vision_commons::Geometry::distance(pj1, pj2) > vision_commons::Geometry::distance(vertical1, vertical2)))
								{
									horizontal1.x = pi1.x;
									horizontal1.y = pi1.y;
									horizontal2.x = pi2.x;
									horizontal2.y = pi2.y;
									vertical1.x = pj1.x;
									vertical1.y = pj1.y;
									vertical2.x = pj2.x;
									vertical2.y = pj2.y;
									found = 1;
								}
							}
						}
					}
				}
				gate_point_message.header.stamp = ros::Time();
				if (found)
				{
					gate_point_message.point.x = pow(sqrt(pow(vision_commons::Geometry::distance(horizontal1, horizontal2), 2) + pow(vision_commons::Geometry::distance(vertical1, vertical2), 2)) / 7526.5, -.92678);
					gate_point_message.point.y = (horizontal1.x + horizontal2.x) / 2 - image_.size().width / 2;
					gate_point_message.point.z = image_.size().height / 2 - (vertical1.y + vertical2.y) / 2;
					ROS_INFO("Gate Center (x, y, z) = (%.2f, %.2f, %.2f)", gate_point_message.point.x, gate_point_message.point.y, gate_point_message.point.z);
					cv::line(image_marked, horizontal1, horizontal2, horizontal_line_color, 3, CV_AA);
					cv::line(image_marked, vertical1, vertical2, vertical_line_color, 3, CV_AA);
				}
				else
				{
					if (abs(vision_commons::Geometry::angleWrtY(longest1, longest2) - 90.0) < front_hough_angle_tolerance_)
					{
						gate_point_message.point.x = pow((vision_commons::Geometry::distance(longest1, longest2) * 1.068) / 7526.5, -.92678);
						gate_point_message.point.y = (longest1.x + longest2.x) / 2 - image_.size().width / 2;
						gate_point_message.point.z = image_.size().height / 2 - (longest1.y + longest2.y) / 2 + 9 * vision_commons::Geometry::distance(longest1, longest2) / 48;
					}
					else
					{
						gate_point_message.point.x = pow((vision_commons::Geometry::distance(longest1, longest2) * 2.848) / 7526.5, -.92678);
						gate_point_message.point.y = (longest1.x + longest2.x) / 2 + 12 * vision_commons::Geometry::distance(longest1, longest2) / 9 - image_.size().width / 2;
						gate_point_message.point.z = image_.size().height / 2 - (longest1.y + longest2.y) / 2;
					}
					cv::line(image_marked, longest1, longest2, hough_line_color, 3, CV_AA);
					ROS_INFO("Couldn't find gate, estimated gate center (x, y, z) = (%.2f, %.2f, %.2f)", gate_point_message.point.x, gate_point_message.point.y, gate_point_message.point.z);
				}
				cv::circle(image_marked, cv::Point(gate_point_message.point.y + image_.size().width / 2, image_.size().height / 2 - gate_point_message.point.z), 1, gate_center_color, 8, 0);
				cv::circle(image_marked, cv::Point(image_.size().width / 2, image_.size().height / 2), 1, image_center_color, 8, 0);
			}
			blue_filtered_pub.publish(cv_bridge::CvImage(gate_point_message.header, "bgr8", blue_filtered).toImageMsg());
			thresholded_pub.publish(cv_bridge::CvImage(gate_point_message.header, "mono8", image_thresholded).toImageMsg());
			canny_pub.publish(cv_bridge::CvImage(gate_point_message.header, "mono8", image_canny).toImageMsg());
			lines_pub.publish(cv_bridge::CvImage(gate_point_message.header, "bgr8", image_lines).toImageMsg());
			coordinates_pub.publish(gate_point_message);
			marked_pub.publish(cv_bridge::CvImage(gate_point_message.header, "bgr8", image_marked).toImageMsg());
		}
		else
			ROS_INFO("Image empty");
		ros::spinOnce();
	}
}

void Gate::imageCallback(const sensor_msgs::Image::ConstPtr &msg)
{
	cv_bridge::CvImagePtr cv_img_ptr;
	try
	{
		image_ = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8)->image;
	}
	catch (cv_bridge::Exception &e)
	{
		ROS_ERROR("cv_bridge exception: %s", e.what());
	}
	catch (cv::Exception &e)
	{
		ROS_ERROR("cv exception: %s", e.what());
	}
};


#include "VoxelGeometry.h"

#include <osg/ShapeDrawable>
#include <osg/Material>

#include <boost/qvm/all.hpp>

#include "cuda_vector_functions_interop.h"

#include "qvm_osg.h"
#include "qvm_cuda.h"

#include "Voxel.h"

VoxelGeometry::VoxelGeometry(const sfs::Voxel * pVoxel) :
	mep_voxel(pVoxel),
	m_voxelGeode(),
	m_material(new osg::Material())
{
	// empty
}

void VoxelGeometry::createGeometry()
{
	this->removeChild(m_voxelGeode);
	m_voxelGeode.release();

	if (mep_voxel->isActive)
	{
		m_voxelGeode = new osg::Geode();
		this->addChild(m_voxelGeode);

		const osg::Vec3 center = boost::qvm::convert_to<osg::Vec3>(mep_voxel->center);
		const osg::Vec3 size = boost::qvm::convert_to<osg::Vec3>(mep_voxel->size);

		osg::ref_ptr<osg::Box> box = new osg::Box(center, size[0], size[1], size[2]);

		osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable(box);

		osg::Vec4 color(1.0, 1.0, 1.0, 1);

		m_material->setDiffuse(osg::Material::FRONT, color);
		getOrCreateStateSet()->setAttribute(m_material);

		m_voxelGeode->addDrawable(sd);
	}
}

void VoxelGeometry::setMaterial(const osg::Material & material)
{
	m_material = new osg::Material(material);
	getOrCreateStateSet()->setAttribute(m_material);
}


#include <rovi_pose_estimator/rovi_pose_estimator.h>

#include <iostream>
#include <vector>
#include <tuple>

#include <Eigen/Eigen>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <std_msgs/Int32.h>
#include <cv_bridge/cv_bridge.h>
#include <eigen_conversions/eigen_msg.h>

#include <opencv4/opencv2/imgproc.hpp>
#include <opencv4/opencv2/imgcodecs.hpp>
#include <opencv4/opencv2/highgui.hpp>
#include <opencv4/opencv2/flann.hpp>
#include <opencv4/opencv2/calib3d.hpp>
#include <opencv4/opencv2/core/eigen.hpp>

#include <rovi_utils/rovi_utils.h>
#include <rovi_gazebo/rovi_gazebo.h>

namespace rovi_pose_estimator
{

std::array<cv::Mat, 2>
M3::get_image_data(const std::string & ns_ros)
{
    static std::mutex mutex_image_l;
    static sensor_msgs::Image image_l;
    static auto thread_left = rovi_utils::create_async_listener(ns_ros + "/left/image_raw", image_l, mutex_image_l);

    static std::mutex mutex_image_r;
    static sensor_msgs::Image image_r;
    static auto thread_right = rovi_utils::create_async_listener(ns_ros + "/right/image_raw", image_r, mutex_image_r);

    static bool first = true;

    std::array<cv::Mat, 2> arr;
    cv::Mat temp_l, temp_r;

    std::lock_guard<std::mutex> lock_l(mutex_image_l);
    std::lock_guard<std::mutex> lock_r(mutex_image_r);

    auto ptr_image_l = boost::make_shared<const sensor_msgs::Image>(image_l);
    auto ptr_image_r = boost::make_shared<const sensor_msgs::Image>(image_r);

    cv_bridge::toCvShare(ptr_image_l, "bgr8")->image.copyTo(temp_l);
    arr[0] = temp_l;
    cv_bridge::toCvShare(ptr_image_r, "bgr8")->image.copyTo(temp_r);
    arr[1] = temp_r;

    return arr;
}

std::array<sensor_msgs::CameraInfo, 2>
M3::get_image_info(const std::string & ns_ros)
{
    std::array<sensor_msgs::CameraInfo, 2> arr;

    for (const auto & [idx, cam] : std::array{std::tuple(0, std::string("/left/camera_info")), std::tuple(1, std::string("/right/camera_info"))})
    {
        // get the message
        auto msg = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(ns_ros + cam);
        arr[idx] = *msg;
    }
    return arr;
}

cv::Mat
M3::create_mask(const cv::Mat & img, const std::vector<cv::Point> & pts)
{
    cv::Mat mask(img.rows, img.cols, CV_8UC1, cv::Scalar(0));
    std::vector<std::vector<cv::Point>> cords;
    cords.push_back(pts);
    cv::drawContours(mask, cords, 0, cv::Scalar(255), -1, 8 );
    return mask;
}

cv::Mat
M3::tsh_mask(const cv::Mat & img_color, const std::array<double, 3> & lower_rgb, const std::array<double, 3> & upper_rgb)
{
    cv::Mat mask;
    std::vector<cv::Mat> masks;
    cv::split(img_color, masks);
    for(auto i = 0; lower_rgb.size() > i; ++i)
        cv::threshold(masks[i], masks[i], lower_rgb[i], upper_rgb[i], cv::THRESH_BINARY);
    cv::bitwise_and(masks[0], masks[1], mask, masks[2]);
    return mask;
}

cv::Mat
M3::find_contour(const cv::Mat & mask)
{
    cv::Mat img_cpy;
    std::vector<std::vector<cv::Point>> contour_pts;
    cv::findContours(mask, contour_pts, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
    cv::Mat contour_mat = cv::Mat::zeros(mask.size(), mask.type());
    if (contour_pts.size() == 0)
        return contour_mat;
    for (auto i = 0; contour_pts.size() > i; i++)
        cv::drawContours(contour_mat, contour_pts, i, cv::Scalar(255), cv::FILLED);
    return contour_mat;
};

double
M3::L2_stereo(const cv::Point2d & left, const cv::Point2d & right, const double y_tsh)
{
    if (left.x < right.x || abs(left.y - right.y) > y_tsh)
        return std::numeric_limits<double>::max();
    else
        return std::pow((left.x - right.x), 2) + std::pow((left.y - right.y), 2);
}

cv::Mat
M3::stereo_triangulation(const std::vector<std::array<cv::Point2d, 2>> & pts, const cv::Mat & Q, const bool & kmeans)
{
    cv::Mat T = ( cv::Mat_<double>(4,4) <<  0.f, 0.f, 1.f, 0.f,
                                           -1.f, 0.f, 0.f, 0.f,
                                            0.f,-1.f, 0.f, 0.f,
                                            0.f, 0.f, 0.f, 1.f );

    // Create disparity points (x, y, d(x,y), 1)
    cv::Mat m( cv::Size( 4, pts.size() ), CV_64FC1);

    // Get the pose of the camera.
    cv::Mat w_T_c;
    Eigen::Affine3d affine;
    tf::poseMsgToEigen(rovi_gazebo::get_model_pose("camera_stereo"), affine);
    cv::eigen2cv(affine.matrix(), w_T_c);

    // Calculate the disparity mean, this assumes 4 corner max, not a lot of corners!
    // if (kmeans == true)
    // {
    //     double k_means = 0;
    //     for(auto i = 0; i < pts.size(); i++)
    //         k_means += (double)(pts[i][0].x - pts[i][1].x);
    //     k_means /= pts.size();
    // }

    // Calculate disparity points
    for(auto i = 0; i < pts.size(); i++)
    {
        m.at<double>(0, i) = pts[i][0].x;
        m.at<double>(1, i) = pts[i][0].y;
        m.at<double>(2, i) = pts[i][0].x - pts[i][1].x;
        m.at<double>(3, i) = 1.f;
    }

    // Constant transformation matrix
    cv::Mat M = w_T_c * T * Q * m;

    // Determine M
    for (auto i = 0; i < pts.size(); i++)
    {
        const double w = M.at<double>(3, i);
        M.at<double>(0, i) = M.at<double>(0, i) / w;
        M.at<double>(1, i) = M.at<double>(1, i) / w;
        M.at<double>(2, i) = M.at<double>(2, i) / w;
        M.at<double>(3, i) = M.at<double>(3, i) / w;
    }

    return M;
}

geometry_msgs::Pose
M3::estimate_pose(const bool & draw, const std::string & img_name, const double & noise, const double & qual, const int & max_number_of_corners, const double & min_dist_features)
{
    geometry_msgs::Pose pose;

    // Get intrinsic / extrinsic parameters / get images
    auto cam_info_arr     = M3::get_image_info();
    auto cam_images_color = M3::get_image_data();
    auto cam_images_gray  = cam_images_color;

    cv::Mat gaussian_noise_left = cv::Mat::zeros(cam_images_color[0].size(), cam_images_color[0].type());
    cv::Mat gaussian_noise_right = gaussian_noise_left;

    std::vector<double> mean = {0, 0, 0};
    std::vector<double> std = {noise, noise, noise};

    cv::theRNG().state = cv::getTickCount();

    cv::randn(gaussian_noise_left, mean, std);
    cv::randn(gaussian_noise_right, mean, std);

    cv::imwrite("left.jpg", gaussian_noise_left);

    cam_images_color[0] += gaussian_noise_left;
    cam_images_color[1] += gaussian_noise_right;
    cam_images_gray[0] += gaussian_noise_left;
    cam_images_gray[1] += gaussian_noise_right;

    // Define ROI masks
    cv::Mat ROI_mask_left  = M3::create_mask(cam_images_gray[LEFT],  ROI_left);
    cv::Mat ROI_mask_right = M3::create_mask(cam_images_gray[RIGHT], ROI_right);

    // Define threshhold masks
    cv::Mat tsh_mask_left  = M3::tsh_mask(cam_images_gray[LEFT],  TSH_LOWER, TSH_UPPER);
    cv::Mat tsh_mask_right = M3::tsh_mask(cam_images_gray[RIGHT], TSH_LOWER, TSH_UPPER);

    // Bitwise and the masks
    cv::Mat left_mask, right_mask;
    cv::bitwise_and(ROI_mask_left, tsh_mask_left, left_mask);
    cv::bitwise_and(ROI_mask_right, tsh_mask_right, right_mask);

    // Convert color to Grayscale
    cv::cvtColor(cam_images_gray[LEFT], cam_images_gray[LEFT], cv::COLOR_BGR2GRAY);
    cv::cvtColor(cam_images_gray[RIGHT], cam_images_gray[RIGHT], cv::COLOR_BGR2GRAY);

    // Remove noise before canny, always.
    cv::GaussianBlur(cam_images_gray[LEFT], cam_images_gray[LEFT], GAUSS_BLUR_KERNEL, GAUSS_BLUR_STD);
    cv::GaussianBlur(cam_images_gray[RIGHT], cam_images_gray[RIGHT], GAUSS_BLUR_KERNEL, GAUSS_BLUR_STD);

    // Morph the images with open, do a extra dilate to ensure.
    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(3,3), cv::Point(1, 1));
    cv::morphologyEx(left_mask, left_mask, cv::MORPH_OPEN, kernel);
    cv::morphologyEx(right_mask, right_mask, cv::MORPH_OPEN, kernel);
    cv::dilate(left_mask, left_mask, kernel);
    cv::dilate(right_mask, right_mask, kernel);

    // Perform canny.
    cv::Canny(left_mask,  left_mask,  0, 100, 3, false);
    cv::Canny(right_mask, right_mask, 0, 100, 3, false);

    // Find contour left_right
    cv::Mat contour_left = M3::find_contour(left_mask);
    cv::Mat contour_right = M3::find_contour(right_mask);

    // Track the good features, this is lambda, very specific no reason for making this a function
    auto good_feature_tracker = [&qual](cv::Mat & img, std::vector<cv::Point2d> & corners, const cv::Mat & mask, const bool & draw, const double & min_dist = 50, const int & max_corner = 4)
    {
        cv::goodFeaturesToTrack( mask, corners, max_corner, qual, min_dist, cv::Mat(), 5, false);
        if(draw)
            for (const auto & corner : corners)
                cv::circle(img, corner, 10, cv::Scalar(128), 2, 8);
        return corners;
    };

    // Find the good corner features
    std::vector<cv::Point2d> corner_pts_left;
    std::vector<cv::Point2d> corner_pts_right;
    auto corner_left = good_feature_tracker(cam_images_gray[LEFT], corner_pts_left, contour_left, draw, min_dist_features, max_number_of_corners);
    auto corner_right = good_feature_tracker(cam_images_gray[RIGHT], corner_pts_right, contour_right, draw, min_dist_features, max_number_of_corners);

    // Brute force matching
    std::vector<std::array<cv::Point2d, 2>> pts;
    for ( const auto & left_pt : corner_pts_left )
    {
        int k = 0;
        double dist = std::numeric_limits<double>::max();
        for ( int i = 0; i < corner_pts_right.size(); ++i )
            if (auto eu_cmp = M3::L2_stereo(left_pt, corner_pts_right[i]); eu_cmp < dist)
            {
                dist = eu_cmp;
                k = i;
            }
        pts.push_back(std::array<cv::Point2d, 2>{left_pt, corner_pts_right[k]});
    }

    // Sort it
    std::sort(pts.begin(), pts.end(), [](const std::array<cv::Point2d, 2> & a, const std::array<cv::Point2d, 2> & b)
    {
        if( a[0].y < b[0].y )
            return true;
        else
            return false;
    });

    if (pts.size() == 0)
    {
        ROS_INFO_STREAM("No solution was found");
        return pose;
    }

    // Stich the images
    const double dx = cam_images_color[LEFT].cols-1;

    // Define the Q matrix
    Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_left(cam_info_arr[LEFT].K.data());

    cv::Mat Q = ( cv::Mat_<double>(4,4) <<  1.0, 0.0, 0.0,-K_left(0,2),
                                            0.0, 1.0, 0.0,-K_left(1,2),
                                            0.0, 0.0, 0.0, K_left(1,1),
                                            0.0, 0.0,-1/BASELINE, 0.0);

    // Stereo triangulation
    cv::Mat M = M3::stereo_triangulation(pts, Q);

    //
    ROS_INFO_STREAM("\n"<< M);

    // Take an arbitrary point, find the diagonal.
    cv::Point2d origo(M.at<double>(0, 0), M.at<double>(1, 0));

    int longest_idx = 0;

    double dist = std::numeric_limits<double>::min();
    for (auto i = 1; i < pts.size(); i++)
    {
        if (auto distance = cv::norm( origo - cv::Point2d(M.at<double>(0, i), M.at<double>(1, i) ) ); dist < distance)
        {
            dist = distance;
            longest_idx = i;
        }
    }

    // Calculate the coordinate system for pose generation by getting the indices
    std::vector<int> pt_idx;
    for (int i = 1; i < 4; ++i)
    {
        if (i != longest_idx)
        {
            pt_idx.push_back(i);
        }
    }

    // Validate the coordinate system
    cv::Point2d l1 = cv::Point2d( M.at<double>(0, pt_idx[0]) - origo.x, M.at<double>(1, pt_idx[0]) - origo.y);
    cv::Point2d l2 = cv::Point2d( M.at<double>(0, pt_idx[1]) - origo.x, M.at<double>(1, pt_idx[1]) - origo.y);

    double cross_val = l1.cross(l2);

    if(cross_val > 0)
    {
        std::swap(pt_idx[0], pt_idx[1]);
        std::swap(l1, l2);
    }

    // Update l1/v2
    auto L1_norm = sqrt(std::pow(l1.x, 2) + std::pow(l1.y, 2));
    auto L2_norm = sqrt(std::pow(l2.x, 2) + std::pow(l2.y, 2));

    l1 /= sqrt(l1.dot(l1));
    l2 /= sqrt(l2.dot(l2));

    auto error = l1.dot(l2);
    auto y_orth = l1 - (error/2)*l2;
    auto x_orth = l2 - (error/2)*l1;

    l1 = 0.5*(3.0-x_orth.dot(x_orth))*x_orth;
    l2 = 0.5*(3.0-y_orth.dot(y_orth))*y_orth;

    if (draw)
    {

        // Imgage stiched
        cv::Mat img_stitched = cv::Mat::zeros(cv::Size(cam_images_gray[LEFT].cols * 2, cam_images_gray[LEFT].rows), CV_8UC3);

        cam_images_color[0].copyTo(img_stitched(cv::Rect(0, 0, cam_images_color[LEFT].cols, cam_images_color[LEFT].rows)));
        cam_images_color[1].copyTo(img_stitched(cv::Rect(dx, 0, cam_images_color[RIGHT].cols, cam_images_color[RIGHT].rows)));

        cv::putText(img_stitched, "Left Image", cv::Point(50, 50), cv::FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 2, cv::LINE_AA);
        cv::putText(img_stitched, "Right Image", cv::Point(50 + dx, 50), cv::FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 2, cv::LINE_AA);

        for(auto pt : pts)
        {
            pt[1].x += dx;
            cv::line(img_stitched, pt[0], pt[1], cv::Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255)));
        }

        auto pt = cv::Point2d(pts[0][0].x + l1.x * 50, pts[0][0].y + l1.y * 50);

        // Draw the pose_estimation, rhs-coordinate system and with a blue arrow
        cv::arrowedLine( img_stitched, pts[0][0], pts[pt_idx[0]][0], cv::Scalar(0, 255, 0), 2 );
        cv::arrowedLine( img_stitched, pts[0][0], pts[pt_idx[1]][0], cv::Scalar(0, 0, 255), 2 );
        cv::arrowedLine( img_stitched, pts[0][0], pts[longest_idx][0], cv::Scalar(255, 0, 0), 2 );
        cv::arrowedLine( img_stitched, cv::Point2d(pts[0][1].x + dx, pts[0][1].y), cv::Point2d(pts[pt_idx[0]][1].x + dx, pts[pt_idx[0]][1].y), cv::Scalar(0, 255, 0), 2 );
        cv::arrowedLine( img_stitched, cv::Point2d(pts[0][1].x + dx, pts[0][1].y), cv::Point2d(pts[pt_idx[1]][1].x + dx, pts[pt_idx[1]][1].y), cv::Scalar(0, 0, 255), 2 );
        cv::arrowedLine( img_stitched, cv::Point2d(pts[0][1].x + dx, pts[0][1].y), cv::Point2d(pts[longest_idx][1].x + dx, pts[longest_idx][1].y), cv::Scalar(255, 0, 0), 2 );

        // Get the longest line, two gripping poses will be available, compute them both.
        cv::imwrite(img_name, img_stitched);
    }

    geometry_msgs::Pose pose_T;

    Eigen::Matrix4d eig_pose = ( Eigen::Matrix4d() << l1.x, l2.x, 0.0, M.at<double>(0, 0) + (M.at<double>(0, longest_idx) - M.at<double>(0, 0))/2.0,
                                                      l1.y, l2.y, 0.0, M.at<double>(1, 0) + (M.at<double>(1, longest_idx) - M.at<double>(1, 0))/2.0,
                                                       0.0, 0.0, 1.0, 0.75,
                                                       0.0, 0.0, 0.0, 1.0 ).finished();

    Eigen::Affine3d affine(eig_pose);
    tf::poseEigenToMsg(affine, pose);

    ROS_INFO_STREAM("\n" << eig_pose);
    // ROS_INFO_STREAM("\n" << eig_pose);

    return pose;
}

}


/*
* If not stated otherwise in this file or this component's LICENSE file the
* following copyright and licenses apply:
*
* Copyright 2021 Sky UK
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
/*
 * File:   DobbyRdkPluginDependencySolver.cpp
 *
 */
#include "DobbyRdkPluginDependencySolver.h"

#include <Logging.h>

#include <boost/graph/topological_sort.hpp>

// -----------------------------------------------------------------------------
/**
 * @brief Adds a plugin to the solver.
 *
 * Each plugin must be known to the solver - added by this method before its dependencies
 * are tracked.
 *
 * @param[in]   name   Name of the plugin
 *
 * @return True if the plugin has been added successfully,
 * false otherwise (the plugin had already been added).
 *
 */
bool DobbyRdkPluginDependencySolver::addPlugin(const std::string &name)
{
    AI_LOG_FN_ENTRY();
    // Plugins names are case-insensitive - use lowercase.
    std::string lowercaseName;
    lowercaseName.resize(name.length());
    std::transform(name.begin(), name.end(), lowercaseName.begin(), ::tolower);

    if (mDescriptorMap.count(lowercaseName) != 0)
    {
        AI_LOG_WARN("Plugin %s already added to the solver", lowercaseName.c_str());
        return false;
    }

    // Each plugin is represented by a vertex of a directed graph.
    const VertexDescriptor descriptor = boost::add_vertex(mDependencyGraph);
    // Store the name of the plugin as a property of the vertex.
    boost::put(boost::vertex_name_t{}, mDependencyGraph, descriptor, lowercaseName);
    mDescriptorMap.insert(StringVertexDescriptorMap::value_type{ lowercaseName, descriptor });

    AI_LOG_FN_EXIT();
    return true;
}

// -----------------------------------------------------------------------------
/**
 * @brief Adds a dependency between two plugins to the solver.
 *
 * If plugin A depends on plugin B, then the call is:
 * @code addDependency(A, B); @endcode
 *
 * @param[in]   pluginName       The name of the plugin which depends on @p dependencyName.
 * @param[in]   dependencyName   The name of the plugin on which @p pluginName depends.
 *
 * @return True if the dependency has been added successfully,
 * false otherwise (one of the plugins had not been added to the solver).
 *
 */
bool DobbyRdkPluginDependencySolver::addDependency(const std::string &pluginName, const std::string &dependencyName)
{
    AI_LOG_FN_ENTRY();
    // Plugins names are case-insensitive - use lowercase.
    std::string lowercaseName, lowercasedependencyName;
    lowercaseName.resize(pluginName.length());
    std::transform(pluginName.begin(), pluginName.end(), lowercaseName.begin(), ::tolower);
    lowercasedependencyName.resize(dependencyName.length());
    std::transform(dependencyName.begin(), dependencyName.end(), lowercasedependencyName.begin(), ::tolower);

    if (mDescriptorMap.count(lowercaseName) == 0)
    {
        AI_LOG_ERROR("Plugin %s unknown", lowercaseName.c_str());
        return false;
    }
    if (mDescriptorMap.count(lowercasedependencyName) == 0)
    {
        AI_LOG_ERROR("Plugin %s unknown", lowercasedependencyName.c_str());
        return false;
    }

    // Our dependency relation is represented by a directed edge (pluginName->dependencyName) of the graph.
    boost::add_edge(mDescriptorMap.at(lowercaseName), mDescriptorMap.at(lowercasedependencyName), mDependencyGraph);

    AI_LOG_FN_EXIT();
    return true;
}

// -----------------------------------------------------------------------------
/**
 * @brief Gets the names of the plugins in order of their dependency.
 *
 * "Order of dependency" here means that if plugin A depends on plugin B, plugin B will be placed
 * before plugin A in the returned vector, e.g. with this code:
 * @code
 * DobbyRdkPluginDependencySolver solver;
 * solver.addPlugin("AppServices");
 * solver.addPlugin("Networking");
 * solver.addPlugin("IPC");
 * solver.addDependency("AppServices", "Networking");
 * solver.addDependency("Networking", "IPC");
 *
 * const std::vector<std::string> inOrder = solver.getOrderOfDependency();
 * @endcode,
 * @code inOrder @endcode is @code { "IPC", "Networking", "AppServices" } @endcode.
 *
 * If there is a dependency cycle, an error message is printed and the function returns an empty vector.
 *
 * @return Names of the plugins in order of dependency. Empty vector if no plugins have been added or a
 * dependency cycle has been detected. Vector with plugin names in the same order in which they were added
 * if no dependencies have been added).
 */
std::vector<std::string> DobbyRdkPluginDependencySolver::getOrderOfDependency() const
{
    AI_LOG_FN_ENTRY();

    std::vector<std::string> namesInOrder;
    std::vector<VertexDescriptor> descriptorsInOrder;
    try
    {
        // If edge (u, v) appears in the graph, a topological sort will put v before u. This is our order of dependency.
        boost::topological_sort(mDependencyGraph, std::back_inserter(descriptorsInOrder));
    }
    catch(const boost::not_a_dag &e)
    {
        // Thrown when a graph is not a directed acyclic graph (DAG). In our case that means we have a dependency cycle.
        AI_LOG_ERROR("Dependency cycle detected");
        return namesInOrder;
    }

    // boost::topological_sort returns vertex descriptors. Convert them to our plugin names.
    const auto nameMap = boost::get(boost::vertex_name, mDependencyGraph);
    std::transform(descriptorsInOrder.begin(), descriptorsInOrder.end(), std::back_inserter(namesInOrder),
                   [&nameMap](const VertexDescriptor &d) -> std::string { return nameMap[d]; });

    AI_LOG_FN_EXIT();
    return namesInOrder;
}

/**
 * @brief Gets the names of the plugins in reversed order of their dependency.
 *
 * "Reversed order of dependency" here means that if plugin A depends on plugin B, plugin B will be placed
 * after plugin A in the returned vector, e.g. with this code:
 * @code
 * DobbyRdkPluginDependencySolver solver;
 * solver.addPlugin("AppServices");
 * solver.addPlugin("Networking");
 * solver.addPlugin("IPC");
 * solver.addDependency("AppServices", "Networking");
 * solver.addDependency("Networking", "IPC");
 *
 * const std::vector<std::string> inReversedOrder = solver.getReversedOrderOfDependency();
 * @endcode,
 * @code inOrder @endcode is @code { "AppServices", "Networking", "IPC" } @endcode.
 *
 * If there is a dependency cycle, an error message is printed and the function returns an empty vector.
 *
 * @return Names of the plugins in reversed order of dependency. Empty vector if no plugins have been added or a
 * dependency cycle has been detected. Vector with plugin names in the same order in which they were added
 * if no dependencies have been added).
 */
std::vector<std::string> DobbyRdkPluginDependencySolver::getReversedOrderOfDependency() const
{
    AI_LOG_FN_ENTRY();

    std::vector<std::string> namesInReversedOrder = getOrderOfDependency();
    std::reverse(namesInReversedOrder.begin(), namesInReversedOrder.end());

    AI_LOG_FN_EXIT();
    return namesInReversedOrder;
}


// $Id$
//
//  Copyright (C) 2013 Paolo Tosco
//
//  Copyright (C) 2004-2006 Rational Discovery LLC
//
//   @@ All Rights Reserved @@
//  This file is part of the RDKit.
//  The contents are covered by the terms of the BSD license
//  which is included in the file license.txt, found at the root
//  of the RDKit source tree.
//
#include "TorsionAngle.h"
#include "TorsionConstraint.h"
#include "Params.h"
#include <cmath>

#include <RDGeneral/BoostStartInclude.h>
#include <boost/math/special_functions/round.hpp>
#include <RDGeneral/BoostEndInclude.h>

#include <ForceField/ForceField.h>
#include <RDGeneral/Invariant.h>

namespace ForceFields {
namespace MMFF {
inline void checkPrecondition(const ForceField *owner, unsigned int idx1,
    unsigned int idx2, unsigned int idx3, unsigned int idx4) {
  PRECONDITION(owner, "bad owner");
  URANGE_CHECK(idx1, owner->positions().size());
  URANGE_CHECK(idx2, owner->positions().size());
  URANGE_CHECK(idx3, owner->positions().size());
  URANGE_CHECK(idx4, owner->positions().size());
}

double TorsionConstraintContrib::computeDihedralTerm(double dihedral) const {
  double dihedralTarget = dihedral;
  if (!(dihedral > d_minDihedralDeg && dihedral < d_maxDihedralDeg)
      && !(dihedral > d_minDihedralDeg && d_minDihedralDeg > d_maxDihedralDeg)
      && !(dihedral < d_maxDihedralDeg && d_minDihedralDeg > d_maxDihedralDeg)) {
    double minDihedralTarget = dihedral - d_minDihedralDeg;
    double maxDihedralTarget = dihedral - d_maxDihedralDeg;
    RDKit::ForceFieldsHelper::normalizeAngleDeg(minDihedralTarget);
    RDKit::ForceFieldsHelper::normalizeAngleDeg(maxDihedralTarget);
    dihedralTarget = (fabs(minDihedralTarget) < fabs(maxDihedralTarget)
      ? d_minDihedralDeg : d_maxDihedralDeg);
  }
  double dihedralTerm = dihedral - dihedralTarget;
  RDKit::ForceFieldsHelper::normalizeAngleDeg(dihedralTerm);
  return dihedralTerm;
}

void TorsionConstraintContrib::setParameters(ForceField *owner, unsigned int idx1,
    unsigned int idx2, unsigned int idx3, unsigned int idx4, double minDihedralDeg,
    double maxDihedralDeg, double forceConst) {
  dp_forceField = owner;
  d_at1Idx = idx1;
  d_at2Idx = idx2;
  d_at3Idx = idx3;
  d_at4Idx = idx4;
  d_minDihedralDeg = minDihedralDeg;
  d_maxDihedralDeg = maxDihedralDeg;
  d_forceConstant = forceConst;
}

TorsionConstraintContrib::TorsionConstraintContrib(
    ForceField *owner, unsigned int idx1, unsigned int idx2, unsigned int idx3,
    unsigned int idx4, double minDihedralDeg, double maxDihedralDeg,
    double forceConst) {
  checkPrecondition(owner, idx1, idx2, idx3, idx4);
  RDKit::ForceFieldsHelper::normalizeAngleDeg(minDihedralDeg);
  RDKit::ForceFieldsHelper::normalizeAngleDeg(maxDihedralDeg);
  setParameters(owner, idx1, idx2, idx3, idx4,
    minDihedralDeg, maxDihedralDeg, forceConst);
}

TorsionConstraintContrib::TorsionConstraintContrib(
    ForceField *owner, unsigned int idx1, unsigned int idx2, unsigned int idx3,
    unsigned int idx4, bool relative, double minDihedralDeg,
    double maxDihedralDeg, double forceConst) {
  checkPrecondition(owner, idx1, idx2, idx3, idx4);
  RDKit::ForceFieldsHelper::normalizeAngleDeg(minDihedralDeg);
  RDKit::ForceFieldsHelper::normalizeAngleDeg(maxDihedralDeg);
  if (relative) {
    double dihedral;
    RDKit::ForceFieldsHelper::computeDihedral(
        owner->positions(), idx1, idx2, idx3, idx4, &dihedral);
    dihedral *= RAD2DEG;
    minDihedralDeg += dihedral;
    maxDihedralDeg += dihedral;
    RDKit::ForceFieldsHelper::normalizeAngleDeg(minDihedralDeg);
    RDKit::ForceFieldsHelper::normalizeAngleDeg(maxDihedralDeg);
  }
  setParameters(owner, idx1, idx2, idx3, idx4,
    minDihedralDeg, maxDihedralDeg, forceConst);
}

double TorsionConstraintContrib::getEnergy(double *pos) const {
  PRECONDITION(dp_forceField, "no owner");
  PRECONDITION(pos, "bad vector");
  double dihedral;
  RDKit::ForceFieldsHelper::computeDihedral(
    pos, d_at1Idx, d_at2Idx, d_at3Idx, d_at4Idx, &dihedral);
  dihedral *= RAD2DEG;
  double dihedralTerm = computeDihedralTerm(dihedral);
  static double const c = 0.5 * DEG2RAD * DEG2RAD;
  double res = c * d_forceConstant * dihedralTerm * dihedralTerm;

  return res;
}

void TorsionConstraintContrib::getGrad(double *pos, double *grad) const {
  PRECONDITION(dp_forceField, "no owner");
  PRECONDITION(pos, "bad vector");
  PRECONDITION(grad, "bad vector");

  double *g[4] = {&(grad[3 * d_at1Idx]), &(grad[3 * d_at2Idx]),
                  &(grad[3 * d_at3Idx]), &(grad[3 * d_at4Idx])};

  RDGeom::Point3D r[4];
  RDGeom::Point3D t[2];
  double d[2];
  double cosPhi;
  double dihedral;
  RDKit::ForceFieldsHelper::computeDihedral(
    pos, d_at1Idx, d_at2Idx, d_at3Idx, d_at4Idx, &dihedral, &cosPhi, r, t, d);
  dihedral *= RAD2DEG;
  double sinPhiSq = 1.0 - cosPhi * cosPhi;
  double sinPhi = ((sinPhiSq > 0.0) ? sqrt(sinPhiSq) : 0.0);
  // dE/dPhi is independent of cartesians:
  double dihedralTerm = computeDihedralTerm(dihedral);
  double dE_dPhi = DEG2RAD * d_forceConstant * dihedralTerm;

  // FIX: use a tolerance here
  // this is hacky, but it's per the
  // recommendation from Niketic and Rasmussen:
  double sinTerm =
      -dE_dPhi * (isDoubleZero(sinPhi) ? (1.0 / cosPhi) : (1.0 / sinPhi));
  Utils::calcTorsionGrad(r, t, d, g, sinTerm, cosPhi);
}
}
}


#ifndef IMAGE_BUFFER_6RYGHM2V
#define IMAGE_BUFFER_6RYGHM2V

#include <stdexcept>
#include <boost/thread/mutex.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/lexical_cast.hpp>

#include <libfreenect/libfreenect.h>

namespace freenect_camera {

  const float RGB_FOCAL_LENGTH_SXGA = 1050;
  const float WIDTH_SXGA = 1280;

  /**
   * \class ImageBuffer
   *
   * \brief Holds an image buffer with all the metadata required to transmit the
   * image over ROS channels
   */
  struct ImageBuffer {
    boost::mutex mutex;
    boost::shared_array<unsigned char> image_buffer;
    int valid;
    freenect_frame_mode metadata;
    float focal_length;
    bool is_registered;
  };

  /**
   * Get RGB Focal length in pixels
   */
  float getRGBFocalLength(int width) {
    float scale = width / WIDTH_SXGA;
    return RGB_FOCAL_LENGTH_SXGA * scale;
  }

  /**
   * Get Depth Focal length in pixels
   */
  float getDepthFocalLength(
      const freenect_registration& registration, int width) {

    float depth_focal_length_sxga =
      registration.zero_plane_info.reference_distance /
      registration.zero_plane_info.reference_pixel_size;
    float scale = width / WIDTH_SXGA;
    return depth_focal_length_sxga * scale;
  }

  /**
   * Reallocate the video buffer if the video format or resolution changes
   */
  void allocateBufferVideo(
      ImageBuffer& buffer,
      const freenect_video_format& format,
      const freenect_resolution& resolution,
      const freenect_registration& registration) {

    // Obtain a lock on the buffer. This is mostly for debugging, as allocate
    // buffer should only be called when the buffer is not being used by the
    // freenect thread
    boost::lock_guard<boost::mutex> buffer_lock(buffer.mutex);

    // Deallocate the buffer incase an exception happens (the buffer should no
    // longer be valid)
    buffer.image_buffer.reset();

    switch (format) {
      case FREENECT_VIDEO_RGB:
      case FREENECT_VIDEO_BAYER:
      case FREENECT_VIDEO_YUV_RGB:
      case FREENECT_VIDEO_IR_8BIT:
      case FREENECT_VIDEO_IR_10BIT:
      case FREENECT_VIDEO_IR_10BIT_PACKED:
        switch (resolution) {
          case FREENECT_RESOLUTION_HIGH:
          case FREENECT_RESOLUTION_MEDIUM:
            buffer.metadata =
              freenect_find_video_mode(resolution, format);
            if (!buffer.metadata.is_valid) {
              throw std::runtime_error("libfreenect: Invalid video fmt, res: " +
                  boost::lexical_cast<std::string>(format) + "," +
                  boost::lexical_cast<std::string>(resolution));
            }
            break;
          default:
            throw std::runtime_error("libfreenect: Invalid video resolution: " +
                boost::lexical_cast<std::string>(resolution));
        }
        break;
      default:
        throw std::runtime_error("libfreenect: Invalid video format: " +
            boost::lexical_cast<std::string>(format));
    }

    // All is good, reallocate the buffer and calculate other pieces of info
    buffer.image_buffer.reset(new unsigned char[buffer.metadata.bytes]);
    switch(format) {
      case FREENECT_VIDEO_RGB:
      case FREENECT_VIDEO_BAYER:
      case FREENECT_VIDEO_YUV_RGB:
        buffer.focal_length = getRGBFocalLength(buffer.metadata.width);
        break;
      case FREENECT_VIDEO_IR_8BIT:
      case FREENECT_VIDEO_IR_10BIT:
      case FREENECT_VIDEO_IR_10BIT_PACKED:
        buffer.focal_length = getDepthFocalLength(registration,
            buffer.metadata.width);
        break;
      default:
        throw std::runtime_error("libfreenect: shouldn't reach here");
    }
    buffer.is_registered = false;
  }

  /**
   * Reallocate the depth buffer if the depth format or resolution changes
   */
  void allocateBufferDepth(
      ImageBuffer& buffer,
      const freenect_depth_format& format,
      const freenect_resolution& resolution,
      const freenect_registration& registration) {

    // Obtain a lock on the buffer. This is mostly for debugging, as allocate
    // buffer should only be called when the buffer is not being used by the
    // freenect thread
    boost::lock_guard<boost::mutex> buffer_lock(buffer.mutex);

    // Deallocate the buffer incase an exception happens (the buffer should no
    // longer be valid)
    buffer.image_buffer.reset();

    switch (format) {
      case FREENECT_DEPTH_11BIT:
      case FREENECT_DEPTH_10BIT:
      case FREENECT_DEPTH_11BIT_PACKED:
      case FREENECT_DEPTH_10BIT_PACKED:
      case FREENECT_DEPTH_REGISTERED:
      case FREENECT_DEPTH_MM:
        switch (resolution) {
          case FREENECT_RESOLUTION_MEDIUM:
            buffer.metadata =
              freenect_find_depth_mode(resolution, format);
            if (!buffer.metadata.is_valid) {
              throw std::runtime_error("libfreenect: Invalid depth fmt, res: " +
                  boost::lexical_cast<std::string>(format) + "," +
                  boost::lexical_cast<std::string>(resolution));
            }
            break;
          default:
            throw std::runtime_error("libfreenect: Invalid depth resolution: " +
                boost::lexical_cast<std::string>(resolution));
        }
        break;
      default:
        throw std::runtime_error("libfreenect: Invalid depth format: " +
            boost::lexical_cast<std::string>(format));
    }

    // All is good, reallocate the buffer and calculate other pieces of info
    buffer.image_buffer.reset(new unsigned char[buffer.metadata.bytes]);
    switch(format) {
      case FREENECT_DEPTH_11BIT:
      case FREENECT_DEPTH_10BIT:
      case FREENECT_DEPTH_11BIT_PACKED:
      case FREENECT_DEPTH_10BIT_PACKED:
      case FREENECT_DEPTH_MM:
        buffer.focal_length =
          getDepthFocalLength(registration, buffer.metadata.width);
        buffer.is_registered = false;
        break;
      case FREENECT_DEPTH_REGISTERED:
        buffer.focal_length = getRGBFocalLength(buffer.metadata.width);
        buffer.is_registered = true;
        break;
      default:
        throw std::runtime_error("libfreenect: shouldn't reach here");
    }
  }

  void fillImage(const ImageBuffer& buffer, void* data) {
    memcpy(data, buffer.image_buffer.get(), buffer.metadata.bytes);
  }

} /* end namespace freenect_camera */

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


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/xray/f_model.h>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>

namespace cctbx { namespace xray { namespace f_model_core_data { namespace boost_python {
  namespace {

    struct f_model_core_data_derivative_holder_wrappers
    {
      typedef f_model_core_data_derivative_holder<double> w_t;

      static void
      wrap()
      {
        using namespace boost::python;
        class_<w_t>("f_model_core_data_derivative_holder")//, no_init)
          .def("ksol",(double(w_t::*)())  &w_t::ksol )
          .def("ksol",(void (w_t::*)(double))  &w_t::ksol )

          .def("usol",(double(w_t::*)())  &w_t::usol )
          .def("usol",(void(w_t::*)(double))  &w_t::usol )

          .def("kpart",(double(w_t::*)()) &w_t::kpart )
          .def("kpart",(void(w_t::*)(double)) &w_t::kpart )

          .def("upart",(double(w_t::*)()) &w_t::upart )
          .def("upart",(void(w_t::*)(double)) &w_t::upart )

          .def("koverall", (double(w_t::*)()) &w_t::koverall )
          .def("koverall", (void(w_t::*)(double)) &w_t::koverall )

          .def("ustar", (scitbx::sym_mat3<double>(w_t::*)()) &w_t::ustar )
          .def("ustar", (void(w_t::*)(scitbx::sym_mat3<double>)) &w_t::ustar )

          .def("accumulate", &w_t::accumulate)
          ;
      }

    };

    struct f_model_core_data_wrappers
    {
      typedef f_model_core_data<double> w_t;

      static void
      wrap()
      {
        using namespace boost::python;

        class_<w_t>("f_model_core_data", no_init)
          .def(init<
               scitbx::af::const_ref< cctbx::miller::index<> > const&,  // 1 indices
               scitbx::af::const_ref< std::complex< double > > const&,  // 2 f_atmos
               scitbx::af::const_ref< std::complex< double > > const&,  // 3 f_mask
               cctbx::uctbx::unit_cell const&,                          // 4 unit cell
               double const&,                                           // 7 overall scale
               scitbx::sym_mat3< double > const&,                       // 8 u star
               double const&,                                           // 9 bs scale
               double const&,                                           // 0 bs u
               scitbx::af::const_ref< std::complex< double > > const&,  // 1 f_part
               double const&,                                           // 2 part scale
               double const&                                            // 3 part u
               >
               ((arg("hkl"),
                 arg("f_atoms"),
                 arg("f_mask"),
                 arg("unit_cell"),
                 arg("k_overall"),
                 arg("u_star"),
                 arg("k_sol"),
                 arg("u_sol"),
                 arg("f_part"),
                 arg("k_part"),
                 arg("u_part") )))

          .def(init<
               scitbx::af::const_ref< cctbx::miller::index<> > const&,  // 1 indices
               scitbx::af::const_ref< std::complex< double > > const&,  // 2 f_atmos
               scitbx::af::const_ref< std::complex< double > > const&,  // 3 f_mask
               cctbx::uctbx::unit_cell const&,                          // 4 unit cell
               double const&,                                           // 7 overall scale
               scitbx::sym_mat3< double > const&,                       // 8 u star
               double const&,                                           // 9 bs scale
               double const&,                                           // 0 bs b
               scitbx::af::const_ref< std::complex< double > > const&,  // 1 f_part
               double const&,                                           // 2 part scale
               double const&                                            // 3 part b
               >
               ((arg("hkl"),
                 arg("f_atoms"),
                 arg("f_mask"),
                 arg("d_star_sq"),
                 arg("k_overall"),
                 arg("u_star"),
                 arg("k_sol"),
                 arg("u_sol"),
                 arg("f_part"),
                 arg("k_part"),
                 arg("u_part") )))

          // derivatives using gradientflags
          .def("d_target_d_all",
               ( f_model_core_data_derivative_holder<double>(w_t::*)
                 (double const&,
                  double const&,
                  std::size_t const&,
                  scitbx::af::const_ref<bool> const& )
               ) &w_t::d_target_d_all)
          .def("d_target_d_all",
               ( f_model_core_data_derivative_holder<double>(w_t::*)
                 (scitbx::af::const_ref<double> const&,
                  scitbx::af::const_ref<double> const&,
                  scitbx::af::const_ref<bool> const&)
               ) &w_t::d_target_d_all)
          /*
          .def("d_target_d_all",
               ( f_model_core_data_derivative_holder<double>(w_t::*)
                 (scitbx::af::const_ref<std::complex<double> > const&,
                  scitbx::af::const_ref<bool> const&)
               ) &w_t::d_target_d_all)
          */
          .def("d_target_d_all",
               ( f_model_core_data_derivative_holder<double>(w_t::*)
                 (scitbx::af::const_ref<double> const&,
                  scitbx::af::const_ref<bool> const&)
                 ) &w_t::d_target_d_all)

          // updating variables
          .def("refresh", &w_t::refresh)
          .def("renew_fatoms", &w_t::renew_fatoms)
          .def("renew_fmask", &w_t::renew_fmask)
          .def("renew_fpart", &w_t::renew_fpart)
          .def("renew_overall_scale_parameters",
               &w_t::renew_overall_scale_parameters)
          .def("renew_bulk_solvent_scale_parameters",
               &w_t::renew_bulk_solvent_scale_parameters)
          .def("renew_partial_structure_scale_parameters",
               &w_t::renew_partial_structure_scale_parameters)
          // get individual scales and sf's
          .def("f_atoms", &w_t::f_atoms)
          .def("f_bulk", &w_t::f_bulk)
          .def("f_part", &w_t::f_part)
          .def("bulk_scale", &w_t::bulk_scale)
          .def("part_scale", &w_t::part_scale)
          .def("f_model", &w_t::get_f_model_core_data)
          .def("overall_scale", &w_t::overall_scale)
          .def("fu_star", &w_t::fu_star)
          .def("d_f_model_core_data_d_f_atoms", &w_t::d_f_model_core_data_d_f_atoms)

          .def("ustar", (scitbx::sym_mat3<double>(w_t::*)()) &w_t::ustar)
          .def("koverall",(double(w_t::*)()) &w_t::koverall)
          .def("ksol", (double(w_t::*)()) &w_t::ksol)
          .def("kpart",(double(w_t::*)()) &w_t::kpart)
          .def("usol", (double(w_t::*)()) &w_t::usol)
          .def("upart",(double(w_t::*)()) &w_t::upart)

          .def("ustar", (void(w_t::*)(scitbx::sym_mat3<double>)) &w_t::ustar)
          .def("koverall",(void(w_t::*)(double)) &w_t::koverall)
          .def("ksol", (void(w_t::*)(double)) &w_t::ksol)
          .def("kpart",(void(w_t::*)(double)) &w_t::kpart)
          .def("usol", (void(w_t::*)(double)) &w_t::usol)
          .def("upart",(void(w_t::*)(double)) &w_t::upart)

          .def("d_star_sq", &w_t::d_star_sq)
          // selection method
          .def("select", &w_t::select)
          ;
      }
    };

  }

}} // namespace cctbx::xray

  namespace boost_python {

    void wrap_f_model_core_data()
    {
      f_model_core_data::boost_python::f_model_core_data_wrappers::wrap();
      f_model_core_data::boost_python::f_model_core_data_derivative_holder_wrappers::wrap();
    }

  }

}}


/*
 * vivepos.cpp
 *
 */

#include "vivepos.h"
#include <am_vive_pos/timeoutserial.h>
#include <std_msgs/Float32.h>
#include "std_msgs/UInt32.h"
#include <string.h>

#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include <boost/lexical_cast.hpp>

#include <tf/transform_datatypes.h>

#define DEG2RAD(DEG) ((DEG) * ((M_PI) / (180.0)))
#define RAD2DEG(RAD) ((RAD) * ((180.0) / (M_PI)))
#define NOISE(n) (((rand() % 1000) / 1000.0) * n - n / 2.0)

#define FIX_ANGLES(a)       \
    if (a > M_PI * 2.0)     \
    {                       \
        a = a - M_PI * 2.0; \
    }                       \
    else if (a < 0.0)       \
    {                       \
        a = a + M_PI * 2.0; \
    }

namespace Husqvarna {

VivePos::VivePos(const ros::NodeHandle& nodeh)
{
	  // Init attributes
    nh = nodeh;

    // Parameters
    ros::NodeHandle n_private("~");

    std::string defPort = "/dev/ttyACM0"; //"/dev/pts/22";
    n_private.param("serialUsbPort", pSerialUsbPort, defPort);

    n_private.param("publishTf", publishTf, 1);
    ROS_INFO("Param: publishTf: [%d]", publishTf);

    odom_pub = nh.advertise<nav_msgs::Odometry>("odom_combined",5);
    pose_pub = nh.advertise<geometry_msgs::PoseStamped>("pose", 5);

    // Initialize the intial pose
    robot_pose.pose.position.x = 0.0;
    robot_pose.pose.position.y = 0.0;
    robot_pose.pose.position.z = 0.0;

    xPos = 0.0;
    yPos = 0.0;
    zPos = 0.0;

}

VivePos::~VivePos()
{
	// TODO Auto-generated destructor stub
}

void VivePos::Run()
{
  try
  {
      TimeoutSerial serial(pSerialUsbPort, 115200);
      serial.setTimeout(boost::posix_time::seconds(15));

      while (ros::ok())
      {
          // Read from serial port
          std::string line;

          try
          {
              line = serial.readStringUntil("\n");
              handleLine(line);
          }
          catch (timeout_exception)
          {
              ROS_WARN("No range data???");
          }
      }
  }
  catch (boost::system::system_error& e)
  {
      ROS_ERROR("Range-serial: %s", e.what());
  }

}

void VivePos::handleLine(std::string line)
{
    // Parse data
    if (boost::starts_with(line, "OBJ0"))
    {
       std::vector<std::string> strs;
       boost::split(strs, line, boost::is_any_of(" \t"));
       if (strs.size() == 7)
       {
           //std::cout << "LINE[" << line << "]" << std::endl;

           double x,y,z;

           try
           {
               x = boost::lexical_cast<double>(strs[3]);
           }
           catch (boost::bad_lexical_cast e)
           {
               return;
           }

           try
           {
               y = boost::lexical_cast<double>(strs[5]);
           }
           catch (boost::bad_lexical_cast e)
           {
               return;
           }

           try
           {
               z = boost::lexical_cast<double>(strs[4]);
           }
           catch (boost::bad_lexical_cast e)
           {
               return;
           }

           xPos = -x;
           yPos = y;
           zPos = z;

           publish();

       }

    }

    // FRONT POS (used for angle/yaw)
    if (boost::starts_with(line, "OBJ1"))
    {
       std::vector<std::string> strs;
       boost::split(strs, line, boost::is_any_of(" \t"));
       if (strs.size() == 7)
       {
           //std::cout << "LINE[" << line << "]" << std::endl;

           double x,y,z;

           try
           {
               x = boost::lexical_cast<double>(strs[3]);
           }
           catch (boost::bad_lexical_cast e)
           {
               return;
           }

           try
           {
               y = boost::lexical_cast<double>(strs[5]);
           }
           catch (boost::bad_lexical_cast e)
           {
               return;
           }

           try
           {
               z = boost::lexical_cast<double>(strs[4]);
           }
           catch (boost::bad_lexical_cast e)
           {
               return;
           }

           // Same coordinate system as xPos
           x = -x;

           double idz = (z+0.05) - zPos;
           double idx = x - xPos;
           double idy = y - yPos;

           // ROLL (TODO)
           roll = 0.0;

           // PITCH
           pitch = -asin(idz/0.435);

           //std::cout << "LINE[" << line << "]" << std::endl;

           //std::cout << "PITCH = " << RAD2DEG(pitch) << std::endl;

           //pitch = 0.0;

           // YAW
           yaw = atan2(idy, idx);
           FIX_ANGLES(yaw);

           //std::cout << "ANGLE[" << yaw << "]" << std::endl;
           publish();

       }
   }

}

void VivePos::publish()
{
	ros::Time current_time = ros::Time::now();

  // ODOM
  nav_msgs::Odometry odom;
  odom.header.stamp = current_time;
  odom.header.frame_id = "odom_combined";

  odom.pose.pose.position.x = xPos;
  odom.pose.pose.position.y = yPos;
  odom.pose.pose.position.z = zPos;

  odom.pose.pose.orientation.x = 0;
  odom.pose.pose.orientation.y = 0;
  odom.pose.pose.orientation.z = 0;
  odom.pose.pose.orientation.w = 1;
  odom_pub.publish(odom);

  // POSE
  //tf::Quaternion q = tf::createQuaternionFromYaw(yaw);
  tf::Quaternion q = tf::createQuaternionFromRPY(roll, pitch, yaw);
  robot_pose.pose.orientation.x = q.x();
  robot_pose.pose.orientation.y = q.y();
  robot_pose.pose.orientation.z = q.z();
  robot_pose.pose.orientation.w = q.w();

  // Set this into the pose
  robot_pose.pose.position.x = xPos;
  robot_pose.pose.position.y = yPos;
  robot_pose.pose.position.z = zPos;

  robot_pose.header.frame_id = "base_link";
  robot_pose.header.stamp = ros::Time::now();
  pose_pub.publish(robot_pose);

  // Send the TF
  if (publishTf)
  {
    // Calculate the TF from the pose...
    transform.setOrigin(
                tf::Vector3(robot_pose.pose.position.x, robot_pose.pose.position.y, robot_pose.pose.position.z));
    tf::Quaternion q;
    tf::quaternionMsgToTF(robot_pose.pose.orientation, q);
    transform.setRotation(q);

      br.sendTransform(tf::StampedTransform(transform, current_time, "odom", "base_link"));
  }

}

} /* namespace Husqvarna */


#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>
#include <mmtbx/tls/tls.h>
#include <scitbx/array_family/boost_python/shared_wrapper.h>
#include <scitbx/boost_python/is_polymorphic_workaround.h>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>
#include <boost/python.hpp>

SCITBX_BOOST_IS_POLYMORPHIC_WORKAROUND(mmtbx::tls::common)

namespace mmtbx { namespace tls {
  namespace bp = boost::python;

namespace {
  boost::python::tuple
  getinitargs(tlso<> const& self)
  {
    return boost::python::make_tuple(self.t, self.l, self.s);
  }

  void init_module()
  {
    using namespace boost::python;
    using boost::python::arg;
    typedef return_value_policy<return_by_value> rbv;

    class_<uaniso_from_tls>("uaniso_from_tls",
                             init<sym_mat3<double> const&,
                                  sym_mat3<double> const&,
                                  mat3<double> const&,
                                  vec3<double> const&,
                                  vec3<double> const&,
                                  bool>())
      .def("u", &uaniso_from_tls::u)
    ;

    class_<tls_ls_derivative_coefficients>("tls_ls_derivative_coefficients",
                           init<vec3<double> const&,
                                af::shared<vec3<double> > const&,
                                af::shared<double> const&>(
                                   (arg("origin"),
                                    arg("sites_cart"),
                                    arg("u_iso"))))
      .add_property("a", make_getter(&tls_ls_derivative_coefficients::a, rbv()))
      .add_property("b", make_getter(&tls_ls_derivative_coefficients::b, rbv()))
    ;

    class_<d_target_d_tls>("d_target_d_tls",
                           init<af::shared<vec3<double> > const&,
                                vec3<double> const&,
                                af::shared<sym_mat3<double> > const&,
                                bool,
                                bool>(
                                   (arg("sites"),
                                    arg("origin"),
                                    arg("d_target_d_uaniso"),
                                    arg("scale_l_and_s"),
                                    arg("use_trace_s_zero_constraint"))))
      .def("grad_T", &d_target_d_tls::grad_T)
      .def("grad_L", &d_target_d_tls::grad_L)
      .def("grad_S", &d_target_d_tls::grad_S)
    ;

    class_<tls_from_uaniso_target_and_grads>("tls_from_uaniso_target_and_grads",
                             init<sym_mat3<double> const&,
                                  sym_mat3<double> const&,
                                  mat3<double> const&,
                                  vec3<double> const&,
                                  af::shared<vec3<double> > const&,
                                  af::shared<sym_mat3<double> > const&>())
      .def("target", &tls_from_uaniso_target_and_grads::target)
      .def("grad_T", &tls_from_uaniso_target_and_grads::grad_T)
      .def("grad_L", &tls_from_uaniso_target_and_grads::grad_L)
      .def("grad_S", &tls_from_uaniso_target_and_grads::grad_S)
    ;
    class_<tls_from_uiso_target_and_grads>("tls_from_uiso_target_and_grads",
                             init<double const&,
                                  sym_mat3<double> const&,
                                  vec3<double> const&,
                                  vec3<double> const&,
                                  af::shared<vec3<double> > const&,
                                  af::shared<double> const&>())
      .def("target", &tls_from_uiso_target_and_grads::target)
      .def("grad_T", &tls_from_uiso_target_and_grads::grad_T)
      .def("grad_L", &tls_from_uiso_target_and_grads::grad_L)
      .def("grad_S", &tls_from_uiso_target_and_grads::grad_S)
    ;

    typedef return_value_policy<return_by_value> rbv;
    class_<tlso<> >("tlso")
      .def(init<scitbx::sym_mat3<double> const&,
                scitbx::sym_mat3<double> const&,
                scitbx::mat3<double> const&,
                scitbx::vec3<double> const& >((arg("t"),arg("l"),arg("s"),
                                               arg("origin"))))
      .add_property("t",      make_getter(&tlso<>::t,      rbv()))
      .add_property("l",      make_getter(&tlso<>::l,      rbv()))
      .add_property("s",      make_getter(&tlso<>::s,      rbv()))
      .add_property("origin", make_getter(&tlso<>::origin, rbv()))
      .enable_pickling()
      .def("__getinitargs__", getinitargs)
    ;
    def("uaniso_from_tls_one_group",
         (af::shared<sym_mat3<double> >(*)
               (tlso<double>,
                af::shared<vec3<double> > const&,
                bool)) uaniso_from_tls_one_group,
                  (arg("tlso"),
                   arg("sites_cart"),
                   arg("zeroize_trace")))
   ;

   def("u_cart_from_xyz",
         (sym_mat3<double>(*)
             (af::shared<vec3<double> > const&)) u_cart_from_xyz,
                (arg("sites_cart")))
   ;

   def("all_vs_all",
         (boost::python::list(*)
             (boost::python::list const&)) all_vs_all,
                (arg("xyz_all")))
   ;

   def("apply_tls_shifts",
         (af::shared<vec3<double> >(*)
               (af::shared<vec3<double> > const&,
                mat3<double> const&,
                mat3<double> const&,
                vec3<double> const&,
                vec3<double> const&,
                vec3<double> const&,
                double const&,
                double const&,
                double const&,
                double const&,
                double const&,
                double const&,
                vec3<double> const&
                )) apply_tls_shifts,
                  (arg("sites_cart"),
                   arg("R_ML_transposed"),
                   arg("R_ML"),
                   arg("d0"),
                   arg("d_r_M_V"),
                   arg("s"),
                   arg("wy_lx"),
                   arg("wz_lx"),
                   arg("wz_ly"),
                   arg("wx_ly"),
                   arg("wx_lz"),
                   arg("wy_lz"),
                   arg("origin")
                   ))
   ;

   class_<tls_parts_one_group>("tls_parts_one_group",
                             init<tlso<double>,
                                  af::shared<vec3<double> > const&>())
      .def("ala",    &tls_parts_one_group::ala)
      .def("assa",   &tls_parts_one_group::assa)
      .def("u_cart", &tls_parts_one_group::u_cart)
      .def("t",      &tls_parts_one_group::t)
      .def("r",      &tls_parts_one_group::r)
    ;
   class_<tls_parts_one_group_as_b_iso>("tls_parts_one_group_as_b_iso",
                             init<tlso<double>,
                                  af::shared<vec3<double> > const&>())
      .def("ala",    &tls_parts_one_group_as_b_iso::ala)
      .def("assa",   &tls_parts_one_group_as_b_iso::assa)
      .def("b_iso", &tls_parts_one_group_as_b_iso::b_iso)
      .def("t",      &tls_parts_one_group_as_b_iso::t)
    ;

   class_<common>("common",init<sym_mat3<double> const&,
                                sym_mat3<double> const&,
                                optional<double> >())
      .def("t", &common::t)
      .def("branch_0",       &common::get_branch_0)
      .def("branch_1",       &common::get_branch_1)
      .def("branch_1_1",     &common::get_branch_1_1)
      .def("branch_1_2",     &common::get_branch_1_2)
      .def("branch_1_2_1",   &common::get_branch_1_2_1)
      .def("branch_1_2_2",   &common::get_branch_1_2_2)
      .def("branch_1_2_3",   &common::get_branch_1_2_3)
      .def("branch_1_2_3_1", &common::get_branch_1_2_3_1)
      .def("branch_1_2_3_2", &common::get_branch_1_2_3_2)
   ;

   def("ls_target_from_iso_tls", (double(*)(
     double const&,
     sym_mat3<double> const&,
     vec3<double> const&,
     vec3<double> const&,
     af::shared<vec3<double> > const&,
     af::shared<double> const&)) ls_target_from_iso_tls, (arg("t"),arg("l"),
       arg("s"),arg("origin"),arg("sites_cart"),arg("u_isos")))
   ;

   def("t_from_u_cart", (sym_mat3<double>(*)(af::shared<sym_mat3<double> > const&, double)) t_from_u_cart, (arg("u_cart"),arg("small")))
   ;
   def("t_from_u_cart", (sym_mat3<double>(*)(af::shared<double> const&, double))            t_from_u_cart, (arg("u_iso"),arg("small")))
   ;

  }

} // namespace <anonymous>
}} // namespace mmtbx::tls

BOOST_PYTHON_MODULE(mmtbx_tls_ext)
{
  mmtbx::tls::init_module();
}