// Copyright University of Warwick 2014
// 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)

// Authors:
// John Back

/*! \file LpcLineSegment.hh
    \brief File containing the declaration of the LpcLineSegment class
*/

/*! \class LpcLineSegment
    \brief Class that defines the line segments used for vertexing/clustering
*/

#ifndef LPC_LINE_SEGMENT_HH
#define LPC_LINE_SEGMENT_HH

#include <Eigen/Dense>

class LpcLineSegment {

public:

    //! Default, empty constructor
    LpcLineSegment();

    //! Constructor defining all information for a line segment
    /*!
      \param [in] centroid The point on the line corresponding to the centroid of the hits
      \param [in] direction The unit direction along the line
      \param [in] residualMean The mean value of the hit-to-lpc residuals for the segment
      \param [in] residualRms The rms value of the hit-to-lpc residuals for the segment
      \param [in] residualCut The cut on the hit-to-lpc residuals to be used for further analysis
    */
    LpcLineSegment(const Eigen::VectorXd& centroid, const Eigen::VectorXd& direction,
		   double residualMean, double residualRms, double residualCut);

    //! Copy constructor
    /*!
      \param [in] other The LpcLineSegment to copy
    */
    LpcLineSegment(const LpcLineSegment& other);

    //! Assignment operator
    /*!
      \param [in] other The LpcLineSegment used for assignment
    */
    LpcLineSegment& operator=(const LpcLineSegment& other);

    //! Destructor
    virtual ~LpcLineSegment();

    // Modifiers

    //! Set the end point of the line. The direction vector can change sign in
    //! order to point to this from the centroid
    void setEndPoint(const Eigen::VectorXd& endPoint);

    // Accessors

    //! Get the centroid position
    /*!
      \returns the centroid as an Eigen VectorXd
    */
    Eigen::VectorXd getCentroid() const {return centroid_;}

    //! Get the unit direction vector
    /*!
      \returns the unit direction vector as an Eigen VectorXd
    */
    Eigen::VectorXd getDirection() const {return direction_;}

    //! Get the mean of the hit-to-lpc residuals for the segment
    /*!
      \returns the mean of the hit-to-lpc residuals
    */
    double getResidualMean() const {return residualMean_;}

    //! Get the rms of the hit-to-lpc residuals for the segment
    /*!
      \returns the rms of the hit-to-lpc residuals
    */
    double getResidualRms() const {return residualRms_;}

    //! Get the selection cut for the hit-to-lpc residuals for the segment
    /*!
      \returns the selection cut for the hit-to-lpc residuals
    */
    double getResidualCut() const {return residualCut_;}

    //! Get the end point of the line
    /*!
      \returns the end point
    */
    Eigen::VectorXd getEndPoint() const {return endPoint_;}

    //! Print out the line segment information
    void print() const;

protected:

private:

    //! The centroid position
    Eigen::VectorXd centroid_;

    //! The direction vector
    Eigen::VectorXd direction_;

    //! The residual mean
    double residualMean_;

    //! The residual rms
    double residualRms_;

    //! The residual cut
    double residualCut_;

    //! The end point of the line
    Eigen::VectorXd endPoint_;

};

#endif


/*!
 * \file PathFigure3D.hpp
 * \author Jun Yoshida
 * \copyright (c) 2019 Jun Yoshida.
 * The project is released under the MIT License.
 * \date Descember 12, 2019: created
 */

#pragma once

#include <Eigen/Dense>

#include "../PathScheme.hpp"

class PathFigure3D : public PathFigure<Eigen::Vector3d>
{
public:
    PathFigure3D() = default;
    virtual ~PathFigure3D() = default;

    virtual void updateProjector(Eigen::Matrix<double,2,3> const &proj) {}
};


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

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

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_STRATEGIES_AGNOSTIC_CONVEX_GRAHAM_ANDREW_HPP
#define BOOST_GEOMETRY_STRATEGIES_AGNOSTIC_CONVEX_GRAHAM_ANDREW_HPP

#include <cstddef>
#include <algorithm>
#include <vector>

#include <boost/range.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/strategies/convex_hull.hpp>

#include <boost/geometry/views/detail/range_type.hpp>

#include <boost/geometry/policies/compare.hpp>

#include <boost/geometry/algorithms/detail/for_each_range.hpp>
#include <boost/geometry/views/reversible_view.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace convex_hull
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template
<
    typename InputRange,
    typename RangeIterator,
    typename StrategyLess,
    typename StrategyGreater
>
struct get_extremes
{
    typedef typename point_type<InputRange>::type point_type;

    point_type left, right;

    bool first;

    StrategyLess less;
    StrategyGreater greater;

    inline get_extremes()
        : first(true)
    {}

    inline void apply(InputRange const& range)
    {
        if (boost::size(range) == 0)
        {
            return;
        }

        // First iterate through this range
        // (this two-stage approach avoids many point copies,
        //  because iterators are kept in memory. Because iterators are
        //  not persistent (in MSVC) this approach is not applicable
        //  for more ranges together)

        RangeIterator left_it = boost::begin(range);
        RangeIterator right_it = boost::begin(range);

        for (RangeIterator it = boost::begin(range) + 1;
            it != boost::end(range);
            ++it)
        {
            if (less(*it, *left_it))
            {
                left_it = it;
            }

            if (greater(*it, *right_it))
            {
                right_it = it;
            }
        }

        // Then compare with earlier
        if (first)
        {
            // First time, assign left/right
            left = *left_it;
            right = *right_it;
            first = false;
        }
        else
        {
            // Next time, check if this range was left/right from
            // the extremes already collected
            if (less(*left_it, left))
            {
                left = *left_it;
            }

            if (greater(*right_it, right))
            {
                right = *right_it;
            }
        }
    }
};

template
<
    typename InputRange,
    typename RangeIterator,
    typename Container,
    typename SideStrategy
>
struct assign_range
{
    Container lower_points, upper_points;

    typedef typename point_type<InputRange>::type point_type;

    point_type const& most_left;
    point_type const& most_right;

    inline assign_range(point_type const& left, point_type const& right)
        : most_left(left)
        , most_right(right)
    {}

    inline void apply(InputRange const& range)
    {
        typedef SideStrategy side;

        // Put points in one of the two output sequences
        for (RangeIterator it = boost::begin(range);
            it != boost::end(range);
            ++it)
        {
            // check if it is lying most_left or most_right from the line

            int dir = side::apply(most_left, most_right, *it);
            switch(dir)
            {
                case 1 : // left side
                    upper_points.push_back(*it);
                    break;
                case -1 : // right side
                    lower_points.push_back(*it);
                    break;

                // 0: on line most_left-most_right,
                //    or most_left, or most_right,
                //    -> all never part of hull
            }
        }
    }
};

template <typename Range>
static inline void sort(Range& range)
{
    typedef typename boost::range_value<Range>::type point_type;
    typedef geometry::less<point_type> comparator;

    std::sort(boost::begin(range), boost::end(range), comparator());
}

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
\brief Graham scan strategy to calculate convex hull
\ingroup strategies
\note Completely reworked version inspired on the sources listed below
\see http://www.ddj.com/architect/201806315
\see http://marknelson.us/2007/08/22/convex
 */
template <typename InputGeometry, typename OutputPoint>
class graham_andrew
{
public :
    typedef OutputPoint point_type;
    typedef InputGeometry geometry_type;

private:

    typedef typename cs_tag<point_type>::type cs_tag;

    typedef typename std::vector<point_type> container_type;
    typedef typename std::vector<point_type>::const_iterator iterator;
    typedef typename std::vector<point_type>::const_reverse_iterator rev_iterator;

    class partitions
    {
        friend class graham_andrew;

        container_type m_lower_hull;
        container_type m_upper_hull;
        container_type m_copied_input;
    };

public:
    typedef partitions state_type;

    inline void apply(InputGeometry const& geometry, partitions& state) const
    {
        // First pass.
        // Get min/max (in most cases left / right) points
        // This makes use of the geometry::less/greater predicates

        // For the left boundary it is important that multiple points
        // are sorted from bottom to top. Therefore the less predicate
        // does not take the x-only template parameter (this fixes ticket #6019.
        // For the right boundary it is not necessary (though also not harmful),
        // because points are sorted from bottom to top in a later stage.
        // For symmetry and to get often more balanced lower/upper halves
        // we keep it.

        typedef typename geometry::detail::range_type<InputGeometry>::type range_type;

        typedef typename boost::range_iterator
            <
                range_type const
            >::type range_iterator;

        detail::get_extremes
            <
                range_type,
                range_iterator,
                geometry::less<point_type>,
                geometry::greater<point_type>
            > extremes;
        geometry::detail::for_each_range(geometry, extremes);

        // Bounding left/right points
        // Second pass, now that extremes are found, assign all points
        // in either lower, either upper
        detail::assign_range
            <
                range_type,
                range_iterator,
                container_type,
                typename strategy::side::services::default_strategy<cs_tag>::type
            > assigner(extremes.left, extremes.right);

        geometry::detail::for_each_range(geometry, assigner);

        // Sort both collections, first on x(, then on y)
        detail::sort(assigner.lower_points);
        detail::sort(assigner.upper_points);

        //std::cout << boost::size(assigner.lower_points) << std::endl;
        //std::cout << boost::size(assigner.upper_points) << std::endl;

        // And decide which point should be in the final hull
        build_half_hull<-1>(assigner.lower_points, state.m_lower_hull,
                extremes.left, extremes.right);
        build_half_hull<1>(assigner.upper_points, state.m_upper_hull,
                extremes.left, extremes.right);
    }

    template <typename OutputIterator>
    inline void result(partitions const& state,
                    OutputIterator out, bool clockwise)  const
    {
        if (clockwise)
        {
            output_range<iterate_forward>(state.m_upper_hull, out, false);
            output_range<iterate_reverse>(state.m_lower_hull, out, true);
        }
        else
        {
            output_range<iterate_forward>(state.m_lower_hull, out, false);
            output_range<iterate_reverse>(state.m_upper_hull, out, true);
        }
    }

private:

    template <int Factor>
    static inline void build_half_hull(container_type const& input,
            container_type& output,
            point_type const& left, point_type const& right)
    {
        output.push_back(left);
        for(iterator it = input.begin(); it != input.end(); ++it)
        {
            add_to_hull<Factor>(*it, output);
        }
        add_to_hull<Factor>(right, output);
    }

    template <int Factor>
    static inline void add_to_hull(point_type const& p, container_type& output)
    {
        typedef typename strategy::side::services::default_strategy<cs_tag>::type side;

        output.push_back(p);
        std::size_t output_size = output.size();
        while (output_size >= 3)
        {
            rev_iterator rit = output.rbegin();
            point_type const last = *rit++;
            point_type const& last2 = *rit++;

            if (Factor * side::apply(*rit, last, last2) <= 0)
            {
                // Remove last two points from stack, and add last again
                // This is much faster then erasing the one but last.
                output.pop_back();
                output.pop_back();
                output.push_back(last);
                output_size--;
            }
            else
            {
                return;
            }
        }
    }

    template <iterate_direction Direction, typename OutputIterator>
    static inline void output_range(container_type const& range,
        OutputIterator out, bool skip_first)
    {
        typedef typename reversible_view<container_type const, Direction>::type view_type;
        view_type view(range);
        bool first = true;
        for (typename boost::range_iterator<view_type const>::type it = boost::begin(view);
            it != boost::end(view); ++it)
        {
            if (first && skip_first)
            {
                first = false;
            }
            else
            {
                *out = *it;
                ++out;
            }
        }
    }

};

}} // namespace strategy::convex_hull

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
template <typename InputGeometry, typename OutputPoint>
struct strategy_convex_hull<InputGeometry, OutputPoint, cartesian_tag>
{
    typedef strategy::convex_hull::graham_andrew<InputGeometry, OutputPoint> type;
};
#endif

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_AGNOSTIC_CONVEX_GRAHAM_ANDREW_HPP


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include <Utils/Properties/Thermochemistry/ThermochemistryCalculator.h>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <boost/optional.hpp>

using namespace Scine::Utils;

