#include <ros/ros.h>
#include <Eigen/Geometry>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/ActuatorControl.h>
#include <kingfisher_msgs/Drive.h>
#include <sensor_msgs/Imu.h>
#include <tf/transform_datatypes.h>
#include <math.h>

using namespace std;

namespace attitude_controller {

class Node {
 public:
  explicit Node(const ros::NodeHandle& pnh);
  void setpoint_cb(const mavros_msgs::AttitudeTarget::ConstPtr& msg);
  void imu_cb(const sensor_msgs::Imu::ConstPtr& msg);

 private:
  ros::NodeHandle pnh_;

  ros::Subscriber setpoint_sub_;
  ros::Time last_setpoint_time_;
  mavros_msgs::AttitudeTarget last_setpoint_;
  double throttle_;
  Eigen::Quaterniond setpoint_q_;
  bool setpoint_set_;

  ros::Publisher drive_pub_;
  ros::Publisher actuator_pub_;

  ros::Subscriber imu_sub_;
  Eigen::Quaterniond imu_q_;

  double kp_;
  double kd_;
  double ki_;

  double last_error_;
  ros::Time last_error_time_;
};

Node::Node(const ros::NodeHandle& pnh) : pnh_(pnh) {
  setpoint_sub_ = pnh_.subscribe("/att_control/attitude_target", 10, &Node::setpoint_cb, this);
  imu_sub_ = pnh_.subscribe("/mavros/imu/data", 10, &Node::imu_cb, this);
  drive_pub_ = pnh_.advertise<kingfisher_msgs::Drive>("/cmd_drive", 10);
  actuator_pub_ = pnh_.advertise<mavros_msgs::ActuatorControl>("/mavros/actuator_control", 10);

  setpoint_set_ = false;

  kp_ = pnh_.param("kp", kp_, 1.0);
  kd_ = pnh_.param("kd", kd_, 1.0);
  ki_ = pnh_.param("ki", ki_, 1.0);

  last_error_ = 0.0;
  last_error_time_ = ros::Time::now();

  ROS_INFO("init attitude_controller");
}

void Node::imu_cb(const sensor_msgs::Imu::ConstPtr &msg)
{
  ROS_INFO("imu recieved");
  imu_q_ = Eigen::Quaterniond(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);

  // perform control
  if (setpoint_set_){
    // convert to tf::Quaternions
    tf::Quaternion imu_tf =
           tf::Quaternion(imu_q_.x(), imu_q_.y(), imu_q_.z(), imu_q_.w());
    tf::Quaternion setpoint_tf =
             tf::Quaternion(setpoint_q_.x(), setpoint_q_.y(), setpoint_q_.z(), setpoint_q_.w());

    // get RPY
    double imu_roll, imu_pitch, imu_yaw;
    double setpoint_roll, setpoint_pitch, setpoint_yaw;
    tf::Matrix3x3(imu_tf).getRPY(imu_roll, imu_pitch, imu_yaw);
    tf::Matrix3x3(setpoint_tf).getRPY(setpoint_roll, setpoint_pitch, setpoint_yaw);
    ROS_INFO_STREAM("imu yaw: " << imu_yaw);
    ROS_INFO_STREAM("setpoint yaw: " << setpoint_yaw);

    // calculate control effort
    double error = asin(sin(setpoint_yaw - imu_yaw));
    double d_error = (error - last_error_)/(ros::Time::now() - last_error_time_).toSec();
    double yaw_effort = 1.0 * error + 0.7 * d_error;
    last_error_ = error;
    last_error_time_ = ros::Time::now();

    // clamp yaw
    yaw_effort = std::min(1.0, yaw_effort);
    yaw_effort = std::max(-1.0, yaw_effort);

    // clamp throttle
    double throttle = throttle_;
    throttle = std::min(1.0, throttle);
    throttle = std::max(-1.0, throttle);

    ROS_INFO_STREAM("yaw effort: " << yaw_effort);
    ROS_INFO_STREAM("throttle: " << throttle);

    double left = throttle - yaw_effort;
    double right = throttle + yaw_effort;

    // clamp left
    left = std::min(1.0, left);
    left = std::max(-1.0, left);

    // clamp right
    right = std::min(1.0, right);
    right = std::max(-1.0, right);

    // publish motor command
    auto drive_msg = boost::make_shared<kingfisher_msgs::Drive>();
    drive_msg->left = left;
    drive_msg->right = right;

    drive_pub_.publish(drive_msg);

    // create array for publishing

    float controls[] = {0, 1, 2, 3, 4, 5, 6, 7};

    // publish actuator control
    auto control_msg = boost::make_shared<mavros_msgs::ActuatorControl>();
    control_msg->group_mix = 0;
    control_msg->header.stamp = ros::Time::now();
    control_msg->controls[1] = -throttle;
    control_msg->controls[2] = -yaw_effort;

    if ((ros::Time::now() - last_setpoint_time_).toSec() < 0.5)
      actuator_pub_.publish(control_msg);
  }
}

void Node::setpoint_cb(const mavros_msgs::AttitudeTarget::ConstPtr &msg)
{
  setpoint_set_ = true;
  ROS_INFO("setpoint recieved");
  last_setpoint_ = *msg;
  setpoint_q_ = Eigen::Quaterniond(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);
  throttle_ = msg->thrust;
  last_setpoint_time_ = ros::Time::now();
}

}  // namespace attitude_controller

int main(int argc, char** argv) {
  ros::init(argc, argv, "attitude_controller");
  ros::NodeHandle pnh("~");
  attitude_controller::Node node(pnh);
  ros::spin();
  return 0;
}


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

// Copyright (c) 2008-2014 Bruno Lalande, Paris, France.
// Copyright (c) 2008-2014 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2009-2014 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2014-2021.
// Modifications copyright (c) 2014-2021, Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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_CARTESIAN_DISTANCE_PROJECTED_POINT_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_DISTANCE_PROJECTED_POINT_HPP

#include <type_traits>

#include <boost/concept_check.hpp>
#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/point_type.hpp>

#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>

#include <boost/geometry/strategies/tags.hpp>
#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/default_distance_result.hpp>
#include <boost/geometry/strategies/cartesian/closest_points_pt_seg.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>
#include <boost/geometry/strategies/cartesian/point_in_point.hpp>
#include <boost/geometry/strategies/cartesian/intersection.hpp>

// Helper geometry (projected point on line)
#include <boost/geometry/geometries/point.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Strategy for distance point to segment
\ingroup strategies
\details Calculates distance using projected-point method, and (optionally) Pythagoras
\author Adapted from: http://geometryalgorithms.com/Archive/algorithm_0102/algorithm_0102.htm
\tparam CalculationType \tparam_calculation
\tparam Strategy underlying point-point distance strategy
\par Concepts for Strategy:
- cartesian_distance operator(Point,Point)
\note If the Strategy is a "comparable::pythagoras", this strategy
    automatically is a comparable projected_point strategy (so without sqrt)

\qbk{
[heading See also]
[link geometry.reference.algorithms.distance.distance_3_with_strategy distance (with strategy)]
}

*/
template
<
    typename CalculationType = void,
    typename Strategy = pythagoras<CalculationType>
>
class projected_point
{
public:
    // The three typedefs below are necessary to calculate distances
    // from segments defined in integer coordinates.

    // Integer coordinates can still result in FP distances.
    // There is a division, which must be represented in FP.
    // So promote.
    template <typename Point, typename PointOfSegment>
    struct calculation_type
        : promote_floating_point
          <
              typename strategy::distance::services::return_type
                  <
                      Strategy,
                      Point,
                      PointOfSegment
                  >::type
          >
    {};

    template <typename Point, typename PointOfSegment>
    inline typename calculation_type<Point, PointOfSegment>::type
    apply(Point const& p, PointOfSegment const& p1, PointOfSegment const& p2) const
    {
        assert_dimension_equal<Point, PointOfSegment>();

        typedef typename calculation_type<Point, PointOfSegment>::type calculation_type;

        auto closest_point = closest_points::detail::compute_closest_point_to_segment
            <calculation_type>::apply(p, p1, p2);

        return Strategy().apply(p, closest_point);
    }

    template <typename CT>
    inline CT vertical_or_meridian(CT const& lat1, CT const& lat2) const
    {
        return lat1 - lat2;
    }

};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename CalculationType, typename Strategy>
struct tag<projected_point<CalculationType, Strategy> >
{
    typedef strategy_tag_distance_point_segment type;
};

template <typename CalculationType, typename Strategy, typename P, typename PS>
struct return_type<projected_point<CalculationType, Strategy>, P, PS>
    : projected_point<CalculationType, Strategy>::template calculation_type<P, PS>
{};

template <typename CalculationType, typename Strategy>
struct comparable_type<projected_point<CalculationType, Strategy> >
{
    // Define a projected_point strategy with its underlying point-point-strategy
    // being comparable
    typedef projected_point
        <
            CalculationType,
            typename comparable_type<Strategy>::type
        > type;
};

template <typename CalculationType, typename Strategy>
struct get_comparable<projected_point<CalculationType, Strategy> >
{
    typedef typename comparable_type
        <
            projected_point<CalculationType, Strategy>
        >::type comparable_type;
public :
    static inline comparable_type apply(projected_point<CalculationType, Strategy> const& )
    {
        return comparable_type();
    }
};

template <typename CalculationType, typename Strategy, typename P, typename PS>
struct result_from_distance<projected_point<CalculationType, Strategy>, P, PS>
{
private :
    typedef typename return_type<projected_point<CalculationType, Strategy>, P, PS>::type return_type;
public :
    template <typename T>
    static inline return_type apply(projected_point<CalculationType, Strategy> const& , T const& value)
    {
        Strategy s;
        return result_from_distance<Strategy, P, PS>::apply(s, value);
    }
};

