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

#include "precomp.hpp"

// Eigen
#include <Eigen/Core>

// OpenCV
#include <opencv2/core/eigen.hpp>
#include <opencv2/sfm/triangulation.hpp>
#include <opencv2/sfm/projection.hpp>

// libmv headers
#include "libmv/multiview/twoviewtriangulation.h"
#include "libmv/multiview/fundamental.h"

using namespace cv;
using namespace std;

namespace cv
{
namespace sfm
{

/** @brief Triangulates the a 3d position between two 2d correspondences, using the DLT.
  @param xl Input vector with first 2d point.
  @param xr Input vector with second 2d point.
  @param Pl Input 3x4 first projection matrix.
  @param Pr Input 3x4 second projection matrix.
  @param objectPoint Output vector with computed 3d point.

  Reference: @cite HartleyZ00 12.2 pag.312
 */
void
triangulateDLT( const Vec2d &xl, const Vec2d &xr,
                const Matx34d &Pl, const Matx34d &Pr,
                Vec3d &point3d )
{
    Matx44d design;
    for (int i = 0; i < 4; ++i)
    {
        design(0,i) = xl(0) * Pl(2,i) - Pl(0,i);
        design(1,i) = xl(1) * Pl(2,i) - Pl(1,i);
        design(2,i) = xr(0) * Pr(2,i) - Pr(0,i);
        design(3,i) = xr(1) * Pr(2,i) - Pr(1,i);
    }

    Vec4d XHomogeneous;
    cv::SVD::solveZ(design, XHomogeneous);

    homogeneousToEuclidean(XHomogeneous, point3d);
}

/** @brief Triangulates the 3d position of 2d correspondences between n images, using the DLT
 * @param x Input vectors of 2d points (the inner vector is per image). Has to be 2xN
 * @param Ps Input vector with 3x4 projections matrices of each image.
 * @param X Output vector with computed 3d point.

 * Reference: it is the standard DLT; for derivation see appendix of Keir's thesis
 */
void
triangulateNViews(const Mat_<double> &x, const std::vector<Matx34d> &Ps, Vec3d &X)
{
    CV_Assert(x.rows == 2);
    unsigned nviews = x.cols;
    CV_Assert(nviews == Ps.size());

    cv::Mat_<double> design = cv::Mat_<double>::zeros(3*nviews, 4 + nviews);
    for (unsigned i=0; i < nviews; ++i) {
        for(char jj=0; jj<3; ++jj)
            for(char ii=0; ii<4; ++ii)
                design(3*i+jj, ii) = -Ps[i](jj, ii);
        design(3*i + 0, 4 + i) = x(0, i);
        design(3*i + 1, 4 + i) = x(1, i);
        design(3*i + 2, 4 + i) = 1.0;
    }

    Mat X_and_alphas;
    cv::SVD::solveZ(design, X_and_alphas);
    homogeneousToEuclidean(X_and_alphas.rowRange(0, 4), X);
}

void
triangulatePoints(InputArrayOfArrays _points2d, InputArrayOfArrays _projection_matrices,
                  OutputArray _points3d)
{
    // check
    size_t nviews = (unsigned) _points2d.total();
    CV_Assert(nviews >= 2 && nviews == _projection_matrices.total());

    // inputs
    size_t n_points;
    std::vector<Mat_<double> > points2d(nviews);
    std::vector<Matx34d> projection_matrices(nviews);
    {
        std::vector<Mat> points2d_tmp;
        _points2d.getMatVector(points2d_tmp);
        n_points = points2d_tmp[0].cols;

        std::vector<Mat> projection_matrices_tmp;
        _projection_matrices.getMatVector(projection_matrices_tmp);

        // Make sure the dimensions are right
        for(size_t i=0; i<nviews; ++i) {
            CV_Assert(points2d_tmp[i].rows == 2 && points2d_tmp[i].cols == n_points);
            if (points2d_tmp[i].type() == CV_64F)
                points2d[i] = points2d_tmp[i];
            else
                points2d_tmp[i].convertTo(points2d[i], CV_64F);

            CV_Assert(projection_matrices_tmp[i].rows == 3 && projection_matrices_tmp[i].cols == 4);
            if (projection_matrices_tmp[i].type() == CV_64F)
              projection_matrices[i] = projection_matrices_tmp[i];
            else
              projection_matrices_tmp[i].convertTo(projection_matrices[i], CV_64F);
        }
    }

    // output
    _points3d.create(3, n_points, CV_64F);
    cv::Mat points3d = _points3d.getMat();

    // Two view
    if( nviews == 2 )
    {
        const Mat_<double> &xl = points2d[0], &xr = points2d[1];

        const Matx34d & Pl = projection_matrices[0];    // left matrix projection
        const Matx34d & Pr = projection_matrices[1];    // right matrix projection

        // triangulate
        for( unsigned i = 0; i < n_points; ++i )
        {
            Vec3d point3d;
            triangulateDLT( Vec2d(xl(0,i), xl(1,i)), Vec2d(xr(0,i), xr(1,i)), Pl, Pr, point3d );
            for(char j=0; j<3; ++j)
                points3d.at<double>(j, i) = point3d[j];
        }
    }
    else if( nviews > 2 )
    {
        // triangulate
        for( unsigned i=0; i < n_points; ++i )
        {
            // build x matrix (one point per view)
            Mat_<double> x( 2, nviews );
            for( unsigned k=0; k < nviews; ++k )
            {
                points2d.at(k).col(i).copyTo( x.col(k) );
            }

            Vec3d point3d;
            triangulateNViews( x, projection_matrices, point3d );
            for(char j=0; j<3; ++j)
                points3d.at<double>(j, i) = point3d[j];
        }
    }
}

} /* namespace sfm */
} /* namespace cv */


//
// Copyright 2005-2007 Adobe Systems Incorporated
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//

#include <boost/gil.hpp>
#include <boost/gil/extension/io/jpeg.hpp>
#include <boost/gil/extension/numeric/sampler.hpp>
#include <boost/gil/extension/numeric/resample.hpp>

// Performs an arbitrary affine transformation on the image.

// This example relies on the matrices and functions available in GIL to define the operation,
// in include/boost/gil/extension/numeric/affine.hpp
// and calls resample_pixels(), avaiable in the numeric extension, to apply it

int main()
{
    namespace gil = boost::gil;

    gil::rgb8_image_t img;
    gil::read_image("test.jpg", img, gil::jpeg_tag());

    // test resample_pixels
    // Transform the image by an arbitrary affine transformation using nearest-neighbor resampling
    gil::rgb8_image_t transf(gil::rgb8_image_t::point_t(gil::view(img).dimensions() * 2));
    gil::fill_pixels(gil::view(transf), gil::rgb8_pixel_t(255, 0, 0)); // the background is red

    gil::matrix3x2<double> mat =
        gil::matrix3x2<double>::get_translate(-gil::point<double>(200,250)) *
        gil::matrix3x2<double>::get_rotate(-15*3.14/180.0);
    gil::resample_pixels(const_view(img), gil::view(transf), mat, gil::nearest_neighbor_sampler());
    gil::write_view("out-affine.jpg", gil::view(transf), gil::jpeg_tag());

    return 0;
}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// 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.

#ifndef NETKET_ONLINESTAT_HPP
#define NETKET_ONLINESTAT_HPP

#include <mpi.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <cassert>
#include <iostream>
#include <vector>
#include "Utils/random_utils.hpp"

namespace netket {

/// Online statistics
// for general types
// this class accumulates results
// with minimal memory requirements
// simple statistics can then be obtained
// or merged with other bins

template <class T>
class OnlineStat {
  // Number of samples in this bin
  int N_;

  // current mean
  T mean_;

  // Running sum of squares of differences from the current mean
  T m2_;

  bool firstcall_;

 public:
  using DataType = T;

  explicit OnlineStat() { Reset(); }

  // Adding data to this bin
  inline void operator<<(const DataType &data) {
    CheckCall(data);

    N_ += 1;
    const T delta = data - mean_;
    mean_ += delta / double(N_);

    const T delta2 = data - mean_;
    m2_ += delta * delta2;
  }

  // Merging with another bin
  inline void operator<<(const OnlineStat<T> &obin) {
    CheckCall(obin.Mean());

    N_ += obin.N();
    const T delta = obin.Mean() - Mean();
    mean_ += delta * obin.N() / double(N_);

    m2_ += obin.m2_ + delta * delta * obin.N() * (1. - obin.N() / double(N_));
  }

  inline int N() const { return N_; }

  inline DataType Mean() const { return mean_; }

  inline DataType Variance() const { return m2_ / double(N_ - 1.); }

  inline DataType ErrorOfMean() const {
    return sqrt(m2_ / double(N_ * (N_ - 1.)));
  }

  void Reset() { firstcall_ = true; }

  void CheckCall(const DataType &data) {
    if (firstcall_) {
      N_ = 0;
      mean_ = DataType(data);
      mean_ = 0;
      m2_ = DataType(data);
      m2_ = 0;
      firstcall_ = false;
    }
  }
};

/// Online statistics
// for scalars
// this class accumulates results
// with minimal memory requirements
// simple statistics can then be obtained
// or merged with other bins
template <>
class OnlineStat<Eigen::VectorXd> {
  // Number of samples in this bin
  int N_;

  // current mean
  Eigen::VectorXd mean_;

  // Running sum of squares of differences from the current mean
  Eigen::VectorXd m2_;

  bool firstcall_;

 public:
  using DataType = Eigen::VectorXd;

  explicit OnlineStat() { Reset(); }

  // Adding data to this bin
  inline void operator<<(const DataType &data) {
    CheckCall(data);

    N_ += 1;
    const DataType delta = data - mean_;
    mean_ += delta / double(N_);

    const DataType delta2 = data - mean_;
    m2_ += delta.cwiseProduct(delta2);
  }

  // Merging with another bin
  inline void operator<<(const OnlineStat<Eigen::VectorXd> &obin) {
    CheckCall(obin.Mean());

    N_ += obin.N();
    const DataType delta = obin.Mean() - Mean();
    mean_ += delta * obin.N() / double(N_);
    m2_ += obin.m2_ +
           delta.cwiseProduct(delta) * obin.N() * (1. - obin.N() / double(N_));
  }

  inline int N() const { return N_; }

  inline DataType Mean() const { return mean_; }

  inline DataType Variance() const { return m2_ / double(N_ - 1.); }

  inline DataType ErrorOfMean() const {
    return m2_.cwiseSqrt() / std::sqrt(double(N_ * (N_ - 1.)));
  }

  void Reset() { firstcall_ = true; }

  void CheckCall(const DataType &data) {
    if (firstcall_) {
      N_ = 0;
      mean_.resize(data.size());
      mean_.setZero();
      m2_.resize(data.size());
      m2_.setZero();
      firstcall_ = false;
    }
  }
};

}  // namespace netket
#endif


// Erwann Rogard, wrote in July 2009:
//
// This iterator is by nbecker and was found in the boost's vault.
// Changes that I made are shown by ER_2007_07
//
// arch-tag: ed320324-7f40-4e99-8364-8a6b7cf4d19e
#ifndef cycle_iterator_ext_HPP_2009
#define cycle_iterator_ext_HPP_2009
#include <boost/type_traits/is_convertible.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/iterator/iterator_adaptor.hpp>
#include <iterator>

// See http://www.nabble.com/cycle-iterators-td25215321.html

namespace boost {