void init_thermochemical_calculator(pybind11::module& m) {
  pybind11::class_<ThermochemistryCalculator> thermochemistryCalculator(m, "ThermochemistryCalculator");
  pybind11::class_<ThermochemicalContainer> thermochemicalContainer(m, "ThermochemicalContainer");
  pybind11::class_<ThermochemicalComponentsContainer> thermochemicalComponentsContainer(
      m, "ThermochemicalComponentsContainer");

  thermochemistryCalculator.def(pybind11::init<const HessianMatrix&, const AtomCollection&, int, double>(),
                                pybind11::arg("hessian"), pybind11::arg("atoms"), pybind11::arg("multiplicity"),
                                pybind11::arg("energy"), "Initialize a thermochemistry calculator");
  thermochemistryCalculator.def(
      pybind11::init<const HessianMatrix&, ElementTypeCollection, const PositionCollection&, int, double>(),
      pybind11::arg("hessian"), pybind11::arg("elements"), pybind11::arg("positions"), pybind11::arg("multiplicity"),
      pybind11::arg("energy"), "Initialize a thermochemistry calculator");
  thermochemistryCalculator.def("set_temperature", &ThermochemistryCalculator::setTemperature);
  thermochemistryCalculator.def("set_molecular_symmetry", &ThermochemistryCalculator::setMolecularSymmetryNumber);
  thermochemistryCalculator.def("calculate", &ThermochemistryCalculator::calculate);

  thermochemicalContainer.def_readonly("entropy", &ThermochemicalContainer::entropy);
  thermochemicalContainer.def_readonly("enthalpy", &ThermochemicalContainer::enthalpy);
  thermochemicalContainer.def_readonly("heat_capacity_p", &ThermochemicalContainer::heatCapacityP);
  thermochemicalContainer.def_readonly("heat_capacity_v", &ThermochemicalContainer::heatCapacityV);
  thermochemicalContainer.def_readonly("gibbs_free_energy", &ThermochemicalContainer::gibbsFreeEnergy);
  thermochemicalContainer.def_readonly("zero_point_vibrational_energy", &ThermochemicalContainer::zeroPointVibrationalEnergy);
  thermochemicalContainer.def_readonly("symmetry_number", &ThermochemicalContainer::symmetryNumber);

  thermochemicalComponentsContainer.def_readonly("vibrational_component",
                                                 &ThermochemicalComponentsContainer::vibrationalComponent);
  thermochemicalComponentsContainer.def_readonly("rotational_component", &ThermochemicalComponentsContainer::rotationalComponent);
  thermochemicalComponentsContainer.def_readonly("translational_component",
                                                 &ThermochemicalComponentsContainer::translationalComponent);
  thermochemicalComponentsContainer.def_readonly("electronic_component", &ThermochemicalComponentsContainer::electronicComponent);
  thermochemicalComponentsContainer.def_readonly("overall", &ThermochemicalComponentsContainer::overall);
}


/*  Sirikata
 *  UniformCoordinateSegmentation.cpp
 *
 *  Copyright (c) 2009, Ewen Cheslack-Postava
 *  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 Sirikata 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 "UniformCoordinateSegmentation.hpp"

#include <algorithm>
#include <boost/tokenizer.hpp>
#include <sirikata/core/options/CommonOptions.hpp>

namespace Sirikata {

template<typename T>
T clamp(T val, T minval, T maxval) {
    if (val < minval) return minval;
    if (val > maxval) return maxval;
    return val;
}

UniformCoordinateSegmentation::UniformCoordinateSegmentation(SpaceContext* ctx, const BoundingBox3f& region, const Vector3ui32& perdim)
 : CoordinateSegmentation(ctx),
   mRegion(region),
   mServersPerDim(perdim)
{
    mContext->serverDispatcher()->registerMessageRecipient(SERVER_PORT_CSEG_CHANGE, this);

  /* Read in the file which maintains how the layout of the region
     changes at different times. */
  std::ifstream infile("layoutChangeFile.txt");

  lastLayoutChangeIdx = 0;

  int  i = 0;
  std::string str;
  while (getline(infile, str)) {

    typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
    boost::char_separator<char> sep(" ");
    tokenizer tokens(str, sep);

    tokenizer::iterator tok_iter = tokens.begin();

    uint32 time_val = atoi((*tok_iter).c_str());

    ++tok_iter;

    std::string layoutAsStr = *tok_iter;

    Vector3ui32 vec;

    std::istringstream is(layoutAsStr);
    is >> vec;

    LayoutChangeEntry lce;
    lce.time = time_val;
    lce.layout = vec;

    mLayoutChangeEntries.push_back(lce);
  }

  infile.close();

}

UniformCoordinateSegmentation::~UniformCoordinateSegmentation() {
    mContext->serverDispatcher()->unregisterMessageRecipient(SERVER_PORT_CSEG_CHANGE, this);
}

ServerID UniformCoordinateSegmentation::lookup(const Vector3f& pos)  {
    Vector3f region_extents = mRegion.extents();
    Vector3f to_point = pos - mRegion.min();

    Vector3i32 server_dim_indices( (int32)((to_point.x/region_extents.x)*mServersPerDim.x),
                                   (int32)((to_point.y/region_extents.y)*mServersPerDim.y),
                                   (int32)((to_point.z/region_extents.z)*mServersPerDim.z) );

    server_dim_indices.x = clamp(server_dim_indices.x, 0, (int32)mServersPerDim.x-1);
    server_dim_indices.y = clamp(server_dim_indices.y, 0, (int32)mServersPerDim.y-1);
    server_dim_indices.z = clamp(server_dim_indices.z, 0, (int32)mServersPerDim.z-1);

    uint32 server_index = (uint32) server_dim_indices.z*mServersPerDim.x*mServersPerDim.y + server_dim_indices.y*mServersPerDim.x + server_dim_indices.x + 1;
    return ServerID(server_index);
}

BoundingBoxList UniformCoordinateSegmentation::serverRegion(const ServerID& server)  {
    BoundingBoxList boundingBoxList;
    //assert( server > 0 && server <= mServersPerDim.x * mServersPerDim.y * mServersPerDim.z );
    if ( server > mServersPerDim.x * mServersPerDim.y * mServersPerDim.z ) {
      boundingBoxList.push_back( BoundingBox3f( Vector3f(0,0,0),Vector3f(0,0,0)) );
      return boundingBoxList;
    }

    ServerID sid = server - 1;
    Vector3i32 server_dim_indices(sid%mServersPerDim.x,
                                  (sid/mServersPerDim.x)%mServersPerDim.y,
                                  (sid/mServersPerDim.x/mServersPerDim.y)%mServersPerDim.z);
    double xsize=mRegion.extents().x/mServersPerDim.x;
    double ysize=mRegion.extents().y/mServersPerDim.y;
    double zsize=mRegion.extents().z/mServersPerDim.z;
    Vector3f region_min(server_dim_indices.x*xsize+mRegion.min().x,
                        server_dim_indices.y*ysize+mRegion.min().y,
                        server_dim_indices.z*zsize+mRegion.min().z);
    Vector3f region_max((1+server_dim_indices.x)*xsize+mRegion.min().x,
                        (1+server_dim_indices.y)*ysize+mRegion.min().y,
                        (1+server_dim_indices.z)*zsize+mRegion.min().z);

    boundingBoxList.push_back(BoundingBox3f(region_min, region_max));

    return boundingBoxList;
}

BoundingBox3f UniformCoordinateSegmentation::region()  {
    return mRegion;
}

uint32 UniformCoordinateSegmentation::numServers()  {
    return mServersPerDim.x * mServersPerDim.y * mServersPerDim.z;
}

std::vector<ServerID> UniformCoordinateSegmentation::lookupBoundingBox(const BoundingBox3f& bbox) {
  //naive implementation

  std::vector<ServerID> serverList;
  for (ServerID i=1; i <= numServers(); i++) {
    BoundingBoxList bboxList = serverRegion(i);

    for (uint32 j=0; j<bboxList.size(); j++) {
      BoundingBox3f jth_bbox = bboxList[j];

      if ( jth_bbox.intersects(bbox) ) {
        serverList.push_back(i);
        break;
      }
    }
  }

  return serverList;
}

void UniformCoordinateSegmentation::service() {
  /* Short-circuited the code for changing the layout at run-time for now
     but its been tested and it works.
   */
  return;

  Time t = mContext->simTime();

  /* The following code changes the segmentation/layout of the region.*/

  for (uint32 i=lastLayoutChangeIdx; i< mLayoutChangeEntries.size(); i++) {
    LayoutChangeEntry lce = mLayoutChangeEntries[i];

    if (  t.raw() >= lce.time ) {

      printf("Changing layout to %s\n", lce.layout.toString().c_str());
      printf("lce.time=%ld, t.raw()=%ld\n", lce.time, t.raw() );

      lastLayoutChangeIdx = i+1;

      mServersPerDim = lce.layout;

      uint32 countServers = numServers();

      std::vector<SegmentationInfo> segInfoVector;

      for (uint32 j = 1; j <= countServers; j++) {
        SegmentationInfo segInfo;
        segInfo.server = j;
        segInfo.region = serverRegion(j);
        segInfoVector.push_back( segInfo );
      }

      notifyListeners(segInfoVector);

      break;
    }
  }
}

void UniformCoordinateSegmentation::receiveMessage(Message* msg) {
    // FIXME what are we doing with these
    assert(msg->dest_port() == SERVER_PORT_CSEG_CHANGE);
    delete msg;
}

} // namespace Sirikata


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_DelayedNeutronEmissionDistributionACEFactory.hpp
//! \author Alex Robinson
//! \brief  Delayed neutron emission distribution factory class declaration
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_DELAYED_NEUTRON_EMISSION_DISTRIBUTION_ACE_FACTORY_HPP
#define MONTE_CARLO_DELAYED_NEUTRON_EMISSION_DISTRIBUTION_ACE_FACTORY_HPP

// Std Lib Includes
#include <string>

// Boost Includes
#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>

// Trilinos Includes
#include <Teuchos_RCP.hpp>
#include <Teuchos_Array.hpp>

// FRENSIE Includes
#include "MonteCarlo_NuclearScatteringDistribution.hpp"
#include "MonteCarlo_NeutronState.hpp"
#include "Utility_OneDDistribution.hpp"

namespace MonteCarlo{

//! The delayed neutron emission distribution factory class
class DelayedNeutronEmissionDistributionACEFactory
{

public:

  //! Constructor
  DelayedNeutronEmissionDistributionACEFactory(
		          const std::string& table_name,
			  const double atomic_weight_ratio,
			  const Teuchos::ArrayView<const double>& bdd_block,
			  const Teuchos::ArrayView<const double>& dnedl_block,
		          const Teuchos::ArrayView<const double>& dned_block );

  //! Destructor
  ~DelayedNeutronEmissionDistributionACEFactory()
  { /* ... */ }

  //! Create the delayed neutron emission distribution
  void createEmissionDistribution(
       Teuchos::RCP<NuclearScatteringDistribution<NeutronState,NeutronState> >&
       distribution ) const;

protected:

  //! Return the precursor group decay constants
  const Teuchos::Array<double>& getPrecursorGroupDecayConsts() const;

  //! Return the precursor group probability distributions
  const Teuchos::Array<Teuchos::RCP<Utility::OneDDistribution> >&
  getPrecursorGroupProbDists() const;

  //! Return the precursor group emission distributions
  const Teuchos::Array<Teuchos::RCP<NuclearScatteringDistribution<NeutronState,NeutronState> > >&
  getPrecursorGroupEmissionDists() const;

private:

  // Initialize basic delayed neutron data
  void initializeBasicDelayedNeutronData(
			   const std::string& table_name,
			   const Teuchos::ArrayView<const double>& bdd_block );

  // Initialize the emission distributions
  void initializeEmissionDistributions(
			  const std::string& table_name,
			  const double atomic_weight_ratio,
			  const Teuchos::ArrayView<const double>& dnedl_block,
		          const Teuchos::ArrayView<const double>& dned_block );

  // The atomic weight ratio
  double d_atomic_weight_ratio;

  // The precursor group decay constants
  Teuchos::Array<double> d_precursor_group_decay_consts;

  // The precursor group probability distributions
  Teuchos::Array<Teuchos::RCP<Utility::OneDDistribution> >
  d_precursor_group_prob_distributions;