// Get default-strategy for point-segment distance calculation
// while still have the possibility to specify point-point distance strategy (PPS)
// It is used in algorithms/distance.hpp where users specify PPS for distance
// of point-to-segment or point-to-linestring.
// Convenient for geographic coordinate systems especially.
template <typename Point, typename PointOfSegment, typename Strategy>
struct default_strategy
    <
        point_tag, segment_tag, Point, PointOfSegment,
        cartesian_tag, cartesian_tag, Strategy
    >
{
    typedef strategy::distance::projected_point
        <
            void,
            std::conditional_t
                <
                    std::is_void<Strategy>::value,
                    typename default_strategy
                        <
                            point_tag, point_tag, Point, PointOfSegment,
                            cartesian_tag, cartesian_tag
                        >::type,
                    Strategy
                >
        > type;
};

template <typename PointOfSegment, typename Point, typename Strategy>
struct default_strategy
    <
        segment_tag, point_tag, PointOfSegment, Point,
        cartesian_tag, cartesian_tag, Strategy
    >
{
    typedef typename default_strategy
        <
            point_tag, segment_tag, Point, PointOfSegment,
            cartesian_tag, cartesian_tag, Strategy
        >::type type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_DISTANCE_PROJECTED_POINT_HPP


//          Copyright Gavin Band 2008 - 2012.
// 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 <Eigen/Core>
#include "genfile/vcf/get_set_eigen.hpp"
#include "components/SampleSummaryComponent/SampleSummaryComputation.hpp"
#include "components/SampleSummaryComponent/MissingnessHeterozygosityComputation.hpp"

namespace sample_stats {

	MissingnessHeterozygosityComputation::MissingnessHeterozygosityComputation():
	 	m_snp_index( 0 ),
		m_threshhold( 0.9 )
	{}

	MissingnessHeterozygosityComputation::MissingnessHeterozygosityComputation( genfile::Chromosome chromosome ):
		m_chromosome( chromosome ),
	 	m_snp_index( 0 ),
		m_threshhold( 0.9 )
	{}

	void MissingnessHeterozygosityComputation::accumulate( genfile::VariantIdentifyingData const& snp, Genotypes const& genotypes, genfile::VariantDataReader& ) {
		if( m_chromosome && m_chromosome.get() != snp.get_position().chromosome() ) {
			return ;
		}

		if( m_snp_index == 0 ) {
			m_total_probabilities.setZero( genotypes.rows() ) ;
			m_total_calls.setZero( genotypes.rows() ) ;
			m_het_snps.setZero( genotypes.rows() ) ;
			m_het_snp_calls.setZero( genotypes.rows() ) ;
			m_ones.setOnes( genotypes.rows() ) ;
		}

		m_total_probabilities += genotypes.rowwise().sum() ;
		m_total_calls.array() += ( genotypes.rowwise().maxCoeff().array() >= m_threshhold ).cast< double >() ;

		m_het_snps += genotypes.col( 1 ) ;
		m_het_snp_calls.array() += ( genotypes.col( 1 ).array() >= m_threshhold ).cast< double >() ;

		m_snp_index++ ;
	}

	void MissingnessHeterozygosityComputation::compute( int sample, ResultCallback callback ) {
		callback( sample, "missing_proportion", ( m_snp_index - m_total_probabilities( sample ) ) / m_snp_index ) ;
		callback( sample, "missing_call_proportion", ( m_snp_index - m_total_calls( sample ) ) / m_snp_index ) ;
		callback( sample, "heterozygous_proportion", m_het_snps( sample ) / m_total_probabilities( sample ) ) ;
		callback( sample, "heterozygous_call_proportion", m_het_snp_calls( sample ) / m_total_calls( sample ) ) ;
	}

	std::string MissingnessHeterozygosityComputation::get_summary( std::string const& prefix, std::size_t column_width ) const {
		return prefix + "MissingnessHeterozygosityComputation" ;
	}
}


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/sqrt.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/mzero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/functions/sqr.hpp>

NT2_TEST_CASE_TPL ( sqrt_real,  (double))//BOOST_SIMD_REAL_TYPES)
{
  using nt2::sqrt;
  using nt2::tag::sqrt_;
  typedef typename std::complex<T> cT;
  typedef typename boost::dispatch::meta::call<sqrt_(cT)>::type r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, cT);

  // specific values testss
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Inf<T>())), cT(nt2::Inf<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Inf<T>(),nt2::Inf<T>())),cT(nt2::Inf<T>(),nt2::Inf<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Nan<T>(),nt2::Nan<T>())),cT(nt2::Nan<T>(),nt2::Nan<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Inf<T>(),nt2::Nan<T>())),cT(nt2::Nan<T>(),nt2::Nan<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Zero<T>(),nt2::Inf<T>())),cT(nt2::Inf<T>(),nt2::Inf<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Minf<T>())), cT(0, nt2::Inf<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Nan<T>())), cT(nt2::Nan<T>(),nt2::Nan<T>()));
  std::complex < T > b(nt2::Zero<T>(), nt2::Inf<T>());
  NT2_TEST_EQUAL(nt2::sqrt(nt2::pure(b)), cT(nt2::Inf<T>(), nt2::Inf<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Minf<T>(),nt2::Zero<T>())), cT(nt2::Zero<T>(),nt2::Inf<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::One<T>(), nt2::Inf<T>())), cT(nt2::Inf<T>(),nt2::Inf<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Inf<T>(), nt2::Inf<T>())), cT(nt2::Inf<T>(),nt2::Inf<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Minf<T>(),nt2::Inf<T>())), cT(nt2::Inf<T>(),nt2::Inf<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Nan <T>(),nt2::Inf<T>())), cT(nt2::Nan<T>(),nt2::Nan<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::One<T>(), nt2::Nan<T>())), cT(nt2::Nan<T>(),nt2::Nan<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Inf<T>(), nt2::Nan<T>())), cT(nt2::Nan<T>(),nt2::Nan<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Minf<T>(),nt2::Nan<T>())), cT(nt2::Nan<T>(),nt2::Nan<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Nan <T>(),nt2::Nan<T>())), cT(nt2::Nan<T>(),nt2::Nan<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Minf<T>(),nt2::One<T>())), cT(nt2::Zero<T>(),nt2::Inf<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Minf<T>(),nt2::Mone<T>())),cT(nt2::Zero<T>(),nt2::Minf<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Inf<T>(),nt2::Nan<T>())),  cT(nt2::Nan<T>(),nt2::Nan<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Nan<T>(),nt2::One<T>())),  cT(nt2::Nan<T>(),nt2::Nan<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Nan<T>(),nt2::Nan<T>())),  cT(nt2::Nan<T>(),nt2::Nan<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::One<T>(), -nt2::Inf<T>())), cT(nt2::Inf<T>(),-nt2::Inf<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Inf<T>(), -nt2::Inf<T>())), cT(nt2::Inf<T>(),-nt2::Inf<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Minf<T>(),-nt2::Inf<T>())), cT(nt2::Inf<T>(),-nt2::Inf<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Nan <T>(),-nt2::Inf<T>())), cT(nt2::Nan<T>(),-nt2::Nan<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::One<T>(), -nt2::Nan<T>())), cT(nt2::Nan<T>(),-nt2::Nan<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Inf<T>(), -nt2::Nan<T>())), cT(nt2::Nan<T>(),-nt2::Nan<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Minf<T>(),-nt2::Nan<T>())), cT(nt2::Nan<T>(),-nt2::Nan<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Nan <T>(),-nt2::Nan<T>())), cT(nt2::Nan<T>(),-nt2::Nan<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Minf<T>(),-nt2::One<T>())), cT(nt2::Zero<T>(),-nt2::Inf<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Minf<T>(),-nt2::Mone<T>())),cT(nt2::Zero<T>(),-nt2::Minf<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Inf<T>(), -nt2::Nan<T>())), cT(nt2::Nan<T>(),nt2::Nan<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Nan<T>(), -nt2::One<T>())), cT(nt2::Nan<T>(),-nt2::Nan<T>()));
  NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Nan<T>(), -nt2::Nan<T>())), cT(nt2::Nan<T>(),-nt2::Nan<T>()));
#endif
   NT2_TEST_ULP_EQUAL(nt2::sqrt(cT(1)), cT(1), 0);
   NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Mone<T>())), cT(0, nt2::One<T>()));
   NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::One<T>())), cT(nt2::One<T>()));
   NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Zero<T>())), cT(nt2::Zero<T>()));
   NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Zero<T>(),nt2::Zero<T>())), cT(nt2::Zero<T>(),nt2::Zero<T>()));
   NT2_TEST_EQUAL(nt2::sqrt(cT(nt2::Mzero<T>(),nt2::Zero<T>())),cT(nt2::Zero<T>(),nt2::Zero<T>()));
   NT2_TEST_ULP_EQUAL(nt2::sqrt(cT(0, 2)), cT(1, 1), 1);
   for(T i=-5; i <=  T(5) ; i+= T(0.5))
   {
     for(T j =-5; j < T(5); j+= T(0.5))
     {
       std::cout << i << "+i*(" << j << ") -> " << nt2::sqrt(cT(i, j)) << std::endl;
       NT2_TEST_ULP_EQUAL(nt2::sqr(nt2::sqrt(cT(i, j))), cT(i, j), 10);
     }
   }
}


/* ************************************************************************
 * Copyright 2015 Vratis, Ltd.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 * ************************************************************************ */

#include <gtest/gtest.h>
#include <vector>
#include <string>
#include <boost/program_options.hpp>
#include <boost/algorithm/string.hpp>

//boost ublas
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include "resources/clsparse_environment.h"
#include "resources/csr_matrix_environment.h"

//#include "resources/uBLAS-linalg/ublas_pcg.hpp"