  //! This is a cycle iterator that does keep track of wraparound.
  template<typename BaseIterator, typename offset_t>
  class cycle_iterator_ext : public boost::iterator_adaptor<cycle_iterator_ext<BaseIterator, offset_t>,
							BaseIterator
						       >
  {
  public:
    typedef typename boost::iterator_adaptor<cycle_iterator_ext<BaseIterator, offset_t>,
					     BaseIterator
					    > super_t;

    typedef typename super_t::difference_type difference_type;
    typedef typename super_t::reference reference;

    explicit cycle_iterator_ext()
    :super_t(),size(0),position(0),wrap(0) // ER_2009_07
    {}

    explicit cycle_iterator_ext (BaseIterator const& _b, BaseIterator const& _e, offset_t offset=0) :
      //base(_b), //ER_2009_07
      super_t(_b), //ER_2009_07
      size (std::distance (_b, _e))
      , wrap(0) //ER_2009_07
    {
      SetPos (offset);
    }

    template <typename OtherBase, typename OtherOffset>
    cycle_iterator_ext (cycle_iterator_ext<OtherBase,OtherOffset> const& other,
		    typename enable_if_convertible<OtherBase, BaseIterator>::type* = 0) :
      super_t(other),       // ER_2009_07
      //base (other.base),  // ER_2009_07
      size (other.size),
      position (other.position),
      wrap (other.wrap)
    {}

    // ER_2009_07
    //template <typename OtherBase, typename OtherOffset>
    //typename enable_if<
    //    is_convertible<OtherBase,BaseIterator>,
    //    cycle_iterator_ext&
    //>::type
    //operator= (cycle_iterator_ext<OtherBase,OtherOffset> const& other)
    //{
    //    if(&other!=this){
    //        super_t& super = static_cast<super_t&>(this);
    //        super = other;
    //        size = (other.size),
    //        position = (other.position),
    //        wrap = (other.wrap);
    //    }
    //    return *this;
    //}

  private:
    friend class boost::iterator_core_access;

    void increment () {
      ++position;
      if (position >= size) {
	++wrap;
	position -= size;
      }
    }

    void decrement () {
      --position;
      if (position < 0) {
	--wrap;
	position += size;
      }
    }

    void SetPos (offset_t newpos) {
      position = newpos % size;
      wrap = newpos / size;
      if (position < 0) {
	--wrap;
	position += size;
      }
    }

    void advance (difference_type n) {
      offset_t newpos = realposition() + n;
      SetPos (newpos);
    }

    template<typename OtherBase, typename OtherOffset>
    difference_type
    distance_to (cycle_iterator_ext<OtherBase, OtherOffset> const& y) const {
      if (size == 0)
	return 0;

      else {
	offset_t pos1 = realposition();
	offset_t pos2 = y.realposition();
	return -(pos1 - pos2);
      }
    }

    template<typename OtherBase, typename OtherOffset>
    bool equal (cycle_iterator_ext<OtherBase, OtherOffset> const& y) const {
      return distance_to (y) == 0;
    }

    reference dereference() const {
       // return *(base + position); // ER_2009_07
        return *(this->base_reference() + position); // ER_2009_07
    }

   offset_t PositiveMod (offset_t x) const {
      offset_t y = x % size;
      if (y < 0)
	y += size;
      return y;
    }

  public:

    reference operator[] (difference_type n) const {
        // return *(base + PositiveMod (position + n)); // ER_2009_07
        return *(this->base_reference() + PositiveMod (position + n)); // ER_2009_07
    }

    offset_t offset() const { return position; }

    offset_t realposition () const {
      return position + wrap * size;
    }

    //  private:

    // BaseIterator base; // ER_2009
    offset_t size;
    offset_t position;
    offset_t wrap;
  };

  template<typename offset_t, typename BaseIterator>
  cycle_iterator_ext<BaseIterator, offset_t> make_cycle_iterator_ext(BaseIterator b, BaseIterator e, offset_t offset=0) {
    return cycle_iterator_ext<BaseIterator, offset_t> (b, e, offset);
  }

  template<typename BaseIterator>
  cycle_iterator_ext<BaseIterator, int> make_cycle_iterator_ext(BaseIterator b, BaseIterator e, int offset=0) {
    return cycle_iterator_ext<BaseIterator, int> (b, e, offset);
  }

} //namespace boost

#endif


#ifndef PARMCB_FORESTINDEX_HPP_
#define PARMCB_FORESTINDEX_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <vector>
#include <map>
#include <queue>
#include <set>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/concept/assert.hpp>

#include <parmcb/detail/spanning_forest.hpp>

namespace parmcb {

    template<class Graph>
    class ForestIndex {

    public:
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename std::size_t size_type;

        explicit ForestIndex(const Graph &g) {
            create_index(g);
        }

        ForestIndex(const ForestIndex &ei) {
            n = ei.n;
            m = ei.m;
            k = ei.k;
            index = ei.index;
            reverse_index = ei.reverse_index;
        }

        ~ForestIndex(void) {
        }

        ForestIndex& operator=(const ForestIndex &ei) {
            if (this == &ei) {
                return *this;
            }
            n = ei.n;
            m = ei.m;
            k = ei.k;
            index = ei.index;
            reverse_index = ei.reverse_index;
            return *this;
        }

        const Edge& operator()(const size_type &i) const {
            return reverse_index[i];
        }

        const size_type& operator()(const Edge &e) const {
            return index.at(e);
        }

        bool is_on_forest(const Edge &e) const {
            return index.at(e) >= cycle_space_dimension();
        }

        size_type cycle_space_dimension() const {
            return m - n + k;
        }

        size_type weak_connected_components() const {
            return k;
        }

    private:
        typedef typename boost::graph_traits<Graph>::edge_iterator EdgeIt;

        size_type n = 0;
        size_type m = 0;
        size_type k = 0;
        std::map<Edge, size_type> index;
        std::vector<Edge> reverse_index;

        void create_index(const Graph &g) {
            std::set<Edge> forest;
            n = boost::num_vertices(g);
            m = boost::num_edges(g);
            k = parmcb::detail::spanning_forest(g, std::inserter(forest, forest.begin()));

            index.clear();
            reverse_index.resize(m);

            size_type csd = m - n + k; // cycle space dimension
            size_type low = 0;
            size_type high = csd;

            EdgeIt ei, eiend;
            for (boost::tie(ei, eiend) = boost::edges(g); ei != eiend; ++ei) {
                auto e = *ei;
                if (forest.find(e) == forest.end()) {
                    index[e] = low;
                    reverse_index[low] = e;
                    low++;
                } else {
                    index[e] = high;
                    reverse_index[high] = e;
                    high++;
                }
            }
        }

    };

} // namespace parmcb

#endif


#ifndef BOOST_ASTRONOMY_IO_IMAGE_HPP
#define BOOST_ASTRONOMY_IO_IMAGE_HPP

#include <valarray>
#include <fstream>
#include <cstddef>
#include <algorithm>
#include <iterator>
#include <cstdint>
#include <string>
#include <cmath>
#include <numeric>

#include <boost/endian/conversion.hpp>
#include <boost/cstdfloat.hpp>

#include <boost/astronomy/io/bitpix.hpp>

namespace boost { namespace astronomy { namespace io {

template <typename PixelType>
struct image_buffer
{
protected:
    std::valarray<PixelType> data; //! stores the image
    std::size_t width; //! width of image
    std::size_t height; //! height of image
    //std::fstream image_file; //! image file

    //! Used purly for Type punning
    union pixel_data
    {
        PixelType pixel;
        std::uint8_t byte[sizeof(PixelType)];
        //char byte[sizeof(PixelType)];
    };

public:
    image_buffer() {}

    image_buffer(std::size_t width, std::size_t height) : width(width), height(height)
    {
        this->data.resize(width*height);
    }

    virtual ~image_buffer() {}

    //! returns the maximum value of all the pixels in the image
    PixelType max() const
    {
        return this->data.max();
    }

    //! returns the manimum value of all the pixels in the image
    PixelType min() const
    {
        return this->data.min();
    }

    //! returns the mean value of all the pixels in image
    double mean() const
    {
        if (this->data.size() == 0)
        {
            return 0;
        }

        return (std::accumulate(std::begin(this->data),
            std::end(this->data), 0.0) / this->data.size());
    }

    //! returns the median of all the pixel values in the image
    //! Note: uses additional space of order O(n) where n is the number of total pixels
    PixelType median() const
    {
        std::valarray<PixelType> soreted_array = this->data;
        std::nth_element(std::begin(soreted_array),
            std::begin(soreted_array) + soreted_array.size() / 2, std::end(soreted_array));

        return soreted_array[soreted_array.size() / 2];
    }

    //! returns the standard deviation of all the pixel values in the image
    //! Note: uses additional space of order O(n) where n is the number of total pixels
    double std_dev() const
    {
        if (this->data.size() == 0)
        {
            return 0;
        }

        double avg = this->mean();

        std::valarray<double> diff(this->data.size());
        for (size_t i = 0; i < diff.size(); i++)
        {
            diff[i] = this->data[i] - avg;
        }

        diff *= diff;
        return std::sqrt(diff.sum() / (diff.size() - 1));
    }

    PixelType operator() (std::size_t x, std::size_t y)
    {
        return this->data[(x*this->width) + y];
    }
};

template<bitpix args>
struct image {};

template <>
struct image<bitpix::B8> : public image_buffer<std::uint8_t>
{
public:
    image() {}

    image(std::string const& file, std::size_t width, std::size_t height, std::streamoff start) :
        image_buffer<std::uint8_t>(width, height)
    {
        std::fstream image_file(file);
        image_file.seekg(start);
        read_image_logic(image_file);
        image_file.close();
    }

    image(std::string const& file, std::size_t width, std::size_t height) :
        image_buffer<std::uint8_t>(width, height)
    {
        std::fstream image_file(file);
        read_image_logic(image_file);
        image_file.close();
    }

    image(std::fstream &file, std::size_t width, std::size_t height, std::streamoff start)
    {
        read_image(file, width, height, start);
    }

    image(std::fstream &file, std::size_t width, std::size_t height)
    {
        read_image(file, width, height);
    }

    void read_image_logic(std::fstream &image_file)
    {
        image_file.read((char*)std::begin(data), width*height);
        //std::copy_n(std::istreambuf_iterator<char>(file.rdbuf()), width*height, std::begin(data));
    }

    void read_image
    (
        std::string const& file,
        std::size_t width,
        std::size_t height,
        std::streamoff start
    )
    {
        std::fstream image_file(file);
        data.resize(width*height);
        image_file.seekg(start);

        read_image_logic(image_file);

        image_file.close();
    }

    void read_image(std::string const& file, std::size_t width, std::size_t height)
    {
        read_image(file, width, height, 0);
    }

    void read_image(std::fstream &file, std::size_t width, std::size_t height, std::streamoff start)
    {
        data.resize(width*height);
        file.seekg(start);

        read_image_logic(file);
    }

    void read_image(std::fstream &file, std::size_t width, std::size_t height)
    {
        read_image(file, width, height, file.tellg());
    }
};

template <>
struct image<bitpix::B16> : public image_buffer<std::int16_t>
{
public:
    image() {}

    image(std::string const& file, std::size_t width, std::size_t height, std::streamoff start) :
        image_buffer<std::int16_t>(width, height)
    {
        std::fstream image_file(file);
        image_file.seekg(start);
        read_image_logic(image_file);
        image_file.close();
    }

    image(std::string const& file, std::size_t width, std::size_t height) :
        image_buffer<std::int16_t>(width, height)
    {
        std::fstream image_file(file);
        read_image_logic(image_file);
        image_file.close();
    }

    image(std::fstream &file, std::size_t width, std::size_t height, std::streamoff start)
    {
        read_image(file, width, height, start);
    }

    image(std::fstream &file, std::size_t width, std::size_t height)
    {
        read_image(file, width, height);
    }

    void read_image_logic(std::fstream &image_file)
    {
        for (std::size_t i = 0; i < height*width; i++)
        {
            image_file.read((char*)&data[i], 2);
            data[i] = boost::endian::big_to_native(data[i]);
        }
    }

    void read_image
    (
        std::string const& file,
        std::size_t width,
        std::size_t height,
        std::streamoff start
    )
    {
        std::fstream image_file(file);
        image_file.open(file);
        data.resize(width*height);
        image_file.seekg(start);

        read_image_logic(image_file);

        image_file.close();
    }

    void read_image(std::string const& file, std::size_t width, std::size_t height)
    {
        read_image(file, width, height, 0);
    }

    void read_image(std::fstream &file, std::size_t width, std::size_t height, std::streamoff start)
    {
        data.resize(width*height);
        file.seekg(start);

        read_image_logic(file);
    }