  // The precursor group emission distributions
  Teuchos::Array<Teuchos::RCP<NuclearScatteringDistribution<NeutronState,NeutronState> > >
  d_precursor_group_emission_distributions;
};

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_DELAYED_NEUTRON_EMISSION_DISTRIBUTION_ACE_FACTORY_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_DelayedNeutronEmissionDistributionACEFactory.hpp
//---------------------------------------------------------------------------//


// Copyright (c) 2016 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <amount.h>

#include <test/test_bitcoin.h>

#include <boost/test/unit_test.hpp>

#include <array>

BOOST_FIXTURE_TEST_SUITE(feerate_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(GetFeeTest) {
    CFeeRate feeRate;

    feeRate = CFeeRate(Amount::zero());
    // Must always return 0
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), Amount::zero());
    BOOST_CHECK_EQUAL(feeRate.GetFee(1e5), Amount::zero());

    feeRate = CFeeRate(1000 * SATOSHI);
    // Must always just return the arg
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), Amount::zero());
    BOOST_CHECK_EQUAL(feeRate.GetFee(1), SATOSHI);
    BOOST_CHECK_EQUAL(feeRate.GetFee(121), 121 * SATOSHI);
    BOOST_CHECK_EQUAL(feeRate.GetFee(999), 999 * SATOSHI);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1000), 1000 * SATOSHI);
    BOOST_CHECK_EQUAL(feeRate.GetFee(9000), 9000 * SATOSHI);

    feeRate = CFeeRate(-1000 * SATOSHI);
    // Must always just return -1 * arg
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), Amount::zero());
    BOOST_CHECK_EQUAL(feeRate.GetFee(1), -SATOSHI);
    BOOST_CHECK_EQUAL(feeRate.GetFee(121), -121 * SATOSHI);
    BOOST_CHECK_EQUAL(feeRate.GetFee(999), -999 * SATOSHI);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1000), -1000 * SATOSHI);
    BOOST_CHECK_EQUAL(feeRate.GetFee(9000), -9000 * SATOSHI);

    feeRate = CFeeRate(123 * SATOSHI);
    // Truncates the result, if not integer
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), Amount::zero());
    // Special case: returns 1 instead of 0
    BOOST_CHECK_EQUAL(feeRate.GetFee(8), SATOSHI);
    BOOST_CHECK_EQUAL(feeRate.GetFee(9), SATOSHI);
    BOOST_CHECK_EQUAL(feeRate.GetFee(121), 14 * SATOSHI);
    BOOST_CHECK_EQUAL(feeRate.GetFee(122), 15 * SATOSHI);
    BOOST_CHECK_EQUAL(feeRate.GetFee(999), 122 * SATOSHI);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1000), 123 * SATOSHI);
    BOOST_CHECK_EQUAL(feeRate.GetFee(9000), 1107 * SATOSHI);

    feeRate = CFeeRate(-123 * SATOSHI);
    // Truncates the result, if not integer
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), Amount::zero());
    // Special case: returns -1 instead of 0
    BOOST_CHECK_EQUAL(feeRate.GetFee(8), -SATOSHI);
    BOOST_CHECK_EQUAL(feeRate.GetFee(9), -SATOSHI);

    // Check ceiling results
    feeRate = CFeeRate(18 * SATOSHI);
    // Truncates the result, if not integer
    BOOST_CHECK_EQUAL(feeRate.GetFeeCeiling(0), Amount::zero());
    BOOST_CHECK_EQUAL(feeRate.GetFeeCeiling(100), 2 * SATOSHI);
    BOOST_CHECK_EQUAL(feeRate.GetFeeCeiling(200), 4 * SATOSHI);
    BOOST_CHECK_EQUAL(feeRate.GetFeeCeiling(1000), 18 * SATOSHI);

    // Check full constructor
    // default value
    BOOST_CHECK(CFeeRate(-SATOSHI, 1000) == CFeeRate(-SATOSHI));
    BOOST_CHECK(CFeeRate(Amount::zero(), 1000) == CFeeRate(Amount::zero()));
    BOOST_CHECK(CFeeRate(SATOSHI, 1000) == CFeeRate(SATOSHI));
    // lost precision (can only resolve satoshis per kB)
    BOOST_CHECK(CFeeRate(SATOSHI, 1001) == CFeeRate(Amount::zero()));
    BOOST_CHECK(CFeeRate(2 * SATOSHI, 1001) == CFeeRate(SATOSHI));
    // some more integer checks
    BOOST_CHECK(CFeeRate(26 * SATOSHI, 789) == CFeeRate(32 * SATOSHI));
    BOOST_CHECK(CFeeRate(27 * SATOSHI, 789) == CFeeRate(34 * SATOSHI));
    // Maximum size in bytes, should not crash
    CFeeRate(MAX_MONEY, std::numeric_limits<size_t>::max() >> 1).GetFeePerK();
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/accumulators/statistics/pot_tail_mean.hpp>


#include "camera_array/FiducialDetectionModel.h"
#include "camplex/CameraCalibration.h"
#include <boost/foreach.hpp>

using namespace argus_msgs;

namespace argus
{

FiducialDetectionModel::FiducialDetectionModel( LookupInterface& interface )
: extrinsicsManager( interface ), fiducialManager( interface ),
targetManager( interface )
{
  // TODO
  minPointSeparation = 60.0;
}

FiducialDetectionModel::Detections
FiducialDetectionModel::GenerateDetections( const std::string& cameraName,
                                            const std::string& targetName,
                                            const PoseSE3& targetToCameraRef )
{
	CheckCamera( cameraName );
	CheckTarget( targetName );

	std::vector<FiducialDetection> detections;

	const PoseSE3& cameraExtrinsics = extrinsicsManager.GetInfo( cameraName ).extrinsics;
	PoseSE3 targetToCam = cameraExtrinsics.Inverse() * targetToCameraRef;

	// TODO
	CameraCalibration cameraModel( "fake", 550, 550, 320, 240, 640, 480 );

	const TargetInfo& info = targetManager.GetInfo( targetName );
	BOOST_FOREACH( const TargetInfo::FiducialGroupRegistry::value_type& group,
	               info.fiducialGroups )
	{
		BOOST_FOREACH( const std::string& fidName, group.second.fiducials )
		{
			const PoseSE3& fidExtrinsics = extrinsicsManager.GetInfo( fidName ).extrinsics;
			const Fiducial& fidIntrinsics = fiducialManager.GetInfo( fidName );
			PoseSE3 fidToCam = targetToCam * fidExtrinsics;
			FiducialDetection detection;
			detection.name = fidName;
			bool valid = ProjectDetection( fidIntrinsics, cameraModel,
	                                       fidToCam, detection );
			// 		ROS_INFO_STREAM( "Camera " << cameraName << " fiducial " << fidName
					 // 			<< " rel pose " << fidToCam );
			// 		for( unsigned int i = 0; i < detection.points.size(); i++ )
			  // 		{
			  // 			ROS_INFO_STREAM( "Detected point: " << detection.points[i].x
						 //	<< ", " << detection.points[i].y );
				// 		}
			if( !valid ) { continue; }
			double minDist = FindMinDistance( detection.points );
			if( minDist < minPointSeparation ) { continue; }
			//		ROS_INFO_STREAM( "Camera " << cameraName << " detects " << fidName );
			detections.push_back( detection );
		}
	}

	return detections;
}

void FiducialDetectionModel::CheckCamera( const std::string& cameraName )
{
	if( !extrinsicsManager.CheckMemberInfo( cameraName ) )
	{
		ROS_ERROR_STREAM( "Could not read extrinsics for camera: " << cameraName );
		exit( -1 );
	}
	// TODO Implement!
// 	if( cameraManager.HasMember( cameraName ) )
// 	{
// 		if( !cameraManager.ReadMemberInfo( cameraName ) )
// 		{
// 			ROS_ERROR_STREAM( "Could not read intrinsics for camera: " << cameraName );
// 			exit( -1 );
// 		}
// 	}
}

void FiducialDetectionModel::CheckTarget( const std::string& targetName )
{
	if( !targetManager.CheckMemberInfo( targetName ) )
	{
		ROS_ERROR_STREAM( "Could not read target info for: " << targetName );
		exit( -1 );
	}

	TargetInfo& info = targetManager.GetInfo( targetName );
	BOOST_FOREACH( const TargetInfo::FiducialGroupRegistry::value_type& group,
	               info.fiducialGroups )
	{
		BOOST_FOREACH( const std::string& fiducialName, group.second.fiducials )
		{
			if( !fiducialManager.CheckMemberInfo( fiducialName ) )
			{
				ROS_ERROR_STREAM( "Could not read fiducial info for: " << fiducialName );
				exit( -1 );
			}
			if( !extrinsicsManager.CheckMemberInfo( fiducialName ) )
			{
				ROS_ERROR_STREAM( "Could not read fiducial extrinsics for: " << fiducialName );
				exit( -1 );
			}
		}
	}
}

}


/*
 * rsxgltest - framebuffer object demo. A cube within a cube, maaaaaaan!
 */

#define GL3_PROTOTYPES
#include <GL3/gl3.h>
#include <GL3/gl3ext.h>

#include "rsxgltest.h"
#include "math3d.h"
#include "sine_wave.h"

#include <stddef.h>
#include "fbo_inner_vert.h"
#include "fbo_inner_frag.h"
#include "fbo_outer_vert.h"
#include "fbo_outer_frag.h"

#include <io/pad.h>

#include <math.h>
#include <Eigen/Geometry>

#include "texture.h"
#include "nagel_bin.h"

const char * rsxgltest_name = "fbomrt";

struct sine_wave_t rgb_waves[3] = {
  { 0.5f,
    0.5f,
    1.0f
  },
  { 0.5f,
    0.5f,
    1.5f
  },
  { 0.5f,
    0.5f,
    2.5f
  }
};

struct sine_wave_t xyz_waves[3] = {
  { 0.5f,
    0.5f,
    1.0f / 4.0f
  },
  { 0.5f,
    0.5f,
    1.5f / 4.0f
  },
  { 0.5f,
    0.5f,
    2.5f / 4.0f
  }
};

GLuint vao = 0;
GLuint buffers[2] = { 0,0 };

Image image;
GLuint textures[3] = { 0,0,0 };
GLuint program = 0;

GLuint fbo = 0, rbo[2] = { 0,0 };

const float geometry[] = {
    // -X
    -0.5,-0.5,-0.5,
    -1.0,0.0,0.0,
    0,0,

    -0.5,0.5,-0.5,
    -1.0,0.0,0.0,
    1,0,

    -0.5,0.5,0.5,
    -1.0,0.0,0.0,
    1,1,

    -0.5,-0.5,0.5,
    -1.0,0.0,0.0,
    0,1,

    // +X
    0.5,-0.5,-0.5,
    1.0,0.0,0.0,
    0,0,

    0.5,-0.5,0.5,
    1.0,0.0,0.0,
    1,0,

    0.5,0.5,0.5,
    1.0,0.0,0.0,
    1,1,

    0.5,0.5,-0.5,
    1.0,0.0,0.0,
    0,1,

    // -Y
    -0.5,-0.5,-0.5,
    0.0,-1.0,0.0,
    0,0,

    -0.5,-0.5,0.5,
    0.0,-1.0,0.0,
    1,0,

    0.5,-0.5,0.5,
    0.0,-1.0,0.0,
    1,1,

    0.5,-0.5,-0.5,
    0.0,-1.0,0.0,
    0,1,

    // +Y
    -0.5,0.5,-0.5,
    0.0,1.0,0.0,
    0,0,

    0.5,0.5,-0.5,
    0.0,1.0,0.0,
    1,0,

    0.5,0.5,0.5,
    0.0,1.0,0.0,
    1,1,

    -0.5,0.5,0.5,
    0.0,1.0,0.0,
    0,1,

    // -Z
    -0.5,-0.5,-0.5,
    0.0,0.0,-1.0,
    0,0,

    -0.5,0.5,-0.5,
    0.0,0.0,-1.0,
    1,0,

    0.5,0.5,-0.5,
    0.0,0.0,-1.0,
    1,1,

    0.5,-0.5,-0.5,
    0.0,0.0,-1.0,
    0,1,

    // +Z
    -0.5,-0.5,0.5,
    0.0,0.0,1.0,
    0,0,

    -0.5,0.5,0.5,
    0.0,0.0,1.0,
    1,0,

    0.5,0.5,0.5,
    0.0,0.0,1.0,
    1,1,

    0.5,-0.5,0.5,
    0.0,0.0,1.0,
    0,1
  };

  const GLuint indices[] = {
    // -X
    0, 1, 2,
    2, 3, 0,

    // +X
    4, 5, 6,
    6, 7, 4,

    // -Y
    8, 9, 10,
    10, 11, 8,

    // +Y
    12, 13, 14,
    14, 15, 12,

    // -Z
    16, 17, 18,
    18, 19, 16,

    // +Z
    20, 21, 22,
    22, 23, 20
  };