clsparseControl ClSparseEnvironment::control = NULL;
cl_command_queue ClSparseEnvironment::queue = NULL;
cl_context ClSparseEnvironment::context = NULL;
//cl_uint ClSparseEnvironment::N = 1024;

static cl_bool explicit_zeroes = true;

namespace po = boost::program_options;
namespace uBLAS = boost::numeric::ublas;

// Solver parameters;
double absoluteTolerance;
double relativeTolerance;
int maxIterations;
PRECONDITIONER precond;
PRINT_MODE printMode;

// Initial values of rhs and x vectors
double initialRhsValue;
double initialUnknownsValue;

template <typename T>
class Solver : public ::testing::Test
{
    using CLSE = ClSparseEnvironment;
    using CSRE = CSREnvironment;

public:

    void SetUp()
    {
        // Setup solver control
        clsparseCreateSolverResult solverResult = clsparseCreateSolverControl(precond,
                                                    maxIterations,
                                                    relativeTolerance,
                                                    absoluteTolerance);
        solverControl = solverResult.control;
        ASSERT_NE(nullptr, solverControl);

        clsparseStatus status = clsparseSolverPrintMode(solverControl, printMode);
        ASSERT_EQ(clsparseSuccess, status);

        // Setup rhs and vector of unknowns

        hX = uBLAS::vector<T>(CSRE::n_cols, (T) initialUnknownsValue);
        hB = uBLAS::vector<T>(CSRE::n_rows, (T) initialRhsValue);

        clsparseInitVector(&gX);
        clsparseInitVector(&gB);

        cl_int cl_status;

        gX.values = clCreateBuffer(CLSE::context,
                                   CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR,
                                   hX.size() * sizeof(T), hX.data().begin(),
                                   &cl_status);
        gX.num_values = hX.size();
        ASSERT_EQ(CL_SUCCESS, cl_status);

        gB.values = clCreateBuffer(CLSE::context,
                                   CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR,
                                   hB.size() * sizeof(T), hB.data().begin(),
                                   &cl_status);
        gB.num_values = hB.size();
        ASSERT_EQ(CL_SUCCESS, cl_status);

    }

    void TearDown()
    {
        clsparseReleaseSolverControl(solverControl);

        ::clReleaseMemObject(gX.values);
        ::clReleaseMemObject(gB.values);

        clsparseInitVector(&gX);
        clsparseInitVector(&gB);
    }

    void test_cg()
    {

        if ( typeid(T) == typeid(cl_float) )
        {
            clsparseStatus status =
                    clsparseScsrcg(&gX, &CSRE::csrSMatrix, &gB, solverControl, CLSE::control);
            ASSERT_EQ(clsparseSuccess, status);

//            std::cout << "Running CPU Solver" << std::endl;
//            std::shared_ptr<Preconditioner<CSRE::sMatrixType, uBLAS::vector<T>> > ublas_precond;

//            if (precond == DIAGONAL )
//            {
//                ublas_precond = std::shared_ptr<Preconditioner<CSRE::sMatrixType, uBLAS::vector<T>> >
//                        (new DiagonalPreconditioner<CSRE::sMatrixType, uBLAS::vector<T>> (CSRE::ublasSCsr));
//            }
//            else
//            {
//                ublas_precond = std::shared_ptr<Preconditioner<CSRE::sMatrixType, uBLAS::vector<T>> >
//                        (new IdentityPreconditioner<CSRE::sMatrixType, uBLAS::vector<T>> (CSRE::ublasSCsr));
//            }

//            size_t iters = pcg_solve(CSRE::ublasSCsr, hX, hB, *ublas_precond,
//                                   maxIterations, relativeTolerance, absoluteTolerance);

//            double ublas_final_residual =
//                    uBLAS::norm_2(prod( CSRE::ublasSCsr, hX) - hB) / uBLAS::norm_2(hB);

//            std::cout << "uBLAS Solver finished calculations" << std::endl;
//            std::cout << "\tfinal residual = " << ublas_final_residual
//                      << "\titerations = " << iters << std::endl;

        }

        if ( typeid(T) == typeid(cl_double))
        {
            clsparseStatus status =
                    clsparseDcsrcg(&gX, &CSRE::csrDMatrix, &gB, solverControl, CLSE::control);
            ASSERT_EQ(clsparseSuccess, status);

//            std::cout << "Running CPU Solver" << std::endl;
//            std::shared_ptr<Preconditioner<CSRE::dMatrixType, uBLAS::vector<T>> > ublas_precond;

//            if (precond == DIAGONAL )
//            {
//                ublas_precond = std::shared_ptr<Preconditioner<CSRE::dMatrixType, uBLAS::vector<T>> >
//                        (new DiagonalPreconditioner<CSRE::dMatrixType, uBLAS::vector<T>> (CSRE::ublasDCsr));
//            }
//            else
//            {
//                ublas_precond = std::shared_ptr<Preconditioner<CSRE::dMatrixType, uBLAS::vector<T>> >
//                        (new IdentityPreconditioner<CSRE::dMatrixType, uBLAS::vector<T>> (CSRE::ublasDCsr));
//            }

//            size_t iters = pcg_solve(CSRE::ublasDCsr, hX, hB, *ublas_precond,
//                                   maxIterations, relativeTolerance, absoluteTolerance);

//            double ublas_final_residual =
//                    uBLAS::norm_2(prod( CSRE::ublasDCsr, hX) - hB) / uBLAS::norm_2(hB);

//            std::cout << "uBLAS Solver finished calculations" << std::endl;
//            std::cout << "\tfinal residual = " << ublas_final_residual
//                      << "\titerations = " << iters << std::endl;

        }

    }

    void test_bicg()
    {

        if ( typeid(T) == typeid(cl_float) )
        {
            clsparseStatus status =
                    clsparseScsrbicgStab(&gX, &CSRE::csrSMatrix, &gB, solverControl, CLSE::control);
            ASSERT_EQ(clsparseSuccess, status);
        }

        if ( typeid(T) == typeid(cl_double))
        {
            clsparseStatus status =
                    clsparseDcsrbicgStab(&gX, &CSRE::csrDMatrix, &gB, solverControl, CLSE::control);
            ASSERT_EQ(clsparseSuccess, status);
        }
    }

    uBLAS::vector<T> hX;
    uBLAS::vector<T> hB;

    cldenseVector gX;
    cldenseVector gB;

    clSParseSolverControl solverControl;

};

typedef ::testing::Types<cl_float, cl_double> TYPES;
TYPED_TEST_CASE(Solver, TYPES);

TYPED_TEST(Solver, cg)
{
    this->test_cg();
}

TYPED_TEST(Solver, bicgstab)
{
    this->test_bicg();
}

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

    using CLSE = ClSparseEnvironment;
    using CSRE = CSREnvironment;

    std::string path;

    std::string strPrecond;
    std::string strPrintMode;

    std::string platform;
    cl_platform_type pID;
    cl_uint dID;

    po::options_description desc("Allowed options");

    desc.add_options()
            ("help,h", "Produce this message.")
            ("path,p", po::value(&path)->required(), "Path to matrix in mtx format.")
            ("platform,l", po::value(&platform)->default_value("AMD"),
             "OpenCL platform: AMD or NVIDIA.")
            ("device,d", po::value(&dID)->default_value(0),
             "Device id within platform.")
            ("relative,r", po::value(&relativeTolerance)->default_value(1e-2),
             "Final residuum value relative to initial (r = b - A*x)")
            ("absolute,a", po::value(&absoluteTolerance)->default_value(1e-5),
             "Absolute level of residuum value")
            ("iterations,i", po::value(&maxIterations)->default_value(100),
             "Maximum number of solver iterations")
            ("precond,c", po::value(&strPrecond)->default_value("Diagonal"),
             "Type of preconditioner, Diagonal or Void")
            ("mode,m", po::value(&strPrintMode)->default_value("Normal"),
             "Solver Print mode, Verbose, Normal, Quiet")
            ("initx,x", po::value(&initialUnknownsValue)->default_value(0),
             "Initial value for vector of unknowns")
            ("initb,b", po::value(&initialRhsValue)->default_value(1),
             "Initial value for rhs vector")
            ("no_zeroes,z", po::bool_switch()->default_value(false),
             "Disable reading explicit zeroes from the input matrix market file.");

    po::variables_map vm;
    po::parsed_options parsed =
            po::command_line_parser( argc, argv ).options( desc ).allow_unregistered( ).run( );

    try {
        po::store(parsed, vm);
        if (vm.count("help"))
        {
            std::cout << desc << std::endl;
            return 0;
        }
        po::notify(vm);
    }
    catch (po::error& error)
    {
        std::cerr << "Parsing command line options..." << std::endl;
        std::cerr << "Error: " << error.what() << std::endl;
        std::cerr << desc << std::endl;
        return false;
    }

    std::vector< std::string > to_pass_further =
            po::collect_unrecognized( parsed.options, po::include_positional );

    //check platform
    if(vm.count("platform"))
    {
        if ("AMD" == platform)
        {
            pID = AMD;
        }
        else if ("NVIDIA" == platform)
        {
            pID = NVIDIA;
        }
        else
        {

            std::cout << "The platform option is missing or is ill defined!\n";
            std::cout << "Given [" << platform << "]" << std::endl;
            platform = "AMD";
            pID = AMD;
            std::cout << "Setting [" << platform << "] as default" << std::endl;
        }

    }

    if (vm["no_zeroes"].as<bool>())
        explicit_zeroes = false;

    //pickup preconditioner
    if ( boost::iequals(strPrecond, "Diagonal"))
    {
        precond = DIAGONAL;
    }
    else if (boost::iequals(strPrecond, "Void"))
    {
        precond = NOPRECOND;
    }

    else
    {
        std::cerr << "Invalid preconditioner [" << strPrecond << "] "
                  << "using Diagonal instead" << std::endl;
        precond = DIAGONAL;
    }

    //pickup solver print mode;
    if ( boost::iequals(strPrintMode, "Normal"))
    {
        printMode = NORMAL;
    }
    else if (boost::iequals(strPrintMode, "Quiet"))
    {
        printMode = QUIET;
    }
    else if (boost::iequals(strPrintMode, "Verbose"))
    {
        printMode = VERBOSE;
    }
    else
    {
        std::cerr << "Invalid print mode given [" << strPrintMode << "]"
                  << " using Quiet instead" << std::endl;
        printMode = QUIET;
    }

    ::testing::InitGoogleTest(&argc, argv);
    //order does matter!
    ::testing::AddGlobalTestEnvironment( new CLSE(pID, dID));
    ::testing::AddGlobalTestEnvironment( new CSRE(path, 0, 0,
                                                  CLSE::queue, CLSE::context, explicit_zeroes));

    return RUN_ALL_TESTS();

}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_PREDECESSOR_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_PREDECESSOR_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing predecessor capabilities

    Returns the n-th greatest element strictly less than the parameter

    @par Semantic:

    @code
    T r = predecessor(x,n);
    @endcode

    computes the n-th greatest representable value strictly less than x in its type.
    n must be positive or null.

    For integer it saturate at @ref Valmin, for floating point numbers, all @ref Minf
    strict predecessors are @ref Nan.

    @see next, prev, successor, nextafter, Minf, Valmin, Nan

  **/
  const boost::dispatch::functor<tag::predecessor_> predecessor = {};
} }
#endif