    void read_image(std::fstream &file, std::size_t width, std::size_t height)
    {
        read_image(file, width, height, file.tellg());
    }
};

template <>
struct image<bitpix::B32> : public image_buffer<std::int32_t>
{
public:
    image() {}

    image(std::string const& file, std::size_t width, std::size_t height, std::streamoff start) :
        image_buffer<std::int32_t>(width, height)
    {
        std::fstream image_file(file);
        image_file.seekg(start);
        read_image_logic(image_file);
        image_file.close();
    }

    image(std::string const& file, std::size_t width, std::size_t height) :
        image_buffer<std::int32_t>(width, height)
    {
        std::fstream image_file(file);
        read_image_logic(image_file);
        image_file.close();
    }

    image(std::fstream &file, std::size_t width, std::size_t height, std::streamoff start)
    {
        read_image(file, width, height, start);
    }

    image(std::fstream &file, std::size_t width, std::size_t height)
    {
        read_image(file, width, height);
    }

    void read_image_logic(std::fstream &image_file)
    {
        for (std::size_t i = 0; i < height*width; i++)
        {
            image_file.read((char*)&data[i], 4);
            data[i] = boost::endian::big_to_native(data[i]);
        }
    }

    //!reads image
    void read_image
    (
        std::string const& file,
        std::size_t width,
        std::size_t height,
        std::streamoff start
    )
    {
        std::fstream image_file(file);
        data.resize(width*height);
        image_file.seekg(start);

        read_image_logic(image_file);

        image_file.close();
    }

    void read_image(std::string const& file, std::size_t width, std::size_t height)
    {
        read_image(file, width, height, 0);
    }

    void read_image(std::fstream &file, std::size_t width, std::size_t height, std::streamoff start)
    {
        data.resize(width*height);
        file.seekg(start);

        read_image_logic(file);
    }

    void read_image(std::fstream &file, std::size_t width, std::size_t height)
    {
        read_image(file, width, height, file.tellg());
    }
};

template <>
struct image<bitpix::_B32> : public image_buffer<boost::float32_t>
{
public:
    image() {}

    image(std::string const& file, std::size_t width, std::size_t height, std::streamoff start) :
        image_buffer<boost::float32_t>(width, height)
    {
        std::fstream image_file(file);
        image_file.seekg(start);
        read_image_logic(image_file);
        image_file.close();
    }

    image(std::string const& file, std::size_t width, std::size_t height) :
        image_buffer<boost::float32_t>(width, height)
    {
        std::fstream image_file(file);
        read_image_logic(image_file);
        image_file.close();
    }

    image(std::fstream &file, std::size_t width, std::size_t height, std::streamoff start)
    {
        read_image(file, width, height, start);
    }

    image(std::fstream &file, std::size_t width, std::size_t height)
    {
        read_image(file, width, height);
    }

    void read_image_logic(std::fstream &image_file)
    {
        pixel_data single_pixel;
        for (std::size_t i = 0; i < height*width; i++)
        {
            image_file.read((char*)single_pixel.byte, 4);
            data[i] = (single_pixel.byte[3] << 0) | (single_pixel.byte[2] << 8) |
                (single_pixel.byte[1] << 16) | (single_pixel.byte[0] << 24);
        }
    }

    void read_image
    (
        std::string const& file,
        std::size_t width,
        std::size_t height,
        std::streamoff start
    )
    {
        std::fstream image_file(file);
        data.resize(width*height);
        image_file.seekg(start);

        read_image_logic(image_file);
        image_file.close();
    }

    void read_image(std::string const& file, std::size_t width, std::size_t height)
    {
        read_image(file, width, height, 0);
    }

    void read_image(std::fstream &file, std::size_t width, std::size_t height, std::streamoff start)
    {
        data.resize(width*height);
        file.seekg(start);

        read_image_logic(file);
    }

    void read_image(std::fstream &file, std::size_t width, std::size_t height)
    {
        read_image(file, width, height, file.tellg());
    }
};

template <>
struct image<bitpix::_B64> : public image_buffer<boost::float64_t>
{
public:
    image() {}

    image(std::string const& file, std::size_t width, std::size_t height, std::streamoff start) :
        image_buffer<boost::float64_t>(width, height)
    {
        std::fstream image_file(file);
        image_file.seekg(start);
        read_image_logic(image_file);
        image_file.close();
    }

    image(std::string const& file, std::size_t width, std::size_t height) :
        image_buffer<boost::float64_t>(width, height)
    {
        std::fstream image_file(file);
        read_image_logic(image_file);
        image_file.close();
    }

    image(std::fstream &file, std::size_t width, std::size_t height, std::streamoff start)
    {
        read_image(file, width, height, start);
    }

    image(std::fstream &file, std::size_t width, std::size_t height)
    {
        read_image(file, width, height);
    }

    void read_image_logic(std::fstream &image_file)
    {
        pixel_data single_pixel;
        for (std::size_t i = 0; i < height*width; i++)
        {
            image_file.read((char*)single_pixel.byte, 8);
            data[i] = (single_pixel.byte[7] << 0) | (single_pixel.byte[6] << 8) |
                (single_pixel.byte[5] << 16) | (single_pixel.byte[4] << 24) |
                (single_pixel.byte[3] << 32) | (single_pixel.byte[2] << 40) |
                (single_pixel.byte[1] << 48) | (single_pixel.byte[0] << 56);
        }
    }

    void read_image
    (
        std::string const& file,
        std::size_t width,
        std::size_t height,
        std::streamoff start
    )
    {
        std::fstream image_file(file);
        data.resize(width*height);
        image_file.seekg(start);

        read_image_logic(image_file);

        image_file.close();
    }

    void read_image(std::string const& file, std::size_t width, std::size_t height)
    {
        read_image(file, width, height, 0);
    }

    void read_image(std::fstream &file, std::size_t width, std::size_t height, std::streamoff start)
    {
        data.resize(width*height);
        file.seekg(start);

        read_image_logic(file);
    }

    void read_image(std::fstream &file, std::size_t width, std::size_t height)
    {
        read_image(file, width, height, file.tellg());
    }
};

}}} //namespace boost::astronomy::io

#endif // !BOOST_ASTRONOMY_IO_IMAGE_HPP


#include "statutil.h"

#include <algorithm>

#define BOOST_MATH_DOMAIN_ERROR_POLICY ignore_error
#include <boost/math/distributions/fisher_f.hpp>

#include "lapack.h"
#include "vectorutil.h"

double fpval(double x, double df1, double df2)
{
    boost::math::fisher_f f(df1, df2);
    return boost::math::cdf(boost::math::complement(f, x));
}

double eps(double x)
{
    if (!std::isfinite(x))
        return std::numeric_limits<double>::quiet_NaN();

    if (std::fabs(x) < std::numeric_limits<double>::min())
        return std::numeric_limits<double>::denorm_min();

    int exponent;
    std::frexp(x, &exponent);

    int digits = std::numeric_limits<double>::digits;

    return std::ldexp(1, exponent - digits);
}


#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>

#include <CGAL/boost/graph/helpers.h>
#include <CGAL/Polygon_mesh_processing/connected_components.h>
#include <CGAL/property_map.h>

#include <boost/property_map/function_property_map.hpp>

#include <iostream>
#include <fstream>
#include <cstring>

namespace PMP = CGAL::Polygon_mesh_processing;

typedef CGAL::Simple_cartesian<double>     Kernel;
typedef Kernel::Point_3                    Point;
typedef Kernel::Compare_dihedral_angle_3   Compare_dihedral_angle_3;
typedef CGAL::Surface_mesh<Point>          Mesh;

template <typename G, typename GT>
struct Constraint
{
  typedef typename GT::FT                                  FT;

  typedef typename boost::graph_traits<G>::edge_descriptor edge_descriptor;
  typedef boost::readable_property_map_tag                 category;
  typedef bool                                             value_type;
  typedef bool                                             reference;
  typedef edge_descriptor                                  key_type;

  Constraint() : g(nullptr), gt(nullptr), bound(0) {}
  Constraint(const G& g, const GT& gt, const FT bound) : g(&g), gt(&gt), bound(bound) {}

  bool operator[](const edge_descriptor e) const
  {
    const Mesh& rg = *g;

    return gt->compare_dihedral_angle_3_object()(
             rg.point(source(e, rg)),
             rg.point(target(e, rg)),
             rg.point(target(next(halfedge(e, rg), rg), rg)),
             rg.point(target(next(opposite(halfedge(e, rg), rg), rg), rg)),
          bound) == CGAL::SMALLER;
  }

  friend inline
  value_type get(const Constraint& m, const key_type k)
  {
    return m[k];
  }

  const G* g;
  const GT* gt;
  FT bound;
};

template <typename G, typename GT>
struct Face_descriptor_area_functor
{
  typedef typename boost::graph_traits<G>::face_descriptor     face_descriptor;

  Face_descriptor_area_functor(const G& g, const GT& gt) : g(g), gt(gt) { }

  typename GT::FT operator()(const face_descriptor f) const
  {
    const auto& vpm = get(CGAL::vertex_point, g);

    return gt.compute_area_3_object()(get(vpm, source(halfedge(f, g), g)),
                                      get(vpm, target(halfedge(f, g), g)),
                                      get(vpm, target(next(halfedge(f, g), g), g)));
  }

  const G& g;
  const GT& gt;
};

void test_CC_with_default_size_map(Mesh sm,
                                   const Kernel& k)
{
  std::cout << " -- test with default size map -- " << std::endl;

  typedef boost::graph_traits<Mesh>::face_descriptor                      face_descriptor;
  typedef Kernel::FT                                                      FT;

  const FT bound = std::cos(0.7 * CGAL_PI);

  std::vector<face_descriptor> cc;
  face_descriptor fd = *faces(sm).first;
  CGAL::Polygon_mesh_processing::connected_component(fd, sm, std::back_inserter(cc));

  std::cerr << "connected components without edge constraints" << std::endl;
  std::cerr << cc.size() << " faces in the CC of " << fd << std::endl;
  assert(cc.size() == 1452);

  std::cerr << "\nconnected components with edge constraints (dihedral angle < 3/4 pi)" << std::endl;
  Mesh::Property_map<face_descriptor,std::size_t> fccmap;
  fccmap = sm.add_property_map<face_descriptor, std::size_t>("f:CC").first;
  std::size_t num = PMP::connected_components(sm, fccmap);

  std::cerr << "The graph has " << num << " connected components (face connectivity)" << std::endl;
  assert(num == 3);

  std::vector<face_descriptor> one_face_per_cc(num);
  std::vector<std::size_t> cc_size(num,0);

  for(face_descriptor f : faces(sm))
  {
    //    std::cout  << f << " in connected component " << fccmap[f] << std::endl;
    std::size_t ccid=fccmap[f];
    if (++cc_size[ccid]==1)
      one_face_per_cc[ccid]=f;
  }

  std::size_t id_of_cc_to_remove = std::distance(cc_size.begin(),
                                                 std::min_element(cc_size.begin(), cc_size.end()));

  Mesh copy1 = sm;
  Mesh copy2 = sm;

  // remove cc from copy1
  std::vector<face_descriptor> ff;
  for (std::size_t i=0;i<num;++i)
    if (i!=id_of_cc_to_remove)
      ff.push_back(one_face_per_cc[i]);

  // default face size map, but explicitely passed
  PMP::keep_connected_components(copy1, ff,
     PMP::parameters::edge_is_constrained_map(Constraint<Mesh, Kernel>(copy1, k, bound))
                     .face_size_map(CGAL::Constant_property_map<face_descriptor, std::size_t>(1)));

  // remove cc from copy2
  ff.clear();
  ff.push_back(one_face_per_cc[id_of_cc_to_remove]);
  PMP::remove_connected_components(copy2, ff,
     PMP::parameters::edge_is_constrained_map(Constraint<Mesh, Kernel>(copy2, k, bound)));

  std::cerr << "We keep the " << num-1 << " largest components" << std::endl;
  PMP::keep_largest_connected_components(sm, num-1,
    PMP::parameters::edge_is_constrained_map(Constraint<Mesh, Kernel>(sm, k, bound)));

  sm.collect_garbage();
  copy1.collect_garbage();
  copy2.collect_garbage();

  assert( num_vertices(sm)==num_vertices(copy1) && num_vertices(copy1)==num_vertices(copy2) );
  assert( num_edges(sm)==num_edges(copy1) && num_edges(copy1)==num_edges(copy2) );
  assert( num_faces(sm)==num_faces(copy1) && num_faces(copy1)==num_faces(copy2) );

  {
    Mesh m;
    Point p(0,0,0), q(1,0,0), r(0,1,0), s(0,0,1);
    CGAL::make_tetrahedron(p,q,r,s,m);
    CGAL::make_triangle(p,q,r,m);
    CGAL::make_tetrahedron(p,q,r,s,m);
    PMP::keep_large_connected_components(m, 4);

    assert(vertices(m).size() == 8);
  }
}