GLuint * client_indices = 0;

#define DTOR(X) ((X)*0.01745329f)
#define RTOD(d) ((d)*57.295788f)

// Projection matrix for the outer cube:
Eigen::Projective3f ProjMatrix(perspective(DTOR(54.3),1920.0 / 1080.0,0.1,1000.0));

// Projection matrix for the inner cube - the image that gets rendered to the FBO:
Eigen::Projective3f ProjMatrix2(perspective(DTOR(35.3),1920.0 / 1080.0,0.1,1000.0));

Eigen::Affine3f ViewMatrixInv =
  Eigen::Affine3f(Eigen::Affine3f::Identity() *
		  (
		   Eigen::Translation3f(1.779,2.221,4.034) *
		   (
		    Eigen::AngleAxisf(DTOR(0),Eigen::Vector3f::UnitZ()) *
		    Eigen::AngleAxisf(DTOR(23.8),Eigen::Vector3f::UnitY()) *
		    Eigen::AngleAxisf(DTOR(-26.738),Eigen::Vector3f::UnitX())
		    )
		   )
		  ).inverse();

const GLfloat light[4] = { 10.0f, 10.0f, 10.0f, 1.0f };

struct program {
  GLuint object;
  int vertex, normal, uv, ProjMatrix, TransMatrix, NormalMatrix, light, tc, texture;

  program()
    : object(0),
      vertex(-1),
      normal(-1),
      uv(-1),
      ProjMatrix(-1),
      TransMatrix(-1),
      NormalMatrix(-1),
      light(-1),
      tc(-1),
      texture(-1) {
  }

  void init(const unsigned char * vp,const size_t vp_len,
	    const unsigned char * fp,const size_t fp_len) {
    GLuint shaders[2] = { 0,0 };

    const GLchar * shader_srcs[2] = { (const GLchar *)vp, (const GLchar *)fp };
    GLint shader_srcs_lengths[2] = { vp_len, fp_len };
    int worked = 0;
    GLchar szInfo[1024];
    GLuint shader_types[2] = {
      GL_VERTEX_SHADER,
      GL_FRAGMENT_SHADER
    };

    for(int i = 0;i < 2;++i) {
      shaders[i] = glCreateShader(shader_types[i]);

      glShaderSource(shaders[i],1,shader_srcs + i,shader_srcs_lengths + i);
      glCompileShader(shaders[i]);

      glGetShaderInfoLog(shaders[i],1024,0,szInfo);
      tcp_printf("info log for shader %u: %s\n",shaders[i],szInfo);

      GLint compiled = 0;
      glGetShaderiv(shaders[i],GL_COMPILE_STATUS,&compiled);

      if(!compiled) continue;

      ++worked;
    }

    if(worked == 2) {
      object = glCreateProgram();
      for(int i = 0;i < 2;++i) {
	glAttachShader(object,shaders[i]);
      }

      glBindAttribLocation(object,0,"vertex");
      glBindAttribLocation(object,1,"normal");
      glBindAttribLocation(object,2,"uv");

      glLinkProgram(object);
      glGetProgramInfoLog(object,1024,0,szInfo);
      tcp_printf("info log for program %u: %s\n",object,szInfo);

      GLint linked = 0;
      glGetProgramiv(object,GL_LINK_STATUS,&linked);

      if(linked) {
	glValidateProgram(object);

	vertex = glGetAttribLocation(object,"vertex");
	normal = glGetAttribLocation(object,"normal");
	tc = glGetAttribLocation(object,"uv");

	tcp_printf("attrib locations: %i %i %i\n",
		   vertex,
		   normal,
		   tc);

	ProjMatrix = glGetUniformLocation(object,"ProjMatrix");
	TransMatrix = glGetUniformLocation(object,"TransMatrix");
	NormalMatrix = glGetUniformLocation(object,"NormalMatrix");
	texture = glGetUniformLocation(object,"texture");
	light = glGetUniformLocation(object,"light");

	tcp_printf("vertex: %i\n",vertex);
	tcp_printf("normal: %i\n",normal);
	tcp_printf("tc: %i\n",tc);
	tcp_printf("ProjMatrix: %i TransMatrix: %i\n",
		   ProjMatrix,TransMatrix);
	tcp_printf("texture: %i\n",texture);
      }
      else {
	glDeleteProgram(object);
	object = 0;
      }
    }
  }
};

struct program inner_program, outer_program;

extern "C"
void
rsxgltest_pad(unsigned int,const padData * paddata)
{
  if(paddata -> BTN_UP) {
    tcp_printf("up\n");
  }
  else if(paddata -> BTN_DOWN) {
    tcp_printf("down\n");
  }
}

extern "C"
void
rsxgltest_init(int argc,const char ** argv)
{
  tcp_printf("%s\n",__PRETTY_FUNCTION__);

  glEnable(GL_DEPTH_TEST);
  glDepthFunc(GL_LEQUAL);

  // Setup programs:
  inner_program.init(fbo_inner_vert,fbo_inner_vert_len,
		     fbo_inner_frag,fbo_inner_frag_len);

  outer_program.init(fbo_outer_vert,fbo_outer_vert_len,
		     fbo_outer_frag,fbo_outer_frag_len);

  // Set up us the vertex data:
  glGenBuffers(2,buffers);

  glBindBuffer(GL_ARRAY_BUFFER,buffers[0]);

  glBufferData(GL_ARRAY_BUFFER,sizeof(float) * 6 * 4 * 8,geometry,GL_STATIC_DRAW);
  glEnableVertexAttribArray(0);
  glVertexAttribPointer(0,3,GL_FLOAT,GL_FALSE,sizeof(float) * 8,0);

  glEnableVertexAttribArray(1);
  glVertexAttribPointer(1,3,GL_FLOAT,GL_FALSE,sizeof(float) * 8,(const GLvoid *)(sizeof(float) * 3));

  glEnableVertexAttribArray(2);
  glVertexAttribPointer(2,2,GL_FLOAT,GL_FALSE,sizeof(float) * 8,(const GLvoid *)(sizeof(float) * 6));

  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,buffers[1]);
  glBufferData(GL_ELEMENT_ARRAY_BUFFER,sizeof(GLuint) * 6 * 2 * 3,indices,GL_STATIC_DRAW);

  client_indices = (GLuint *)malloc(sizeof(GLuint) * 6 * 2 * 3);
  memcpy(client_indices,indices,sizeof(GLuint) * 6 * 2 * 3);

  // Texture map:
  image = loadPng(nagel_bin);
  tcp_printf("image size: %u %u\n",image.width,image.height);

  glGenTextures(3,textures);

  tcp_printf("\ttextures: %u %u %u\n",textures[0],textures[1],textures[2]);

  // image asset:
  glBindTexture(GL_TEXTURE_2D,textures[0]);

  glTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,image.width,image.height,0,GL_RGBA,GL_UNSIGNED_BYTE,image.data);

  glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR);

  // rendering surface:
  for(int i = 1;i < 3;++i) {
    glBindTexture(GL_TEXTURE_2D,textures[i]);

    //glTexStorage2D(GL_TEXTURE_2D,1,GL_RGBA,image.width,image.height);
    glTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,image.width,image.height,0,GL_RGBA,GL_UNSIGNED_BYTE,0);

    glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
    glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR);
  }

  // Framebuffer object:
  glGenRenderbuffers(2,rbo);
  report_glerror("glGenRenderbuffers");

  //glBindRenderbuffer(GL_RENDERBUFFER,rbo[0]);
  //glRenderbufferStorage(GL_RENDERBUFFER,GL_RGBA,image.width,image.height);

  glBindRenderbuffer(GL_RENDERBUFFER,rbo[1]);
  glRenderbufferStorage(GL_RENDERBUFFER,GL_DEPTH_COMPONENT,image.width,image.height);

  glBindRenderbuffer(GL_RENDERBUFFER,0);

  //
  glGenFramebuffers(1,&fbo);

  glBindFramebuffer(GL_FRAMEBUFFER,fbo);

  GLenum bufs[2] = {
    GL_COLOR_ATTACHMENT0,
    GL_COLOR_ATTACHMENT1
  };
  glDrawBuffers(2,bufs);

  //glFramebufferRenderbuffer(GL_FRAMEBUFFER,GL_COLOR_ATTACHMENT0,GL_RENDERBUFFER,rbo[0]);
  //glFramebufferTexture(GL_FRAMEBUFFER,GL_COLOR_ATTACHMENT0,texture,0);
  glFramebufferTexture2D(GL_FRAMEBUFFER,GL_COLOR_ATTACHMENT0,GL_TEXTURE_2D,textures[1],0);
  glFramebufferTexture2D(GL_FRAMEBUFFER,GL_COLOR_ATTACHMENT1,GL_TEXTURE_2D,textures[2],0);
  glFramebufferRenderbuffer(GL_FRAMEBUFFER,GL_DEPTH_ATTACHMENT,GL_RENDERBUFFER,rbo[1]);

  glBindFramebuffer(GL_FRAMEBUFFER,0);

  glBindTexture(GL_TEXTURE_2D,0);
}