#include <boost/simd/function/scalar/predecessor.hpp>
#include <boost/simd/function/simd/predecessor.hpp>

#endif


//  Copyright John Maddock 2006.
//  Use, modification and distribution are 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_STATS_EXPONENTIAL_HPP
#define BOOST_STATS_EXPONENTIAL_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/config/no_tr1/cmath.hpp>

#ifdef BOOST_MSVC
# pragma warning(push)
# pragma warning(disable: 4702) // unreachable code (return after domain_error throw).
#endif

#include <utility>

namespace boost{ namespace math{

namespace detail{
//
// Error check:
//
template <class RealType, class Policy>
inline bool verify_lambda(const char* function, RealType l, RealType* presult, const Policy& pol)
{
   if(l <= 0)
   {
      *presult = policies::raise_domain_error<RealType>(
         function,
         "The scale parameter \"lambda\" must be > 0, but was: %1%.", l, pol);
      return false;
   }
   return true;
}

template <class RealType, class Policy>
inline bool verify_exp_x(const char* function, RealType x, RealType* presult, const Policy& pol)
{
   if(x < 0)
   {
      *presult = policies::raise_domain_error<RealType>(
         function,
         "The random variable must be >= 0, but was: %1%.", x, pol);
      return false;
   }
   return true;
}

} // namespace detail

template <class RealType = double, class Policy = policies::policy<> >
class exponential_distribution
{
public:
   typedef RealType value_type;
   typedef Policy policy_type;

   exponential_distribution(RealType lambda = 1)
      : m_lambda(lambda)
   {
      RealType err;
      detail::verify_lambda("boost::math::exponential_distribution<%1%>::exponential_distribution", lambda, &err, Policy());
   } // exponential_distribution

   RealType lambda()const { return m_lambda; }

private:
   RealType m_lambda;
};

typedef exponential_distribution<double> exponential;

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const exponential_distribution<RealType, Policy>& /*dist*/)
{ // Range of permissible values for random variable x.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(static_cast<RealType>(0), max_value<RealType>());
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const exponential_distribution<RealType, Policy>& /*dist*/)
{ // Range of supported values for random variable x.
   // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
   using boost::math::tools::max_value;
   using boost::math::tools::min_value;
   return std::pair<RealType, RealType>(min_value<RealType>(),  max_value<RealType>());
   // min_value<RealType>() to avoid a discontinuity at x = 0.
}

template <class RealType, class Policy>
inline RealType pdf(const exponential_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   static const char* function = "boost::math::pdf(const exponential_distribution<%1%>&, %1%)";

   RealType lambda = dist.lambda();
   RealType result;
   if(0 == detail::verify_lambda(function, lambda, &result, Policy()))
      return result;
   if(0 == detail::verify_exp_x(function, x, &result, Policy()))
      return result;
   result = lambda * exp(-lambda * x);
   return result;
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const exponential_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   static const char* function = "boost::math::cdf(const exponential_distribution<%1%>&, %1%)";

   RealType result;
   RealType lambda = dist.lambda();
   if(0 == detail::verify_lambda(function, lambda, &result, Policy()))
      return result;
   if(0 == detail::verify_exp_x(function, x, &result, Policy()))
      return result;
   result = -boost::math::expm1(-x * lambda, Policy());

   return result;
} // cdf

template <class RealType, class Policy>
inline RealType quantile(const exponential_distribution<RealType, Policy>& dist, const RealType& p)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   static const char* function = "boost::math::quantile(const exponential_distribution<%1%>&, %1%)";

   RealType result;
   RealType lambda = dist.lambda();
   if(0 == detail::verify_lambda(function, lambda, &result, Policy()))
      return result;
   if(0 == detail::check_probability(function, p, &result, Policy()))
      return result;

   if(p == 0)
      return 0;
   if(p == 1)
      return policies::raise_overflow_error<RealType>(function, 0, Policy());

   result = -boost::math::log1p(-p, Policy()) / lambda;
   return result;
} // quantile

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<exponential_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   static const char* function = "boost::math::cdf(const exponential_distribution<%1%>&, %1%)";

   RealType result;
   RealType lambda = c.dist.lambda();
   if(0 == detail::verify_lambda(function, lambda, &result, Policy()))
      return result;
   if(0 == detail::verify_exp_x(function, c.param, &result, Policy()))
      return result;
   result = exp(-c.param * lambda);

   return result;
}

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<exponential_distribution<RealType, Policy>, RealType>& c)
{
   BOOST_MATH_STD_USING // for ADL of std functions

   static const char* function = "boost::math::quantile(const exponential_distribution<%1%>&, %1%)";

   RealType result;
   RealType lambda = c.dist.lambda();
   if(0 == detail::verify_lambda(function, lambda, &result, Policy()))
      return result;

   RealType q = c.param;
   if(0 == detail::check_probability(function, q, &result, Policy()))
      return result;

   if(q == 1)
      return 0;
   if(q == 0)
      return policies::raise_overflow_error<RealType>(function, 0, Policy());

   result = -log(q) / lambda;
   return result;
}

template <class RealType, class Policy>
inline RealType mean(const exponential_distribution<RealType, Policy>& dist)
{
   RealType result;
   RealType lambda = dist.lambda();
   if(0 == detail::verify_lambda("boost::math::mean(const exponential_distribution<%1%>&)", lambda, &result, Policy()))
      return result;
   return 1 / lambda;
}

template <class RealType, class Policy>
inline RealType standard_deviation(const exponential_distribution<RealType, Policy>& dist)
{
   RealType result;
   RealType lambda = dist.lambda();
   if(0 == detail::verify_lambda("boost::math::standard_deviation(const exponential_distribution<%1%>&)", lambda, &result, Policy()))
      return result;
   return 1 / lambda;
}

template <class RealType, class Policy>
inline RealType mode(const exponential_distribution<RealType, Policy>& /*dist*/)
{
   return 0;
}

template <class RealType, class Policy>
inline RealType median(const exponential_distribution<RealType, Policy>& dist)
{
   using boost::math::constants::ln_two;
   return ln_two<RealType>() / dist.lambda(); // ln(2) / lambda
}

template <class RealType, class Policy>
inline RealType skewness(const exponential_distribution<RealType, Policy>& /*dist*/)
{
   return 2;
}

template <class RealType, class Policy>
inline RealType kurtosis(const exponential_distribution<RealType, Policy>& /*dist*/)
{
   return 9;
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const exponential_distribution<RealType, Policy>& /*dist*/)
{
   return 6;
}

} // namespace math
} // namespace boost

#ifdef BOOST_MSVC
# pragma warning(pop)
#endif

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_EXPONENTIAL_HPP


#include "stdafx.h"

#ifdef USE_EIGEN
//#define EIGEN_DONT_VECTORIZE
//#define EIGEN_DONT_PARALLELIZE
#include <Eigen/Eigen>
#include <Eigen/SVD>
#include <opencv2/core/eigen.hpp>
#endif

using namespace std;
using namespace cv;

namespace cp
{
	void plotDCTKernel(string wname, bool isWait, const double* GCn, const int radius, const int order, const double G0, const double sigma)
	{
		const int size = (order) * (radius + 1);
		AutoBuffer<float> G(size);
		for (int i = 0; i < size; i++)G[i] = (float)GCn[i];
		plotDCTKernel(wname, isWait, G, radius, order, (float)G0, sigma);
	}