void test_CC_with_area_size_map(Mesh sm,
                                const Kernel& k)
{
  std::cout << " -- test with area size map -- " << std::endl;

  typedef boost::graph_traits<Mesh>::face_descriptor                      face_descriptor;

  Face_descriptor_area_functor<Mesh, Kernel> f(sm, k);
  std::vector<face_descriptor> faces_to_remove; // faces that would be removed but are not because we're doing a dry run
  std::size_t nv = num_vertices(sm);
  std::size_t num = PMP::internal::number_of_connected_components(sm);

  std::cout << "We keep the " << 2 << " largest components" << std::endl;
  std::size_t res = PMP::keep_largest_connected_components(sm, 2,
                                                           PMP::parameters::face_size_map(boost::make_function_property_map<face_descriptor>(f))
                                                                           .dry_run(true)
                                                                           .output_iterator(std::back_inserter(faces_to_remove)));

  // didn't actually remove anything
  assert(PMP::internal::number_of_connected_components(sm) == num);
  assert(num_vertices(sm) == nv);

  if(num > 2)
  {
    assert(res == num - 2);
    assert(!faces_to_remove.empty());
  }

  PMP::keep_largest_connected_components(sm, 2,
                                         PMP::parameters::face_size_map(
                                           boost::make_function_property_map<face_descriptor>(f)));
  assert(vertices(sm).size() == 1459);

  {
    Mesh m;

    Point p(0,0,0), q(1,0,0), r(0,1,0), s(0,0,1);
    CGAL::make_tetrahedron(p,q,r,s,m);
    CGAL::make_tetrahedron(p,q,r,s,m);

    Point t(100,100,100);
    CGAL::make_triangle(p,q,t,m);

    Face_descriptor_area_functor<Mesh, Kernel> f(m, k);
    PMP::keep_large_connected_components(m, 10,
                                         CGAL::parameters::face_size_map(
                                           boost::make_function_property_map<face_descriptor>(f)));
    assert(vertices(m).size() == 3);

    PMP::keep_largest_connected_components(m, 1);
    assert(PMP::internal::number_of_connected_components(m) == 1);
    PMP::keep_largest_connected_components(m, 0);
    assert(is_empty(m));
    assert(PMP::internal::number_of_connected_components(m) == 0);
  }
}

int main(int /*argc*/, char** /*argv*/)
{
  const std::string filename = CGAL::data_file_path("meshes/blobby_3cc.off");
  Mesh sm;
  std::ifstream in(filename);
  assert(in.good());
  in >> sm;

  Kernel k;

  std::cout << "VEF " << num_vertices(sm) << " " << num_edges(sm) << " " << num_faces(sm) << "\n";

  test_CC_with_default_size_map(sm, k);
  test_CC_with_area_size_map(sm, k);

  return EXIT_SUCCESS;
}


/*
 * Copyright 2018 Pedro Proenza <p.proenca@surrey.ac.uk> (University of Surrey)
 *
 */

#include <iostream>
#include <cstdio>
#define _USE_MATH_DEFINES
#include <math.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>

#include "CAPE/CAPE.h"
#include "RSCamera.h"

bool done = false;
float COS_ANGLE_MAX = cos(M_PI/12);
float MAX_MERGE_DIST = 50.0f;
bool cylinder_detection= true;
CAPE * plane_detector;
std::vector<cv::Vec3b> color_code;

// void draw3DCoordinateAxes(cv::Mat image, const std::vector<cv::Point2f> &list_points2d){
//     cv::Scalar red(0, 0, 255);
//     cv::Scalar green(0,255,0);
//     cv::Scalar blue(255,0,0);
//     cv::Scalar black(0,0,0);

//     cv::Point2i origin = list_points2d[0];
//     cv::Point2i pointX = list_points2d[1];
//     cv::Point2i pointY = list_points2d[2];
//     cv::Point2i pointZ = list_points2d[3];

//     drawArrow(image, origin, pointX, red, 9, 2);
//     drawArrow(image, origin, pointY, green, 9, 2);
//     drawArrow(image, origin, pointZ, blue, 9, 2);
//     cv::circle(image, origin, radius/2, black, -1, lineType );
// }

bool loadCalibParameters(std::string filepath, cv:: Mat & intrinsics_rgb, cv::Mat & dist_coeffs_rgb, cv:: Mat & intrinsics_ir, cv::Mat & dist_coeffs_ir, cv::Mat & R, cv::Mat & T){

    cv::FileStorage fs(filepath,cv::FileStorage::READ);
    if (fs.isOpened()){
        fs["RGB_intrinsic_params"]        >> intrinsics_rgb;
        fs["RGB_distortion_coefficients"] >> dist_coeffs_rgb;
        fs["IR_intrinsic_params"]         >> intrinsics_ir;
        fs["IR_distortion_coefficients"]  >> dist_coeffs_ir;
        fs["Rotation"]                    >> R;
        fs["Translation"]                 >> T;
        fs.release();
        return true;
    }else{
        std::cerr << "Calibration file missing" << std::endl;
        return false;
    }
}

int main(int argc, char ** argv){

    RSCamera* camera = new RSCamera();
    if(!camera->init(0)){
        std::cout << "Error inicializating camera \n";
        return -1;
    }
    for(int i = 0 ; i < 10 ; i++)
        camera->grab();

    // Create window
    cv::namedWindow("Segmentation",cv::WINDOW_AUTOSIZE);
    cv::namedWindow("mask",cv::WINDOW_AUTOSIZE);
    cvStartWindowThread();

    cv::Mat intrinsics, coeffs;
    camera->leftCalibration(intrinsics, coeffs);

    // Get intrinsics
    cv::Mat K_rgb, K_ir, dist_coeffs_rgb, dist_coeffs_ir, R_stereo, t_stereo;
    std::stringstream calib_path;
    calib_path << "/home/grvc/programming/CAPE/Data/pipe/calib_params_2.xml";
    loadCalibParameters(calib_path.str(), K_rgb, dist_coeffs_rgb, K_ir, dist_coeffs_ir, R_stereo, t_stereo);
    float fx_ir  = K_ir.at<double>(0,0);  float fy_ir  = K_ir.at<double>(1,1);
    float cx_ir  = K_ir.at<double>(0,2);  float cy_ir  = K_ir.at<double>(1,2);
    float fx_rgb = K_rgb.at<double>(0,0); float fy_rgb = K_rgb.at<double>(1,1);
    float cx_rgb = K_rgb.at<double>(0,2); float cy_rgb = K_rgb.at<double>(1,2);

    cv::Mat left, rigth;
    if(!camera->rgb(left, rigth)){
        std::cout << "Error getting first color frame \n";
        return -1;
    }

    cv::Mat depth;
    if(!camera->depth(depth)){
        std::cout << "Error getting first depth frame \n";
        return -1;
    }

    int width, height;
    if(left.data){
        width = left.cols;
        height = left.rows;
    }else{
        std::cout << "Error loading image color dimensions \n";
        return -1;
    }
    int PATCH_SIZE = 20;
    int nr_horizontal_cells = width/PATCH_SIZE;
    int nr_vertical_cells = height/PATCH_SIZE;

    // Pre-computations for backprojection
    cv::Mat_<float> X_pre(height,width);
    cv::Mat_<float> Y_pre(height,width);
    cv::Mat_<float> U(height,width);
    cv::Mat_<float> V(height,width);
    for (int r=0;r<height; r++){
        for (int c=0;c<width; c++){
            // Not efficient but at this stage doesn t matter
            X_pre.at<float>(r,c) = (c-cx_ir)/fx_ir;
            Y_pre.at<float>(r,c) = (r-cy_ir)/fy_ir;
        }
    }

    // Pre-computations for maping an image point cloud to a cache-friendly array where cell's local point clouds are contiguous
    cv::Mat_<int> cell_map(height,width);

    for (int r=0;r<height; r++){
        int cell_r = r / PATCH_SIZE;
        int local_r = r % PATCH_SIZE;

        for (int c=0;c<width; c++){
            int cell_c = c/PATCH_SIZE;
            int local_c = c%PATCH_SIZE;
            cell_map.at<int>(r,c) = (cell_r*nr_horizontal_cells+cell_c)*PATCH_SIZE*PATCH_SIZE + local_r*PATCH_SIZE + local_c;
        }
    }

    cv::Mat_<float> X(height, width);
    cv::Mat_<float> Y(height, width);
    cv::Mat_<float> X_t(height, width);
    cv::Mat_<float> Y_t(height, width);
    Eigen::MatrixXf cloud_array(width * height, 3);
    Eigen::MatrixXf cloud_array_organized(width * height, 3);

    // Populate with random color codes
    for(int i=0; i<100;i++){
        cv::Vec3b color;
        color[0]=rand()%255;
        color[1]=rand()%255;
        color[2]=rand()%255;
        color_code.push_back(color);
    }

    // Add specific colors for planes
    color_code[0][0] = 0;   color_code[0][1] = 0;   color_code[0][2] = 255;
    color_code[1][0] = 255; color_code[1][1] = 0;   color_code[1][2] = 204;
    color_code[2][0] = 255; color_code[2][1] = 100; color_code[2][2] = 0;
    color_code[3][0] = 0;   color_code[3][1] = 153; color_code[3][2] = 255;
    // Add specific colors for cylinders
    color_code[50][0] = 178; color_code[50][1] = 255; color_code[50][2] = 0;
    color_code[51][0] = 255; color_code[51][1] = 0;   color_code[51][2] = 51;
    color_code[52][0] = 0;   color_code[52][1] = 255; color_code[52][2] = 51;
    color_code[53][0] = 153; color_code[53][1] = 0;   color_code[53][2] = 255;

    // Initialize CAPE
    plane_detector = new CAPE(height, width, PATCH_SIZE, PATCH_SIZE, cylinder_detection, COS_ANGLE_MAX, MAX_MERGE_DIST);

    while(1){
        camera->grab();

        cv::Mat left, rigth;
        if(!camera->rgb(left, rigth)){
            std::cout << "Error color frame \n";
            return -1;
        }

        cv::Mat depth;
        if(!camera->depth(depth)){
            std::cout << "Error depth frame \n";
            return -1;
        }
        depth.convertTo(depth, CV_32F);

        // Backproject to point cloud
        X = X_pre.mul(depth); Y = Y_pre.mul(depth);
        cloud_array.setZero();

        // The following transformation+projection is only necessary to visualize RGB with overlapped segments
        // Transform point cloud to color reference frame
        X_t = ((float)R_stereo.at<double>(0,0))*X + ((float)R_stereo.at<double>(0,1))*Y + ((float)R_stereo.at<double>(0,2))*depth + (float)t_stereo.at<double>(0);
        Y_t = ((float)R_stereo.at<double>(1,0))*X + ((float)R_stereo.at<double>(1,1))*Y + ((float)R_stereo.at<double>(1,2))*depth + (float)t_stereo.at<double>(1);
        depth = ((float)R_stereo.at<double>(2,0))*X + ((float)R_stereo.at<double>(2,1))*Y + ((float)R_stereo.at<double>(2,2))*depth + (float)t_stereo.at<double>(2);

        CAPE::projectPointCloud(X_t, Y_t, depth, U, V, fx_rgb, fy_rgb, cx_rgb, cy_rgb, t_stereo.at<double>(2), cloud_array);

        cv::Mat_<cv::Vec3b> seg_rz = cv::Mat_<cv::Vec3b>(height,width,cv::Vec3b(0,0,0));
        cv::Mat_<uchar> seg_output = cv::Mat_<uchar>(height,width,uchar(0));
        cv::Mat_<uchar> seg_output_cylinder = cv::Mat_<uchar>(height,width,uchar(0));

        // Run CAPE
        int nr_planes, nr_cylinders;
        std::vector<PlaneSeg> plane_params;
        std::vector<CylinderSeg> cylinder_params;
        double t1 = cv::getTickCount();
        CAPE::organizePointCloudByCell(cloud_array, cloud_array_organized, cell_map);
        plane_detector->process(cloud_array_organized, nr_planes, nr_cylinders, seg_output, plane_params, cylinder_params);
        double t2 = cv::getTickCount();
        double time_elapsed = (t2-t1)/(double)cv::getTickFrequency();
        std::cout<<"Total time elapsed: " << time_elapsed << " sec" << std::endl;

        /* Uncomment this block to print model params
        for(int p_id=0; p_id<nr_planes;p_id++){
            cout<<"[Plane #"<<p_id<<"] with ";
            cout<<"normal: ("<<plane_params[p_id].normal[0]<<" "<<plane_params[p_id].normal[1]<<" "<<plane_params[p_id].normal[2]<<"), ";
            cout<<"d: "<<plane_params[p_id].d<<endl;
        }

        */
        for(int c_id=0; c_id<nr_cylinders;c_id++){
            std::cout << "[Cylinder #"<<c_id<<"] with ";
            std::cout << "axis: ("<<cylinder_params[c_id].axis[0]<<" "<<cylinder_params[c_id].axis[1]<<" "<<cylinder_params[c_id].axis[2]<<"), ";
            std::cout << "center: (" << cylinder_params[c_id].centers[0].transpose()<<"), ";
            std::cout << "radius: " << cylinder_params[c_id].radii[0] << std::endl;
        }

        // loop to extract cylinder mask
        for(int ii=0; ii < seg_output.rows; ii++){
            for(int jj=0; jj < seg_output.cols; jj++){
                cv::Scalar px = seg_output.at<uchar>(ii,jj);
                if (px[0] > 50){ // cylinder threshold
                    seg_output_cylinder.at<uchar>(ii,jj) = uchar(255);
                }else{
                    seg_output_cylinder.at<uchar>(ii,jj) = uchar(0);
                }
            }
        }
        cv::imshow("mask", seg_output_cylinder);

        // Map segments with color codes and overlap segmented image w/ RGB
        uchar * sCode;
        uchar * dColor;
        uchar * srgb;
        int code;
        for(int r=0; r<  height; r++){
            dColor = seg_rz.ptr<uchar>(r);
            sCode  = seg_output.ptr<uchar>(r);
            srgb   = left.ptr<uchar>(r);
            for(int c=0; c< width; c++){
                code = *sCode;
                if (code>0){
                    dColor[c*3] =   color_code[code-1][0]/2 + srgb[0]/2;
                    dColor[c*3+1] = color_code[code-1][1]/2 + srgb[1]/2;
                    dColor[c*3+2] = color_code[code-1][2]/2 + srgb[2]/2;
                }else{
                    dColor[c*3]   = srgb[0];
                    dColor[c*3+1] = srgb[1];
                    dColor[c*3+2] = srgb[2];
                }
                sCode++; srgb++; srgb++; srgb++;
            }
        }

        // Show frame rate and labels
        cv::rectangle(seg_rz,  cv::Point(0,0),cv::Point(width,20), cv::Scalar(0,0,0),-1);
        std::stringstream fps;
        fps << (int)(1/time_elapsed+0.5) << " fps";
        cv::putText(seg_rz, fps.str(), cv::Point(15,15), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255,255,255,1));
        std::cout << "Number of cylinders: " << nr_cylinders << std::endl;
        int cylinder_code_offset = 50;
        // show cylinder labels
        if (nr_cylinders>0){
            std::stringstream text;
            text<<"Cylinders: ";

            cv::putText(seg_rz, text.str(), cv::Point(width/2,15), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255,255,255,1));
            for(int j=0;j<nr_cylinders;j++){
                cv::rectangle(seg_rz,  cv::Point(width/2 + 80+15*j,6),cv::Point(width/2 + 90+15*j,16), cv::Scalar(color_code[cylinder_code_offset+j][0],color_code[cylinder_code_offset+j][1],color_code[cylinder_code_offset+j][2]),-1);
            }
        }
        cv::imshow("Segmentation", seg_rz);
    }

    cv::destroyWindow("Segmentation");
    cv::destroyWindow("mask");

    return 0;
}