extern "C"
int
rsxgltest_draw()
{
  tcp_printf("%s\n",__PRETTY_FUNCTION__);

  float rgb[3] = {
    compute_sine_wave(rgb_waves,rsxgltest_elapsed_time),
    compute_sine_wave(rgb_waves + 1,rsxgltest_elapsed_time),
    compute_sine_wave(rgb_waves + 2,rsxgltest_elapsed_time)
  };

  float xyz[3] = {
    compute_sine_wave(xyz_waves,rsxgltest_elapsed_time),
    compute_sine_wave(xyz_waves + 1,rsxgltest_elapsed_time),
    compute_sine_wave(xyz_waves + 2,rsxgltest_elapsed_time)
  };

  Eigen::Affine3f rotmat =
    Eigen::Affine3f::Identity() *
    Eigen::AngleAxisf(DTOR(xyz[2]) * 360.0f,Eigen::Vector3f::UnitZ()) *
    Eigen::AngleAxisf(DTOR(xyz[1]) * 360.0f,Eigen::Vector3f::UnitY()) *
    Eigen::AngleAxisf(DTOR(xyz[0]) * 360.0f,Eigen::Vector3f::UnitX());

  //Eigen::Affine3f modelview = ViewMatrixInv * (Eigen::Affine3f::Identity() * rotmat * Eigen::UniformScaling< float >(3.0));

  // Render to FBO:
  glBindFramebuffer(GL_FRAMEBUFFER,fbo);

  if(glCheckFramebufferStatus(GL_DRAW_FRAMEBUFFER) == GL_FRAMEBUFFER_COMPLETE) {
    //
    glViewport(0,0,image.width,image.height);

    glUseProgram(inner_program.object);

    glUniform1i(inner_program.texture,0);
    glUniform4fv(inner_program.light,1,light);

    glUniformMatrix4fv(inner_program.ProjMatrix,1,GL_FALSE,ProjMatrix2.data());

    Transform3f modelview = ViewMatrixInv * (Transform3f::Identity() * rotmat * Eigen::UniformScaling< float >(3.0));
    glUniformMatrix4fv(inner_program.TransMatrix,1,GL_FALSE,modelview.data());

    Eigen::Affine3f normal = Eigen::Affine3f::Identity();
    normal.linear() = modelview.linear().inverse().transpose();
    glUniformMatrix4fv(inner_program.NormalMatrix,1,GL_FALSE,normal.data());

    glClearColor(1.0 - rgb[0],1.0 - rgb[1],1.0 - rgb[2],1.0);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    glBindTexture(GL_TEXTURE_2D,textures[0]);

    glDrawElements(GL_TRIANGLES,36,GL_UNSIGNED_INT,0 /*client_indices*/);
  }
  else {
    tcp_printf("fbo is incomplete\n");
  }

  // Render to main framebuffer:
  //
  glBindFramebuffer(GL_FRAMEBUFFER,0);
  glViewport(0,0,rsxgltest_width,rsxgltest_height);

  glClearColor(rgb[0],rgb[1],rgb[2],1.0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  {
    glUseProgram(outer_program.object);

    glUniform1i(outer_program.texture,0);
    glUniform4fv(outer_program.light,1,light);

    glUniformMatrix4fv(outer_program.ProjMatrix,1,GL_FALSE,ProjMatrix.data());
  }

  {
    glBindTexture(GL_TEXTURE_2D,textures[1]);

    Transform3f modelview = ViewMatrixInv * (Transform3f::Identity() * Eigen::Translation3f(-5,0,0) * rotmat * Eigen::UniformScaling< float >(3.0));
    glUniformMatrix4fv(outer_program.TransMatrix,1,GL_FALSE,modelview.data());

    Eigen::Affine3f normal = Eigen::Affine3f::Identity();
    normal.linear() = modelview.linear().inverse().transpose();
    glUniformMatrix4fv(outer_program.NormalMatrix,1,GL_FALSE,normal.data());

    glUniformMatrix4fv(outer_program.ProjMatrix,1,GL_FALSE,ProjMatrix.data());
    glDrawElements(GL_TRIANGLES,36,GL_UNSIGNED_INT,0 /*client_indices*/);
  }

  {
    glBindTexture(GL_TEXTURE_2D,textures[2]);

    Transform3f modelview = ViewMatrixInv * (Transform3f::Identity() * Eigen::Translation3f(5,0,0) * rotmat * Eigen::UniformScaling< float >(3.0));
    glUniformMatrix4fv(outer_program.TransMatrix,1,GL_FALSE,modelview.data());

    Eigen::Affine3f normal = Eigen::Affine3f::Identity();
    normal.linear() = modelview.linear().inverse().transpose();
    glUniformMatrix4fv(outer_program.NormalMatrix,1,GL_FALSE,normal.data());

    glUniformMatrix4fv(outer_program.ProjMatrix,1,GL_FALSE,ProjMatrix.data());
    glDrawElements(GL_TRIANGLES,36,GL_UNSIGNED_INT,0 /*client_indices*/);
  }

  return 1;
}

extern "C"
void
rsxgltest_exit()
{
}


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

#ifndef independent_population_model_hpp
#define independent_population_model_hpp

#include <vector>
#include <functional>

#include <boost/optional.hpp>

#include "config/common.hpp"
#include "core/types/haplotype.hpp"
#include "core/types/indexed_haplotype.hpp"
#include "core/types/genotype.hpp"
#include "genotype_prior_model.hpp"
#include "individual_model.hpp"
#include "core/models/haplotype_likelihood_array.hpp"
#include "containers/probability_matrix.hpp"
#include "containers/mappable_block.hpp"
#include "logging/logging.hpp"

namespace octopus { namespace model {

class IndependentPopulationModel
{
public:
    using SampleVector            = std::vector<SampleName>;
    using GenotypeVector          = MappableBlock<Genotype<IndexedHaplotype<>>>;
    using GenotypeVectorReference = std::reference_wrapper<const GenotypeVector>;

    struct Latents
    {
        using GenotypeProbabilityVector = std::vector<double>;
        std::vector<GenotypeProbabilityVector> genotype_probabilities;
    };

    struct InferredLatents
    {
        Latents posteriors;
        double log_evidence;
    };

    IndependentPopulationModel() = delete;

    IndependentPopulationModel(const GenotypePriorModel& genotype_prior_model,
                               boost::optional<logging::DebugLogger> debug_log = boost::none,
                               boost::optional<logging::TraceLogger> trace_log = boost::none);

    IndependentPopulationModel(const IndependentPopulationModel&)            = delete;
    IndependentPopulationModel& operator=(const IndependentPopulationModel&) = delete;
    IndependentPopulationModel(IndependentPopulationModel&&)                 = delete;
    IndependentPopulationModel& operator=(IndependentPopulationModel&&)      = delete;

    ~IndependentPopulationModel() = default;

    // All samples have same ploidy
    InferredLatents
    evaluate(const SampleVector& samples,
             const GenotypeVector& genotypes,
             const HaplotypeLikelihoodArray& haplotype_likelihoods) const;

    // Samples have different ploidy
    InferredLatents
    evaluate(const SampleVector& samples,
             const std::vector<unsigned>& sample_ploidies,
             const GenotypeVector& genotypes,
             const HaplotypeLikelihoodArray& haplotype_likelihoods) const;

private:
    IndividualModel individual_model_;
};

} // namesapce model
} // namespace octopus

#endif


#include <cctbx/boost_python/flex_fwd.h>
#include <scitbx/boost_python/container_conversions.h>
#include <scitbx/array_family/boost_python/shared_wrapper.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>

#include "cut.h"
#include "direct_space_asu.h"

namespace cctbx { namespace sgtbx { namespace asu { namespace {

  void wrap_cut()
  {
    typedef cut w_t;

    using namespace boost::python;
    typedef return_value_policy<return_by_value> rbv;

    bool (w_t::*const cut_is_inside1)( const rvector3_t &) const =
      &w_t::is_inside;
    short (w_t::*const cut_where_is1)( const scitbx::af::int3 &,
      const scitbx::af::int3 & ) const = &w_t::where_is;

    class_<w_t>("cut", no_init)
      .def(init<
        int3_t const&,
        rational_t ,
        optional< bool > >((
          arg("n"),
          arg("c"),
          arg("inclusive") )))
      .def(init<
        int_type, int_type, int_type,
        rational_t ,
        optional< bool > >((
          arg("x"),arg("y"),arg("z"),
          arg("c"),
          arg("inclusive") )))
      .add_property("n", make_getter(&w_t::n, rbv()))
      .def_readonly("c", &w_t::c)
      .def_readonly("inclusive", &w_t::inclusive)
      .def("__pos__", &w_t::operator+)
      .def("__neg__", &w_t::operator-)
      .def("__inv__", &w_t::operator~)
      .def("__mul__", &w_t::operator*)
      .def("__div__", &w_t::operator/)
      .def("__repr__", &w_t::as_string)
      .def("one", &w_t::one)
      .def("is_inside",cut_is_inside1)
      .def("where_is", cut_where_is1)
      .def("get_point_in_plane", &w_t::get_point_in_plane)
      .def("change_basis", &w_t::change_basis)
      .def("evaluate", &w_t::evaluate)
    ;
  }

  direct_space_asu asu_copy(const direct_space_asu &a)
  {
    return direct_space_asu(a);
  }

  scitbx::af::shared<rvector3_t> asu_shape_vertices(const direct_space_asu &a)
  {
    set_rvector3_t v;
    a.shape_vertices(v);
    scitbx::af::shared<rvector3_t> result;
    for(set_rvector3_t::const_iterator it=v.begin(); it!=v.end(); ++it)
      result.push_back(*it);
    return result;
  }

  scitbx::af::long3 asu_grid_limits(const direct_space_asu &a)
  {
    scitbx::af::long3 result;
    a.get_optimized_grid_limits(result);
    return result;
  }

  void wrap_direct_space_asu()
  {
    typedef direct_space_asu w_t;

    using namespace boost::python;
    typedef return_value_policy<return_by_value> rbv;

    bool (w_t::*const is_inside1)( const rvector3_t &) const = &w_t::is_inside;
    bool (w_t::*const is_inside2)( const rvector3_t &, bool ) const =
      &w_t::is_inside;
    bool (w_t::*const is_inside_shape_only1)(const rvector3_t &point) const =
      &w_t::is_inside_shape_only;
    bool (w_t::*const is_inside_shape_only2)(const scitbx::af::double3 &point,
      double tol) const = &w_t::is_inside_shape_only;
    short (w_t::*const where_is1)(const scitbx::int3 &num,
      const scitbx::int3 &den) const = &w_t::where_is;
    short (w_t::*const where_is2)(const scitbx::int3 &num) const =
      &w_t::where_is;

    class_<w_t>("direct_space_asu", no_init)
      .def(init< const std::string& >(( arg("group_symbol") )))
      .def(init< const space_group_type& >(( arg("group_type") )))
      .def_readonly("hall_symbol", &w_t::hall_symbol)
      .def("is_inside", is_inside1)
      .def("is_inside", is_inside2)
      .def("is_inside_shape_only", is_inside_shape_only1)
      .def("is_inside_shape_only", is_inside_shape_only2)
      .def("where_is", where_is1)
      .def("where_is", where_is2)
      .def("change_basis", &w_t::change_basis)
      .def("get_nth_plane", &w_t::get_nth_plane)
      .def("shape_only", &w_t::shape_only)
      .def("in_which_planes", &w_t::in_which_planes)
      .def("in_which_facets", &w_t::in_which_facets, rbv())
      .def("n_faces", &w_t::n_faces)
      .def("shape_vertices", asu_shape_vertices)
      .def("box_max", &w_t::box_max)
      .def("box_min", &w_t::box_min)
      .def("as_string", &w_t::as_string)
      .def("as_float_asu", &w_t::as_float_asu)
      .def("__copy__", asu_copy)
      .def("optimize_for_grid", &w_t::optimize_for_grid)
      .def("is_optimized", &w_t::is_optimized)
      .def("get_optimized_grid_limits", asu_grid_limits)
    ;
  }

  void init_module()
  {
    wrap_cut();
    wrap_direct_space_asu();
    scitbx::af::boost_python::shared_wrapper< cut >::wrap("cut_shared_array");
    scitbx::af::boost_python::shared_wrapper< rvector3_t >::wrap(
        "rational3_shared_array");
  }

}}}} // namespace cctbx::sgtbx::asu

BOOST_PYTHON_MODULE(cctbx_sgtbx_asu_ext)
{
  cctbx::sgtbx::asu::init_module();
}


#include <boost/accumulators/statistics/rolling_sum.hpp>


//
// Copyright 2013 Christian Henning
//
// 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/toolbox/color_spaces/ycbcr.hpp>
#include <boost/gil/extension/toolbox/image_types/subchroma_image.hpp>

#include <boost/core/lightweight_test.hpp>
#include <boost/mp11.hpp>

#include <vector>

namespace gil = boost::gil;
namespace mp11 = boost::mp11;

void test_subchroma_image()
{
    {
        gil::ycbcr_601_8_pixel_t a(10, 20, 30);
        gil::rgb8_pixel_t b;
        gil::bgr8_pixel_t c;

        gil::color_convert(a, b);
        gil::color_convert(a, c);
        BOOST_TEST(gil::static_equal(b, c));

        gil::color_convert(b, a);
    }
    {
        gil::ycbcr_709_8_pixel_t a(10, 20, 30);
        gil::rgb8_pixel_t b;
        gil::bgr8_pixel_t c;

        gil::color_convert(a, b);
        gil::color_convert(a, c);
        BOOST_TEST(gil::static_equal(b, c));

        gil::color_convert(b, a);
    }

    {
        using pixel_t = gil::rgb8_pixel_t;
        using image_t = gil::subchroma_image<pixel_t>;
        image_t img(320, 240);
        gil::fill_pixels(view(img), pixel_t(10, 20, 30));

        // TODO: Add BOOST_TEST checkpoints
    }
    {
        using pixel_t = gil::rgb8_pixel_t;

        gil::subchroma_image<pixel_t, mp11::mp_list_c<int, 4, 4, 4>> a(640, 480);
        static_assert(a.ss_X == 1 && a.ss_Y == 1, "");
        gil::subchroma_image<pixel_t, mp11::mp_list_c<int, 4, 4, 0>> b(640, 480);
        static_assert(b.ss_X == 1 && b.ss_Y == 2, "");
        gil::subchroma_image<pixel_t, mp11::mp_list_c<int, 4, 2, 2>> c(640, 480);
        static_assert(c.ss_X == 2 && c.ss_Y == 1, "");
        gil::subchroma_image<pixel_t, mp11::mp_list_c<int, 4, 2, 0>> d(640, 480);
        static_assert(d.ss_X == 2 && d.ss_Y == 2, "");
        gil::subchroma_image<pixel_t, mp11::mp_list_c<int, 4, 1, 1>> e(640, 480);
        static_assert(e.ss_X == 4 && e.ss_Y == 1, "");
        gil::subchroma_image<pixel_t, mp11::mp_list_c<int, 4, 1, 0>> f(640, 480);
        static_assert(f.ss_X == 4 && f.ss_Y == 2, "");

        gil::fill_pixels(view(a), pixel_t(10, 20, 30));
        gil::fill_pixels(view(b), pixel_t(10, 20, 30));
        gil::fill_pixels(view(c), pixel_t(10, 20, 30));
        gil::fill_pixels(view(d), pixel_t(10, 20, 30));
        gil::fill_pixels(view(e), pixel_t(10, 20, 30));
        gil::fill_pixels(view(f), pixel_t(10, 20, 30));

        // TODO: Add BOOST_TEST checkpoints
    }
    {
        using pixel_t = gil::ycbcr_601_8_pixel_t;
        using factors_t = mp11::mp_list_c<int, 4, 2, 2>;
        using image_t = gil::subchroma_image<pixel_t, factors_t>;

        std::size_t const y_width = 320;
        std::size_t const y_height = 240;

        std::size_t image_size = (y_width * y_height)
            + (y_width / image_t::ss_X) * (y_height / image_t::ss_Y)
            + (y_width / image_t::ss_X) * (y_height / image_t::ss_Y);

        std::vector<unsigned char> data(image_size); // TODO: Initialize? --mloskot

        image_t::view_t v = gil::subchroma_view<pixel_t, factors_t>(y_width, y_height, &data.front());
        //gil::rgb8_pixel_t p;  // TODO: Why RGB?? --mloskot
        //p = *v.xy_at(0, 0);
        auto p = *v.xy_at(0, 0);

        // TODO: Add BOOST_TEST checkpoints
    }
}

int main()
{
    test_subchroma_image();

    return ::boost::report_errors();
}


/*
 * KameleonInterpolator_compute_gradient.cpp
 *
 *  Created on: Sep 28, 2009
 *      Author: David Berrios
 */

#include "KameleonInterpolator.h"
#include <vector>
#include <boost/algorithm/string.hpp>

namespace ccmc
{

	/**
	 * @param variable
	 * @param positionComponent1 The first component of the position
	 * @param positionComponent2 The second component of the position
	 * @param positionComponent3 The third component of the position
	 * @return
	 */
	float KameleonInterpolator::compute_gradient(const std::string& variable, const float& positionComponent1,
			const float& positionComponent2, const float& positionComponent3)
	{
		float dComponent1, dComponent2, dComponent3;
		return compute_gradient(variable, positionComponent1, positionComponent2, positionComponent3, dComponent1,
				dComponent2, dComponent3);
	}

	/**
	 * @param variable
	 * @param c0 The first component of the position
	 * @param c1 The second component of the position
	 * @param c2 The third component of the position
	 * @param dComponent1 The delta of the block size for component1 at the position specified.
	 * @param dComponent2 The delta of the block size for component2 at the position specified.
	 * @param dComponent3 The delta of the block size for component3 at the position specified.
	 * @return
	 */
	float KameleonInterpolator::compute_gradient(const std::string& variable, const float& c0,
			const float& c1, const float& c2, float& dc0, float& dc1,
			float& dc2)
	{
		//parse the string, and attempt to calculate the gradient of the parsed variable

		std::vector<std::string> tokens;
		boost::split(tokens, variable, boost::is_any_of("."), boost::token_compress_on);

		/*t = interpolate("t", positionComponent1, positionComponent2, positionComponent3, dComponent1, dComponent2,
				dComponent3);
		if (t == missingValue)
			return missingValue;
		n = interpolate("n", positionComponent1, positionComponent2, positionComponent3, dComponent1, dComponent2,
				dComponent3);
		if (n == missingValue)
			return missingValue;
		float s = t / pow(n, 2.0f / 3.0f);*/
		//cout << "gradient variable: '" << tokens[1] << "'" << endl;

		//super expensive to calculation this. Try to store the components of the gradient, and speed up the calculations
		//if the components are requested in sequential order
		float pointValue = interpolate(tokens[1],c0,c1,c2,dc0,dc1,dc2);
		float direction;
		float dc0_scaled = dc0*.5f;
		float dc1_scaled = dc1*.5f;
		float dc2_scaled = dc2*.5f;
		float positions_c0[8];
		float positions_c1[8];
		float positions_c2[8];

		positions_c0[0] = c0 + dc0_scaled;
		positions_c0[1] = c0 + dc0_scaled;
		positions_c0[2] = c0 - dc0_scaled;
		positions_c0[3] = c0 - dc0_scaled;
		positions_c0[4] = c0 + dc0_scaled;
		positions_c0[5] = c0 + dc0_scaled;
		positions_c0[6] = c0 - dc0_scaled;
		positions_c0[7] = c0 - dc0_scaled;

		positions_c1[0] = c1 - dc1_scaled;
		positions_c1[1] = c1 - dc1_scaled;
		positions_c1[2] = c1 - dc1_scaled;
		positions_c1[3] = c1 - dc1_scaled;
		positions_c1[4] = c1 + dc1_scaled;
		positions_c1[5] = c1 + dc1_scaled;
		positions_c1[6] = c1 + dc1_scaled;
		positions_c1[7] = c1 + dc1_scaled;

		positions_c2[0] = c2 - dc2_scaled;
		positions_c2[1] = c2 + dc2_scaled;
		positions_c2[2] = c2 + dc2_scaled;
		positions_c2[3] = c2 - dc2_scaled;
		positions_c2[4] = c2 - dc2_scaled;
		positions_c2[5] = c2 + dc2_scaled;
		positions_c2[6] = c2 + dc2_scaled;
		positions_c2[7] = c2 - dc2_scaled;

		//direction and magnitude of greatest change
		float p0 = c0;
		float p1 = c1;
		float p2 = c2;
		float current_max = -999999999999.f;
		for (int i = 0; i < 8; i++)
		{

			direction = interpolate(tokens[1],positions_c0[i],positions_c1[i],positions_c2[i]);
			//cout << "*i: " << i << endl;
			float current_diff = direction - pointValue;
			if (current_diff > current_max )
			{
				current_max = current_diff;
				p0 = positions_c0[i];
				p1 = positions_c1[i];
				p2 = positions_c2[i];
			}
		}

		return current_max;

	}
}


#include "pinocchio_model.h"

#include <urdf_parser/urdf_parser.h>

#include "urdf_utils.h"
//#include <boost/property_tree/xml_parser.hpp>
//#include <boost/property_tree/ptree.hpp>

#define DEFINE_TEMPLATE_PM(DATATYPE) template class PinocchioModelTpl<DATATYPE>;

DEFINE_TEMPLATE_PM(double)

DEFINE_TEMPLATE_PM(float)

template<typename DATATYPE>
void PinocchioModelTpl<DATATYPE>::dfs_parse_tree(urdf::LinkConstSharedPtr link, UrdfVisitorBase &visitor) {
    const urdf::JointConstSharedPtr joint = urdf::const_pointer_cast<urdf::Joint>(link->parent_joint);
    if (verbose)
        std::cout << link->name << " " << joint.get() << std::endl;
    if (joint) {
        PINOCCHIO_CHECK_INPUT_ARGUMENT(link->getParent() && link);

        const std::string &joint_name = joint->name;
        const std::string &link_name = link->name;
        const std::string &parent_link_name = link->getParent()->name;
        FrameIndex parent_frame_id = visitor.getBodyId(parent_link_name);

        // Transformation from the parent link to the joint origin
        const SE3 joint_placement = pose_to_se3<DATATYPE>(joint->parent_to_joint_origin_transform);
        const Inertia Y = convert_inertial<DATATYPE>(link->inertial);

        const DATATYPE infty = std::numeric_limits<DATATYPE>::infinity();
        VectorX max_effort(1), max_velocity(1), min_config(1), max_config(1);
        VectorX friction(VectorX::Constant(1, 0.)), damping(VectorX::Constant(1, 0.));
        Vector3 axis(joint->axis.x, joint->axis.y, joint->axis.z);

        std::ostringstream joint_info;

        switch (joint->type) {
            case urdf::Joint::FLOATING:
                joint_info << "joint FreeFlyer";

                max_effort = VectorX::Constant(6, infty);
                max_velocity = VectorX::Constant(6, infty);
                min_config = VectorX::Constant(7, -infty);
                max_config = VectorX::Constant(7, infty);
                min_config.tail(4).setConstant(-1.01);
                max_config.tail(4).setConstant(1.01);

                friction = VectorX::Constant(6, 0.);
                damping = VectorX::Constant(6, 0.);

                visitor.addJointAndBody(UrdfVisitorBase::FLOATING, axis, parent_frame_id, joint_placement, joint->name,
                                        Y, link->name, max_effort, max_velocity, min_config, max_config,
                                        friction, damping);
                break;

            case ::urdf::Joint::REVOLUTE:
                joint_info << "joint REVOLUTE with axis";

                // TODO I think the URDF standard forbids REVOLUTE with no limits.
                ASSERT(joint->limits, "REVOLUTE without limits");
                if (joint->limits) {
                    max_effort << joint->limits->effort;
                    max_velocity << joint->limits->velocity;
                    min_config << joint->limits->lower;
                    max_config << joint->limits->upper;
                }

                if (joint->dynamics) {
                    friction << joint->dynamics->friction;
                    damping << joint->dynamics->damping;
                }

                visitor.addJointAndBody(UrdfVisitorBase::REVOLUTE, axis, parent_frame_id, joint_placement, joint->name,
                                        Y, link->name, max_effort, max_velocity, min_config, max_config,
                                        friction, damping);
                break;

            case ::urdf::Joint::CONTINUOUS: // Revolute joint with no joint limits
                joint_info << "joint CONTINUOUS with axis";

                min_config.resize(2);
                max_config.resize(2);
                min_config << -1.01, -1.01;
                max_config << 1.01, 1.01;

                if (joint->limits) {
                    max_effort << joint->limits->effort;
                    max_velocity << joint->limits->velocity;
                } else {
                    max_effort << infty;
                    max_velocity << infty;
                }

                if (joint->dynamics) {
                    friction << joint->dynamics->friction;
                    damping << joint->dynamics->damping;
                }

                visitor.addJointAndBody(UrdfVisitorBase::CONTINUOUS, axis, parent_frame_id, joint_placement,
                                        joint->name,
                                        Y, link->name, max_effort, max_velocity, min_config, max_config,
                                        friction, damping);
                break;

            case ::urdf::Joint::PRISMATIC:
                joint_info << "joint PRISMATIC with axis";

                // TODO I think the URDF standard forbids REVOLUTE with no limits.
                ASSERT(joint->limits, "PRISMATIC without limits");
                if (joint->limits) {
                    max_effort << joint->limits->effort;
                    max_velocity << joint->limits->velocity;
                    min_config << joint->limits->lower;
                    max_config << joint->limits->upper;
                }

                if (joint->dynamics) {
                    friction << joint->dynamics->friction;
                    damping << joint->dynamics->damping;
                }

                visitor.addJointAndBody(UrdfVisitorBase::PRISMATIC, axis,
                                        parent_frame_id, joint_placement, joint->name,
                                        Y, link->name,
                                        max_effort, max_velocity, min_config, max_config,
                                        friction, damping);
                break;

            case ::urdf::Joint::PLANAR:
                joint_info << "joint PLANAR with normal axis along Z";

                max_effort = VectorX::Constant(3, infty);
                max_velocity = VectorX::Constant(3, infty);
                min_config = VectorX::Constant(4, -infty);
                max_config = VectorX::Constant(4, infty);

                min_config.tail(2).setConstant(-1.01);
                max_config.tail(2).setConstant(1.01);

                friction = VectorX::Constant(3, 0.);
                damping = VectorX::Constant(3, 0.);

                visitor.addJointAndBody(UrdfVisitorBase::PLANAR, axis,
                                        parent_frame_id, joint_placement, joint->name,
                                        Y, link->name,
                                        max_effort, max_velocity, min_config, max_config,
                                        friction, damping);
                break;

            case ::urdf::Joint::FIXED:
                joint_info << "fixed joint";
                visitor.addFixedJointAndBody(parent_frame_id, joint_placement, joint_name, Y, link_name);
                break;

            default:
                throw std::invalid_argument("The type of joint " + joint_name + " is not supported.");
        }

        FrameIndex frame_id = visitor.getBodyId(link->name);
        visitor << "Adding Body" << '\n'
                << '\"' << link_name << "\" connected to \"" << parent_link_name << "\" through joint \"" << joint_name
                << "\"\n"
                << "joint type: " << joint_info.str() << '\n'
                << "joint placement:\n" << joint_placement << '\n'
                << "body info: " << '\n'
                << "  mass: " << Y.mass() << '\n'
                << "  lever: " << Y.lever().transpose() << '\n'
                << "  inertia elements (Ixx,Iyx,Iyy,Izx,Izy,Izz): " << Y.inertia().data().transpose() << '\n' << '\n';
    } else if (link->getParent())
        throw std::invalid_argument(link->name + " - joint information missing.");
    for (auto child : link->child_links)
        dfs_parse_tree(child, visitor);
    if (link->child_links.empty())
        leaf_links.push_back(link->name);
}

template<typename DATATYPE>
void PinocchioModelTpl<DATATYPE>::init(urdf::ModelInterfaceSharedPtr const &urdfTree, Vector3 const &gravity) {
    urdf_model = urdfTree;

    pinocchio::urdf::details::UrdfVisitor<DATATYPE, 0, pinocchio::JointCollectionDefaultTpl> visitor(model);
    if (verbose)
    {
        std::cout << "Begin to parse URDF" << std::endl;
        visitor.log = &std::cout;
    }
    if (not urdf_model)
        throw std::invalid_argument("The XML stream does not contain a valid URDF model.");

    visitor.setName(urdf_model->getName());
    urdf::LinkConstSharedPtr root_link = urdf_model->getRoot();
    visitor.addRootJoint(convert_inertial<DATATYPE>(root_link->inertial), root_link->name);
    for (auto child: root_link->child_links)
        dfs_parse_tree(child, visitor);

    model.gravity = {gravity, Vector3{0, 0, 0}};
    data = Data(model);
    if (verbose)
        std::cout << "Begin to set joint order and link order" << std::endl;
    setJointOrder(getJointNames(false));
    setLinkOrder(getLinkNames(false));
}

template<typename DATATYPE>
PinocchioModelTpl<DATATYPE>::PinocchioModelTpl(std::string const &urdf_filename, Vector3 const &gravity,
                                               bool const &verbose) : verbose(verbose) {
    //std::cout << "Verbose" << verbose << std::endl;
    urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDFFile(urdf_filename);
    init(urdf, gravity);
}

template<typename DATATYPE>
PinocchioModelTpl<DATATYPE>::PinocchioModelTpl(urdf::ModelInterfaceSharedPtr const &urdfTree, Vector3 const &gravity,
                                               bool const &verbose) : verbose(verbose) {
    init(urdfTree, gravity);
}

template<typename DATATYPE>
Eigen::Matrix<DATATYPE, Eigen::Dynamic, 1>
PinocchioModelTpl<DATATYPE>::qposUser2Pinocchio(VectorX const &q_user) {
    VectorX q_pinocchio(model.nq);
    size_t count = 0;
    for (size_t i = 0; i < joint_index_user2pinocchio.size(); i++) {
        auto start_idx = model.idx_qs[joint_index_user2pinocchio[i]];
        switch (nqs[i]) {
            case 0: // FIX
                break;
            case 1: // REVOLUTE OR PRISMATIC
                ASSERT(start_idx < model.nq, "posS2P out of bound");
                q_pinocchio[start_idx] = q_user[count];
                break;
            case 2: // CONTINUOUS
                ASSERT(start_idx + 1 < model.nq, "posS2P out of bound");
                q_pinocchio[start_idx] = std::cos(q_user(count));
                q_pinocchio[start_idx + 1] = std::sin(q_user(count));
                break;
            default:
                throw std::runtime_error(
                        "Unsupported joint in computation. Currently support: fixed, revolute, prismatic");
        }
        count += nvs[i];
    }
    ASSERT(count == q_user.size(), "Qpos user2pinocchio failed");
    return q_pinocchio;
}

template<typename DATATYPE>
Eigen::Matrix<DATATYPE, Eigen::Dynamic, 1>
PinocchioModelTpl<DATATYPE>::qposPinocchio2User(VectorX const &q_pinocchio) {
    VectorX q_user(model.nv);
    uint32_t count = 0;
    for (size_t i = 0; i < joint_index_user2pinocchio.size(); i++) {
        auto start_idx = model.idx_qs[joint_index_user2pinocchio[i]];
        //std::cout << "Joint " << i << " " << start_idx << " " << joint_tangent_dim[i] << " " << count << std::endl;
        switch (nqs[i]) {
            case 0: // FIX
                break;
            case 1: // REVOLUTE OR PRISMATIC
                ASSERT(count < model.nv, "posP2S out of bound");
                q_user[count] = q_pinocchio[start_idx];
                break;
            case 2: // CONTINUOUS
                ASSERT(count < model.nv, "posS2P out of bound");
                q_user[count] = std::atan2(q_pinocchio[start_idx + 1], q_pinocchio[start_idx]);
                break;
            default:
                throw std::runtime_error(
                        "Unsupported joint in computation. Currently support: fixed, revolute, prismatic");
        }
        count += nvs[i];
    }
    ASSERT(count == q_user.size(), "Qpos pinocchio2user failed");
    return q_user;
}

template<typename DATATYPE>
void PinocchioModelTpl<DATATYPE>::setJointOrder(std::vector<std::string> const &names) {
    user_joint_names = names;
    v_index_user2pinocchio = VectorXI(model.nv);
    vidx = VectorXI(names.size());
    qidx = VectorXI(names.size());
    nvs = VectorXI(names.size());
    nqs = VectorXI(names.size());
    parents = VectorXI(names.size());
    joint_index_user2pinocchio = VectorXI(names.size());
    joint_index_pinocchio2user = VectorXI::Constant(model.njoints, -1);

    //joint_start_index = Eigen::VectorXi(names.size());
    //joint_tangent_dim = Eigen::VectorXi(names.size());
    //joint_dim = Eigen::VectorXi(names.size());

    size_t len_v = 0, len_q = 0;
    for (size_t i = 0; i < names.size(); i++) {
        auto pinocchio_idx = model.getJointId(names[i]);
        if (i == model.njoints)
            throw std::invalid_argument(names[i] + " is a invalid name in setJointOrder");
        vidx[i] = len_v;
        qidx[i] = len_q;
        nvs[i] = model.nvs[pinocchio_idx];
        nqs[i] = model.nqs[pinocchio_idx];
        parents[i] = model.parents[pinocchio_idx];
        joint_index_user2pinocchio[i] = pinocchio_idx;
        joint_index_pinocchio2user[pinocchio_idx] = i;
        len_q += model.nqs[pinocchio_idx];
        for (int j = 0; j < model.nvs[pinocchio_idx]; j++)
            v_index_user2pinocchio[len_v++] = model.idx_vs[pinocchio_idx] + j;
    }
    ASSERT(len_v == model.nv, "setJointOrder failed");
    v_map_user2pinocchio = Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic>(v_index_user2pinocchio);
}

template<typename DATATYPE>
void PinocchioModelTpl<DATATYPE>::setLinkOrder(std::vector<std::string> const &names) {
    user_link_names = names;
    link_index_user2pinocchio = VectorXI(names.size());
    for (size_t i = 0; i < names.size(); i++) {
        auto pinocchio_idx = model.getFrameId(names[i], pinocchio::BODY);
        if (pinocchio_idx == model.nframes)
            throw std::invalid_argument(names[i] + " is a invalid names in setLinkOrder");
        link_index_user2pinocchio[i] = pinocchio_idx;
    }
}

template<typename DATATYPE>
void PinocchioModelTpl<DATATYPE>::printFrames(void) {
    std::cout << "Joint dim " << model.joints.size() << " " << model.nv << " " << model.nvs.size() << " " << model.idx_vs.size() << std::endl;
    std::cout << "Joint Tangent dim " << model.nq << " " << model.nqs.size() << " " << model.idx_qs.size() << std::endl;
    std::cout << "Joint Limit " << model.lowerPositionLimit.size() << " " << model.upperPositionLimit.size() << std::endl;
    for (size_t i = 0; i < model.frames.size(); i++) {
        std::string type_name = "NONE";
        auto frame = model.frames[i];
        if (frame.type == pinocchio::OP_FRAME)
            type_name = "OP_FRAME";
        else if (frame.type == pinocchio::JOINT)
            type_name = "JOINT";
        else if (frame.type == pinocchio::FIXED_JOINT)
            type_name = "FIXED_JOINT";
        else if (frame.type == pinocchio::BODY)
            type_name = "BODY";
        else if (frame.type == pinocchio::SENSOR)
            type_name = "BODY";
        std::cout << "Frame " << i << " " << frame.name << " " << frame.parent << " " << type_name << " "
                  << frame.previousFrame
                  << std::endl;
    }
}

template<typename DATATYPE>
std::vector<std::size_t> PinocchioModelTpl<DATATYPE>::getChainJointIndex(const std::string &end_effector) {
    auto frame_id = model.getBodyId(end_effector);
    std::vector<std::size_t> index_pinocchio = model.supports[model.frames[frame_id].parent];
    std::vector<std::size_t> ret;
    for (auto index: index_pinocchio)
        if (joint_index_pinocchio2user[index] != -1)
            ret.push_back(joint_index_pinocchio2user[index]);
    return ret;
}

template<typename DATATYPE>
std::vector<std::string> PinocchioModelTpl<DATATYPE>::getChainJointName(const std::string &end_effector) {
    auto frame_id = model.getBodyId(end_effector);
    std::vector<std::size_t> index_pinocchio = model.supports[model.frames[frame_id].parent];
    std::vector<std::string> ret;
    for (auto index: index_pinocchio)
        if (joint_index_pinocchio2user[index] != -1)
            ret.push_back(model.names[index]);
    return ret;
}

template<typename DATATYPE>
Eigen::Matrix<DATATYPE, Eigen::Dynamic, 1> PinocchioModelTpl<DATATYPE>::getRandomConfiguration() {
    auto qpos = pinocchio::randomConfiguration(model);
    return qposPinocchio2User(qpos);
}

template<typename DATATYPE>
void PinocchioModelTpl<DATATYPE>::computeForwardKinematics(VectorX const &qpos) {
    pinocchio::forwardKinematics(model, data, qposUser2Pinocchio(qpos));
}

template<typename DATATYPE>
Eigen::Matrix<DATATYPE, 7, 1> PinocchioModelTpl<DATATYPE>::getLinkPose(size_t const &index) {
    ASSERT(index < link_index_user2pinocchio.size(), "The link index is out of bound!");
    auto frame = link_index_user2pinocchio[index];
    auto parent_joint = model.frames[frame].parent;
    auto link2joint = model.frames[frame].placement;
    auto joint2world = data.oMi[parent_joint];
    //std::cout << parent_joint << " " << link2joint << std::endl;
    //std::cout << joint2world << std::endl;

/*
    auto parent_joint2 = model2.frames[frame].parent;
    auto link2joint2 = model2.frames[frame].placement;
    auto joint2world2 = data2.oMi[parent_joint];

    std::cout << "Pi link " << model.frames[frame].name << std::endl;
    std::cout << parent_joint << " " << parent_joint2 << std::endl;
    std::cout << link2joint << " " << link2joint2 << std::endl;
    std::cout << joint2world << " " << joint2world2 << std::endl;
*/
    auto link2world = joint2world * link2joint;
    auto p = link2world.translation();
    auto q = Quaternion(link2world.rotation());

    /*
    auto link2world2 = joint2world2 * link2joint2;
    auto p2 = link2world2.translation();
    auto q2 = Eigen::Quaternion<double>(link2world2.rotation());
    std::cout << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << std::endl;
    std::cout << q2.w() << " " << q2.x() << " " << q2.y() << " " << q2.z() << std::endl;
*/

    DATATYPE pose[] = {p.x(), p.y(), p.z(), q.w(), q.x(), q.y(), q.z()};
    return Vector7(pose);
}

template<typename DATATYPE>
Eigen::Matrix<DATATYPE, 7, 1> PinocchioModelTpl<DATATYPE>::getJointPose(size_t const &index) {
    ASSERT(index < joint_index_user2pinocchio.size(), "The link index is out of bound!");
    auto frame = joint_index_user2pinocchio[index];

   /* std::cout << index << " " << user_joint_names[index] << " "<< frame << " " << model.getJointId(user_joint_names[index]) << std::endl;
    std::cout << data.oMi[0] << std::endl;

    std::cout << data.oMi[1] << std::endl;
*/
    //using Data = pinocchio::DataTpl<DATATYPE>

    auto joint2world = data.oMi[frame];
    auto p = joint2world.translation();
    auto q = Quaternion(joint2world.rotation());
    DATATYPE pose[] = {p.x(), p.y(), p.z(), q.w(), q.x(), q.y(), q.z()};
    return Vector7(pose);
}

template<typename DATATYPE>
void PinocchioModelTpl<DATATYPE>::computeFullJacobian(const VectorX &qpos) {
    pinocchio::computeJointJacobians(model, data, qposUser2Pinocchio(qpos));
}

template<typename DATATYPE>
Eigen::Matrix<DATATYPE, 6, Eigen::Dynamic> PinocchioModelTpl<DATATYPE>::getLinkJacobian(size_t const &index,
                                                                                bool const &local) {
    ASSERT(index < link_index_user2pinocchio.size(), "link index out of bound");
    auto frameId = link_index_user2pinocchio[index];
    auto jointId = model.frames[frameId].parent;

    auto link2joint = model.frames[frameId].placement;
    auto joint2world = data.oMi[jointId];
    auto link2world = joint2world * link2joint;

    Matrix6x J(6, model.nv);
    J.fill(0);

    pinocchio::getJointJacobian(model, data, jointId, pinocchio::ReferenceFrame::WORLD, J);
    if (local) {
        J = link2world.toActionMatrixInverse() * J;
    }
    return J * v_map_user2pinocchio;
}

template<typename DATATYPE>
Eigen::Matrix<DATATYPE, 6, Eigen::Dynamic>
PinocchioModelTpl<DATATYPE>::computeSingleLinkLocalJacobian(VectorX const &qpos, size_t const &index) {
    ASSERT(index < link_index_user2pinocchio.size(), "link index out of bound");
    auto frameId = link_index_user2pinocchio[index];
    auto jointId = model.frames[frameId].parent;
    auto link2joint = model.frames[frameId].placement;

    Eigen::Matrix<DATATYPE, 6, Eigen::Dynamic> J(6, model.nv);
    J.fill(0);
    pinocchio::computeJointJacobian(model, data, qposUser2Pinocchio(qpos), jointId, J);
    return link2joint.toActionMatrixInverse() * J * v_map_user2pinocchio;
}

template<typename DATATYPE>
std::tuple<Eigen::Matrix<DATATYPE, Eigen::Dynamic, 1>, bool, Eigen::Matrix<DATATYPE, 6, 1>>
PinocchioModelTpl<DATATYPE>::computeIKCLIK(size_t const &index, Vector7 const &pose, VectorX const &q_init, double const &eps,
                                                        int const &maxIter, double const &dt, double const &damp) {
    ASSERT(index < link_index_user2pinocchio.size(), "link index out of bound");
    auto frameId = link_index_user2pinocchio[index];
    auto jointId = model.frames[frameId].parent;

    SE3 link_pose;
    link_pose.translation({pose[0], pose[1], pose[2]});
    // w, x, y, z
    link_pose.rotation(Quaternion(pose[3], pose[4], pose[5], pose[6]).toRotationMatrix());
    auto link2joint = model.frames[frameId].placement;

    SE3 joint_pose = link_pose * link2joint.inverse();
    VectorX q = qposUser2Pinocchio(q_init); //pinocchio::neutral(model);
    Matrix6x J(6, model.nv);
    J.setZero();

    bool success = false;
    Vector6 err;
    VectorX v(model.nv);

    for (int i = 0;; i++) {
        pinocchio::forwardKinematics(model, data, q);
        const SE3 dMi = joint_pose.actInv(data.oMi[jointId]);
        err = pinocchio::log6(dMi).toVector();
        if (err.norm() < eps) {
            success = true;
            break;
        }
        if (i >= maxIter) {
            success = false;
            break;
        }
        pinocchio::computeJointJacobian(model, data, q, jointId, J);
        Matrix6 JJt;
        JJt.noalias() = J * J.transpose();
        JJt.diagonal().array() += damp;
        v.noalias() = -J.transpose() * JJt.ldlt().solve(err);
        q = pinocchio::integrate(model, q, v * dt);
    }
    return {qposPinocchio2User(q), success, err};
}

template<typename DATATYPE>
std::tuple<Eigen::Matrix<DATATYPE, Eigen::Dynamic, 1>, bool, Eigen::Matrix<DATATYPE, 6, 1>>
PinocchioModelTpl<DATATYPE>::computeIKCLIKJL(size_t const &index, Vector7 const &pose, VectorX const &q_init, VectorX const &q_min, VectorX const &q_max,
                                                        double const &eps, int const &maxIter, double const &dt, double const &damp) {
    ASSERT(index < link_index_user2pinocchio.size(), "link index out of bound");
    auto frameId = link_index_user2pinocchio[index];
    auto jointId = model.frames[frameId].parent;

    SE3 link_pose;
    link_pose.translation({pose[0], pose[1], pose[2]});
    // w, x, y, z
    link_pose.rotation(Quaternion(pose[3], pose[4], pose[5], pose[6]).toRotationMatrix());
    auto link2joint = model.frames[frameId].placement;

    SE3 joint_pose = link_pose * link2joint.inverse();
    VectorX q = qposUser2Pinocchio(q_init); //pinocchio::neutral(model);
    VectorX qmin = qposUser2Pinocchio(q_min);
    VectorX qmax = qposUser2Pinocchio(q_max);
    Matrix6x J(6, model.nv);
    J.setZero();

    bool success = false;
    Vector6 err;
    VectorX v(model.nv);

    for (int i = 0;; i++) {
        pinocchio::forwardKinematics(model, data, q);
        const SE3 dMi = joint_pose.actInv(data.oMi[jointId]);
        err = pinocchio::log6(dMi).toVector();
        if (err.norm() < eps) {
            success = true;
            break;
        }
        if (i >= maxIter) {
            success = false;
            break;
        }
        pinocchio::computeJointJacobian(model, data, q, jointId, J);
        Matrix6 JJt;
        JJt.noalias() = J * J.transpose();
        JJt.diagonal().array() += damp;
        v.noalias() = -J.transpose() * JJt.ldlt().solve(err);
        q = pinocchio::integrate(model, q, v * dt);
        for (int j = 0; j < q.rows(); j++) {
            if (q[j] < qmin[j]) {
                if (abs(qmin[j] + 3.1415926) < 1e-3)
                    q[j] = 3.1415926;
                else
                    q[j] = qmin[j];
            }
            if (q[j] > qmax[j]) {
                if (abs(qmin[j] - 3.1415926) < 1e-3)
                    q[j] = -3.1415926;
                else
                    q[j] = qmax[j];
            }
        }
    }
    return {qposPinocchio2User(q), success, err};
}
/*

Eigen::MatrixXd PinocchioModelTpl::computeGeneralizedMassMatrix(const Eigen::VectorXd &qpos) {
    pinocchio::crba(model, data, posS2P(qpos));
    data.M.triangularView<Eigen::StrictlyLower>() =
            data.M.transpose().triangularView<Eigen::StrictlyLower>();
    return indexS2P.transpose() * data.M * indexS2P;
}

Eigen::MatrixXd PinocchioModelTpl::computeCoriolisMatrix(const Eigen::VectorXd &qpos, const Eigen::VectorXd &qvel) {
    return indexS2P.transpose() *
           pinocchio::computeCoriolisMatrix(model, data, posS2P(qpos), indexS2P * qvel) * indexS2P;
}

Eigen::VectorXd PinocchioModelTpl::computeInverseDynamics(const Eigen::VectorXd &qpos,
                                                         const Eigen::VectorXd &qvel,
                                                         const Eigen::VectorXd &qacc) {
    return indexS2P.transpose() *
           pinocchio::rnea(model, data, posS2P(qpos), indexS2P * qvel, indexS2P * qacc);
}

Eigen::VectorXd PinocchioModelTpl::computeForwardDynamics(const Eigen::VectorXd &qpos,
                                                         const Eigen::VectorXd &qvel,
                                                         const Eigen::VectorXd &qf) {
    return indexS2P.transpose() *
           pinocchio::aba(model, data, posS2P(qpos), indexS2P * qvel, indexS2P * qf);
}

*/


//=============================================================================================================
/**
 * @file     spectrogram.cpp
 * @author   Lorenz Esch <lesch@mgh.harvard.edu>
 * @version  dev
 * @date     September, 2015
 *
 * @section  LICENSE
 *
 * Copyright (C) 2015, Lorenz Esch. 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 MNE-CPP authors 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.
 *
 *
 * @brief    Definition of spectrogram class.
 */

//*************************************************************************************************************
//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "spectrogram.h"

//*************************************************************************************************************
//=============================================================================================================
// Eigen INCLUDES
//=============================================================================================================

#include <Eigen/SparseCore>
#include <unsupported/Eigen/FFT>

//*************************************************************************************************************
//=============================================================================================================
// Qt INCLUDES
//=============================================================================================================

#include <QDebug>
#include <QElapsedTimer>
#include <QThread>
#include <QtConcurrent>

//*************************************************************************************************************
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace UTILSLIB;
using namespace Eigen;

//*************************************************************************************************************
//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

MatrixXd Spectrogram::makeSpectrogram(VectorXd signal, qint32 windowSize = 0)
{
    //QElapsedTimer timer;
    //timer.start();

    signal.array() -= signal.mean();
    QList<SpectogramInputData> lData;
    int iThreadSize = QThread::idealThreadCount()*2;
    int iStepsSize = signal.rows()/iThreadSize;
    int iResidual = signal.rows()%iThreadSize;

    SpectogramInputData dataTemp;
    dataTemp.vecInputData = signal;
    dataTemp.window_size = windowSize;
    if(dataTemp.window_size == 0) {
        dataTemp.window_size = signal.rows()/15;
    }

    for (int i = 0; i < iThreadSize; ++i) {
        dataTemp.iRangeLow = i*iStepsSize;
        dataTemp.iRangeHigh = i*iStepsSize+iStepsSize;
        lData.append(dataTemp);
    }

    dataTemp.iRangeLow = iThreadSize*iStepsSize;
    dataTemp.iRangeHigh = iThreadSize*iStepsSize+iResidual;
    lData.append(dataTemp);

    QFuture<MatrixXd> resultMat = QtConcurrent::mappedReduced(lData,
                                                              compute,
                                                              reduce);
    resultMat.waitForFinished();

    //qDebug() << "Spectrogram::make_spectrogram - timer.elapsed()" << timer.elapsed();
    return resultMat.result();
}

//*************************************************************************************************************

VectorXd Spectrogram::gaussWindow(qint32 sample_count, qreal scale, quint32 translation)
{
    VectorXd gauss = VectorXd::Zero(sample_count);

    for(qint32 n = 0; n < sample_count; n++)
    {
        qreal t = (qreal(n) - translation) / scale;
        gauss[n] = exp(-3.14 * pow(t, 2))*pow(sqrt(scale),(-1))*pow(qreal(2),(0.25));
    }

    return gauss;
}

//*************************************************************************************************************

MatrixXd Spectrogram::compute(const SpectogramInputData& inputData)
{
    #ifdef EIGEN_FFTW_DEFAULT
        fftw_make_planner_thread_safe();
    #endif

    Eigen::FFT<double> fft;
    MatrixXd tf_matrix = MatrixXd::Zero(inputData.vecInputData.rows()/2, inputData.vecInputData.rows());
    VectorXd envelope, windowed_sig, real_coeffs;
    VectorXcd fft_win_sig;
    qint32 window_size = inputData.window_size;

    for(quint32 translate = inputData.iRangeLow; translate < inputData.iRangeHigh; translate++) {
        envelope = gaussWindow(inputData.vecInputData.rows(), window_size, translate);

        windowed_sig = VectorXd::Zero(inputData.vecInputData.rows());
        fft_win_sig = VectorXcd::Zero(inputData.vecInputData.rows());

        windowed_sig = inputData.vecInputData.array() * envelope.array();\

        fft.fwd(fft_win_sig, windowed_sig);

        real_coeffs = fft_win_sig.segment(0,inputData.vecInputData.rows()/2).array().abs2();

        tf_matrix.col(translate) = real_coeffs;
    }

    return tf_matrix;
}

//*************************************************************************************************************

void Spectrogram::reduce(MatrixXd &resultData,
                         const MatrixXd &data)
{
    if(resultData.size() == 0) {
        resultData = data;
    } else {
        resultData += data;
    }
}