	void plotDCTKernel(string wname, bool isWait, const float* GCn, const int radius, const int order, const float G0, const double sigma)
	{
		Mat gauss(radius + 1, 1, CV_64F);
		cp::setGaussKernelHalf(gauss, radius, sigma, true);

		static int pre_order = 0;
		cp::Plot p;
		cv::namedWindow(wname);
		const int allIndex = order + 2;
		const int totalIndex = allIndex - 1;
		static int fk = allIndex; createTrackbar("k", wname, &fk, allIndex);
		if (pre_order != order)
		{
			pre_order = order;
			fk = allIndex;
			setTrackbarMax("k", wname, allIndex);
			setTrackbarPos("k", wname, fk);
		}

		int key = 0;
		for (int k = 0; k <= order; k++)
		{
			p.setPlotTitle(k, format("order %d", k));
		}
		p.setPlotTitle(order + 1, "total");

		if (G0 == 0.f)
		{
			while (key != 'q')
			{
				double error = 0.0;
				if (fk == allIndex)
				{
					for (int k = 0; k <= order; k++)
					{
						for (int n = -radius; n <= radius; n++)
						{
							//p.push_back(n, GCn[(order + 1) * abs(n) + order - k], k);
							p.push_back(n, GCn[(order + 1) * abs(n) + k], k);
						}
					}
					//total
					for (int n = -radius; n <= radius; n++)
					{
						float sum = 0.f;
						for (int k = 0; k <= order; k++)
						{
							sum += GCn[(order + 1) * abs(n) + k];
						}
						p.push_back(n, sum, totalIndex);
						double v = gauss.at<double>(abs(n)) - sum;
						error += v * v;
					}
				}
				else
				{
					if (fk == totalIndex)
					{
						for (int n = -radius; n <= radius; n++)
						{
							float sum = 0.f;
							for (int k = 0; k <= order; k++)
							{
								sum += GCn[(order + 1) * abs(n) + k];
							}
							p.push_back(n, sum, totalIndex);
							double v = gauss.at<double>(abs(n)) - sum;
							error += v * v;
						}
					}
					else
					{
						for (int n = -radius; n <= radius; n++)
						{
							p.push_back(n, GCn[(order + 1) * abs(n) + order - fk], fk);
						}
					}
				}
				p.plot(wname, false, "", format("10log10(1/mse) %f", 10 * log10(1 / error)));
				p.clear();
				key = waitKey(1);
				if (!isWait)break;
			}
		}
		else //DCT1,5
		{
			while (key != 'q')
			{
				double error = 0.0;
				if (fk == allIndex)
				{
					for (int n = -radius; n <= radius; n++)
					{
						p.push_back(n, G0, 0);
					}
					for (int k = 0; k < order; k++)
					{
						for (int n = -radius; n <= radius; n++)
						{
							p.push_back(n, GCn[order * abs(n) + k], k + 1);
						}
					}
					//total
					for (int n = -radius; n <= radius; n++)
					{
						float sum = G0;
						for (int k = 0; k < order; k++)
						{
							sum += GCn[order * abs(n) + k];
						}
						p.push_back(n, sum, totalIndex);
						double v = gauss.at<double>(abs(n)) - sum;
						error += v * v;
					}
				}
				else
				{
					if (fk == totalIndex)
					{
						for (int n = -radius; n <= radius; n++)
						{
							float sum = G0;
							for (int k = 0; k < order; k++)
							{
								sum += GCn[(order)*abs(n) + k];
							}
							p.push_back(n, sum, totalIndex);
							double v = gauss.at<double>(abs(n)) - sum;
							error += v * v;
						}
					}
					else if (fk == 0)
					{
						for (int n = -radius; n <= radius; n++)
						{
							p.push_back(n, G0, 0);
						}
					}
					else
					{
						for (int n = -radius; n <= radius; n++)
						{
							p.push_back(n, GCn[(order)*abs(n) + fk - 1], fk);
						}
					}
				}
				p.plot(wname, false, "", format("10log10(1/mse) %f", 10 * log10(1 / error)));

				p.clear();
				key = waitKey(1);
				if (!isWait)break;
			}
		}
	}

	void computeSpectrumGaussianClosedForm(const double sigma, const int K, const int R, const int dcttype, double* destSpect)
	{
		double omega = 0.0;
		double k0 = 0.0;
		switch (dcttype)
		{
		case 1:	omega = CV_2PI / (2.0 * R + 0.0); k0 = 0.0; break;
		case 3:	omega = CV_2PI / (2.0 * R + 2.0); k0 = 0.5; break;
		case 5:	omega = CV_2PI / (2.0 * R + 1.0); k0 = 0.0; break;
		case 7:	omega = CV_2PI / (2.0 * R + 1.0); k0 = 0.5; break;
			//default: throw "Unsupported DCT type"; break;
		}

		for (int k = 0; k <= K; k++)
		{
			double coeff = sigma * omega * (k + k0);
			destSpect[k] = 2.0 * exp(-0.5 * coeff * coeff);
		}

		if (dcttype == 1 || dcttype == 5)
			destSpect[0] = 1.0;
	}

	void computeCtWCinv(Mat& dest, const int K, const int R, const int dctType)
	{
		Mat Minv = Mat::eye(K, K, CV_64F);

		double T = 0.0;
		switch (dctType)
		{
		case 1:
		{
			Minv.at<double>(0, 0) = 0.5;
			//Minv.at<double>(K - 1, K - 1) = 0.5;
			T = (2.0 * R + 0.0); break;
		}
		case 3:
		{
			//Minv.at<double>(0, 0) = 0.5;
			T = (2.0 * R + 2.0); break;
		}
		case 5:
		{
			Minv.at<double>(0, 0) = 0.5;
			T = (2.0 * R + 1.0); break;
		}
		case 7:
		{
			//Minv.at<double>(0, 0) = 0.5;
			//Minv.at<double>(K - 1, K - 1) = 0.5;
			T = (2.0 * R + 1.0); break;
		}
		default: throw "Unsupported DCT type"; break;
		}

		if (dctType == 1)
		{
			Mat s = Mat::ones(K, 1, CV_64F);
			for (int i = 1; i < K; i += 2)s.at<double>(i) = -1.0;

			Mat Minvs = Minv * s;
			//cv::trace(Minv).val[0]=K-2+0.5+0.5
			Mat((Minv - (Minvs * Minvs.t()) / (T + (double)K - 1.0))).copyTo(dest);
		}
		else
		{
			//Mat(4/T * Minv).copyTo(dest);
			Minv.copyTo(dest);//scaling (4/T) is not required due to other rescaling.
		}
	}

	void setGaussKernelHalf(Mat& dest, const int R, const double sigma, bool isNormalize)
	{
		CV_Assert(dest.depth() == CV_64F);
		double sum = 0.0;
		const double coeff = 1.0 / (-2.0 * sigma * sigma);
		for (int n = 1; n <= R; ++n)
		{
			const double v = exp((double)(n * n) * coeff);
			//const double v = exp(-(double)(abs(u)) / (sigma));
			//int n = 1;
			//const double v = exp(-(double)pow(abs(u),(double)n) / (n*pow(sigma,double(n))));
			dest.at<double>(n, 0) = v;
			sum += v;
		}
		sum *= 2.0;
		dest.at<double>(0, 0) = 1.0;//u=0
		sum += 1.0;//u=0

		const int rend = int(10.0 * sigma);
		double eout = 0.0;
		for (int i = R + 1; i <= rend; i++)
		{
			const double v = exp(i * i * coeff);
			eout += v;
		}
		dest.at<double>(R, 0) += eout;

		if (isNormalize)
		{
			for (int n = 0; n <= R; ++n)
			{
				dest.at<double>(n, 0) /= sum;
			}
		}
	}

	//without normalize
	void generateCosKernel(double* dest, double& totalInv, const int dctType, const double* Gk, const int radius, const int order)
	{
		double k0;
		double omega;
		switch (dctType)
		{
		case 1:	omega = CV_2PI / (2.0 * radius + 0.0); k0 = 0.0; break;
		case 3:	omega = CV_2PI / (2.0 * radius + 2.0); k0 = 0.5; break;
		case 5:	omega = CV_2PI / (2.0 * radius + 1.0); k0 = 0.0; break;
		case 7:	omega = CV_2PI / (2.0 * radius + 1.0); k0 = 0.5; break;
		default: throw "Unsupported DCT type"; break;
		}

		double sum = 0.0;
		if (dctType == 1 || dctType == 5)
		{
			sum = Gk[0] * double(2 * radius + 1);//k=0
			for (int r = 0; r <= radius; ++r)
			{
				for (int k = 1; k <= order; ++k)
				{
					const double coeff = cos((k + k0) * omega * r) * Gk[k];
					dest[order * r + k - 1] = coeff;

					if (r == 0) sum += coeff;
					else sum += 2.0 * coeff;
				}
			}
		}
		else
		{
			for (int r = 0; r <= radius; ++r)
			{
				for (int k = 0; k <= order; ++k)
				{
					const double coeff = cos((k + k0) * omega * r) * Gk[k];

#ifdef COEFFICIENTS_SMALLEST_FIRST
					dest[(order + 1) * r + (order)-k] = coeff;
#else
					dest[(order + 1) * r + k] = coeff;
#endif

					if (r == 0) sum += coeff;
					else sum += 2.0 * coeff;
				}
			}
		}

		totalInv = (1.0 / sum);
	}