//
// Created by Alex Beccaro on 22/01/2019.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/101-150/123/problem123.hpp"

BOOST_AUTO_TEST_SUITE( Problem123 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem123::solve(9);
        BOOST_CHECK_EQUAL(res, 7037);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem123::solve();
        BOOST_CHECK_EQUAL(res, 21035);
    }

BOOST_AUTO_TEST_SUITE_END()


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/literal.hpp>
#include <fcppt/math/matrix/comparison.hpp>
#include <fcppt/math/matrix/delete_row_and_column.hpp>
#include <fcppt/math/matrix/delete_row_and_column_static.hpp>
#include <fcppt/math/matrix/index.hpp>
#include <fcppt/math/matrix/output.hpp>
#include <fcppt/math/matrix/row.hpp>
#include <fcppt/math/matrix/static.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_matrix_delete_row_and_column
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::math::matrix::static_<
		int,
		4,
		3
	>
	large_matrix_type;

	typedef
	fcppt::math::matrix::static_<
		int,
		3,
		2
	>
	small_matrix_type;

	large_matrix_type const t(
		fcppt::math::matrix::row(
			1, 2, 3
		),
		fcppt::math::matrix::row(
			4, 5, 6
		),
		fcppt::math::matrix::row(
			7, 8, 9
		),
		fcppt::math::matrix::row(
			10, 11, 12
		)
	);

	BOOST_CHECK_EQUAL(
		fcppt::math::matrix::delete_row_and_column(
			t,
			fcppt::literal<
				large_matrix_type::size_type
			>(
				2
			),
			fcppt::literal<
				large_matrix_type::size_type
			>(
				1
			)
		),
		small_matrix_type(
			fcppt::math::matrix::row(
				1, 3
			),
			fcppt::math::matrix::row(
				4, 6
			),
			fcppt::math::matrix::row(
				10, 12
			)
		)
	);

	BOOST_CHECK_EQUAL(
		fcppt::math::matrix::delete_row_and_column(
			t,
			fcppt::literal<
				large_matrix_type::size_type
			>(
				0
			),
			fcppt::literal<
				large_matrix_type::size_type
			>(
				0
			)
		),
		small_matrix_type(
			fcppt::math::matrix::row(
				5, 6
			),
			fcppt::math::matrix::row(
				8, 9
			),
			fcppt::math::matrix::row(
				11, 12
			)
		)
	);
}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_matrix_delete_row_and_column_static
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::math::matrix::static_<
		int,
		4,
		3
	>
	large_matrix_type;

	typedef
	fcppt::math::matrix::static_<
		int,
		3,
		2
	>
	small_matrix_type;

	large_matrix_type const t(
		fcppt::math::matrix::row(
			1, 2, 3
		),
		fcppt::math::matrix::row(
			4, 5, 6
		),
		fcppt::math::matrix::row(
			7, 8, 9
		),
		fcppt::math::matrix::row(
			10, 11, 12
		)
	);

	BOOST_CHECK_EQUAL(
		fcppt::math::matrix::delete_row_and_column_static(
			t,
			fcppt::math::matrix::index<
				2,
				1
			>{}
		),
		small_matrix_type(
			fcppt::math::matrix::row(
				1, 3
			),
			fcppt::math::matrix::row(
				4, 6
			),
			fcppt::math::matrix::row(
				10, 12
			)
		)
	);

	BOOST_CHECK_EQUAL(
		fcppt::math::matrix::delete_row_and_column_static(
			t,
			fcppt::math::matrix::index<
				0,
				0
			>{}
		),
		small_matrix_type(
			fcppt::math::matrix::row(
				5, 6
			),
			fcppt::math::matrix::row(
				8, 9
			),
			fcppt::math::matrix::row(
				11, 12
			)
		)
	);
}


// File generated on Thu Aug 21, 2014 06:00:18 PM by xcpp.
#define XC_CPP
#include <armadillo>
#include <xc>

namespace excentury {
XC_DUMP_TEMPLATED_TENSOR(class elementType, arma::Mat<elementType>, m, m.mem[0]) {
    size_t ndims = 2;
    unsigned char rm = 0;
    XC_BYTE(rm);
    XC_SIZE(ndims);
    size_t size = m.n_elem;
    size_t dim[2] = {m.n_rows, m.n_cols};
    XC_ARRAY(dim, ndims);
    XC_ARRAY(m.mem, size);
}
XC_LOAD_TEMPLATED_TENSOR(class elementType, arma::Mat<elementType>, m) {
    size_t ndims;
    unsigned char rm;
    XC_BYTE(rm);
    if (rm != 0) {
        char msg[500];
        sprintf(msg, "Armadillo Mat::load:\n"
                "    RM mismatch, got %d, needs %d.", rm, 0);
        excentury::error(msg);
    }
    XC_SIZE(ndims);
    if (ndims != 2) {
        char msg[500];
        sprintf(msg, "Armadillo Mat::load('%s'):\n" \
                "    dimension mismatch, needs dim = 2", varname);
        excentury::error(msg);
    }
    size_t* dim = new size_t[ndims];
    XC_ARRAY(dim, ndims);
    m.resize(dim[0], dim[1]);
    elementType* mem = const_cast<elementType*>(m.mem);
    XC_ARRAY(mem, m.n_elem);
    delete [] dim;
}
}
void xc_help() {
    fprintf(stderr,
    "program: arma-ex1\n"
    "\ndescription:\n"
    "    Armadillo example 2: Computes the determinant and inverse of a\n"
    "    matrix.\n"
    "\nparameters:\n"
    "    `A`: input matrix\n"
    "\n");
}
void xc_input() {
    xc_help();
    excentury::TextInterface<excentury::dump_mode> XC_DI_(stdout);
    arma::mat A(2, 2); XC_DI_.dump(A, "A", A(0,0));
    XC_DI_.close();
}
int main(int argc, char** argv) {
    /*Armadillo example 2: Computes the determinant and inverse of a
    matrix.*/
    excentury::check_inputs(argc);
    excentury::print_help(argv, xc_help);
    excentury::print_inputs(argv, xc_input);
    excentury::STextInterface<excentury::load_mode> XC_LI_(argv[1]);
    arma::mat A(2, 2); XC_LI_.load(A, A(0,0));
    XC_LI_.close();

    double d = det(A);
    arma::mat Ainv;
    try {
        Ainv = inv(A);
    } catch (std::runtime_error& run_error) {
        excentury::error(run_error.what());
    }

    excentury::TextInterface<excentury::dump_mode> XC_DI_(stdout);
    XC_DI_.dump(d, "detA");
    XC_DI_.dump(Ainv, "Ainv", Ainv(0, 0));
    XC_DI_.close();
}


//
// Created by jieming on 20.10.20.
//

#include <array>
#include <cmath>
#include <iostream>

#include <Eigen/Dense>
using namespace std;
using Eigen::MatrixXd;

void jointImpedanceControl(){
    // Compliance parameters
    const double translational_stiffness{150};
    const double rotational_stiffness{10};
    MatrixXd stiffness(6,6), damping(6,6);
    stiffness.setZero();
    stiffness.topLeftCorner(3,3) << translational_stiffness*MatrixXd::Identity(3,3);
    stiffness.bottomRightCorner(3,3) << rotational_stiffness*MatrixXd::Identity(3,3);
    damping.setZero();
    damping.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) * MatrixXd::Identity(3,3);
    damping.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) * MatrixXd::Identity(3,3);

}