	bool optimizeSpectrum(const double sigma, const int K, const int R, const int dcttype, double* destSpect, const int M)
	{
		/*if (K > R)
		{
			computeSpectrumGaussianClosedForm(sigma, K, R, dcttype, destSpect);
			return false;
		}*/

		double omega, ratio, k0, n0 = 0.0;
		switch (dcttype)
		{
		case 1:	omega = CV_2PI / (2.0 * R + 0.0); ratio = 2.0 / sqrt(2.0 * R + 0.0); k0 = 0.0; break;
		case 3:	omega = CV_2PI / (2.0 * R + 2.0); ratio = 2.0 / sqrt(2.0 * R + 2.0); k0 = 0.5; break;
		case 5:	omega = CV_2PI / (2.0 * R + 1.0); ratio = 2.0 / sqrt(2.0 * R + 1.0); k0 = 0.0; break;
		case 7:	omega = CV_2PI / (2.0 * R + 1.0); ratio = 2.0 / sqrt(2.0 * R + 1.0); k0 = 0.5; break;
		default: throw "Unsupported DCT type"; break;
		}

		//kernel
		cv::Mat1d h0(R + 1, 1);//R+1
		setGaussKernelHalf(h0, R, sigma, false);

		//weight matrix
		cv::Mat1d W = cv::Mat1d::eye(R + 1, R + 1);
		W(0, 0) = 0.5;

		// DCT matrix
		cv::Mat1d C(R + 1, K + 1);
		for (int n = 0; n <= R; ++n)
		{
			for (int k = 0; k <= K; ++k)
			{
				C(n, k) = cos(omega * (k + k0) * (n + n0));
			}
		}
		C *= ratio;

		cv::Mat1d CWCinv;//K+1 x K+1
		cv::Mat1d a_ls;

#ifdef USE_OPTIMIZE_DCT_SWICH
		static int method = 1;
#ifdef USE_EIGEN
		createTrackbar("opt method", "", &method, 4);
#else
		createTrackbar("opt method", "", &method, 2);
#endif
#else
		int method = 0;//OpenCV direct
		//int method = 1;//fast method ICASSP2018 universal(but DCT1 and 7 has some bugs?)
		//int method = 2;//OpenCV SVD
		//int method = 3;//Eigen direct
		//int method = 4;//Eigen SVD
#endif
		//print_debug3(K, R, dcttype);
		switch (method)
		{
		default:
		case 0:
		{
			// (CWC)^-1
			CWCinv = (C.t() * W * C).inv(DECOMP_CHOLESKY);
			//cv::Mat1d CWCinv = (C.t() * W * C).inv(DECOMP_LU);
			// L2 minimization with moment preservation
			a_ls = CWCinv * C.t() * W * h0;
			//showMat64F(CWCinv, true);

			break;
		}
		case 1:
		{
			computeCtWCinv(CWCinv, K + 1, R + 1, dcttype);
			//showMat64F(CWCinv, true);
			a_ls = CWCinv * C.t() * W * h0;

			break;
		}
		case 2:
		{
			Mat w, u, vt;
			W(0, 0) = sqrt(0.5);
			//with weight W
			cv::SVDecomp(W * C, w, u, vt, SVD::FULL_UV);
			SVD::backSubst(w, u, vt, W * h0, a_ls);

			//without weight W
			//cv::SVDecomp(C, w, u, vt, SVD::FULL_UV);
			//SVD::backSubst(w, u, vt, h0, a_ls);
			break;
		}
		case 3:
		{
#ifdef USE_EIGEN
			Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> CE;
			cv::cv2eigen(C, CE);
			Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> WE;
			cv::cv2eigen(W, WE);
			Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> h0E;
			cv::cv2eigen(h0, h0E);

			Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> CtE;
			cv::cv2eigen(C.t(), CtE);

			Eigen::MatrixXd temp = CtE * WE * CE;
			Eigen::MatrixXd temp2 = temp.inverse() * CtE * WE * h0E;
			cv::eigen2cv(temp2, a_ls);
#endif
			break;
		}
		case 4:
		{

#ifdef USE_EIGEN
			W(0, 0) = sqrt(0.5);
			Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> CE;
			cv::cv2eigen(C, CE);
			Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> WE;
			cv::cv2eigen(W, WE);
			Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> h0E;
			cv::cv2eigen(h0, h0E);

			Eigen::BDCSVD<Eigen::MatrixXd> svd(WE * CE, Eigen::ComputeThinU | Eigen::ComputeThinV);
			Eigen::MatrixXd a = svd.solve(WE * h0E);
			cv::eigen2cv(a, a_ls);
#endif
			break;
		}
		}

		const double sp0 = (dcttype == 1 || dcttype == 5) ? 1.0 : 2.0;
		//const double v = sp0 / a_ls(0, 0);

#ifdef PRINT_RANK_DEFICIENT
		if (a_ls(0, 0) == 0)
		{
			cout << "PRINT_RANK_DEFICIENT" << endl;
			print_debug4(sigma, K, R, dcttype);
		}
#endif
		if (a_ls(0, 0) == 0)
		{
			computeSpectrumGaussianClosedForm(sigma, K, R, dcttype, destSpect);
			return false;
		}

		const double v = sp0 / a_ls(0, 0);

		for (int k = 0; k <= K; ++k)
		{
			//cout << k << ": " << spect[k] << ": ";
			destSpect[k] = a_ls(k, 0) * v;
			//destSpect[k] = a_ls(k, 0)*ratio;
			//cout << spect[k] << endl;
		}

		if (1 <= M)
		{
			cv::Mat1d a = a_ls.clone();

			//ideal gaussian moment vector
			cv::Mat1d mu(M, 1, 1.0);
			for (auto m = 1; m < M; ++m)
				mu(m, 0) = mu(m - 1, 0) * (2 * m - 1) * sigma * sigma;

			// Vandermonde
			cv::Mat1d V(R + 1, M);
			for (auto m = 0; m < M; ++m)
			{
				for (auto u = 0; u <= R; ++u)
					V(u, m) = pow(u, 2 * m);
				//V(0, m) /= 2.0; // moved from W
			}

			// Moment constraint
			cv::Mat1d U = V.t() * W * C;
			cv::Mat1d S = U * CWCinv * U.t();
			cv::Mat1d Sinv = S.inv();
			a -= CWCinv * U.t() * Sinv * (U * a - 0.5 * mu);

			//print_matrix(U, "[U]");
			//print_matrix(S, "[S]");
			//print_matrix(Sinv, "[S^-1]");
			//print_matrix(U*a - 0.5*mu, "[U*a - 0.5*mu]");

			for (auto k = 0; k <= K; ++k)
			{
				//cout <<k<<": "<< spect[k] << ": ";
				destSpect[k] = a(k, 0);
				//cout << spect[k] << endl;
			}

			if (dcttype == 1 || dcttype == 5)
				destSpect[0] = 1.0;
		}

		//std::cerr << "[Spectra]" << std::endl;
		//for (auto k = 0; k < K; ++k)
		//	std::cerr << cv::format("k=%d:  %10.8f  %10.8f", k, a_ls(k, 0), a(k, 0)) << std::endl;
		return true;
	}

	class SearchFullDCTRadius :public cp::Search1DInt
	{
		double sigma;
		int K;
		int dcttype;
		double* spect;

		bool isOptimize;

		double getErrorDCT(const double sigma, const int K, const int R, const int dcttype, const double* spect)
		{
			const int ROut = (int)ceil(9.0 * sigma);
			//const int ROut = (int)ceil(7.5 * sigma);
			//const int ROut = (int)ceil(6.0 * sigma);
			AutoBuffer<double> ans(ROut + 1);
			AutoBuffer<double> approx(ROut + 1);
			AutoBuffer<double> e2(ROut + 1);

			double sum = 1.0;
			ans[0] = 1.0;
			for (int i = ROut; i >= 1; --i)
			{
				double v = exp(i * i / (-2.0 * sigma * sigma));
				ans[i] = v;
				sum += 2.0 * v;
			}
			for (int i = 0; i <= ROut; i++)
			{
				ans[i] /= sum;
			}

			double errorTruncation = 0.0;
			for (int i = ROut; i >= R + 1; --i)
			{
				double e = ans[i];
				errorTruncation = fma(e, e, errorTruncation);
			}

			double phi, k0;//n0=0
			switch (dcttype)
			{
			case 1:	phi = CV_2PI / (2.0 * R + 0.0); k0 = 0.0; break;
			case 3:	phi = CV_2PI / (2.0 * R + 2.0); k0 = 0.5; break;
			case 5:	phi = CV_2PI / (2.0 * R + 1.0); k0 = 0.0; break;
			case 7:	phi = CV_2PI / (2.0 * R + 1.0); k0 = 0.5; break;

			default: throw "Unsupported DCT type"; break;
			}

			sum = 0.0;
			for (int i = R; i >= 1; --i)
			{
				double s = 0.0;
				for (int k = K; k >= 0; --k)
				{
					s = fma(cos(phi * (k + k0) * (i)), spect[k], s);//DCT1,3,5,7: n0 = 0.
				}
				sum += 2.0 * s;
				approx[i] = s;
			}
			{
				double s = 0.0;
				for (int k = K; k >= 0; --k)
				{
					s += spect[k];
				}
				sum += s;
				approx[0] = s;
			}
			for (int i = 0; i <= R; i++)
			{
				approx[i] /= sum;
			}

			double errorFrec = 0.0;
			for (int i = R; i >= 1; --i)
			{
				errorFrec = fma((ans[i] - approx[i]), (ans[i] - approx[i]), errorFrec);
			}
			errorFrec *= 2.0;
			errorFrec += (ans[0] - approx[0]) * (ans[0] - approx[0]);

			return errorFrec + errorTruncation;
		}

		double getError(int x)
		{
			if (isOptimize)optimizeSpectrum(sigma, K, x, dcttype, spect);
			else computeSpectrumGaussianClosedForm(sigma, K, x, dcttype, spect);

			return getErrorDCT(sigma, K, x, dcttype, spect);
		}

	public:
		SearchFullDCTRadius(const double sigma, const int K, const int dcttype, const double* spect, bool isOptimize)
		{
			this->sigma = sigma;
			this->K = K;
			this->dcttype = dcttype;
			this->spect = (double*)spect;
			this->isOptimize = isOptimize;
		}
	};

	int test_ratio(const double sigma, const int K, const int dcttype)
	{
		double a, b, c_1, c_2, c_3, c_4;
		int dest;
		if (dcttype == 1)
			b = 0.467347, c_1 = 0.0007, c_2 = -0.0277, c_3 = 0.6053, c_4 = 1.8088;
		else if (dcttype == 3)
			b = -0.16509, c_1 = 0.0005, c_2 = -0.0221, c_3 = 0.5458, c_4 = 2.1403;
		else if (dcttype == 5)
			b = 0.00696, c_1 = 0.0008, c_2 = -0.0289, c_3 = 0.6159, c_4 = 1.7929;
		else if (dcttype == 7)
			b = -0.029, c_1 = 0.0005, c_2 = -0.0217, c_3 = 0.5412, c_4 = 2.1912;
		else
		{
			cout << "This is not Sliding DCT" << endl;
			return 0;
		}
		a = c_1 * K * K * K + c_2 * K * K + c_3 * K + c_4;
		return dest = int(a * sigma + b);
	}