#include <armadillo>
#include <cstring>

#include "matrix/utility.hh"

#include "gyro/gyro_rocket6g.hh"

#include "stochastic.hh"

#include <ctime>

GyroRocket6G::GyroRocket6G(Data_exchang &input)
    : VECTOR_INIT(EUG, 3),
      VECTOR_INIT(EWG, 3),
      VECTOR_INIT(EWALKG, 3),
      VECTOR_INIT(EUNBG, 3),
      VECTOR_INIT(EMISG, 3),
      VECTOR_INIT(ESCALG, 3),
      VECTOR_INIT(EBIASG, 3),
      VECTOR_INIT(ITA1, 3),
      VECTOR_INIT(ITA2, 3),
      VECTOR_INIT(BETA, 3) {
  snprintf(name, sizeof(name), "Rocket6G Gyro Sensor Model");
  srand(static_cast<unsigned int>(time(NULL)));
  data_exchang = &input;
}

GyroRocket6G::GyroRocket6G(const GyroRocket6G &other) {
  this->WBICB = other.WBICB;
  this->EWBIB = other.EWBIB;
  this->EUG = other.EUG;
  this->EWG = other.EWG;
  this->EWALKG = other.EWALKG;
  this->EUNBG = other.EUNBG;
  this->EMISG = other.EMISG;
  this->ESCALG = other.ESCALG;
  this->EBIASG = other.EBIASG;
  this->ITA1 = other.ITA1;
  this->ITA2 = other.ITA2;
  this->BETA = other.BETA;
  this->data_exchang = other.data_exchang;
}

GyroRocket6G &GyroRocket6G::operator=(const GyroRocket6G &other) {
  if (&other == this) return *this;

  this->WBICB = other.WBICB;
  this->EWBIB = other.EWBIB;
  this->EUG = other.EUG;
  this->EWG = other.EWG;
  this->EWALKG = other.EWALKG;
  this->EUNBG = other.EUNBG;
  this->EMISG = other.EMISG;
  this->ESCALG = other.ESCALG;
  this->EBIASG = other.EBIASG;
  this->ITA1 = other.ITA1;
  this->ITA2 = other.ITA2;
  this->BETA = other.BETA;
  this->data_exchang = other.data_exchang;

  return *this;
}

void GyroRocket6G::algorithm(double int_step) {
  arma::vec3 WBIB = grab_WBIB();
  arma::vec3 FSPB = grab_FSPB();

  //-------------------------------------------------------------------------
  // ARW RRW
  double sig(1.0);
  double RRW(0.0130848811);  // 0.4422689813  7.6072577e-3
  double ARW(0.2828427125);  // 0.07071067812  7.90569415e-3
  double Freq(200.0);

  for (int i = 0; i < 3; i++) {
    ITA2(i) =
        gauss(0, 1.0) * RRW * RAD;  // distribution(generator) * RRW * RAD;
    BETA(i) = 0.9999 * BETA(i) + ITA2(i) * int_step;
    ITA1(i) = gauss(0, 1.0) * (ARW * sqrt(Freq) / 60 * (1 / sig)) *
              RAD;  // distribution(generator) * (ARW * sqrt(Freq) / 60 * (1 /
                    // sig)) * RAD;
  }

  // combining all uncertainties
  this->EWBIB = ITA1 + BETA;  // EMSBG + EUG + EWG;

  this->WBICB = WBIB + EWBIB;

  data_exchang->hset("WBICB", WBICB);
  return;
}

int GyroRocket6G::write_to_(const char *bus_name) {
  int rc = 0;
  rc |= nxbus_mset(NXBUS_DOUBLE, "Gyro:_WBICB", 3, _WBICB);
  rc |= nxbus_mset(NXBUS_DOUBLE, "Gyro:_EWBIB", 3, _EWBIB);
  return rc;
}


/*
 * PoseOptimizationGeometric.cpp
 *
 *  Created on: Aug 30, 2017
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich
 */

#include <free_gait_core/pose_optimization/PoseOptimizationGeometric.hpp>

#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>
#include <kindr/Core>

using namespace Eigen;

namespace free_gait {

PoseOptimizationGeometric::PoseOptimizationGeometric(const AdapterBase& adapter)
    : PoseOptimizationBase(adapter)
{
}

PoseOptimizationGeometric::~PoseOptimizationGeometric()
{
}

void PoseOptimizationGeometric::setStanceForOrientation(const Stance& stance)
{
  stanceForOrientation_ = stance;
}

bool PoseOptimizationGeometric::optimize(Pose& pose)
{
  checkSupportRegion();

  // Planar position: Use geometric center of support region.
  Position center;
  center.vector().head(2) = supportRegion_.getCentroid();

  // Height: Average of stance plus default height.
  for (const auto& foot : stance_) {
    center.z() += foot.second.z() - nominalStanceInBaseFrame_.at(foot.first).z();
  }
  center.z() /= (double) stance_.size();
  pose.getPosition() = center;

  // Orientation: Squared error minimization (see sec. 4.2.2 from Bloesch, Technical
  // Implementations of the Sense of Balance, 2016).
  // Notes on (38):
  // - i: Identity pose (I),
  // - j: Solution (B),
  // - t = I_r_IB,
  // - q = q_IB,
  // - a_k = I_f_k: Foot position for leg k in inertial frame,
  // - b_k = B_\hat_f_K: Nominal foot position for leg k in base frame.

  Eigen::Matrix4d C(Eigen::Matrix4d::Zero()); // See (45).
  Eigen::Matrix4d A(Eigen::Matrix4d::Zero()); // See (46).
  for (const auto& foot : stance_) {
    kindr::QuaternionD footPositionInertialFrame(0.0, foot.second.vector()); // \bar_a_k = S^T * a_k (39).
    kindr::QuaternionD defaultFootPositionBaseFrame(0.0, nominalStanceInBaseFrame_.at(foot.first).vector()); // \bar_b_k = S^T * b_k.
    Eigen::Matrix4d Ak = footPositionInertialFrame.getQuaternionMatrix() - defaultFootPositionBaseFrame.getConjugateQuaternionMatrix(); // See (46).
    C += Ak * Ak; // See (45).
    A += Ak; // See (46).
  }
  A = A / ((double) stance_.size()); // Error in (46).
  C -= stance_.size() * A * A; // See (45).
  Eigen::EigenSolver<Eigen::Matrix4d> eigenSolver(C);
  int maxCoeff;
  eigenSolver.eigenvalues().real().maxCoeff(&maxCoeff);
  // Eigen vector corresponding to max. eigen value.
  pose.getRotation() = RotationQuaternion(eigenSolver.eigenvectors().col(maxCoeff).real());
  pose.getRotation().setUnique();

  // Get yaw rotation desired heading direction with respect to the target feet.
  const Position positionForeFeetMidPointInWorld = (stanceForOrientation_.at(LimbEnum::LF_LEG) + stanceForOrientation_.at(LimbEnum::RF_LEG)) * 0.5;
  const Position positionHindFeetMidPointInWorld = (stanceForOrientation_.at(LimbEnum::LH_LEG) + stanceForOrientation_.at(LimbEnum::RH_LEG)) * 0.5;
  Vector desiredHeadingDirectionInWorld = Vector(positionForeFeetMidPointInWorld - positionHindFeetMidPointInWorld);
  desiredHeadingDirectionInWorld.z() = 0.0;
  RotationQuaternion desiredHeading;
  desiredHeading.setFromVectors(Vector::UnitX().toImplementation(), desiredHeadingDirectionInWorld.vector());

  // Apply roll/pitch adaptation factor (~0.7).
  const RotationQuaternion yawRotation(RotationVector(RotationVector(pose.getRotation()).vector().cwiseProduct(Eigen::Vector3d::UnitZ())));
  const RotationQuaternion rollPitchRotation(RotationVector(0.7 * RotationVector(yawRotation.inverted() * pose.getRotation()).vector()));
//  pose.getRotation() = yawRotation * rollPitchRotation; // Alternative.
  pose.getRotation() = desiredHeading * rollPitchRotation;

  return true;
}

} /* namespace */


#include "jetEmission.h"

// Standard C++ libraries
#include <iomanip>

// Boost libraries
#include <boost/property_tree/ptree.hpp>
#include <boost/math/tools/roots.hpp>

// Standard user-made libraries
#include <fparameters/SpaceIterator.h>
#include <fparameters/parameters.h>
#include <fmath/physics.h>
#include <fparameters/Dimension.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_sf_gamma.h>
#include "absorption.h"
// Project headers
#include "globalVariables.h"
#include "messages.h"
#include "read.h"
#include "adafFunctions.h"
#include "write.h"
#include <fmath/RungeKutta.h>

#include <fluminosities/thermalSync.h>
#include <fluminosities/blackBody.h>
#include "absorption.h"

double n_pl(double g, double gammaC, double gammaMin, double Npl, double pJet)
{
	if (gammaMin > gammaC) {
		double factor = 0.0;
		if (g > gammaC && g < gammaMin)
			factor = pow(gammaMin,1.0-pJet)/(g*g);
		else if (g > gammaMin)
			factor = pow(g,-(pJet+1.0));
		return Npl*(pJet-1.0)*gammaC*factor;
	} else {
		double factor = 0.0;
		if (g >= gammaMin && g <= gammaC)
			factor = 1.0;
		else if (g > gammaC)
			factor = gammaC/g;
		return Npl*(pJet-1.0)*factor*pow(g,-pJet);
	}
}

double n_pl_2(double g, double Npl, double pJet)
{
	return Npl*(pJet-1.0)*pow(g,-pJet);
}

double jSyJet(double g, double nu, double magf)
{
	double U_B = magf*magf/(8.0*pi);
	double nuB = electronCharge*magf/(2*pi*electronMass*cLight);
	double t = nu / (3.0*g*g*nuB);
	double K_43,K_13;
	if (t > 30) {
		K_43 = K_13 = exp(-t) * sqrt(0.5*pi/t);
	} else if (t < 0.01*sqrt(2)) {
		K_13 = 0.5*gsl_sf_gamma(1.0/3.0) * pow(2.0/t,1.0/3.0);
		K_43 = 0.5*gsl_sf_gamma(4.0/3.0) * pow(2.0/t,4.0/3.0);
	} else {
		K_43 = gsl_sf_bessel_Knu(4.0/3.0,t);
		K_13 = gsl_sf_bessel_Knu(1.0/3.0,t);
	}
	return 3.0*sqrt(3.0)/pi * thomson * cLight * U_B * t*t * (K_43*K_13-3*t/5 * (K_43*K_43-K_13*K_13));
}

double jSyJet2(double g, double nu, double magf)
{
	double nuC = 3.0/(4.0*pi)*electronCharge*magf/(electronMass*cLight) * g*g;
	double x = nu/nuC;
	return sqrt(3.0)*P3(electronCharge)/electronRestEnergy * magf *1.85*pow(x,1.0/3.0)*exp(-x);
}

double sigma_Sy(double g, double nu, double magf)
{
	double nuB = electronCharge*magf/(2*pi*electronMass*cLight);
	double t = nu/(3*g*g*nuB);
	double K_43,K_13;
	if (t > 30) {
		K_43 = K_13 = exp(-t) * sqrt(0.5*pi/t);
	} else if (t < 0.01*sqrt(2)) {
		K_13 = 0.5*gsl_sf_gamma(1.0/3.0) * pow(2.0/t,1.0/3.0);
		K_43 = 0.5*gsl_sf_gamma(4.0/3.0) * pow(2.0/t,4.0/3.0);
	} else {
		K_43 = gsl_sf_bessel_Knu(4.0/3.0,t);
		K_13 = gsl_sf_bessel_Knu(1.0/3.0,t);
	}
	return 8.0*sqrt(3.0)/15 *pi*pi*electronCharge / magf * t / pow(g,5.0) * (K_43*K_43-K_13*K_13);
}