	int argminR_BruteForce_DCT(const double sigma, const int K, const int dcttype, const double* spect, const bool isOptimize, const bool isGoldenSelectionSearch)
	{
		//case K<=R:OK
		//case K>R:NG
		int r = argminR_ContinuousForm_DCT(sigma, K, dcttype, isGoldenSelectionSearch);

		const int rmin = max(1, (int)floor(0.6 * r));
		const int rmax = (int)ceil(1.4 * r);
		int argmin_r = 0;

		SearchFullDCTRadius s(sigma, K, dcttype, spect, isOptimize);

		//		s.plotError("test", rmin, rmax);
				//debug
				/*{
					int r_gs = s.goldenSearch(rmin, rmax);
					int r_li = s.linearSearch(rmin, rmax);
					if (r_li != r_gs)
					{
						cout << "NG: (sigma, k) = " << sigma << "," << K << endl;
						print_debug2(r_li, r_gs);
						print_debug2(rmin, rmax);
						s.plotError("error", rmin, rmax);
					}
					//cout << "dct" << dcttype << ": K=" << K << ", r=" << argmin_r << ", sigma=" << argmin_r / sigma << endl;
				}*/

		if (isGoldenSelectionSearch)
		{
			argmin_r = s.goldenSearch(rmin, rmax);
			//cout << "dct" << dcttype << ": K=" << K << ", r=" << argmin_r << ", sigma=" << argmin_r / sigma << endl;
		}
		else
		{
			argmin_r = s.linearSearch(rmin, rmax);
			//cout << "dct" << dcttype << ": K=" << K << ", r=" << argmin_r << ", sigma=" << argmin_r / sigma << endl;
		}

		//cout << "Linear search radius is" << test_ratio(sigma, K, dcttype) << endl;
		//cout << "True number radius is" << argmin_r << endl;
		return argmin_r;
	}

#pragma region ContinuousForm
	class SearchDCTRadiusContinuousForm :public cp::Search1DInt
	{
		double sigma;
		int K;
		double* spect = nullptr;

		double getError(int x)
		{
			double T = 2.0 * x + 1.0;
			double Es = erfc(T / (2.0 * sigma));
			double Ef = erfc(sigma * CV_PI * (2.0 * K + 1.0) / T);
			return Es + Ef;
		}

	public:
		SearchDCTRadiusContinuousForm(const double sigma, const int K)
		{
			this->sigma = sigma;
			this->K = K;
		}
	};

	int argminR_ContinuousForm_DCT(const double sigma, const int K, const int dcttype, const bool isGoldenSelectionSearch)
	{
		int argmin_r = 0;

		int rmin = max(K, (int)floor(1.0 * sigma));
		int rmax = (int)ceil(10.0 * sigma);
		if (rmin > rmax)return rmin;
		SearchDCTRadiusContinuousForm s(sigma, K);

		if (isGoldenSelectionSearch)
		{
			argmin_r = s.goldenSearch(rmin, rmax);
			//cout << "dct" << dcttype << ": K=" << K << ", r=" << argmin_r << ", sigma=" << argmin_r / sigma << endl;
		}
		else
		{
			argmin_r = s.linearSearch(rmin, rmax);
			//cout << "dct" << dcttype << ": K=" << K << ", r=" << argmin_r << ", sigma=" << argmin_r / sigma << endl;
		}
		return argmin_r;
	}
#pragma endregion
}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/ext/std/integer_sequence.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/foldable.hpp>

#include <type_traits>
#include <utility>
using namespace boost::hana;

using T = int;
using U = long long;

int main() {
    //////////////////////////////////////////////////////////////////////////
    // Setup for the laws below
    //////////////////////////////////////////////////////////////////////////
    auto sequences = make<Tuple>(
        std::integer_sequence<T>{},
        std::integer_sequence<U>{},

        std::integer_sequence<T, 0>{},
        std::integer_sequence<U, 0>{},

        std::integer_sequence<T, 0, 1>{},
        std::integer_sequence<U, 0, 1>{},

        std::integer_sequence<T, 1, 0, 2>{},
        std::integer_sequence<U, 1, 0, 2>{}
    );

    //////////////////////////////////////////////////////////////////////////
    // Foldable
    //////////////////////////////////////////////////////////////////////////
    {
        // unpack
        {
            test::_injection<0> f{};

            BOOST_HANA_CONSTANT_CHECK(equal(
                unpack(std::integer_sequence<T>{}, f),
                f()
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                unpack(std::integer_sequence<T, 0>{}, f),
                f(std::integral_constant<T, 0>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                unpack(std::integer_sequence<T, 0, 1>{}, f),
                f(std::integral_constant<T, 0>{}, std::integral_constant<T, 1>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                unpack(std::integer_sequence<T, 0, 1, 2>{}, f),
                f(std::integral_constant<T, 0>{}, std::integral_constant<T, 1>{},
                  std::integral_constant<T, 2>{})
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                unpack(std::integer_sequence<T, 0, 1, 2, 3>{}, f),
                f(std::integral_constant<T, 0>{}, std::integral_constant<T, 1>{},
                  std::integral_constant<T, 2>{}, std::integral_constant<T, 3>{})
            ));
        }

        // laws
        test::TestFoldable<ext::std::IntegerSequence>{sequences};
    }
}


#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <aslam/cameras/GridCalibrationTargetCirclegrid.hpp>

namespace aslam {
namespace cameras {

/// \brief Construct a calibration target
GridCalibrationTargetCirclegrid::GridCalibrationTargetCirclegrid(size_t rows, size_t cols, double spacingMeters,
                                                                 const CirclegridOptions &options)
    : GridCalibrationTargetBase(rows, cols),
      _spacing(spacingMeters),
      _options(options) {
  SM_ASSERT_GT(Exception, spacingMeters, 0.0, "spacingMetric has to be positive");

  // allocate memory for the grid points
  _points.resize(size(), 3);

  //initialize a normal grid
  createGridPoints();

  //start the output window if requested
  initialize();
}

/// \brief initialize the object
void GridCalibrationTargetCirclegrid::initialize()
{
  if (_options.showExtractionVideo) {
    cv::namedWindow("Circlegrid corners", cv::WINDOW_AUTOSIZE);
    cv::resizeWindow("Circlegrid corners", 640, 480);
    cv::startWindowThread();
  }
}

/// \brief initialize a checkerboard grid (cols*rows = (cols)*(rows) internal grid points)
void GridCalibrationTargetCirclegrid::createGridPoints() {
  for (unsigned int r = 0; r < _rows; r++)
    for (unsigned int c = 0; c < _cols; c++)
      _points.row(gridCoordinatesToPoint(r, c)) = Eigen::Matrix<double, 1, 3>(
          _spacing * r, _spacing * c, 0.0);

  if(_options.useAsymmetricCirclegrid)
  {
    //asymmetric grid
    for (unsigned int r = 0; r < _rows; r++)
      for (unsigned int c = 0; c < _cols; c++)
        _points.row(gridCoordinatesToPoint(r, c)) = Eigen::Matrix<double, 1, 3>(double((2*c + r % 2)*_spacing), double(r*_spacing), 0.0);
  } else {
    //symmetric grid
    for (unsigned int r = 0; r < _rows; r++)
      for (unsigned int c = 0; c < _cols; c++)
        _points.row(gridCoordinatesToPoint(r, c)) = Eigen::Matrix<double, 1, 3>(_spacing * r, _spacing * c, 0.0);
  }
}

/// \brief extract the calibration target points from an image and write to an observation
bool GridCalibrationTargetCirclegrid::computeObservation(const cv::Mat & image,
           Eigen::MatrixXd & outImagePoints, std::vector<bool> &outCornerObserved) const {

  // extract the circle grid corners
  cv::Size patternSize(cols(), rows());
  cv::Mat centers(size(), 2, CV_64FC1);

  bool success = false;
  if(_options.useAsymmetricCirclegrid)
    success = cv::findCirclesGrid( image, patternSize, centers, cv::CALIB_CB_ASYMMETRIC_GRID );
  else
    success = cv::findCirclesGrid( image, patternSize, centers );

  //draw corners
  if (_options.showExtractionVideo) {
    //image with refined (blue) and raw corners (red)
    cv::Mat imageCopy1 = image.clone();
    cv::cvtColor(imageCopy1, imageCopy1, cv::COLOR_GRAY2RGB);
    cv::drawChessboardCorners(imageCopy1, cv::Size(rows(), cols()), centers, true);

    // write error msg
    if (!success)
      cv::putText(imageCopy1, "Detection failed! (frame not used)",
                  cv::Point(50, 50), cv::FONT_HERSHEY_SIMPLEX, 0.8,
                  CV_RGB(255,0,0), 3, 8, false);

    cv::imshow("Circlegrid corners", imageCopy1);  // OpenCV call
    cv::waitKey(1);
  }

  //exit here if there is an error
  if (!success)
    return success;

  //set all points as observed (circlegrid is only usable in that case)
  std::vector<bool> allGood(size(), true);
  outCornerObserved = allGood;

  //convert to eigen for output
  outImagePoints.resize(size(), 2);
  for (unsigned int i = 0; i < size(); i++)
    outImagePoints.row(i) = Eigen::Matrix<double, 1, 2>(
        centers.row(i).at<float>(0), centers.row(i).at<float>(1));

  return success;
}

}  // namespace cameras
}  // namespace aslam

//export explicit instantions for all included archives
#include <sm/boost/serialization.hpp>
#include <boost/serialization/export.hpp>
BOOST_CLASS_EXPORT_IMPLEMENT(aslam::cameras::GridCalibrationTargetCirclegrid);
BOOST_CLASS_EXPORT_IMPLEMENT(aslam::cameras::GridCalibrationTargetCirclegrid::CirclegridOptions);


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_TOINTS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_TOINTS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing toints capabilities

    Convert to integer by saturated truncation.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    as_integer_t<T> r = toints(x);
    @endcode

    The code is similar to:

    @code
    as_integer_t<T> r = static_cast<as_integer_t<T> >(saturate<as_integer_t<T> >(x))
    @endcode

    @par Notes:

    - The @ref Inf, @ref Minf and @ref Nan values are treated properly and go respectively to
    @ref Valmax, @ref Valmin and @ref Zero of the destination integral type.

    - All values superior (resp.) less than @ref Valmax (resp. @ref Valmin) of the return type
    are saturated accordingly.

    - If you do not care about invalid values or overflows, toint is faster.

    @par Alias

    @c ifix,  @c itrunc

    @see  toint
  **/
  const boost::dispatch::functor<tag::toints_> toints = {};
} }
#endif

#include <boost/simd/function/scalar/toints.hpp>
#include <boost/simd/function/simd/toints.hpp>

#endif


#pragma once
#include <type_traits>

#include <boost/hana.hpp>

struct get_index_of_first_matching_t
{
	template <typename Iterable, typename T>
	constexpr auto operator()(Iterable iterable, T element) const
	{
		using boost::hana::_;

		auto size = boost::hana::size(boost::hana::drop_while(iterable, [&element](auto elem)
															  {
																  return elem != element;
															  }));

		return boost::hana::size(iterable) - size;
	}
};
constexpr get_index_of_first_matching_t get_index_of_first_matching{};

template <typename T>
constexpr auto remove_dups(T tuple)
{
	auto pred = [](auto currentTuple, auto newItem)
	{
		return boost::hana::if_(boost::hana::contains(currentTuple, newItem), currentTuple,
								boost::hana::append(currentTuple, newItem));
	};
	return decltype(boost::hana::fold(tuple, boost::hana::make_tuple(), pred)){};
}

template <typename T>
constexpr auto is_tuple(T typeToCheck = T{})
{
	return boost::hana::bool_c<boost::hana::is_a<boost::hana::tuple_tag, decltype(typeToCheck)>>;
}

namespace detail
{
template <typename Tup, typename F, typename Index>
constexpr bool for_each_index_IMPL(Tup&& tup, F&& func, Index current_index)
{
	using namespace boost::hana::literals;

	std::forward<F>(func)(std::forward<Tup>(tup)[current_index]);

	return boost::hana::if_(
		boost::hana::size(tup) == current_index, true,
		for_each_index_IMPL(std::forward<Tup>(tup), std::forward<F>(func), current_index + 1_c));
}
}

template <typename Tup, typename F>
constexpr void for_each_index(Tup&& tuple, F&& func)
{
	using namespace boost::hana::literals;

	return for_each_index_IMPL(std::forward<Tup>(tuple), std::forward<F>(func), 0_c);
}


//headers for stl
#include <sstream>
//headers for gazebo
#include <gazebo/gazebo.hh>
#include <gazebo/physics/Model.hh>
#include <gazebo/physics/Link.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/math/gzmath.hh>
//headers for sdf
#include <sdf/Param.hh>
#include <sdf/sdf.hh>
//headers fot boost
#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>
//headers for ROS
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <std_msgs/Float32.h>

namespace gazebo
{
  class simple_buoyancy_plugin : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr sdf)
    {
      // Store the pointer to the model
      this->model = _parent;
      // Listen to the update event. This event is broadcast every
      // simulation iteration.
      if(this->LoadParams(sdf,"target_link",target_link_name))
      {
        this->target_link = this->model->GetLink(target_link_name);
        nh.param<float>("water_surface_height", this->water_surface_height,1.0);
        this->LoadParams(sdf,"cob_x",this->cob_x);
        this->LoadParams(sdf,"cob_y",this->cob_y);
        this->LoadParams(sdf,"cob_z",this->cob_z);
        this->LoadParams(sdf,"bbox_x",this->bbox_x);
        this->LoadParams(sdf,"bbox_y",this->bbox_y);
        this->LoadParams(sdf,"bbox_z",this->bbox_z);
        float default_offset_value = 0;
        this->LoadParams(sdf,"force_offset_x",this->force_offset_x,default_offset_value);
        this->LoadParams(sdf,"force_offset_y",this->force_offset_y,default_offset_value);
        this->LoadParams(sdf,"force_offset_z",this->force_offset_z,default_offset_value);
        this->LoadParams(sdf,"publish_data",this->publish_data);
        std::string default_prefix = "";
        this->LoadParams(sdf,"bbox_prefix",this->bbox_prefix,default_prefix);
        this->LoadParams(sdf,"position_reference_link",this->position_reference_link_name,this->target_link_name);
        this->position_reference_link = this->model->GetLink(this->target_link_name);

        if(!this->target_link)
        {
          ROS_ERROR_STREAM("Cannot find target link!! Check your URDF and gzclient panels");
        }
        else
        {
          if(this->publish_data = true)
          {
            if(this->bbox_prefix == "")
            {
              this->bounding_box_pub = nh.advertise<visualization_msgs::Marker>("/"+target_link_name+"/bounding_box", 1);
              this->buoyancy_pub = nh.advertise<std_msgs::Float32>("/"+target_link_name+"/buoyancy", 1);
            }
            else
            {
              this->bounding_box_pub = nh.advertise<visualization_msgs::Marker>("/"+this->bbox_prefix+"/"+target_link_name+"/bounding_box", 1);
              this->buoyancy_pub = nh.advertise<std_msgs::Float32>("/"+this->bbox_prefix+"/"+target_link_name+"/buoyancy", 1);
            }
          }
          this->target_link_bounding_box = this->target_link->GetBoundingBox();
          this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&simple_buoyancy_plugin::OnUpdate, this, _1));
        }
      }
    }

    // Called by the world update start event
    public: void OnUpdate(const common::UpdateInfo & /*_info*/)
    {
      this->link_pose = this->position_reference_link->GetWorldPose();
      std_msgs::Float32 buoyancy;
      if((link_pose.pos.z+this->cob_z-this->bbox_z/2) >= this->water_surface_height)
      {
        buoyancy.data = 0;
      }
      else
      {
        buoyancy.data = (this->water_surface_height-(link_pose.pos.z+this->cob_z-this->bbox_z/2))*9.8*1000*this->bbox_x*this->bbox_y;
        //this->target_link->AddForce(math::Vector3(0, 0, buoyancy.data));
        this->target_link->AddForceAtRelativePosition(math::Vector3(0, 0, buoyancy.data),math::Vector3(this->force_offset_x,this->force_offset_y,this->force_offset_z));
      }
      if(this->publish_data == true)
      {
        this->PublishMarker();
        this->buoyancy_pub.publish(buoyancy);
      }
    }

    template <typename T>
    bool LoadParams(sdf::ElementPtr sdf,std::string key,T& param)
    {
      std::string param_str;
      if(!sdf->HasElement(key))
      {
        ROS_WARN_STREAM("failed to get " << key);
        return false;
      }
      else
      {
        param_str = sdf->GetElement(key)->GetValue()->GetAsString();
      }
      try
      {
        param = boost::lexical_cast<T>(param_str);
      }
      catch(boost::bad_lexical_cast &)
      {
        ROS_WARN_STREAM("failed to casting " << key);
      }
      return true;
    }

    template <typename T>
    bool LoadParams(sdf::ElementPtr sdf,std::string key,T& param, T default_param)
    {
      std::string param_str;
      if(!sdf->HasElement(key))
      {
        param = default_param;
        ROS_INFO_STREAM("unable to get " << key << " ,set default_value = " << default_param);
        return false;
      }
      else
      {
        param_str = sdf->GetElement(key)->GetValue()->GetAsString();
      }
      try
      {
        param = boost::lexical_cast<T>(param_str);
      }
      catch(boost::bad_lexical_cast &)
      {
        ROS_WARN_STREAM("failed to casting " << key);
        param = default_param;
      }
      return true;
    }

    private: void PublishMarker()
    {
      visualization_msgs::Marker marker;
      marker.type = marker.CUBE;
      marker.header.stamp = ros::Time::now();
      marker.header.frame_id = position_reference_link_name;
      marker.action = marker.ADD;
      marker.color.r = 0;
      marker.color.g = 1;
      marker.color.b = 0;
      marker.color.a = 0.5;
      marker.frame_locked = true;
      marker.scale.x = bbox_x;
      marker.scale.y = bbox_y;
      marker.scale.z = bbox_z;
      marker.pose.position.x = cob_x;
      marker.pose.position.y = cob_y;
      marker.pose.position.z = cob_z;
      marker.pose.orientation.x = 0;
      marker.pose.orientation.y = 0;
      marker.pose.orientation.z = 0;
      marker.pose.orientation.w = 1;
      bounding_box_pub.publish(marker);
    }

    // Pointer to the model
    private: physics::ModelPtr model;

    //Pointer to the link
    private: physics::LinkPtr target_link,position_reference_link;

    private: math::Pose link_pose;

    //bounding box of target link
    private: math::Box target_link_bounding_box;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;

    //parameters
    private: std::string position_reference_link_name,target_link_name,bbox_prefix;
    private: float water_surface_height;
    private: float cob_x,cob_y,cob_z,bbox_x,bbox_y,bbox_z;
    private: float force_offset_x,force_offset_y,force_offset_z;
    private: bool publish_data;

    //publishers
    private: ros::Publisher bounding_box_pub,buoyancy_pub;
    private: ros::NodeHandle nh;
  };

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(simple_buoyancy_plugin)
}