double tau_Sy(double deltaR, double nu, double gammaC, double gammaMin, double Npl, double pJet, double magf)
{
	return deltaR * RungeKuttaSimple(gammaMin,1.0e10,[&nu,&gammaC,&gammaMin,&Npl,&pJet,&magf]
						(double g) {return sigma_Sy(g,nu,magf)*n_pl(g,gammaC,gammaMin,Npl,pJet);});
}

void jetProcesses(State& st, const string& filename)
{
	double accRateFraction = GlobalConfig.get<double>("nonThermal.jet.accRateFraction");
	double accRate = accRateFraction*accRateADAF(schwRadius);
	double openingAngle = GlobalConfig.get<double>("nonThermal.jet.openingAngle");
	double bulkLorentzFactor = GlobalConfig.get<double>("nonThermal.jet.lorentzFactor");
	double inclinationAngle = GlobalConfig.get<double>("nonThermal.jet.inclination")*(pi/180.0);
	double nt_electronFraction = GlobalConfig.get<double>("nonThermal.jet.etaInj");
	double eB = GlobalConfig.get<double>("nonThermal.jet.magneticEnergyFraction");
	double eElectrons = GlobalConfig.get<double>("nonThermal.jet.electronEnergyFraction");
	double pJet = GlobalConfig.get<double>("nonThermal.jet.pIndex");
	double zMin = GlobalConfig.get<double>("nonThermal.jet.zMin");

	double z0 = P2(bulkLorentzFactor)*schwRadius*zMin;
	double zMax = z0*1.0e4;
	size_t nZ = 100;

	double gamma2 = sqrt(0.5*(P2(bulkLorentzFactor)+1.0));
	double betaJet = sqrt(1.0-1.0/P2(bulkLorentzFactor));
	double vJet = betaJet*cLight;
	double n1 = accRate/(pi*z0*z0*openingAngle*openingAngle*vJet)/protonMass;
	double n2 = (4.0*gamma2+3.0)*n1;
	double Ti = 6.3e11;
	double Te = 1.0e9;

	double pasoZ = pow(zMax/z0,1.0/nZ);
	double z = z0;
	size_t jZ = 0;
	Matrix lumBeamed;
	matrixInit(lumBeamed,nE,nZ,0.0);
	while (z < zMax) {
		z *= pasoZ;

		double dz = z*(pasoZ-1.0);
		double area = pi*P2(openingAngle*z);
		double vol = area*dz;
		double dens = n2 * P2(z0/z);
		double magf = sqrt(8.0*pi*eB*(gamma2-1.0)*dens*protonMass*cLight2);

		// NONTHERMAL POPULATION
		double gammaMin = max((gamma2-1.0)*(pJet-2.0)/(pJet-1.0) * (protonMass/electronMass) *
								(eElectrons/nt_electronFraction),1.0);
		gammaMin = 2.0;
		double tDyn = z/cLight;
		double Npl = nt_electronFraction*dens*pow(gammaMin,pJet-1.0);
		double gammaC = 6*pi*electronRestEnergy/(magf*magf*thomson*z);

		size_t jE = 0;
		st.photon.ps.iterate([&](const SpaceIterator& i) {
			double nu = i.val(DIM_E)/planck;
			double beta = sqrt(1.0-1.0/P2(bulkLorentzFactor));
			double doppler = 1.0/(bulkLorentzFactor*(1.0-beta*cos(inclinationAngle)));
			double nuDoppler = nu/doppler;

			double lumLocal = 0.0;
			double gamma1 = 2.0;
			double gammaMax = 1.0e7;
			size_t nnE = 1000;
			double pasoG = pow(gammaMax/gamma1,1.0/nnE);
			double gamma = gamma1;
			for (size_t jjE=0;jjE<nnE;jjE++) {
				double dgamma = gamma*(pasoG-1.0);
				//if (jE==0)
				//	cout << z/schwRadius << "\t" << gamma << "\t" << n_pl(gamma,gammaC,gammaMin,Npl,pJet)*dgamma << endl;
				lumLocal += dgamma*n_pl(gamma,gammaC,gammaMin,Npl,pJet)*jSyJet2(gamma,nuDoppler,magf);
				//lumLocal += dgamma*n_pl_2(gamma,Npl,pJet)*jSyJet2(gamma,nuDoppler,magf);
				gamma *= pasoG;
			}
			lumLocal *= vol;

			//double lumLocal = vol*RungeKuttaSimple(gammaC,gammaC*1.0e5,[&gammaC,&gammaMin,&Npl,
			//					&pJet,&magf,&nuDoppler](double g){return n_pl(g,gammaC,gammaMin,Npl,pJet)*
			//					jSyJet2(g,nuDoppler,magf);});

			double a_th = jSync(planck*nu,Te,magf,dens)/bb(nu,Te);
			double a_pl = RungeKuttaSimple(gammaMin,gammaMin*1.0e10,[&](double g)
						{return n_pl(g,gammaC,gammaMin,Npl,pJet)*sigma_Sy(g,nu,magf);});
			double tau = z*openingAngle*(a_pl+a_th);
			//tau = tau_Sy(z*openingAngle,nu,gammaC,gammaMin,Npl,pJet,magf);
			lumLocal *= (tau > 1.0e-10 ? (1.0-exp(-tau))/tau : 1.0);
			lumBeamed[jE][jZ] = lumLocal * doppler*doppler*doppler;
			jE++;
		},{-1,0,0});
		jZ++;
	}

	ofstream file;
	file.open(filename.c_str(),ios::out);
	size_t jE = 0.0;
	st.photon.ps.iterate([&](const SpaceIterator& i) {
		double nu = i.val(DIM_E)/planck;
		double lum = 0.0;
		for (size_t jZ=0;jZ<nZ;jZ++) {
			lum += lumBeamed[jE][jZ];
		}
		file << nu << "\t" << nu*lum << endl;
		jE++;
	},{-1,0,0});
	file.close();
}

void jetProcesses2(State& st, const string& filename)
{
	double accRateFraction = GlobalConfig.get<double>("nonThermal.jet.accRateFraction");
	double accRate = accRateFraction*accRateADAF(schwRadius);
	double openingAngle = GlobalConfig.get<double>("nonThermal.jet.openingAngle");
	double bulkLorentzFactor = GlobalConfig.get<double>("nonThermal.jet.lorentzFactor");
	double inclinationAngle = GlobalConfig.get<double>("nonThermal.jet.inclination")*(pi/180.0);
	double nt_electronFraction = GlobalConfig.get<double>("nonThermal.jet.etaInj");
	double pJet = GlobalConfig.get<double>("nonThermal.jet.pIndex");
	double mB = 1.5;
	size_t nZ = 100;

	double z0 = 50*schwRadius;
	double r0 = openingAngle*z0;
	double betaJet = sqrt(1.0-1.0/P2(bulkLorentzFactor));
	double vJet = betaJet*cLight;
	double B0 = sqrt(accRate*cLight2*8/(r0*r0*vJet));
	double n0 = accRate/(bulkLorentzFactor*pi*r0*r0*vJet*protonMass);
	double zMax = z0*1.0e5;

	double pasoZ = pow(zMax/z0,1.0/nZ);
	double z = z0;
	size_t jZ = 0;
	Matrix lumBeamed;
	matrixInit(lumBeamed,nE,nZ,0.0);
	while (z < zMax) {
		z *= pasoZ;
		double dz = z*(pasoZ-1.0);
		double area = pi*P2(openingAngle*z);
		double vol = area*dz;
		double dens = n0 * P2(z0/z);
		double magf = B0 * pow(z0/z,mB);

		// NONTHERMAL POPULATION
		double gammaMin = 2.0;
		double Npl = nt_electronFraction*dens*pow(gammaMin,pJet-1.0);
		double gammaC = 6*pi*electronRestEnergy/(magf*magf*thomson*z);

		size_t jE = 0;
		st.photon.ps.iterate([&](const SpaceIterator& i) {
			double nu = i.val(DIM_E)/planck;
			double beta = sqrt(1.0-1.0/P2(bulkLorentzFactor));
			double doppler = 1.0/(bulkLorentzFactor*(1.0-beta*cos(inclinationAngle)));
			double nuDoppler = nu/doppler;
			double lumLocal = vol*RungeKuttaSimple(gammaMin,gammaMin*1.0e10,[&gammaC,&gammaMin,&Npl,
									&pJet,&magf,&nuDoppler](double g){return n_pl(g,gammaC,gammaMin,Npl,pJet)*
										jSyJet2(g,nuDoppler,magf);});
			double a_pl = RungeKuttaSimple(gammaMin,gammaMin*1.0e10,[&](double g)
							{return n_pl(g,gammaC,gammaMin,Npl,pJet)*sigma_Sy(g,nu,magf);});
			double tau = z*openingAngle*a_pl;
			//tau = tau_Sy(z*openingAngle,nu,gammaC,gammaMin,Npl,pJet,magf);
			lumLocal *= (tau > 1.0e-10 ? (1.0-exp(-tau))/tau : 1.0);
			lumBeamed[jE][jZ] = lumLocal * doppler*doppler*doppler;
			jE++;
		},{-1,0,0});
		jZ++;
	}

	ofstream file;
	file.open(filename.c_str(),ios::out);
	size_t jE = 0.0;
	st.photon.ps.iterate([&](const SpaceIterator& i) {
		double nu = i.val(DIM_E)/planck;
		double lum = 0.0;
		for (size_t jZ=0;jZ<nZ;jZ++) {
			lum += lumBeamed[jE][jZ];
		}
		file << nu << "\t" << nu*lum << endl;
		jE++;
	},{-1,0,0});
	file.close();
}


#ifndef ___COLORSPACEADJUSTMENT_H___
#define ___COLORSPACEADJUSTMENT_H___
#include <iostream>
#include <math.h>
//Eigen Library
#include <Eigen/Dense>
#include <Eigen/Geometry>
//OpenCV Library
#include <stdio.h>
#include <cstdlib>
// #include <opencv/cv.h>
// #include <opencv/highgui.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

/**
 * @brief Class that contains the functions to fully create the color space "oRGB"
 *
 */
class ConvertTooRGB {
 public:
  ConvertTooRGB(void) = default;

/**
 * @brief Function that normalizes the values
 *
 */
bool setNormalizeImage(cv::Mat img1);

/**
 * @brief function that returns a vector after it has been multiplied with a specific matrix given because of the formula
 *
 * @param img
 * @return cv::Mat
 */
bool setLinearImage();

/**
 * @brief contains a matrix of rotation, that we will use to rotate a vector in the next function
 *
 * @param angle
 * @return Eigen::Matrix3d
 */
    Eigen::Matrix3d rotatePoint(double angle);

/**
 * @brief based on the angle taken from the vector of the loaded image, it rotates that vector using the previous rotatePoint function
 *
 * @param img
 * @return cv::Mat
 */
bool fullRotation();

/**
 * @brief It changes the values of ths second and third vector of the image
 *
 * @param cb
 * @param crg
 * @param img
 * @return cv::Mat
 */

//cv::Mat filter(double cb, double crg, cv::Mat img);

/**
 * @brief Set the Filter values- it changes the value of the vectors Cyb and Crg
 *
 * @param img
 * @param cyb
 * @param crg
 * @return cv::Mat
 */
cv::Mat setFilter(cv::Mat img,double cyb, double crg);

/**
 * @brief Returns the matrix value from the private variable 'normalizedImage', which has been previously set by SetNormalizedImage function.
 *
 * @return cv::Mat
 */
cv::Mat getnormalizedImage();

/**
 * @brief Returns the matrix value from the private variable 'linearImage', which has been previously set by setLinearImage function.
 *
 * @return cv::Mat
 */
cv::Mat getLinearImage();

/**
 * @brief Returns the matrix value from the private variable 'rotatedImage', which has been previously set by fullRotation() function.
 *
 * @return cv::Mat
 */
cv::Mat getRotatedImage();

enum  channel {L, Cyb, Crg};

/**
 * @brief Function to separate channels of the image
 *
 * @param img
 * @param c
 * @return cv::Mat
 */
cv::Mat channelExtraction( cv::Mat img1, channel c);

private:
cv::Mat normalizedImage;
cv::Mat linearImage;
cv::Mat rotatedImage;
};

#endif


#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <ros/ros.h>

int main(int argc,char **argv)
{

    ros::init(argc, argv, "test");

    ros::NodeHandle n;

    Eigen::MatrixXd MatAdf;
    Eigen::MatrixXd MatBdf;
    Eigen::MatrixXd MatCdf;

    MatAdf.resize(100,100);
    MatAdf.setRandom();

    MatBdf.resize(100,100);
    MatBdf.setRandom();

    Eigen::SparseMatrix<double> MatAsf;
    Eigen::SparseMatrix<double> MatBsf;
    Eigen::SparseMatrix<double> MatCsf;

    MatAsf.resize(100,100);
    MatAsf.reserve(100*100);

    MatBsf.resize(100,100);
    MatBsf.reserve(100*100);

    {
    std::vector<Eigen::Triplet<double>> tripletList;
    tripletList.reserve(100*100);
    for(int i=0;i<100;i++)
    {
      for(int j=0;j<100;j++)
        tripletList.push_back(Eigen::Triplet<double>(i,j,i+j));
    }
    MatAsf.setFromTriplets(tripletList.begin(), tripletList.end());
    MatBsf.setFromTriplets(tripletList.begin(), tripletList.end());
    }

    Eigen::MatrixXd MatAde;
    Eigen::MatrixXd MatBde;
    Eigen::MatrixXd MatCde;

    MatAde.resize(100,100);
    MatAde=Eigen::MatrixXd::Identity(100,100);

    MatBde.resize(100,100);
    MatBde=Eigen::MatrixXd::Identity(100,100);

    Eigen::SparseMatrix<double> MatAse;
    Eigen::SparseMatrix<double> MatBse;
    Eigen::SparseMatrix<double> MatCse;

    MatAse.resize(100,100);
    MatAse.reserve(100);

    MatBse.resize(100,100);
    MatBse.reserve(100);

    {
    std::vector<Eigen::Triplet<double>> tripletList;
    tripletList.reserve(100);
    for(int i=0;i<100;i++)
    {
      for(int j=0;j<100;j++)
          if(i==j)
        tripletList.push_back(Eigen::Triplet<double>(i,j,1));
    }
    MatAse.setFromTriplets(tripletList.begin(), tripletList.end());
    MatBse.setFromTriplets(tripletList.begin(), tripletList.end());
    }

    {
        std::cout<<"Test 01: df x df"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCdf=MatAdf*MatBdf;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 02: sf x sf"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCsf=MatAsf*MatBsf;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 03: sf x df"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCdf=MatAsf*MatBdf;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 04: df x sf"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCdf=MatAdf*MatBsf;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 05: de x de"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCde=MatAde*MatBde;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 06: se x se"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCse=MatAse*MatBse;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 07: se x de"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCde=MatAse*MatBde;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 08: de x se"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCde=MatAde*MatBse;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 09: df x de"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCdf=MatAdf*MatBde;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 10: sf x se"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCsf=MatAsf*MatBse;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 11: sf x de"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCdf=MatAsf*MatBde;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 12: df x se"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCdf=MatAdf*MatBse;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 13: de x df"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCdf=MatAde*MatBdf;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 14: se x sf"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCsf=MatAse*MatBsf;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 15: se x df"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCdf=MatAse*MatBdf;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 16: de x sf"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCdf=MatAde*MatBsf;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 17: se x sf x se"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {
            MatCsf=MatAse*MatBsf*MatAse;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 18: de x df x de"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCdf=MatAde*MatBdf*MatAde;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 19: se x df x se"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCdf=MatAse*MatBdf*MatAse;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    {
        std::cout<<"Test 20: de x sf x de"<<std::endl;
        ros::Time time_before=ros::Time::now();
        for(int i=0; i<100; i++)
        {

            MatCdf=MatAde*MatBsf*MatAde;
        }
        ros::Time time_after=ros::Time::now();

        std::cout<<"\t delta time="<<(time_after-time_before)<<" ns"<<std::endl;
    }

    return 0;
}


// BLAS level 3
// hermitian matrices, her2k

#include <stddef.h>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/atlas/cblas3.hpp>
#include <boost/numeric/bindings/traits/ublas_hermitian.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace atlas = boost::numeric::bindings::atlas;
namespace traits = boost::numeric::bindings::traits;

using std::cout;
using std::cin;
using std::endl;

typedef float real_t;
typedef std::complex<real_t> cmplx_t;

typedef ublas::matrix<cmplx_t, ublas::column_major> ccm_t;
typedef ublas::matrix<cmplx_t, ublas::row_major> crm_t;
typedef ublas::hermitian_adaptor<ccm_t, ublas::upper> cucha_t;
typedef ublas::hermitian_adaptor<ccm_t, ublas::lower> clcha_t;
typedef ublas::hermitian_adaptor<crm_t, ublas::upper> curha_t;
typedef ublas::hermitian_adaptor<crm_t, ublas::lower> clrha_t;

int main() {

  // complex

  const int n1 = 3;
  const int k1 = 2;

  ccm_t cac (n1, k1);
  crm_t car (n1, k1);
  cac(0,0) = car(0,0) = cmplx_t (1., 1.);
  cac(1,0) = car(1,0) = cmplx_t (2., 1.);
  cac(2,0) = car(2,0) = cmplx_t (3., 1.);
  cac(0,1) = car(0,1) = cmplx_t (1., 1.);
  cac(1,1) = car(1,1) = cmplx_t (2., 1.);
  cac(2,1) = car(2,1) = cmplx_t (3., 1.);
  print_m (cac, "cac");
  cout << endl;
  print_m (car, "car");
  cout << endl << endl;

  ccm_t cbc (n1, k1);
  crm_t cbr (n1, k1);
  cbc(0,0) = cbr(0,0) = cmplx_t (0., -1.);
  cbc(1,0) = cbr(1,0) = cmplx_t (0., -1.);
  cbc(2,0) = cbr(2,0) = cmplx_t (0., -1.);
  cbc(0,1) = cbr(0,1) = cmplx_t (0., -1.);
  cbc(1,1) = cbr(1,1) = cmplx_t (0., -1.);
  cbc(2,1) = cbr(2,1) = cmplx_t (0., -1.);
  print_m (cbc, "cbc");
  cout << endl;
  print_m (cbr, "cbr");
  cout << endl << endl;

  ccm_t ccmu (n1, n1);
  ccm_t ccml (n1, n1);
  crm_t crmu (n1, n1);
  crm_t crml (n1, n1);
  cucha_t cucha (ccmu);
  clcha_t clcha (ccml);
  curha_t curha (crmu);
  clrha_t clrha (crml);

  atlas::her2k (CblasNoTrans, cac, cbc, cucha);
  atlas::her2k (CblasNoTrans, cmplx_t(1,0), cac, cbc, 0., clcha);
  atlas::her2k (CblasNoTrans, cmplx_t(1,0), car, cbr, 0., curha);
  atlas::her2k (CblasNoTrans, car, cbr, clrha);

  print_m (cucha, "cucha");
  cout << endl;
  print_m (clcha, "clcha");
  cout << endl;
  print_m (curha, "curha");
  cout << endl;
  print_m (clrha, "clrha");
  cout << endl << endl;

  // part 2

  ccm_t cact (ublas::herm (cac));
  crm_t cart (ublas::herm (car));
  print_m (cact, "cact");
  cout << endl;
  print_m (cart, "cart");
  cout << endl << endl;

  ccm_t cbct (ublas::herm (cbc));
  crm_t cbrt (ublas::herm (cbr));
  print_m (cbct, "cbct");
  cout << endl;
  print_m (cbrt, "cbrt");
  cout << endl << endl;

  init_m (ccmu, const_val<cmplx_t> (cmplx_t (0, 0)));
  init_m (ccml, const_val<cmplx_t> (cmplx_t (0, 0)));
  init_m (crmu, const_val<cmplx_t> (cmplx_t (0, 0)));
  init_m (crml, const_val<cmplx_t> (cmplx_t (0, 0)));

  atlas::her2k (CblasUpper, CblasConjTrans, 1.0, cact, cbct, 0.0, ccmu);
  atlas::her2k (CblasLower, CblasConjTrans, 1.0, cact, cbct, 0.0, ccml);
  atlas::her2k (CblasUpper, CblasConjTrans, 1.0, cart, cbrt, 0.0, crmu);
  atlas::her2k (CblasLower, CblasConjTrans, 1.0, cart, cbrt, 0.0, crml);

  print_m (ccmu, "ccmu");
  cout << endl;
  print_m (ccml, "ccml");
  cout << endl;
  print_m (crmu, "crmu");
  cout << endl;
  print_m (crml, "crml");
  cout << endl;
}


#define BOOST_TEST_MODULE RandomNumbers
#include <boost/test/unit_test.hpp>
#include <vexcl/vector.hpp>
#include <vexcl/element_index.hpp>
#include <vexcl/random.hpp>
#include <vexcl/reductor.hpp>
#include <vexcl/tagged_terminal.hpp>
#include <vexcl/temporary.hpp>
#include <vexcl/function.hpp>
#include <boost/math/constants/constants.hpp>
#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(random_numbers)
{
    const size_t N = 1 << 20;

    vex::Reductor<size_t, vex::SUM> sumi(ctx);
    vex::Reductor<double, vex::SUM> sumd(ctx);

    vex::Random<cl_int> rand0;
    vex::vector<cl_uint> x0(ctx, N);
    x0 = rand0(vex::element_index(), std::rand());

    vex::Random<cl_float4> rand1;
    vex::vector<cl_float4> x1(ctx, N);
    x1 = rand1(vex::element_index(), std::rand());

    vex::Random<cl_double4> rand2;
    vex::vector<cl_double4> x2(ctx, N);
    x2 = rand2(vex::element_index(), std::rand());

    vex::Random<cl_double> rand3;
    vex::vector<cl_double> x3(ctx, N);
    x3 = rand3(vex::element_index(), std::rand());

    // X in [0,1]
    BOOST_CHECK(sumi(x3 > 1) == 0);
    BOOST_CHECK(sumi(x3 < 0) == 0);

    // mean = 0.5
    BOOST_CHECK(std::abs((sumd(x3) / N) - 0.5) < 1e-2);

    vex::RandomNormal<cl_double> rand4;
    vex::vector<cl_double> x4(ctx, N);
    x4 = rand4(vex::element_index(), std::rand());

    // E(X ~ N(0,s)) = 0
    BOOST_CHECK(std::abs(sumd(x4)/N) < 1e-2);

    // E(abs(X) ~ N(0,s)) = sqrt(2/M_PI) * s
    BOOST_CHECK(std::abs(sumd(fabs(x4))/N - std::sqrt(2 / boost::math::constants::pi<double>())) < 1e-2);

    vex::Random<cl_double, vex::random::threefry> rand5;
    vex::vector<cl_double> x5(ctx, N);
    x5 = rand5(vex::element_index(), std::rand());

    BOOST_CHECK(std::abs(sumd(x5)/N - 0.5) < 1e-2);
}

BOOST_AUTO_TEST_CASE(monte_carlo_pi)
{
    vex::Random<double, vex::random::threefry> rnd;

    vex::Reductor<size_t, vex::SUM> sum(ctx);

    const size_t n = 1 << 20;

    auto i = vex::tag<0>(vex::element_index(0, n));

    auto x = vex::make_temp<1>(rnd(i, std::rand()));
    auto y = vex::make_temp<2>(rnd(i, std::rand()));

    double pi = 4.0 * sum( (x * x + y * y) < 1 ) / n;

    BOOST_CHECK_CLOSE(pi, boost::math::constants::pi<double>(), 0.5);
}

BOOST_AUTO_TEST_SUITE_END()