#pragma once

#include <ros/ros.h>

#include <memory>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/StdVector>

#include "dynamic_motion_planner/chomp_trajectory.hpp"
#include "utils/utility_functions.hpp"

namespace hdi_plan {
class ChompCost {
public:
	ChompCost(const std::shared_ptr<ChompTrajectory>& trajectory, const std::vector<double>& derivative_costs, double ridge_factor = 0.0);
	~ChompCost();

	double getMaxQuadCostInvValue() const {
		return this->quad_cost_inv_.maxCoeff();
	};
	Eigen::MatrixXd getQuadraticCostInverse() const {
		return this->quad_cost_inv_;
	}
	Eigen::MatrixXd getQuadraticCost() const {
		return this->quad_cost_;
	}
	void scale(double scale);
	double getCost(const Eigen::MatrixXd::ColXpr& joint_trajectory) const;

	Eigen::MatrixXd getDerivative(const Eigen::MatrixXd::ColXpr& joint_trajectory) const;
private:
	Eigen::MatrixXd quad_cost_full_;
	Eigen::MatrixXd quad_cost_;
	Eigen::MatrixXd quad_cost_inv_;

	Eigen::MatrixXd getDiffMatrix(int size, const double* diff_rule) const;

};
}


#define BOOST_TEST_MODULE FourierTransformTest
#include <boost/test/unit_test.hpp>

// For IO
#include <stdlib.h>
#include <math.h>
#include <iostream>
#include <unistd.h>

// For measuring elapsed time
#include <chrono>

// For Random Float Generator
#include <time.h>

// For object under test
#include "ofdmcodec.h"
#include "common.h"

#define CONFIDENCE_INTERVAL 0.0000000000001

// Integration Tests
BOOST_AUTO_TEST_SUITE(IntegrationTests)

/**
*  This test simulates entire encoding and decoding process
*  of only one symbol, this test does not transmit the data
*  through a physical medium.
*
*/
BOOST_AUTO_TEST_CASE(EncodeDecode)
{
    printf("Testing OFDM Encoder & Decoder Object...\n");
    // Initialize ofdm coder setting structs and objects
    OFDMSettings encoderSettings;
    encoderSettings.type = FFTW_BACKWARD;
    encoderSettings.EnergyDispersalSeed = 0;
    encoderSettings.nPoints = 512;
	encoderSettings.pilotToneStep = 8;
    encoderSettings.pilotToneAmplitude = 2.0;
    encoderSettings.guardInterval = 0;
    encoderSettings.QAMSize = 2;
    encoderSettings.cyclicPrefixSize = 128;

    OFDMSettings decoderSettings = encoderSettings;
    decoderSettings.type = FFTW_FORWARD;

    OFDMCodec encoder(encoderSettings);
    OFDMCodec decoder(decoderSettings);

    size_t symbolSize = (encoderSettings.nPoints*2);
    size_t symbolSizeWithPrefix = symbolSize + encoderSettings.cyclicPrefixSize;
    size_t rxSignalSize = symbolSizeWithPrefix * 10;
    size_t rxLastAllowedIndex = symbolSizeWithPrefix * 9;

    // Randomly generated prefix start position
    size_t prefixStart = rand() % rxLastAllowedIndex;
    printf("Randomly Generated Prefix Start = %lu\n",prefixStart);

    // Calculate max nBytes
    size_t nAvaiablePoints = (encoderSettings.nPoints - ((size_t)(encoderSettings.nPoints / encoderSettings.pilotToneStep)));
    size_t nBytes = (nAvaiablePoints*encoderSettings.QAMSize) / 8;

    std::cout << "Encoding nBytes = " << nBytes << std::endl;

    // Byte input & output buffers
    ByteVec txIn(nBytes);
    ByteVec rxOut(nBytes);

    // Signal buffer
    DoubleVec txData(symbolSizeWithPrefix);
    DoubleVec rxSignal(rxSignalSize);

    // Setup random byte generator
    srand( (unsigned)time( NULL ) );

    // Generate array of random bytes
    for (size_t i = 0; i < nBytes; i++)
    {
        txIn[i] = rand() % 255;
    }

    // Encode 1 ofdm symbol
    auto start = std::chrono::steady_clock::now();
    txData = encoder.Encode(txIn, nBytes);
    auto end = std::chrono::steady_clock::now();

    std::cout << "Encode elapsed time: "
    << std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count()
    << " ns" << std::endl;

    // Copy the symbol with prefix into Rx signal buffer
    std::copy(txData.begin(), txData.begin()+symbolSizeWithPrefix, rxSignal.begin()+prefixStart);

    // Check the smybol has been copied correctly
    for (size_t i = 0; i < encoderSettings.nPoints*2; i++)
    {
        //printf("Copied vs encoded sample: %lu %+9.5f vs. %+9.5f\n",
        //i, rxSignal[prefixStart+i], txData[i]);

        // Check if real and complex element match within defined precision of each other
        BOOST_CHECK_MESSAGE( (rxSignal[prefixStart+i]  == txData[i] ), "Copied Symbol differs!" );
    }

    // Decode
    start = std::chrono::steady_clock::now();
    rxOut = decoder.Decode(rxSignal, nBytes);
    end = std::chrono::steady_clock::now();

    std::cout << "Decode elapsed time: "
    << std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count()
    << " ns" << std::endl;

    // Check the input and output are within threshold
    for (size_t i = 0; i < nBytes; i++)
    {
        //printf("Recovered Sample: %lu %d vs. %d\n", i, txIn[i], rxOut[i]);

        // Check if real and complex element match within defined precision of each other
        //BOOST_CHECK_MESSAGE( (txIn[i] == rxOut[i]), "Bytes difffer!" );
    }

}
BOOST_AUTO_TEST_SUITE_END()


#ifndef INCLUDED_PDS_DENSITY_HPP
#define INCLUDED_PDS_DENSITY_HPP

#include <Danvil/Tools/FunctionCache.h>
#include <Eigen/Dense>
#include <vector>

namespace density
{

	/** Loads a density function from a file (image or tsv) */
	Eigen::MatrixXf LoadDensity(const std::string& fn);

	/** Saves a density function to a file (image or tsv) */
	void SaveDensity(const std::string& fn, const Eigen::MatrixXf& m);

	constexpr float KernelRange = 2.5f;
	constexpr float cPi = 3.141592654f;

	inline float KernelImpl(float d) {
		return std::exp(-cPi*d*d);
	}

	inline float Kernel(float d) {
		static Danvil::FunctionCache<float,1> cache(0.0f, KernelRange, &KernelImpl);
		return cache(std::abs(d));
	}

	inline float KernelSquareImpl(float d2) {
		return std::exp(-cPi*d2);
	}

	inline float KernelSquare(float d2) {
		static Danvil::FunctionCache<float,1> cache(0.0f, KernelRange*KernelRange, &KernelSquareImpl);
		return cache(d2);
	}

	/** Computes density approximation for a set of points  */
	Eigen::MatrixXf PointDensity(const std::vector<Eigen::Vector2f>& points, const Eigen::MatrixXf& density);

}

#endif


/** @file
 *****************************************************************************

 Sample parameters for the lattice-based vector encryption scheme for the
 lattice-based R1CS ppSNARG. The LWE parameters are chosen to provide 80-bits
 of security, and correctness error 2^{-40} for verifying QAPs with degree up
 to 10000 (over a finite field of size ~10000). Parameter selection based on
 the security analysis in [LP10].

 The plaintext dimension is chosen based on the number of queries needed to
 acheive soundness error 2^{-40} for the QAP-based linear PCP for verifying
 R1CS systems with up to 10000 constraints (and a field of size ~10000).

 References:

  [LP10]: Richard Lindner and Chris Peikert. Better Key Sizes (and Attacks) for
          LWE-Based Encryption. In CT-RSA, 2011.

 *****************************************************************************
 * @author     Samir Menon, Brennan Shacklett, and David J. Wu
 * @copyright  MIT license (see LICENSE file)
 *****************************************************************************/

#ifndef LWE_PARAM_HPP_
#define LWE_PARAM_HPP_

#include <math.h>
#include <stdint.h>
#include <NTL/ZZ.h>

namespace LWE {

// Lattice dimension (parameters chosen to ensure 80-bits of security)
const uint32_t n = 1455;

// Noise distribution standard deviation
const double stddev = 6.0;

// 15 queries (~ 2^-40 soundness error for circuits of size < 10000)
const uint32_t l = 15;
const uint32_t pt_dim = l*4;

// Plaintext modulus
const uint64_t p_int = 65537;
const NTL::ZZ p(p_int);

// Ciphertext modulus
const NTL::ZZ q(1ul << 58);
}

#endif // LWE_PARAM_HPP_


#include <learning/independences/discrete/chi_square.hpp>
#include <factors/discrete/discrete_indices.hpp>
#include <boost/math/distributions/chi_squared.hpp>

namespace learning::independences::discrete {

double ChiSquare::pvalue(const std::string& v1, const std::string& v2) const {
    std::vector<std::string> dummy_v2{v2};
    auto [cardinality, strides] = factors::discrete::create_cardinality_strides(m_df, v1, dummy_v2);
    auto joint_counts = factors::discrete::joint_counts(m_df, v1, dummy_v2, cardinality, strides);

    auto v1_marg = factors::discrete::marginal_counts(joint_counts, 0, cardinality, strides);
    auto v2_marg = factors::discrete::marginal_counts(joint_counts, 1, cardinality, strides);

    auto inv_obs = 1. / joint_counts.sum();

    double statistic = 0;
    for (int i = 0; i < cardinality(0); ++i) {
        for (int j = 0; j < cardinality(1); ++j) {
            auto expected = static_cast<double>(v1_marg(i) * v2_marg(j)) * inv_obs;

            if (expected != 0) {
                auto index = i + j * strides(1);

                auto d = joint_counts(index) - expected;
                statistic += d * d / expected;
            }
        }
    }

    auto df = (cardinality(0) - 1) * (cardinality(1) - 1);

    boost::math::chi_squared_distribution chidist(static_cast<double>(df));
    return cdf(complement(chidist, statistic));
}

double ChiSquare::pvalue(const std::string& v1, const std::string& v2, const std::string& ev) const {
    std::vector<std::string> dummy_vars{v2, ev};
    auto [cardinality, strides] = factors::discrete::create_cardinality_strides(m_df, v1, dummy_vars);
    auto joint_counts = factors::discrete::joint_counts(m_df, v1, dummy_vars, cardinality, strides);

    auto evidence_marg = factors::discrete::marginal_counts(joint_counts, 2, cardinality, strides);

    auto evidence_configurations = cardinality(2);
    auto vars_configurations = strides(2);

    double statistic = 0;

    for (auto k = 0; k < evidence_configurations; ++k) {
        if (evidence_marg(k) == 0) continue;

        auto offset = k * vars_configurations;
        auto evidence_segment = joint_counts.segment(offset, vars_configurations);

        auto v1_marg = factors::discrete::marginal_counts(evidence_segment, 0, cardinality, strides);
        auto v2_marg = factors::discrete::marginal_counts(evidence_segment, 1, cardinality, strides);

        auto inv_obs = 1. / evidence_marg(k);

        for (int i = 0; i < cardinality(0); ++i) {
            for (int j = 0; j < cardinality(1); ++j) {
                auto expected = static_cast<double>(v1_marg(i) * v2_marg(j)) * inv_obs;

                if (expected != 0) {
                    auto index = offset + i + j * strides(1);

                    auto d = joint_counts(index) - expected;
                    statistic += d * d / expected;
                }
            }
        }
    }

    auto df = (cardinality(0) - 1) * (cardinality(1) - 1) * cardinality(2);

    boost::math::chi_squared_distribution chidist(static_cast<double>(df));
    return cdf(complement(chidist, statistic));
}

double ChiSquare::pvalue(const std::string& v1, const std::string& v2, const std::vector<std::string>& ev) const {
    std::vector<std::string> dummy_vars{v2};
    dummy_vars.reserve(ev.size() + 1);
    dummy_vars.insert(dummy_vars.end(), ev.begin(), ev.end());

    auto [cardinality, strides] = factors::discrete::create_cardinality_strides(m_df, v1, dummy_vars);
    auto joint_counts = factors::discrete::joint_counts(m_df, v1, dummy_vars, cardinality, strides);

    auto evidence_configurations = cardinality.tail(ev.size()).prod();
    auto vars_configurations = cardinality(0) * cardinality(1);

    double statistic = 0;

    for (auto k = 0; k < evidence_configurations; ++k) {
        auto offset = k * vars_configurations;

        int total_sum = 0;
        auto marginal_v1 = VectorXi::Zero(cardinality(0)).eval();
        auto marginal_v2 = VectorXi::Zero(cardinality(1)).eval();

        for (auto i = 0; i < cardinality(0); ++i) {
            for (auto j = 0; j < cardinality(1); ++j) {
                auto c = joint_counts(offset + i + j * strides(1));
                marginal_v1(i) += c;
                marginal_v2(j) += c;
                total_sum += c;
            }
        }

        if (total_sum == 0) continue;

        auto inv_obs = 1. / static_cast<double>(total_sum);

        for (auto i = 0; i < cardinality(0); ++i) {
            for (auto j = 0; j < cardinality(1); ++j) {
                auto expected = static_cast<double>(marginal_v1(i) * marginal_v2(j)) * inv_obs;

                if (expected != 0) {
                    auto c = joint_counts(offset + i + j * strides(1));
                    auto d = c - expected;

                    statistic += d * d / expected;
                }
            }
        }
    }

    auto df = (cardinality(0) - 1) * (cardinality(1) - 1) * evidence_configurations;

    boost::math::chi_squared_distribution chidist(static_cast<double>(df));
    return cdf(complement(chidist, statistic));
}

}  // namespace learning::independences::discrete


//
// Copyright (c) 2019 INRIA
//

#include <pinocchio/math/quaternion.hpp>
#include <pinocchio/spatial/se3.hpp>

#include <boost/variant.hpp> // to avoid C99 warnings

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_assignQuaternion)
{
  using namespace pinocchio;
  const int max_tests = 1e5;
  for(int k = 0; k < max_tests; ++k)
  {
    const SE3 M(SE3::Random());
    SE3::Quaternion quat_ref(M.rotation());

    SE3::Quaternion quat;
    quaternion::assignQuaternion(quat,M.rotation());

    BOOST_CHECK(quat.coeffs().isApprox(quat_ref.coeffs()));
  }
}

BOOST_AUTO_TEST_CASE(test_uniformRandom)
{
  srand(0);

  using namespace pinocchio;
  Eigen::Quaternion<double> q;

  for (int i = 0; i < (1 << 10); ++i) {
    quaternion::uniformRandom(q);
    BOOST_CHECK_MESSAGE((q.coeffs().array().abs() <= 1).all(),
        "Quaternion coeffs out of bounds: " << i << ' ' << q.coeffs().transpose());
  }
}

BOOST_AUTO_TEST_SUITE_END()


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2014 Anton Bikineev
//  Copyright 2014 Christopher Kormanyos
//  Copyright 2014 John Maddock
//  Copyright 2014 Paul Bristow
//  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)
//
#ifndef BOOST_MATH_DETAIL_HYPERGEOMETRIC_SERIES_HPP
#define BOOST_MATH_DETAIL_HYPERGEOMETRIC_SERIES_HPP

#include <cmath>
#include <cstdint>
#include <boost/math/tools/series.hpp>
#include <boost/math/special_functions/trunc.hpp>
#include <boost/math/policies/error_handling.hpp>

  namespace boost { namespace math { namespace detail {

  // primary template for term of Taylor series
  template <class T, unsigned p, unsigned q>
  struct hypergeometric_pFq_generic_series_term;

  // partial specialization for 0F1
  template <class T>
  struct hypergeometric_pFq_generic_series_term<T, 0u, 1u>
  {
    typedef T result_type;

    hypergeometric_pFq_generic_series_term(const T& b, const T& z)
       : n(0), term(1), b(b), z(z)
    {
    }

    T operator()()
    {
      BOOST_MATH_STD_USING
      const T r = term;
      term *= ((1 / ((b + n) * (n + 1))) * z);
      ++n;
      return r;
    }

  private:
    unsigned n;
    T term;
    const T b, z;
  };

  // partial specialization for 1F0
  template <class T>
  struct hypergeometric_pFq_generic_series_term<T, 1u, 0u>
  {
    typedef T result_type;

    hypergeometric_pFq_generic_series_term(const T& a, const T& z)
       : n(0), term(1), a(a), z(z)
    {
    }

    T operator()()
    {
      BOOST_MATH_STD_USING
      const T r = term;
      term *= (((a + n) / (n + 1)) * z);
      ++n;
      return r;
    }

  private:
    unsigned n;
    T term;
    const T a, z;
  };

  // partial specialization for 1F1
  template <class T>
  struct hypergeometric_pFq_generic_series_term<T, 1u, 1u>
  {
    typedef T result_type;

    hypergeometric_pFq_generic_series_term(const T& a, const T& b, const T& z)
       : n(0), term(1), a(a), b(b), z(z)
    {
    }

    T operator()()
    {
      BOOST_MATH_STD_USING
      const T r = term;
      term *= (((a + n) / ((b + n) * (n + 1))) * z);
      ++n;
      return r;
    }

  private:
    unsigned n;
    T term;
    const T a, b, z;
  };

  // partial specialization for 1F2
  template <class T>
  struct hypergeometric_pFq_generic_series_term<T, 1u, 2u>
  {
    typedef T result_type;

    hypergeometric_pFq_generic_series_term(const T& a, const T& b1, const T& b2, const T& z)
       : n(0), term(1), a(a), b1(b1), b2(b2), z(z)
    {
    }

    T operator()()
    {
      BOOST_MATH_STD_USING
      const T r = term;
      term *= (((a + n) / ((b1 + n) * (b2 + n) * (n + 1))) * z);
      ++n;
      return r;
    }

  private:
    unsigned n;
    T term;
    const T a, b1, b2, z;
  };

  // partial specialization for 2F0
  template <class T>
  struct hypergeometric_pFq_generic_series_term<T, 2u, 0u>
  {
    typedef T result_type;

    hypergeometric_pFq_generic_series_term(const T& a1, const T& a2, const T& z)
       : n(0), term(1), a1(a1), a2(a2), z(z)
    {
    }

    T operator()()
    {
      BOOST_MATH_STD_USING
      const T r = term;
      term *= (((a1 + n) * (a2 + n) / (n + 1)) * z);
      ++n;
      return r;
    }

  private:
    unsigned n;
    T term;
    const T a1, a2, z;
  };

  // partial specialization for 2F1
  template <class T>
  struct hypergeometric_pFq_generic_series_term<T, 2u, 1u>
  {
    typedef T result_type;

    hypergeometric_pFq_generic_series_term(const T& a1, const T& a2, const T& b, const T& z)
       : n(0), term(1), a1(a1), a2(a2), b(b), z(z)
    {
    }

    T operator()()
    {
      BOOST_MATH_STD_USING
      const T r = term;
      term *= (((a1 + n) * (a2 + n) / ((b + n) * (n + 1))) * z);
      ++n;
      return r;
    }

  private:
    unsigned n;
    T term;
    const T a1, a2, b, z;
  };

  // we don't need to define extra check and make a polinom from
  // series, when p(i) and q(i) are negative integers and p(i) >= q(i)
  // as described in functions.wolfram.alpha, because we always
  // stop summation when result (in this case numerator) is zero.
  template <class T, unsigned p, unsigned q, class Policy>
  inline T sum_pFq_series(detail::hypergeometric_pFq_generic_series_term<T, p, q>& term, const Policy& pol)
  {
    BOOST_MATH_STD_USING
    std::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();

    const T result = boost::math::tools::sum_series(term, boost::math::policies::get_epsilon<T, Policy>(), max_iter);

    policies::check_series_iterations<T>("boost::math::hypergeometric_pFq_generic_series<%1%>(%1%,%1%,%1%)", max_iter, pol);
    return result;
  }

  template <class T, class Policy>
  inline T hypergeometric_0F1_generic_series(const T& b, const T& z, const Policy& pol)
  {
    detail::hypergeometric_pFq_generic_series_term<T, 0u, 1u> s(b, z);
    return detail::sum_pFq_series(s, pol);
  }

  template <class T, class Policy>
  inline T hypergeometric_1F0_generic_series(const T& a, const T& z, const Policy& pol)
  {
    detail::hypergeometric_pFq_generic_series_term<T, 1u, 0u> s(a, z);
    return detail::sum_pFq_series(s, pol);
  }

  template <class T, class Policy>
  inline T log_pochhammer(T z, unsigned n, const Policy pol, int* s = 0)
  {
     BOOST_MATH_STD_USING
#if 0
     if (z < 0)
     {
        if (n < -z)
        {
           if(s)
            *s = (n & 1 ? -1 : 1);
           return log_pochhammer(T(-z + (1 - (int)n)), n, pol);
        }
        else
        {
           int cross = itrunc(ceil(-z));
           return log_pochhammer(T(-z + (1 - cross)), cross, pol, s) + log_pochhammer(T(cross + z), n - cross, pol);
        }
     }
     else
#endif
     {
        if (z + n < 0)
        {
           T r = log_pochhammer(T(-z - n + 1), n, pol, s);
           if (s)
              *s *= (n & 1 ? -1 : 1);
           return r;
        }
        int s1, s2;
        T r = boost::math::lgamma(T(z + n), &s1, pol) - boost::math::lgamma(z, &s2, pol);
        if(s)
           *s = s1 * s2;
        return r;
     }
  }

  template <class T, class Policy>
  inline T hypergeometric_1F1_generic_series(const T& a, const T& b, const T& z, const Policy& pol, long long& log_scaling, const char* function)
  {
     BOOST_MATH_STD_USING
     T sum(0), term(1), upper_limit(sqrt(boost::math::tools::max_value<T>())), diff;
     T lower_limit(1 / upper_limit);
     unsigned n = 0;
     long long log_scaling_factor = lltrunc(boost::math::tools::log_max_value<T>()) - 2;
     T scaling_factor = exp(T(log_scaling_factor));
     T term_m1 = 0;
     long long local_scaling = 0;
     //
     // When a is very small, then (a+n)/n => 1 faster than
     // z / (b+n) => 1, as a result the series starts off
     // converging, then at some unspecified time very gradually
     // starts to diverge, potentially resulting in some very large
     // values being missed.  As a result we need a check for small
     // a in the convergence criteria.  Note that this issue occurs
     // even when all the terms are positive.
     //
     bool small_a = fabs(a) < 0.25;

     unsigned summit_location = 0;
     bool have_minima = false;
     T sq = 4 * a * z + b * b - 2 * b * z + z * z;
     if (sq >= 0)
     {
        T t = (-sqrt(sq) - b + z) / 2;
        if (t > 1)  // Don't worry about a minima between 0 and 1.
           have_minima = true;
        t = (sqrt(sq) - b + z) / 2;
        if (t > 0)
           summit_location = itrunc(t);
     }

     if (summit_location > boost::math::policies::get_max_series_iterations<Policy>() / 4)
     {
        //
        // Skip forward to the location of the largest term in the series and
        // evaluate outwards from there:
        //
        int s1, s2;
        term = log_pochhammer(a, summit_location, pol, &s1) + summit_location * log(z) - log_pochhammer(b, summit_location, pol, &s2) - lgamma(T(summit_location + 1), pol);
        //std::cout << term << " " << log_pochhammer(boost::multiprecision::mpfr_float(a), summit_location, pol, &s1) + summit_location * log(boost::multiprecision::mpfr_float(z)) - log_pochhammer(boost::multiprecision::mpfr_float(b), summit_location, pol, &s2) - lgamma(boost::multiprecision::mpfr_float(summit_location + 1), pol) << std::endl;
        local_scaling = lltrunc(term);
        log_scaling += local_scaling;
        term = s1 * s2 * exp(term - local_scaling);
        //std::cout << term << " " << exp(log_pochhammer(boost::multiprecision::mpfr_float(a), summit_location, pol, &s1) + summit_location * log(boost::multiprecision::mpfr_float(z)) - log_pochhammer(boost::multiprecision::mpfr_float(b), summit_location, pol, &s2) - lgamma(boost::multiprecision::mpfr_float(summit_location + 1), pol) - local_scaling) << std::endl;
        n = summit_location;
     }
     else
        summit_location = 0;

     T saved_term = term;
     long long saved_scale = local_scaling;

     do
     {
        sum += term;
        //std::cout << n << " " << term * exp(boost::multiprecision::mpfr_float(local_scaling)) << " " << rising_factorial(boost::multiprecision::mpfr_float(a), n) * pow(boost::multiprecision::mpfr_float(z), n) / (rising_factorial(boost::multiprecision::mpfr_float(b), n) * factorial<boost::multiprecision::mpfr_float>(n)) << std::endl;
        if (fabs(sum) >= upper_limit)
        {
           sum /= scaling_factor;
           term /= scaling_factor;
           log_scaling += log_scaling_factor;
           local_scaling += log_scaling_factor;
        }
        if (fabs(sum) < lower_limit)
        {
           sum *= scaling_factor;
           term *= scaling_factor;
           log_scaling -= log_scaling_factor;
           local_scaling -= log_scaling_factor;
        }
        term_m1 = term;
        term *= (((a + n) / ((b + n) * (n + 1))) * z);
        if (n - summit_location > boost::math::policies::get_max_series_iterations<Policy>())
           return boost::math::policies::raise_evaluation_error(function, "Series did not converge, best value is %1%", sum, pol);
        ++n;
        diff = fabs(term / sum);
     } while ((diff > boost::math::policies::get_epsilon<T, Policy>()) || (fabs(term_m1) < fabs(term)) || (small_a && n < 10));

     //
     // See if we need to go backwards as well:
     //
     if (summit_location)
     {
        //
        // Backup state:
        //
        term = saved_term * exp(T(local_scaling - saved_scale));
        n = summit_location;
        term *= (b + (n - 1)) * n / ((a + (n - 1)) * z);
        --n;

        do
        {
           sum += term;
           //std::cout << n << " " << term * exp(boost::multiprecision::mpfr_float(local_scaling)) << " " << rising_factorial(boost::multiprecision::mpfr_float(a), n) * pow(boost::multiprecision::mpfr_float(z), n) / (rising_factorial(boost::multiprecision::mpfr_float(b), n) * factorial<boost::multiprecision::mpfr_float>(n)) << std::endl;
           if (n == 0)
              break;
           if (fabs(sum) >= upper_limit)
           {
              sum /= scaling_factor;
              term /= scaling_factor;
              log_scaling += log_scaling_factor;
              local_scaling += log_scaling_factor;
           }
           if (fabs(sum) < lower_limit)
           {
              sum *= scaling_factor;
              term *= scaling_factor;
              log_scaling -= log_scaling_factor;
              local_scaling -= log_scaling_factor;
           }
           term_m1 = term;
           term *= (b + (n - 1)) * n / ((a + (n - 1)) * z);
           if (summit_location - n > boost::math::policies::get_max_series_iterations<Policy>())
              return boost::math::policies::raise_evaluation_error(function, "Series did not converge, best value is %1%", sum, pol);
           --n;
           diff = fabs(term / sum);
        } while ((diff > boost::math::policies::get_epsilon<T, Policy>()) || (fabs(term_m1) < fabs(term)));
     }

     if (have_minima && n && summit_location)
     {
        //
        // There are a few terms starting at n == 0 which
        // haven't been accounted for yet...
        //
        unsigned backstop = n;
        n = 0;
        term = exp(T(-local_scaling));
        do
        {
           sum += term;
           //std::cout << n << " " << term << " " << sum << std::endl;
           if (fabs(sum) >= upper_limit)
           {
              sum /= scaling_factor;
              term /= scaling_factor;
              log_scaling += log_scaling_factor;
           }
           if (fabs(sum) < lower_limit)
           {
              sum *= scaling_factor;
              term *= scaling_factor;
              log_scaling -= log_scaling_factor;
           }
           //term_m1 = term;
           term *= (((a + n) / ((b + n) * (n + 1))) * z);
           if (n > boost::math::policies::get_max_series_iterations<Policy>())
              return boost::math::policies::raise_evaluation_error(function, "Series did not converge, best value is %1%", sum, pol);
           if (++n == backstop)
              break; // we've caught up with ourselves.
           diff = fabs(term / sum);
        } while ((diff > boost::math::policies::get_epsilon<T, Policy>())/* || (fabs(term_m1) < fabs(term))*/);
     }
     //std::cout << sum << std::endl;
     return sum;
  }

  template <class T, class Policy>
  inline T hypergeometric_1F2_generic_series(const T& a, const T& b1, const T& b2, const T& z, const Policy& pol)
  {
    detail::hypergeometric_pFq_generic_series_term<T, 1u, 2u> s(a, b1, b2, z);
    return detail::sum_pFq_series(s, pol);
  }

  template <class T, class Policy>
  inline T hypergeometric_2F0_generic_series(const T& a1, const T& a2, const T& z, const Policy& pol)
  {
    detail::hypergeometric_pFq_generic_series_term<T, 2u, 0u> s(a1, a2, z);
    return detail::sum_pFq_series(s, pol);
  }

  template <class T, class Policy>
  inline T hypergeometric_2F1_generic_series(const T& a1, const T& a2, const T& b, const T& z, const Policy& pol)
  {
    detail::hypergeometric_pFq_generic_series_term<T, 2u, 1u> s(a1, a2, b, z);
    return detail::sum_pFq_series(s, pol);
  }

  } } } // namespaces

#endif // BOOST_MATH_DETAIL_HYPERGEOMETRIC_SERIES_HPP


#pragma once

#include <cmath>

#include <ros/ros.h>
#include <ct/optcon/optcon.h>
#include <lqr_controller/declarations_euler.hpp>
#include <nav_msgs/Odometry.h>
#include <Eigen/Geometry>

namespace LQR {
class LQR_Solver {
  public:
    /*!
     * Constructor.
     * @param nodeHandle the ROS node handle.
     */
    LQR_Solver(ros::NodeHandle& nodeHandle);

    /*!
     * Destructor.
     */
    virtual ~LQR_Solver();

    control_vector_t output;

    ct::core::FeedbackMatrix<nStates, nControls> K_;
    control_vector_t uref_;
    state_vector_t xerror;

   private:

    /*!
     * ROS topic callback method.
     * @param message the received message.
     */
    void topicCallback(const nav_msgs::Odometry::ConstPtr& msg);

    //! ROS node handle.
    ros::NodeHandle& nodeHandle_;

    //! ROS topic subscriber.
    ros::Subscriber subscriber_;

    //! ROS topic name to subscribe to.
    std::string subscriberTopic_;

    //! State and control matrix dimensions
    const size_t state_dim = nStates;
    const size_t control_dim = nControls;

    control_vector_t u_;

    state_matrix_t A_;
    control_gain_matrix_t B_;
    ros::Time callBack_;
    state_vector_t x_;
    state_vector_t xref_;

    ct::optcon::TermQuadratic<nStates, nControls> quadraticCost_;
    ct::optcon::TermQuadratic<nStates, nControls>::state_matrix_t Q_;
    ct::optcon::TermQuadratic<nStates, nControls>::control_matrix_t R_;
    ct::optcon::LQR<nStates, nControls> lqrSolver_;

    state_matrix_t A_quadrotor(const state_vector_t& x, const control_vector_t& u);
    control_gain_matrix_t B_quadrotor(const state_vector_t& x, const control_vector_t& u);
  };

} /* namespace */


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    long long int n, C; cin >> n >> C;
    vector<pair<long long int, long long int>> v;
    for (int i = 0; i < n; i++) {
        long long int a, b, c; cin >> a >> b >> c;
        v.emplace_back(a - 1, c), v.emplace_back(b, -c);
    }
    sort(v.begin(), v.end());
    cpp_int ans = 0;
    long long int p = 0, t = 0;
    for (auto [x, y] : v) {
        if (x != t) ans += min(C, p) * (x - t), t = x;
        p += y;
    }
    cout << ans << endl;
}


#include <igl/barycenter.h>
#include <igl/boundary_facets.h>
#include <igl/parula.h>
#include <igl/readMESH.h>
#include <igl/slice.h>
#include <igl/slice_tets.h>
#include <igl/winding_number.h>
#include <igl/opengl/glfw/Viewer.h>
#include <Eigen/Sparse>
#include <iostream>

#include "tutorial_shared_path.h"

Eigen::MatrixXd V,BC;
Eigen::VectorXd W;
Eigen::MatrixXi T,F,G;
double slice_z = 0.5;
enum OverLayType
{
  OVERLAY_NONE = 0,
  OVERLAY_INPUT = 1,
  OVERLAY_OUTPUT = 2,
  NUM_OVERLAY = 3,
} overlay = OVERLAY_NONE;

void update_visualization(igl::opengl::glfw::Viewer & viewer)
{
  using namespace Eigen;
  using namespace std;
  Eigen::Vector4d plane(
    0,0,1,-((1-slice_z)*V.col(2).minCoeff()+slice_z*V.col(2).maxCoeff()));
  MatrixXd V_vis;
  MatrixXi F_vis;
  VectorXi J;
  {
    SparseMatrix<double> bary;
    // Value of plane's implicit function at all vertices
    const VectorXd IV =
      (V.col(0)*plane(0) +
        V.col(1)*plane(1) +
        V.col(2)*plane(2)).array()
      + plane(3);
    igl::slice_tets(V,T,IV,V_vis,F_vis,J,bary);
  }
  VectorXd W_vis;
  igl::slice(W,J,W_vis);
  MatrixXd C_vis;
  // color without normalizing
  igl::parula(W_vis,false,C_vis);

  const auto & append_mesh = [&C_vis,&F_vis,&V_vis](
    const Eigen::MatrixXd & V,
    const Eigen::MatrixXi & F,
    const RowVector3d & color)
  {
    F_vis.conservativeResize(F_vis.rows()+F.rows(),3);
    F_vis.bottomRows(F.rows()) = F.array()+V_vis.rows();
    V_vis.conservativeResize(V_vis.rows()+V.rows(),3);
    V_vis.bottomRows(V.rows()) = V;
    C_vis.conservativeResize(C_vis.rows()+F.rows(),3);
    C_vis.bottomRows(F.rows()).rowwise() = color;
  };
  switch(overlay)
  {
    case OVERLAY_INPUT:
      append_mesh(V,F,RowVector3d(1.,0.894,0.227));
      break;
    case OVERLAY_OUTPUT:
      append_mesh(V,G,RowVector3d(0.8,0.8,0.8));
      break;
    default:
      break;
  }
  viewer.data().clear();
  viewer.data().set_mesh(V_vis,F_vis);
  viewer.data().set_colors(C_vis);
  viewer.data().set_face_based(true);
}

bool key_down(igl::opengl::glfw::Viewer& viewer, unsigned char key, int mod)
{
  switch(key)
  {
    default:
      return false;
    case ' ':
      overlay = (OverLayType)((1+(int)overlay)%NUM_OVERLAY);
      break;
    case '.':
      slice_z = std::min(slice_z+0.01,0.99);
      break;
    case ',':
      slice_z = std::max(slice_z-0.01,0.01);
      break;
  }
  update_visualization(viewer);
  return true;
}

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;

  cout<<"Usage:"<<endl;
  cout<<"[space]  toggle showing input mesh, output mesh or slice "<<endl;
  cout<<"         through tet-mesh of convex hull."<<endl;
  cout<<"'.'/','  push back/pull forward slicing plane."<<endl;
  cout<<endl;

  // Load mesh: (V,T) tet-mesh of convex hull, F contains facets of input
  // surface mesh _after_ self-intersection resolution
  igl::readMESH(TUTORIAL_SHARED_PATH "/big-sigcat.mesh",V,T,F);

  // Compute barycenters of all tets
  igl::barycenter(V,T,BC);

  // Compute generalized winding number at all barycenters
  cout<<"Computing winding number over all "<<T.rows()<<" tets..."<<endl;
  igl::winding_number(V,F,BC,W);

  // Extract interior tets
  MatrixXi CT((W.array()>0.5).count(),4);
  {
    size_t k = 0;
    for(size_t t = 0;t<T.rows();t++)
    {
      if(W(t)>0.5)
      {
        CT.row(k) = T.row(t);
        k++;
      }
    }
  }
  // find bounary facets of interior tets
  igl::boundary_facets(CT,G);
  // boundary_facets seems to be reversed...
  G = G.rowwise().reverse().eval();

  // normalize
  W = (W.array() - W.minCoeff())/(W.maxCoeff()-W.minCoeff());

  // Plot the generated mesh
  igl::opengl::glfw::Viewer viewer;
  update_visualization(viewer);
  viewer.callback_key_down = &key_down;
  viewer.launch();
}


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file qle/math/stabilisedglls.hpp
    \brief Numerically stabilised general linear least squares
    \ingroup math
*/

#ifndef quantext_stabilised_glls_hpp
#define quantext_stabilised_glls_hpp

#include <ql/math/array.hpp>
#include <ql/math/comparison.hpp>
#include <ql/math/generallinearleastsquares.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/make_shared.hpp>
#include <boost/type_traits.hpp>

#include <vector>

namespace QuantExt {
using namespace QuantLib;
using namespace boost::accumulators;

//! Numerically stabilised general linear least squares
/*! The input data is lineaerly transformed before performing the linear least squares fit.
  The linear least squares fit on the transformed data is done using the
  GeneralLinearLeastSquares class.
    \ingroup math
 */

class StabilisedGLLS {
public:
    enum Method {
        None,      // No stabilisation
        MaxAbs,    // Divide x and y values by max of abs of values (per x coordinate, y)
        MeanStdDev // Subtract mean and divide by std dev (per x coordinate, y)
    };
    template <class xContainer, class yContainer, class vContainer>
    StabilisedGLLS(const xContainer& x, const yContainer& y, const vContainer& v, const Method method = MeanStdDev);

    const Array& transformedCoefficients() const { return glls_->coefficients(); }
    const Array& transformedResiduals() const { return glls_->residuals(); }
    const Array& transformedStandardErrors() const { return glls_->standardErrors(); }
    const Array& transformedError() const { return glls_->error(); }

    //! Transformation parameters (u => (u + shift) * multiplier for u = x, y)
    const Array& xMultiplier() const { return xMultiplier_; }
    const Array& xShift() const { return xShift_; }
    const Real yMultiplier() const { return yMultiplier_; }
    const Real yShift() const { return yShift_; }

    Size size() const { return glls_->residuals().size(); }
    Size dim() const { return glls_->coefficients().size(); }

    //! evaluate regression function in terms of original x, y
    template <class xType, class vContainer>
    Real eval(xType x, vContainer& v, typename boost::enable_if<typename boost::is_arithmetic<xType>::type>::type* = 0);

    //! evaluate regression function in terms of original x, y
    template <class xType, class vContainer>
    Real eval(xType x, vContainer& v,
              typename boost::disable_if<typename boost::is_arithmetic<xType>::type>::type* = 0);

protected:
    Array a_, err_, residuals_, standardErrors_, xMultiplier_, xShift_;
    Real yMultiplier_, yShift_;
    Method method_;
    boost::shared_ptr<GeneralLinearLeastSquares> glls_;

    template <class xContainer, class yContainer, class vContainer>
    void calculate(
        xContainer x, yContainer y, vContainer v,
        typename boost::enable_if<typename boost::is_arithmetic<typename xContainer::value_type>::type>::type* = 0);

    template <class xContainer, class yContainer, class vContainer>
    void calculate(
        xContainer x, yContainer y, vContainer v,
        typename boost::disable_if<typename boost::is_arithmetic<typename xContainer::value_type>::type>::type* = 0);
};

template <class xContainer, class yContainer, class vContainer>
inline StabilisedGLLS::StabilisedGLLS(const xContainer& x, const yContainer& y, const vContainer& v,
                                      const Method method)
    : a_(v.end() - v.begin(), 0.0), err_(v.end() - v.begin(), 0.0), residuals_(y.end() - y.begin()),
      standardErrors_(v.end() - v.begin()), method_(method) {
    calculate(x, y, v);
}

template <class xContainer, class yContainer, class vContainer>
void StabilisedGLLS::calculate(
    xContainer x, yContainer y, vContainer v,
    typename boost::enable_if<typename boost::is_arithmetic<typename xContainer::value_type>::type>::type*) {

    std::vector<Real> xData(x.end() - x.begin(), 0.0), yData(y.end() - y.begin(), 0.0);
    xMultiplier_ = Array(1, 1.0);
    xShift_ = Array(1, 0.0);
    yMultiplier_ = 1.0;
    yShift_ = 0.0;

    switch (method_) {
    case None:
        break;
    case MaxAbs: {
        Real mx = 0.0, my = 0.0;
        for (Size i = 0; i < static_cast<Size>(x.end() - x.begin()); ++i) {
            mx = std::max(std::abs(x[i]), mx);
        }
        if (!close_enough(mx, 0.0))
            xMultiplier_[0] = 1.0 / mx;
        for (Size i = 0; i < static_cast<Size>(y.end() - y.begin()); ++i) {
            my = std::max(std::abs(y[i]), my);
        }
        if (!close_enough(my, 0.0))
            yMultiplier_ = 1.0 / my;
        break;
    }
    case MeanStdDev: {
        accumulator_set<Real, stats<tag::mean, tag::variance> > acc;
        for (Size i = 0; i < static_cast<Size>(x.end() - x.begin()); ++i) {
            acc(x[i]);
        }
        xShift_[0] = -mean(acc);
        Real tmp = variance(acc);
        if (!close_enough(tmp, 0.0))
            xMultiplier_[0] = 1.0 / std::sqrt(tmp);
        accumulator_set<Real, stats<tag::mean, tag::variance> > acc2;
        for (Size i = 0; i < static_cast<Size>(y.end() - y.begin()); ++i) {
            acc2(y[i]);
        }
        yShift_ = -mean(acc2);
        Real tmp2 = variance(acc2);
        if (!close_enough(tmp2, 0.0))
            yMultiplier_ = 1.0 / std::sqrt(tmp2);
        break;
    }
    default:
        QL_FAIL("unknown stabilisation method");
    }

    for (Size i = 0; i < static_cast<Size>(x.end() - x.begin()); ++i) {
        xData[i] = (x[i] + xShift_[0]) * xMultiplier_[0];
    }
    for (Size i = 0; i < static_cast<Size>(y.end() - y.begin()); ++i) {
        yData[i] = (y[i] + yShift_) * yMultiplier_;
    }

    glls_ = boost::make_shared<GeneralLinearLeastSquares>(xData, yData, v);
}

template <class xContainer, class yContainer, class vContainer>
void StabilisedGLLS::calculate(
    xContainer x, yContainer y, vContainer v,
    typename boost::disable_if<typename boost::is_arithmetic<typename xContainer::value_type>::type>::type*) {

    QL_REQUIRE(x.end() - x.begin() > 0, "StabilisedGLLS::calculate(): x container is empty");
    QL_REQUIRE(x[0].end() - x[0].begin() > 0, "StabilisedGLLS:calculate(): x contains empty point(s)");

    std::vector<Array> xData(x.end() - x.begin(), Array(x[0].end() - x[0].begin(), 0.0));
    std::vector<Real> yData(y.end() - y.begin(), 0.0);
    xMultiplier_ = Array(x[0].end() - x[0].begin(), 1.0);
    xShift_ = Array(x[0].end() - x[0].begin(), 0.0);
    yMultiplier_ = 1.0;
    yShift_ = 0.0;

    switch (method_) {
    case None:
        break;
    case MaxAbs: {
        Array m(x[0].end() - x[0].begin(), 0.0);
        Real my = 0.0;
        for (Size i = 0; i < static_cast<Size>(x.end() - x.begin()); ++i) {
            for (Size j = 0; j < m.size(); ++j) {
                m[j] = std::max(std::abs(x[i][j]), m[j]);
            }
        }
        for (Size j = 0; j < m.size(); ++j) {
            if (!close_enough(m[j], 0.0))
                xMultiplier_[j] = 1.0 / m[j];
        }
        for (Size i = 0; i < static_cast<Size>(y.end() - y.begin()); ++i) {
            my = std::max(std::abs(y[i]), my);
        }
        if (!close_enough(my, 0.0))
            yMultiplier_ = 1.0 / my;
        break;
    }
    case MeanStdDev: {
        std::vector<accumulator_set<Real, stats<tag::mean, tag::variance> > > acc(x[0].end() - x[0].begin());
        for (Size i = 0; i < static_cast<Size>(x.end() - x.begin()); ++i) {
            for (Size j = 0; j < acc.size(); ++j) {
                acc[j](x[i][j]);
            }
        }
        for (Size j = 0; j < acc.size(); ++j) {
            xShift_[j] = -mean(acc[j]);
            Real tmp = variance(acc[j]);
            if (!close_enough(tmp, 0.0))
                xMultiplier_[j] = 1.0 / std::sqrt(tmp);
        }
        accumulator_set<Real, stats<tag::mean, tag::variance> > acc2;
        for (Size i = 0; i < static_cast<Size>(y.end() - y.begin()); ++i) {
            acc2(y[i]);
        }
        yShift_ = -mean(acc2);
        Real tmp2 = variance(acc2);
        if (!close_enough(tmp2, 0.0))
            yMultiplier_ = 1.0 / std::sqrt(tmp2);
        break;
    }
    default:
        QL_FAIL("unknown stabilisation method");
        break;
    }

    for (Size i = 0; i < static_cast<Size>(x.end() - x.begin()); ++i) {
        for (Size j = 0; j < xMultiplier_.size(); ++j) {
            xData[i][j] = (x[i][j] + xShift_[j]) * xMultiplier_[j];
        }
    }
    for (Size i = 0; i < static_cast<Size>(y.end() - y.begin()); ++i) {
        yData[i] = (y[i] + yShift_) * yMultiplier_;
    }

    glls_ = boost::make_shared<GeneralLinearLeastSquares>(xData, yData, v);
}

template <class xType, class vContainer>
Real StabilisedGLLS::eval(xType x, vContainer& v,
                          typename boost::enable_if<typename boost::is_arithmetic<xType>::type>::type*) {
    QL_REQUIRE(v.size() == glls_->dim(),
               "StabilisedGLLS::eval(): v size (" << v.size() << ") must be equal to dim (" << glls_->dim());
    Real tmp = 0.0;
    for (Size i = 0; i < v.size(); ++i) {
        tmp += glls_->coefficients()[i] * v[i]((x + xShift_[0]) * xMultiplier_[0]);
    }
    return tmp / yMultiplier_ - yShift_;
}

template <class xType, class vContainer>
Real StabilisedGLLS::eval(xType x, vContainer& v,
                          typename boost::disable_if<typename boost::is_arithmetic<xType>::type>::type*) {
    QL_REQUIRE(v.size() == glls_->dim(),
               "StabilisedGLLS::eval(): v size (" << v.size() << ") must be equal to dim (" << glls_->dim());
    Real tmp = 0.0;
    for (Size i = 0; i < v.size(); ++i) {
        xType xNew(x.end() - x.begin());
        for (Size j = 0; j < static_cast<Size>(x.end() - x.begin()); ++j) {
            xNew[j] = (x[j] + xShift_[j]) * xMultiplier_[j];
        }
        tmp += glls_->coefficients()[i] * v[i](xNew);
    }
    return tmp / yMultiplier_ - yShift_;
}

} // namespace QuantExt

#endif


#include <algorithm>
#include <boost/math/constants/constants.hpp>
#include <boost/test/unit_test.hpp>
#include <cmath>
#include <complex>
#include <iostream>
#include "../../JeanBaptiste/include/basic/SineCosine.h"
#include <memory>
#include <string>

namespace constants = boost::math::constants;
namespace jb = jeanbaptiste;
namespace jbb = jeanbaptiste::basic;
namespace ut = boost::unit_test;

class SinCosFixture
{
public:
    SinCosFixture()
    {}

    ~SinCosFixture()
    {}
};

BOOST_FIXTURE_TEST_SUITE(SinCosTestSuite, SinCosFixture)

    BOOST_AUTO_TEST_CASE(float_sine_from_minus_2pi_to_plus_2_pi)
    {
        BOOST_TEST_MESSAGE("Running sine computation in float range [-2pi ... 2pi].");

        for (float x = -constants::two_pi<float>(); x <=constants::two_pi<float>(); x += 0.1)
            BOOST_TEST(std::fabs(std::sin(x) - jbb::sine<float>(x)) < 0.00001);
    }

    BOOST_AUTO_TEST_CASE(double_sine_from_minus_2pi_to_plus_2_pi)
    {
        BOOST_TEST_MESSAGE("Running sine computation in double range [-2pi ... 2pi].");

        for (double x = -constants::two_pi<double>(); x <=constants::two_pi<double>(); x += 0.1)
            BOOST_TEST(std::fabs(std::sin(x) - jbb::sine<double>(x)) < 0.000000000001);
    }

    BOOST_AUTO_TEST_CASE(float_cosine_from_minus_2pi_to_plus_2_pi)
    {
        BOOST_TEST_MESSAGE("Running cosine computation in float range [-2pi ... 2pi].");

        for (float x = -constants::two_pi<float>(); x <=constants::two_pi<float>(); x += 0.1)
            BOOST_TEST(std::fabs(std::cos(x) - jbb::cosine<float>(x)) < 0.0001);
    }

    BOOST_AUTO_TEST_CASE(double_cosine_from_minus_2pi_to_plus_2_pi)
    {
        BOOST_TEST_MESSAGE("Running cosine computation in double range [-2pi ... 2pi].");

        for (double x = -constants::two_pi<double>(); x <=constants::two_pi<double>(); x += 0.1)
            BOOST_TEST(std::fabs(std::cos(x) - jbb::cosine<double>(x)) < 0.00000000001);
    }

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE FourierTransformTest
#include <boost/test/unit_test.hpp>

// For IO
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <cmath>
#include <iostream>
#include <unistd.h>
#include <vector>

// For measuring elapsed time
#include <chrono>

// For Random Float Generator
#include <time.h>

// For object under test
#include "qam-modulator.h"
#include "common.h"
#include "fftw3.h"

/**
* Test NYQUIST MODULATOR
*
*/
BOOST_AUTO_TEST_SUITE(QAM_MODULATOR)

/**
* Generate random data execute QAM Modulator and demodulator.
*
*/
BOOST_AUTO_TEST_CASE(QamModToDemod)
{
    printf("\nTesting QAM Modulation to Demodulation...\n");
    printf("\nMdoulator:\n");

    size_t nPoints = 512;
    size_t pilotToneStep = 8;
    size_t energyDispersalSeed = 10;
    size_t bitsPerSymbol = 2;
    size_t nAvaiableifftPoints = (nPoints - (int)(nPoints/pilotToneStep));
    size_t nMaxEncodedBytes = (int)((nAvaiableifftPoints *  bitsPerSymbol)  / 8);
    size_t nData = nMaxEncodedBytes;
    double pilotToneAmplitude = 2.0;

    // Setup random float generator
    srand( (unsigned)time( NULL ) );

    std::vector<unsigned char> TxCharArray(nData);
    std::vector<unsigned char> RxCharArray(nData);

    for(size_t i = 0; i < nData; i++ )
    {
        TxCharArray[i] = (unsigned char) rand() % 255;
    }

    DoubleVec QamOutput(nPoints*2);

    QamModulator qam(nPoints, pilotToneStep, pilotToneAmplitude, energyDispersalSeed, bitsPerSymbol);

    auto start = std::chrono::steady_clock::now();
    qam.Modulate(TxCharArray, QamOutput, nData);
    auto end = std::chrono::steady_clock::now();

    std::cout << "QAM Modulator elapsed time: "
    << std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count()
    << " ns" << std::endl;

    printf("\nDemodulator:\n");

    start = std::chrono::steady_clock::now();
    qam.Demodulate(QamOutput, RxCharArray, nData);
    end = std::chrono::steady_clock::now();

    std::cout << "QAM Demodulator elapsed time: "
    << std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count()
    << " ns" << std::endl;

    for(size_t i = 0; i < nData; i++)
    {
        //printf("TxCharArray[%lu] = %d ,RxCharArray[%lu] = %d\n"
        //,i,(int)TxCharArray[i] ,i, (int)RxCharArray[i] );

        BOOST_CHECK_MESSAGE( (TxCharArray[i] == RxCharArray[i] ),
        "Elements differ! - Occured at index: " << i );
    }

}

BOOST_AUTO_TEST_SUITE_END()


#include "Normalize.hh"
#include "TypesFunctions.hh"
#include <Eigen/Core>
#include <cmath>

#ifdef GNA_CUDA_SUPPORT
#include "cuElementary.hh"
#include "DataLocation.hh"
#endif

/**
 * @brief Default constructor.
 *
 * Initializes the transformation for the whole histogram normalization.
 */
Normalize::Normalize() {
    transformation_("normalize")
        .input("inp")
        .output("out")
        .types(TypesFunctions::pass<0>)
        .func(&Normalize::doNormalize)
#ifdef GNA_CUDA_SUPPORT
        .func("gpu", &Normalize::doNormalize_gpu, DataLocation::Device)
#endif
        ;
}

/**
 * @brief Subhistogram normalization constructor.
 *
 * Start and length are defined the same way they are defined for the segment method of Eigen.
 *
 * @param start  -- subhistogram first bin.
 * @param length -- number of bins to normalize to.
 */
Normalize::Normalize(size_t start, size_t length) : m_start{start}, m_length{length} {
    transformation_("normalize")
        .input("inp")
        .output("out")
        .types(TypesFunctions::pass<0>, &Normalize::checkLimits)
        .func(&Normalize::doNormalize_segment)
        ;
}

/**
 * @brief Normalize the whole histogram.
 *
 * Divides each bin to the sum of bins.
 */
void Normalize::doNormalize(FunctionArgs& fargs){
    auto& in=fargs.args[0].x;
    fargs.rets[0].x=in/in.sum();
}

#ifdef GNA_CUDA_SUPPORT
void Normalize::doNormalize_gpu(FunctionArgs& fargs) {
    fargs.args.touch();
    auto& gpuargs = fargs.gpu;
    gpuargs->provideSignatureDevice();
    cunormalize(gpuargs->args, gpuargs->rets, fargs.args[0].arr.size());
}
#endif

/**
 * @brief Normalize subhistogram.
 *
 * Divides each bin to the sum of bins in a range [start, start+length-1].
 */
void Normalize::doNormalize_segment(FunctionArgs& fargs){
    auto& in=fargs.args[0].x;
    fargs.rets[0].x=in/in.segment(m_start, m_length).sum();
}

/**
 * @brief typesFunction to check histogram limits for subhistogram mode.
 * @exception SourceTypeError in case the input array is not 1d.
 * @exception SourceTypeError in case the start is outside of the data limits.
 * @exception SourceTypeError in case the end is outside of the data limits.
 */
void Normalize::checkLimits(TypesFunctionArgs& fargs) {
  auto& args=fargs.args;
    auto& dtype = args[0];
    if( dtype.shape.size()!=1u ){
        throw args.error(dtype, "Accept only 1d arrays in case a segment is specified");
    }
    auto length=dtype.shape[0];
    if( m_start>=length ){
        throw args.error(dtype, "Segment start is outside of the data limits");
    }
    if( (m_start+m_length)>length ){
        throw args.error(dtype, "Segment end is outside of the data limits");
    }
}


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

  @copyright 2016 NumScale SAS

  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_CONSTANT_GREATESTNONINTEGER_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_GREATESTNONINTEGER_HPP_INCLUDED

/*!
  @ingroup group-constant
  @defgroup constant-Greatestnoninteger Greatestnoninteger (function template)

  Generates the greatest representable non-integral value

  @headerref{<boost/simd/constant/greatestnoninteger.hpp>}

  @par Description

  1.  @code
      template<typename T> T Greatestnoninteger();
      @endcode

  2.  @code
      template<typename T> T Greatestnoninteger( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T which is exactly representable and which its successor has
  integral value. By definition, all floating numbers greater than `Greatestnoninteger<T>()` have
  integral value.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c as_integer_t<T> that evaluates to:

  | Type                | double                        | float         |
  |--------------------:|:------------------------------|---------------|
  | value               |   4503599627370495.5          | 8388607.5f    |

  @par Requirements
  - **T** models IEEEValue
**/

#include <boost/simd/constant/scalar/greatestnoninteger.hpp>
#include <boost/simd/constant/simd/greatestnoninteger.hpp>

#endif


#ifndef BODY_HPP
#define BODY_HPP

#include <armadillo>
#include "Math.hpp"
#include <boost/shared_ptr.hpp>
#include <boost/enable_shared_from_this.hpp>

class Body : public boost::enable_shared_from_this<Body>
{
public:
    Body();
    virtual ~Body() = default;

    arma::mat get_TBI();
    arma::vec get_POSITION();
    arma::vec get_VELOCITY();
    arma::vec get_ACCELERATION();
    arma::vec get_ANGLE_VEL();
    arma::vec get_ANGLE();
    arma::vec get_ANGLE_ACC();
    arma::vec get_FORCE();
    arma::vec get_TORQUE();
    arma::mat get_M();
    arma::vec get_TBI_Q();
    arma::vec get_TBID_Q();
    unsigned int get_num();

    void set_POSITION(const arma::vec &PosIn);
    void set_VELOCITY(const arma::vec &VelIn);
    void set_ACCELERATION(const arma::vec &AccIn);
    void set_ANGLE(const arma::vec &AngIn);
    void set_ANGLE_VEL(const arma::vec &AngvelIn);
    void set_ANGLE_ACC(const arma::vec &AngaccIn);
    void set_TBI(const arma::mat &TBIIn);

    virtual void update(arma::vec PosIn, arma::vec VelIn, arma::vec AttIn
        , arma::vec ANG_VEL_In) = 0;

protected:

    unsigned int type;  // type define   0: Ground body, 1: Mobilized body
    unsigned int num;  // No. body

    arma::vec POSITION;
    arma::vec VELOCITY;
    arma::vec ACCELERATION;
    arma::vec ANGLE;
    arma::vec ANGLE_VEL;
    arma::vec ANGLE_ACC;
    arma::mat M;
    arma::vec FORCE;
    arma::vec TORQUE;
    arma::vec APPILED_TORQUE;
    arma::mat TBI;
    arma::vec TBI_Q;
    arma::vec TBID_Q;
};

class Ground : public Body
{
public:
    Ground(unsigned int NumIn);
    ~Ground() {};
    virtual void update(arma::vec PosIn, arma::vec VelIn, arma::vec AttIn
        , arma::vec ANG_VEL_In) {};
};

class Mobilized_body : public Body
{
public:
    Mobilized_body(unsigned int NumIn, arma::vec PosIn, arma::vec VelIn, arma::vec AccIn, arma::vec AttIn
        , arma::vec ANG_VEL_In, arma::vec ANG_ACC_In, double MIn, arma::vec IIn
        , arma::vec F_In, arma::vec T_In);
    ~Mobilized_body() {};

    virtual void update(arma::vec PosIn, arma::vec VelIn, arma::vec TBI_QIn
        , arma::vec ANG_VEL_In) override;
};

typedef boost::shared_ptr<Body> BodyPtr;
#endif  //BODY_HPP


#include "ECF_base.h"
#include "floatingpoint/FloatingPoint.h"
#include "AlgCuckooSearch.h"
#include <boost/random/normal_distribution.hpp>
#include <boost/random.hpp>
#include <ctime>
#include <cstdlib>
#include <vector>

CuckooSearch::CuckooSearch()
{
	name_ = "CuckooSearch";
	selBestOp = static_cast<SelectionOperatorP> (new SelBestOp);
}

void CuckooSearch::registerParameters(StateP state)
{
	registerParameter(state, "pa", (voidP) new double(0.75), ECF::DOUBLE);
}

bool CuckooSearch::initialize(StateP state)
{
	selBestOp->initialize(state);

	voidP pDiscovery = getParameterValue(state, "pa");
	pa = *((double*)pDiscovery.get());
	if (pa < 0 || pa > 1)
	{
		ECF_LOG_ERROR(state, "Error - pa must be in interval [0,1]");
		throw "";
	}

	// reading boudaries and problem dimension
	voidP lBound = state->getGenotypes()[0]->getParameterValue(state, "lbound");
	lbound = *((double*)lBound.get());
	voidP uBound = state->getGenotypes()[0]->getParameterValue(state, "ubound");
	ubound = *((double*)uBound.get());
	voidP sptr = state->getGenotypes()[0]->getParameterValue(state, "dimension");
	numDimension = *((uint*)sptr.get());

	// algorithm accepts a single FloatingPoint or Binary genotype
	// or a genotype derived from the abstract RealValueGenotype class
	GenotypeP activeGenotype = state->getGenotypes()[0];
	RealValueGenotypeP rv = boost::dynamic_pointer_cast<RealValueGenotype> (activeGenotype);
	if(!rv) {
		ECF_LOG_ERROR(state, "Error: Cuckoo Search algorithm accepts only a RealValueGenotype derived genotype! (FloatingPoint or Binary)");
		throw ("");
	}

	return true;
}

bool CuckooSearch::advanceGeneration(StateP state, DemeP deme)
{
	double sigma = 0.696574502;
	boost::mt19937 rng;
	boost::normal_distribution<> nd(0.0, 1.0);
	boost::variate_generator<boost::mt19937&,
	boost::normal_distribution<> > var_nor(rng, nd);

	IndividualP best = selBestOp->select(*deme);
	FloatingPointP bestFp = boost::static_pointer_cast<FloatingPoint::FloatingPoint> (best->getGenotype(0));

	// cuckoos via Levy flights (by Mantegna's algorithm)
	// new individual is added to population only if it is better than original individual
	for (uint i = 0; i < deme->size(); i++) {
		IndividualP trial = (IndividualP)deme->at(i)->copy();
		FloatingPointP trialFp = boost::static_pointer_cast<FloatingPoint::FloatingPoint> (trial->getGenotype(0));
		for (uint j = 0; j < numDimension; j++)	{
			double u = var_nor() * sigma;
			double v = var_nor();
			double step = u / pow(fabs(v), 2 / (double)3);
			double randn = var_nor();
			double diff = trialFp->realValue[j] - bestFp->realValue[j];
			double stepsize = 0.01 * step * diff;
			trialFp->realValue[j] = trialFp->realValue[j] + stepsize*randn;
			if (trialFp->realValue[j] > ubound)
				trialFp->realValue[j] = ubound;
			if (trialFp->realValue[j] < lbound)
				trialFp->realValue[j] = lbound;
		}
		evaluate(trial);
		if (trial->fitness->isBetterThan(deme->at(i)->fitness))
			replaceWith(deme->at(i), trial);
	}

	// copy all individuals
	std::vector<IndividualP> nest1;
	std::vector<IndividualP> nest2;
	for (uint i = 0; i < deme->size(); i++) {
		IndividualP indCp = (IndividualP)deme->at(i)->copy();
		nest1.push_back(indCp);
		nest2.push_back(indCp);
	}

	// replace some individuals/nests by constructing new nests
	// nest is replaced only if it is better than original
	random_shuffle(nest1.begin(), nest1.end());
	random_shuffle(nest2.begin(), nest2.end());
	double randNum = (double)rand() / RAND_MAX;
	for (uint i = 0; i < deme->size(); i++) {
		IndividualP trial = (IndividualP)deme->at(i)->copy();
		FloatingPointP trialFp1 = boost::static_pointer_cast<FloatingPoint::FloatingPoint> (nest1.at(i)->getGenotype(0));
		FloatingPointP trialFp2 = boost::static_pointer_cast<FloatingPoint::FloatingPoint> (nest2.at(i)->getGenotype(0));
		FloatingPointP trialFp = boost::static_pointer_cast<FloatingPoint::FloatingPoint> (trial->getGenotype(0));

		for (uint j = 0; j < numDimension; j++) {
			if ((double)rand() / RAND_MAX < pa) {
				double stepsize = (trialFp1->realValue[j] - trialFp2->realValue[j])*randNum;
				trialFp->realValue[j] += stepsize;
				if (trialFp->realValue[j] > ubound)
					trialFp->realValue[j] = ubound;
				if (trialFp->realValue[j] < lbound)
					trialFp->realValue[j] = lbound;
			}
		}

		evaluate(trial);
		if (trial->fitness->isBetterThan(deme->at(i)->fitness))
			replaceWith(deme->at(i), trial);
	}
	return true;

}


// Author: Daisuke Kanaizumi
// Affiliation: Department of Applied Mathematics, Waseda University

#ifndef QGAMMA_HPP
#define QGAMMA_HPP

#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/complex.hpp>
#include <kv/constants.hpp>
#include <limits>
#include <algorithm>
#include <kv/Heine.hpp>
#include <kv/Pochhammer.hpp>
#include <kv/qPochhammerVer2.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
namespace ub = boost::numeric::ublas;
namespace kv{
template <class T> interval<T> q_gamma(const interval<T>& z,const interval<T>& q){
   // q must be positive
   // verification program for q-gamma function
   interval<T>res;
   if(q<1 && q>0){
     if(pow(q,z)<1){
       res=pow(1-q,1-z)*Karpelevich(interval<T>(pow(q,z)),interval<T>(q));
     }
     else{
       res=Euler(interval<T>(q))*pow(1-q,1-z)/infinite_qPochhammer(interval<T>(pow(q,z)),interval<T>(q));
     }
     /*if(abs(res).upper()==std::numeric_limits<T>::infinity()){
       // Use asymptotic expansion
       // M Mansour (2006) An asymptotic expansion of the q-gamma function Γ q (x), Journal of Nonlinear Mathematical Physics, 13:4, 479-483, DOI: 10.2991/jnmp.2006.13.4.2
       res=sqrt(1+q)*pow(1-q,0.5-z)*Euler(interval<T>(q*q))*pow(1-q*q,0.5)/infinite_qPochhammer(interval<T>(pow(q*q,0.5)),interval<T>(q*q))*interval<T>(1.,(exp(pow(q,z)/(1-q-pow(q,z)))).upper());
       }*/

   }
   if(q>1){ // Moak q-gamma function
     if(pow(q,-z)<1){
       res=pow(q-1,1-z)*pow(q,z*(z-1)/2)*Karpelevich(interval<T>(pow(q,-z)),interval<T>(1/q));
     }
     else{
       res=Euler(interval<T>(1/q))*pow(q-1,1-z)*pow(q,z*(z-1)/2)/infinite_qPochhammer(interval<T>(pow(q,-z)),interval<T>(1/q));
     }
   }
   return res;
 }

  template <class T> complex<interval<T> >q_gamma(const complex<interval<T> >& z,const interval<T>& q){
    complex<interval<T> >res;
    if(q<1 && q>0){
      res=Euler(interval<T>(q))*pow(1-q,1-z)/infinite_qPochhammer(complex<interval<T> >(pow(q,z)),interval<T>(q));
    }
    if(q>1){
      res=Euler(interval<T>(1/q))*pow(q-1,1-z)*pow(q,z*(z-1)/2)/infinite_qPochhammer(complex<interval<T> >(pow(q,-z)),interval<T>(1/q));
    }
    return res;
 }
template <class T> ub::matrix<interval<T> >MExp(const ub::matrix<interval<T> >& A){
  int n,M;
  M=100;
  n=A.size1();//A:square matrix
  ub::matrix< interval<T> > B(n, n),res(n, n),sum(n, n),pro(n,n);
  interval<T> error,norm;
  T b;
  for(int i=0;i<n;i++){
    for(int j=0;j<n;j++){
      sum(i,j)=0.;
      if(i==j)pro(i,j)=1.;
      else pro(i,j)=0.;
    }
  }
  for(int N=0;N<=M;N++){
    sum+=(1./Pochhammer(interval<T>(1.,1.),N))*pro;
    pro=prod(pro,A);
  }
  norm=abs(A(0,0));
  for(int i1=0;i1<n;i1++){
    for(int j1=0;j1<n;j1++){
      if (A(i1,j1)>abs(norm)) norm=abs(A(i1,j1));
    }
  }

  error=exp(norm)*pow(norm,M+1)/Pochhammer(interval<T>(1.,1.),M+1);
  b=(abs(error)).upper();

  for(int k1=0;k1<n;k1++){
    for(int l1=0;l1<n;l1++){
      B(k1,l1).assign(-1.,1.);
      B(k1,l1)=b*B(k1,l1);
    }
  }
  res=sum+B;
  return res;
}
template <class T> ub::matrix<interval<T> >q_gamma(const ub::matrix<interval<T> >& A,const interval<T>& q){
  int n;
  n=A.size1();//A:square matrix
  interval<T>buf;
  ub::matrix< interval<T> > I(n, n),B(n, n),res(n, n),exp(n,n),inv(n,n),qp(n,n),qa(n,n),AA(n,n);

  for(int i=0;i<n;i++){
    for(int j=0;j<n;j++){
      if(i==j){
	I(i,j)=1.;
	inv(i,j)=1.;
      }
      else{
	I(i,j)=0.;
	inv(i,j)=0.;
      }
    }
  }
  AA=log(q)*A;
  qa=MExp(AA);
  qp=infinite_qPochhammer(ub::matrix<interval<T> > (qa), interval<T> (q));
//std::cout<<qp<<std::endl;
  for(int i1=0;i1<n;i1++){
    buf=1./qp(i1,i1);
    for(int j1=0;j1<n;j1++){
      qp(i1,j1)*=buf;
      inv(i1,j1)*=buf;
    }

    for(int j2=0;j2<n;j2++){
      if(i1!=j2){
	buf=qp(j2,i1);
	for(int k=0;k<n;k++){
	  qp(j2,k)-=qp(i1,k)*buf;
	  inv(j2,k)-=inv(i1,k)*buf;
	}
      }
    }
  }
  B=I-A;
  B=log(1-q)*B;
  std::cout<<B<<std::endl;
  exp=MExp(B);
  res=infinite_qPochhammer(interval<T>(q),interval<T>(q))
    *prod(inv,exp);
  return res;
}
  template <class T> complex<interval<T> >qgamma_Gauss_multi(const complex<interval<T> >& z,const interval<T>& q, int  p=3){
  if(q<1 && q>0){
    // M Mansour (2006) An asymptotic expansion of the q-gamma function Γ q (x), Journal of Nonlinear Mathematical Physics, 13:4, 479-483, DOI: 10.2991/jnmp.2006.13.4.2
    // G Gasper , M Rahman, Basic Hypergeometric Series 2nd Edition, Cambridge University Press, 2004.
    interval<T> pq,pro2;
    pq=(1-pow(q,p))/(1-q);//pq OK
    complex<interval<T> >res,pro1;
    pro1=1.;
    pro2=1.;
    for(int i=0;i<=p-1;i++){
      pro1=pro1*q_gamma(complex<interval<T> >((z+i)/p),interval<T>(pow(q,p)));
      // pro1 OK
    }
    for(int j=1;j<=p-1;j++){
      interval<T> jj;
      jj=j;
      pro2=pro2*q_gamma(interval<T> (jj/p),interval<T>(pow(q,p)));
    }

    res=pro1*pow(pq,z-1)/pro2;
    return res;
  }
  else{
    throw std::domain_error("implemented for 0<q<1");
  }
}
template <class T> complex<interval<T> >qgamma_Legendre(const complex<interval<T> >& z,const interval<T>& q){
  if(q<1 && q>0){
    interval<T>qg;
    qg=q_gamma(interval<T>(0.5),interval<T>(q*q));
    complex<interval<T> >res;

    res=q_gamma(complex<interval<T> >(z*0.5),interval<T>(q*q))
      *q_gamma(complex<interval<T> >((z+1)*0.5),interval<T>(q*q))
      *pow(1+q,z-1)/qg;

    return res;
  }
  else{
    throw std::domain_error("implemented for 0<q<1");
  }
}
  template <class T> complex<interval<T> >qgamma_shift(const complex<interval<T> >& z,const interval<T>& q, int p=3){
    // computing the q-gamma function with functional equation
    // G Gasper , M Rahman, Basic Hypergeometric Series 2nd Edition, Cambridge University Press, 2004.
    complex<interval<T> >pro;
    pro=1.;
    for(int i=1;i<=p;i++){
      pro=pro*(1-pow(q,z-i))/(1-q);
    }
    pro=pro*q_gamma(complex<interval<T> >(z-p),interval<T>(q));
  }
  template <class T> interval<T> q_digamma(const interval<T>& x,const interval<T>& q){
    // q,x must be positive
   // verification program for q-digamma function
   // Reference: Kamel Brahim (2009), Turan-Type Inequalities for some q-Special Functions
   // Journal of inequalities in pure and applied mathematics, Volume 10
   interval<T>res,sum,qq,first,ratio;
   T rad;
   int N=100;
   sum=0.;
   qq=1.;
    if (q>=1){
     throw std::domain_error("value of q must be under 1");
   }
   if (q<=0){
     throw std::domain_error("q must be positive");
   }
   if (x<=0){
     throw std::domain_error("implemented for positive x");
   }
   for(int n=1;n<=N-1;n++){
     qq=qq*q;
     sum=sum+pow(q,n*x)/(1-qq);
   }
   qq=qq*q;
   first=pow(q,N*x)/(1-qq);
   ratio=(1-qq)*pow(q,x)/(1-qq*q);
 if(abs(ratio)<1){
      rad=(first/(1-ratio)).upper();
      res=-log(1-q)+log(q)*(sum+rad*interval<T>(-1.,1.));
      return res;
    }
    else{
      std::cout<<"ratio is more than 1"<<std::endl;
    }
 }
 template <class T> interval<T> q_beta(const interval<T>& a,const interval<T>& b,const interval<T>& q){
   // q must be positive
   // verification program for q-beta function
   interval<T>res;
   res=q_gamma(interval<T>(a),interval<T>(q))*q_gamma(interval<T>(b),interval<T>(q))/q_gamma(interval<T>(a+b),interval<T>(q));
   return res;
 }
  template <class T> complex<interval<T> >q_beta(const complex<interval<T> >& a,const complex<interval<T> >& b,const interval<T>& q){
   // q must be positive
   // verification program for q-beta function
    complex<interval<T> >res;
    res=q_gamma(complex<interval<T> >(a),interval<T>(q))*q_gamma(complex<interval<T> >(b),interval<T>(q))/q_gamma(complex<interval<T> >(a+b),interval<T>(q));
    return res;
  }
 template <class T> interval<T> symmetric_q_gamma(const interval<T>& z,const interval<T>& q){
   // verification program for symmetric q-gamma function
   // reference
   // Brahim and Sidomou, On Some Symmetric q-Special Functions, 2013
   interval<T>res;
   res=pow(q,-(z-1)*(z-2)/2)*q_gamma(interval<T>(z),interval<T>(q*q));
   return res;
 }
 template <class T> interval<T> symmetric_q_beta(const interval<T>& a,const interval<T>& b,const interval<T>& q){
   // q,a,b must be positive
   // verification program for symmetric q-beta function
   // reference
   // Brahim and Sidomou, On Some Symmetric q-Special Functions, 2013
   interval<T>res;
   res=symmetric_q_gamma(interval<T>(a),interval<T>(q))*symmetric_q_gamma(interval<T>(b),interval<T>(q))/symmetric_q_gamma(interval<T>(a+b),interval<T>(q));
   return res;
 }
  template <class T> complex<interval<T> >symmetric_q_gamma(const complex<interval<T> >& z,const interval<T>& q){
   // verification program for symmetric q-gamma function
   // reference
   // Brahim and Sidomou, On Some Symmetric q-Special Functions, 2013
    complex<interval<T> >res;
    res=pow(q,-(z-1)*(z-2)/2)*q_gamma(complex<interval<T> >(z),interval<T>(q*q));
   return res;
 }
  template <class T> complex<interval<T> >incomplete_q_gamma(const complex<interval<T> >& z,const complex<interval<T> >& a,const interval<T>& q){
    // verification program for incomplete q-gamma function
    // expansion formula is used
    // reference
    // Ahmed Salem, A q-analogue of the exponential integral, 2013
    // warning: "a" should neither be negative integer nor zero
    complex<interval<T> >res,qq;
    qq=q;
    res=pow(z*(1-q),a)*q_gamma(complex<interval<T> >(a),interval<T>(q))
      *Heine(complex<interval<T> >(z*(1-q)),complex<interval<T> >(pow(q,a)),complex<interval<T> >(0.),interval<T>(q),complex<interval<T> >(qq));
    return res;
  }
 template <class T> complex<interval<T> >elliptic_gamma(const complex<interval<T> >& z,const interval<T> & p ,const interval<T> & q){
    // verification program for elliptic gamma function
    // reference: M. A. Bershtein, A. I. Shechechkin (arXiv, 2016)
    // q-deformed Painlev\`e \tau function and q-deformed conformal blocks, Appendix A
    complex<interval<T> >res;
    /* if (abs(z)>=1){
      throw std::domain_error("implemented only for |z|<1");
      }*/
    if (abs(q)>=1){
      throw std::domain_error("absolute value of q must be under 1");
    }
    if (abs(p)>=1){
      throw std::domain_error("absolute value of p must be under 1");
    }
    res=inf_elliptic_Pochhammer(complex<interval<T> >(p*q/z),complex<interval<T> >(p),complex<interval<T> >(q))
      /inf_elliptic_Pochhammer(complex<interval<T> >(z),complex<interval<T> >(p),complex<interval<T> >(q));
    return res;
  }
  template <class T> complex<interval<T> > modified_Jacobi_theta(const complex<interval<T> >&a,const interval<T> &q){
    complex<interval<T> >res;
    res=infinite_qPochhammer(complex<interval<T> >(a),interval<T>(q))
      *infinite_qPochhammer(complex<interval<T> >(q/a),interval<T>(q));
    return res;
  }
  template <class T> interval<T>  modified_Jacobi_theta(const interval<T> &a,const interval<T> &q){
    interval<T> res;
    res=qPVer2(interval<T> (a),interval<T>(q))
      *qPVer2(interval<T> (q/a),interval<T>(q));
    return res;
  }
  template <class T> complex<interval<T> >elliptic_gamma_tilde(const complex<interval<T> >& z,const interval<T> & p ,const interval<T> & q){
    complex<interval<T> >res;
    if (abs(z)>=1){
      throw std::domain_error("implemented only for |z|<1");
    }
    if (abs(q)>=1){
      throw std::domain_error("absolute value of q must be under 1");
    }
    if (abs(p)>=1){
      throw std::domain_error("absolute value of p must be under 1");
    }
    res=infinite_qPochhammer(interval<T> (q),interval<T> (q))/infinite_qPochhammer(interval<T> (p),interval<T> (p))
      *pow(modified_Jacobi_theta(q,p),1-log(z)/log(q))*elliptic_gamma(complex<interval<T> >(z),interval<T>(p),interval<T>(q));
    return res;
  }
  template <class T> complex<interval<T> >elliptic_gamma_shift(const complex<interval<T> >& z,const interval<T> & p ,const interval<T> & q,int n){
    complex<interval<T> >res,pro1,pro2;
    interval<T> r;
    r=pow(q,n);
    if (abs(q)>=1){
      throw std::domain_error("absolute value of q must be under 1");
    }
    if (abs(p)>=1){
      throw std::domain_error("absolute value of p must be under 1");
    }
    for(int i=1;i<=n-1;i++){
      pro1=pro1*elliptic_gamma_tilde(complex<interval<T> >(i/T(n)),interval<T>(p),interval<T>(r));
    }
    for(int j=0;j<=n-1;j++){
      pro2=pro2*elliptic_gamma_tilde(complex<interval<T> >((z+j)/T(n)),interval<T>(p),interval<T>(r));
    }
    res=pow(modified_Jacobi_theta(r,p)/modified_Jacobi_theta(q,p),z-1)*pro2/pro1;
    return res;
  }
 template <class T> complex<interval<T> >elliptic_gamma_shift2(const complex<interval<T> >& z,const interval<T> & p ,const interval<T> & q,int n){
    complex<interval<T> >res,pro1,pro2;
    interval<T> r;
    r=pow(q,n);
    if (abs(q)>=1){
      throw std::domain_error("absolute value of q must be under 1");
    }
    if (abs(p)>=1){
      throw std::domain_error("absolute value of p must be under 1");
    }
    for(int i=1;i<=n-1;i++){
      pro1=pro1*elliptic_gamma_tilde(complex<interval<T> >(T(i)/T(n)),interval<T>(p),interval<T>(r));
    }
    for(int j=0;j<=n-1;j++){
      pro2=pro2*elliptic_gamma_shift(complex<interval<T> >((z+T(j))/T(n)),interval<T>(p),interval<T>(r),int(n));
    }
    res=pow(modified_Jacobi_theta(r,p)/modified_Jacobi_theta(q,p),z-1)*pro2/pro1;
    return res;
  }
}

#endif


#define DEBUG 1
/**
 * File    : E.cpp
 * Author  : Kazune Takahashi
 * Created : 12/14/2019, 10:43:31 PM
 * Powered by Visual Studio Code
 */
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <vector>
#include <string>
#include <complex>
#include <tuple>
#include <queue>
#include <stack>
#include <map>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <bitset>
#include <functional>
#include <random>
#include <chrono>
#include <cctype>
#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstdlib>
// ----- boost -----
#include <boost/rational.hpp>
// ----- using directives and manipulations -----
using boost::rational;
using namespace std;
using ll = long long;
ll MOD{0};
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{x % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator<=(const Mint &a) const { return x <= a.x; }
  bool operator>(const Mint &a) const { return x > a.x; }
  bool operator>=(const Mint &a) const { return x >= a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  bool operator!=(const Mint &a) const { return !(*this == a); }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
istream &operator>>(istream &stream, Mint &a) { return stream >> a.x; }
ostream &operator<<(ostream &stream, const Mint &a) { return stream << a.x; }

// ----- main() -----

// I wrote this code referring to yataka1999-san's solution
// https://atcoder.jp/contests/agc035/submissions/6380939

Mint solve_even(ll N, ll K);
Mint solve_odd(ll N, ll K);

Mint solve_even(ll N, ll K)
{
  Mint ans{1};
  for (auto t = 0LL; t < 2; t++)
  {
    auto L{(N - t + 1) / 2};
    vector<Mint> to(L + 1), from(L + 1);
    to[0] = 1;
    for (auto i = 0LL; i < L; i++)
    {
      swap(to, from);
      to = vector<Mint>(L + 1);
      for (auto j = 0LL; j <= i; j++)
      {
        to[j + 1] += from[j];
        to[0] += from[j];
      }
      for (auto j = K / 2 + 1; j <= i + 1; j++)
      {
        to[j] = 0;
      }
    }
    Mint sum{0};
    for (auto j = 0LL; j <= L; j++)
    {
      sum += to[j];
    }
    ans *= sum;
  }
  return ans;
}

Mint solve_odd(ll N, ll K)
{
  vector<vector<vector<Mint>>> from(N + 1, vector<vector<Mint>>(N + 1, vector<Mint>(N + 1)));
  vector<vector<vector<Mint>>> to(N + 1, vector<vector<Mint>>(N + 1, vector<Mint>(N + 1)));
  to[0][0][N] = 1;
  for (auto i = 0LL; i < N; i++)
  {
    swap(to, from);
    to = vector<vector<vector<Mint>>>(N + 1, vector<vector<Mint>>(N + 1, vector<Mint>(N + 1)));
    auto even{i / 2 + 1};
    auto odd{(i + 1) / 2};
    for (auto j = 0LL; j <= even; j++)
    {
      for (auto k = 0LL; k <= odd; k++)
      {
        for (auto t = i; t <= N; t++)
        {
          if (from[j][k][t] == 0)
          {
            continue;
          }
          auto dst{(t % 2 == i % 2) ? N : t};
          to[0][j][dst] += from[j][k][t];
          if (t != i)
          {
            dst = j >= K / 2 + 1 ? min(t, i - 2 * k + K) : t;
            to[k + 1][j][dst] += from[j][k][t];
          }
        }
      }
    }
  }
  Mint ans{0};
  for (auto j = 0LL; j <= N; j++)
  {
    for (auto k = 0LL; k <= N; k++)
    {
      ans += to[j][k][N];
    }
  }
  return ans;
}

int main()
{
  ll N, K;
  cin >> N >> K >> MOD;
  if (K % 2 == 0)
  {
    cout << solve_even(N, K) << endl;
  }
  else
  {
    cout << solve_odd(N, K) << endl;
  }
}


#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <iostream>
#include <libv/lma/time/tictoc.hpp>

typedef Eigen::MatrixXd Matrix;
typedef Eigen::VectorXd Vector;

Vector llt(Matrix u, Vector x)
{
  for(int i = 0 ; i < u.rows() ; ++i)
  {
    for(int k = 0 ; k < i ; ++k)
      u(i,i) -= u(k,i) * u(k,i);

    assert(u(i,i)>0);
    u(i,i) = std::sqrt(u(i,i));

    for(int j = i + 1; j < u.cols() ; ++j)
    {
      for(int k = 0 ; k < i ; ++k)
        u(i,j) -= u(k,i) * u(k,j);
      u(i,j) /= u(i,i);
    }
  }

  for(int j = 0 ; j < x.size() ; ++j)
  {
    for(int i = 0 ; i < j ; ++i)
      x(j) -= u(i,j) * x(i);
    x(j) /= u(j,j);
  }

  for(int j = x.size() - 1 ; j >=0  ; --j)
  {
    for(int i = j+1 ; i < x.size() ; ++i)
      x(j) -= u(j,i) * x(i);
    x(j) /= u(j,j);
  }

  return x;
}

int main()
{
  size_t n = 10;
  Matrix a(n,n);
  Vector b(n),x(n);

  a = Matrix::Random(n,n);
  a = (a + a.transpose()).eval();

  for(size_t i = 0 ; i < n ; ++i)
  {
    if (a(i,i)<0) a(i,i) = - a(i,i);
    a(i,i) += 10.0;
    x(i) = i+1;
  }

  b = a * x;

  for(size_t i = 0 ; i < n ; ++i)
    for(size_t j = 0 ; j < n ; ++j)
    {
      if (j<i) a(i,j) = 0;
    }
  std::cout << a << std::endl;
  std::cout << " determinant " << a.determinant() << std::endl;
//   std::cout << "\nb = " << b.transpose() << std::endl;

  size_t N = 1000000;
//   size_t N = 1;

  utils::Tic<true> tic("llt");
  for(size_t i = 0 ; i < N; ++i)
    x = llt(a,b);
  tic.disp();
  std::cout << "\nX = " << x.transpose() << std::endl;

  Vector X;

  utils::Tic<true> tic2("LLT");
  for(size_t i = 0 ; i < N ; ++i)
  {
    Eigen::LLT<Matrix,Eigen::Upper> LLT(a);
    X = LLT.solve(b);
  }
  tic2.disp();
//   std::cout << "\nX = " << x.transpose() << std::endl;

//   std::cout << "A * x = " << (a * x).transpose() << std::endl;

//   Matrix L = LLT.matrixL();
//   Matrix U = LLT.matrixU();
//   std::cout << "\nL =\n" << L << std::endl;
//   std::cout << "\nU =\n" << U << std::endl;
  std::cout << "\nX = " << X.transpose() << std::endl;
//   std::cout << " CHECK " << (a*X - b).transpose() << std::endl;

  return x == X;
}


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2020 INRIA.
 *
 * 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 "SiconosConfig.h"

#include "EigenProblemsTest.hpp"
#include "SiconosAlgebra.hpp"
#include "SimpleMatrixFriends.hpp"
#include "bindings_utils.hpp"
#include <limits>
#include <iostream>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include "SiconosVector.hpp"

#define CPPUNIT_ASSERT_NOT_EQUAL(message, alpha, omega) \
  if ((alpha) == (omega)) CPPUNIT_FAIL(message);

// Note FP : add tests for complex matrices and geev, if needed (?)

CPPUNIT_TEST_SUITE_REGISTRATION(EigenProblemsTest);

void EigenProblemsTest::setUp()
{
  size = 5;
  A.reset(new SimpleMatrix(size,size));
  // Initialize A with random values.
  A->randomize();
  Aref.reset(new SimpleMatrix(*A));
}

void EigenProblemsTest::tearDown()
{}

void EigenProblemsTest::testSyev()
{
  std::cout << "--> Test: syev." <<std::endl;

  // turn A to a symmetric matrix
  A->randomize_sym();
  *Aref = *A;

  // Initialize EigenVectors with A
  SP::SiconosVector EigenValues(new SiconosVector(size));
  SP::SimpleMatrix EigenVectors(new SimpleMatrix(*A));
//  *EigenVectors = *A;

  Siconos::eigenproblems::syev(*EigenValues, *EigenVectors);

  DenseVect error(size);
  error *= 0.0;

  for(unsigned int i = 0; i < size; ++i)
  {
    error.plus_assign(ublas::prod(*A->dense(), column(*EigenVectors->dense(), i)));
    error.minus_assign((*EigenValues->dense())(i)*column(*EigenVectors->dense(),i));
  }
  // Check ...
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testSyev 1: ", norm_2(error) < 10 * std::numeric_limits< double >::epsilon(), true);
  // Check if A has not been modified
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testSyev 2: ", (*A) == (*Aref), true);

  // Now compute only eigenvalues
  SP::SiconosVector RefEigenValues(new SiconosVector(*EigenValues));
  *EigenVectors = *A;
  *EigenValues *= 0.0;
  Siconos::eigenproblems::syev(*EigenValues, *EigenVectors, false);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testSyev 3: ", ((*EigenValues) - (*RefEigenValues)).norm2() < 10 * std::numeric_limits< double >::epsilon(), true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testSyev 4: ", (*A) == (*Aref), true);
  std::cout << "--> Syev test ended with success." <<std::endl;
}

void EigenProblemsTest::testGeev1()
{
  std::cout << "--> Test: geev1." <<std::endl;
  // Compute only right eigenvectors.
  complex_matrix fake(1,1), rightV(size,size);
  complex_vector eigenval(size);
  Siconos::eigenproblems::geev(*A, eigenval, fake, rightV);
  complex_vector error(size);
  for(unsigned int i = 0; i < size; ++i) error(i) = 0.0;
  for(unsigned int i = 0; i < size; ++i)
  {
    error.plus_assign(ublas::prod(*A->dense(), column(rightV, i)));
    error.minus_assign(eigenval(i)*column(rightV,i));
  }

  // Check ...
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testGeev1 1: ", norm_2(error) < 10 * std::numeric_limits< double >::epsilon(), true);
  // Check if A has not been modified
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testGeev1 2: ", (*A) == (*Aref), true);
  // Now compute only eigenvalues
  complex_vector RefEigenValues(size);
  RefEigenValues = eigenval;
  eigenval *= 0.0;
  Siconos::eigenproblems::geev(*A, eigenval, fake, fake, false, false);

  CPPUNIT_ASSERT_EQUAL_MESSAGE("testGeev1 3: ", norm_2(eigenval - RefEigenValues) < 10 * std::numeric_limits< double >::epsilon(), true);
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testGeev1 4: ", (*A) == (*Aref), true);

  std::cout << "--> geev1 test ended with success." <<std::endl;
}

void EigenProblemsTest::testGeev2()
{
  std::cout << "--> Test: geev2." <<std::endl;
  // Compute only left eigenvectors.
  complex_matrix fake(1,1), leftV(size,size);
  complex_vector eigenval(size);
  Siconos::eigenproblems::geev(*A, eigenval, leftV, fake, true, false);
  complex_vector error(size);
  for(unsigned int i = 0; i < size; ++i) error(i) = 0.0;
  for(unsigned int i = 0; i < size; ++i)
  {
    error.plus_assign(ublas::prod(conj(column(leftV, i)), *A->dense()));
    error.minus_assign(eigenval(i)*conj(column(leftV,i)));
  }
  // Check ...
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testGeev2 1: ", norm_2(error) < 10 * std::numeric_limits< double >::epsilon(), true);
  // Check if A has not been modified
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testGeev2 2: ", (*A) == (*Aref), true);

  std::cout << "--> geev1 test ended with success." <<std::endl;
}

void EigenProblemsTest::testGeev3()
{
  std::cout << "--> Test: geev3." <<std::endl;

  // Compute left and right eigenvectors.
  complex_matrix leftV(size,size), rightV(size,size);
  complex_vector eigenval(size);
  Siconos::eigenproblems::geev(*A, eigenval, leftV, rightV, true, true);
  complex_vector error(size);
  for(unsigned int i = 0; i < size; ++i) error(i) = 0.0;
  for(unsigned int i = 0; i < size; ++i)
  {
    error.plus_assign(ublas::prod(*A->dense(), column(rightV, i)));
    error.minus_assign(eigenval(i)*column(rightV,i));
    error.plus_assign(ublas::prod(conj(column(leftV, i)), *A->dense()));
    error.minus_assign(eigenval(i)*conj(column(leftV,i)));
  }
  std::cout << norm_2(error) << std::endl;
  // Check ...
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testGeev3 1: ", norm_2(error) < size * 10 * std::numeric_limits< double >::epsilon(), true);
  // Check if A has not been modified
  CPPUNIT_ASSERT_EQUAL_MESSAGE("testGeev3 2: ", (*A) == (*Aref), true);

  std::cout << "--> geev3 test ended with success." <<std::endl;
}

void EigenProblemsTest::End()
{
  std::cout << "======================================" <<std::endl;
  std::cout << " ===== End of EigenProblems tests ===== " <<std::endl;
  std::cout << "======================================" <<std::endl;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  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)

//! \file
//!\brief Tests for the trigonometric arcsine function of (fixed_point) for a small digit range.

#include <cmath>

#define BOOST_TEST_MODULE test_negatable_func_arcsine_small
#define BOOST_LIB_DIAGNOSTIC

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_CASE(test_negatable_func_arcsine_small)
{
  typedef boost::fixed_point::negatable<7, -24> fixed_point_type;
  typedef fixed_point_type::float_type          float_point_type;

  const fixed_point_type tol = ldexp(fixed_point_type(1), fixed_point_type::resolution + 7);

  // Check positive arguments.
  for(int i = 0; i <= 32; ++i)
  {
    const fixed_point_type x = asin(fixed_point_type(i) / 32);

    using std::asin;
    const float_point_type y = asin(float_point_type(i) / 32);

    BOOST_CHECK_CLOSE_FRACTION(x, fixed_point_type(y), tol);
  }

  // Check negative arguments.
  for(int i = 0; i <= 32; ++i)
  {
    const fixed_point_type x = asin(fixed_point_type(-i) / 32);

    using std::asin;
    const float_point_type y = asin(float_point_type(-i) / 32);

    BOOST_CHECK_CLOSE_FRACTION(x, fixed_point_type(y), tol);
  }
}


/*
 * KMeans.cpp
 *
 *  Created on: Mar 12, 2016
 *      Author: zxi
 */

#include "Clustering.h"
#include <Eigen/Eigenvalues>

#include <iostream>
#include <algorithm>
#include <cfloat>
#include <ctime>

namespace masc {
namespace clustering {

#define TIMING(code, verbosity, output) \
  { \
    auto s = clock(); \
    (code); \
    auto e = clock(); \
    if (this->m_verbosity >= (verbosity) ) \
      std::cout<< (output) << " takes " << (e-s)*1.0 / CLOCKS_PER_SEC << " s" << std::endl; \
  } \

template<class T>
VectorXi ClusteringBase<T>::labelsInertia(const MatrixXd& X,
    const MatrixXd& centriods, VectorXd* distances, double* inertia) {

  const int n_samples = X.rows();
  const int n_k = centriods.rows();

  *distances = VectorXd(n_samples);
  *inertia = 0.0;

  VectorXi labels(n_samples);

  for (int i = 0; i < n_samples; ++i) {
    double min_dist = FLT_MAX;
    int label = -1;
    for (int j = 0; j < n_k; ++j) {
      double dist = (X.row(i) - centriods.row(j)).norm();
      if (dist < min_dist) {
        label = j;
        min_dist = dist;
      }
    }

    *inertia += min_dist;
    labels(i) = label;
    (*distances)(i) = min_dist;
  }

  return labels;
}

template<class T>
KMeansBase<T>::KMeansBase(int n_clusters, int n_init, int max_iter, double tol) :
    m_n_clusters(n_clusters), m_n_init(n_init), m_max_iter(max_iter), m_tol(tol) {
//TODO
}

template<class T>
KMeansBase<T>::~KMeansBase() {
  // nothing to do here
}

template<class T>
MatrixXd KMeansBase<T>::initCentroids(const MatrixXd& X) {
  const int n_samples = X.rows();
  const int n_features = X.cols();

  MatrixXd centroids(m_n_clusters, n_features);

  MatrixXd best_centroids;
  VectorXd distences;
  double best_score = FLT_MAX;

  std::uniform_int_distribution<int> dist(0, n_samples);

  for (int r = 0; r < m_n_init; ++r) {

    for (int i = 0; i < m_n_clusters; ++i) {
      int sample = dist(this->m_rd);
      centroids.row(i) = X.row(sample);
    }

    double score;
    this->labelsInertia(X, centroids, &distences, &score);

    if (score < best_score) {
      best_score = score;
      best_centroids = centroids;
    }

  }

  return best_centroids;
}

template<class T>
MatrixXd KMeansBase<T>::updateCenters(const MatrixXd& X,
    const VectorXi& labels) {
  const int n_samples = X.rows();
  const int n_features = X.cols();

  MatrixXd centroids = Eigen::MatrixXd::Zero(m_n_clusters, n_features);
  VectorXi count = Eigen::VectorXi::Zero(m_n_clusters);

  for (int i = 0; i < n_samples; ++i) {
    centroids.row(labels[i]) += X.row(i);
    count(labels[i]) += 1;
  }

  for (int i = 0; i < m_n_clusters; ++i) {
    if (count(i) > 0)
      centroids.row(i) /= count(i);
  }

  return centroids;
}

template<class T>
T& KMeansBase<T>::fit(const MatrixXd& X) {
  auto centers = this->initCentroids(X);

  if (this->m_verbosity >= 2)
    std::cout << "KMeansBase::fit - init centers = " << std::endl << centers << std::endl;

  VectorXd distances;

  double best_inertia = FLT_MAX / 2.0;
  VectorXi best_labels;
  MatrixXd best_centers;

  for (int i = 0; i < m_max_iter; ++i) {
    auto old_centers = centers;
    double inertia = 0.0;
    auto labels = this->labelsInertia(X, old_centers, &distances, &inertia);
    centers = this->updateCenters(X, labels);

    if (this->m_verbosity >= 1)
      std::cout << "KMeansBase::fit - iter " << i << " inertia = " << inertia << std::endl;

    if (inertia < best_inertia) {
      best_inertia = inertia;
      best_labels = labels;
      best_centers = centers;
    }

    auto shift = (old_centers - centers).norm();

    if (shift * shift < this->m_tol) {
      if (this->m_verbosity >= 1)
        std::cout << "KMeansBase::fit - Converged at iteration " << i << std::endl;
      break;
    }
  }

  this->m_cluster_centers = best_centers;
  this->m_labels = this->labelsInertia(X, best_centers, &distances,
      &best_inertia);
  this->m_inertia = best_inertia;

  return static_cast<T&>(*this);
}

///////////////////////////////////////////////////////////////////////////////
// KMeans
///////////////////////////////////////////////////////////////////////////////
KMeans::KMeans(int n_clusters, int n_init, int max_iter, double tol) :
    KMeansBase<KMeans>(n_clusters, n_init, max_iter, tol) {

}

KMeans::~KMeans() {
  //TODO
}

///////////////////////////////////////////////////////////////////////////////
// SpectralClustering
///////////////////////////////////////////////////////////////////////////////
SpectralClustering::SpectralClustering(int n_clusters, double gamma,
    AffinityType affinity_type) :
    KMeansBase<SpectralClustering>(n_clusters), m_affinity_type(affinity_type), m_gamma(
        gamma) {
//TODO
}

SpectralClustering::~SpectralClustering() {
  //TODO
}

MatrixXd SpectralClustering::constructAffinityMatrix(const MatrixXd& X,
    AffinityType affinity_type) {

  const int n_samples = X.rows();

  MatrixXd W(n_samples, n_samples);

  switch (affinity_type) {
  case AffinityType::RBF:

    // compute pairwise distance
    for (int i = 0; i < n_samples; ++i) {
      W(i, i) = 1.0;
      for (int j = i + 1; j < n_samples; ++j) {
        double dist = (X.row(i) - X.row(j)).norm();
        W(i, j) = W(j, i) = exp(-(this->m_gamma) * dist * dist);
      }
    }

    break;
  default:
    std::cerr << "Unsupported affinity type " << (int) affinity_type
        << std::endl;
    break;
  }

  return W;
}

SpectralClustering& SpectralClustering::fit(const MatrixXd& X) {
  const int n_samples = X.rows();
  const int n_features = X.cols();

  // Affinity matrix
  MatrixXd W;
  TIMING(W = this->constructAffinityMatrix(X, this->m_affinity_type), 1,
      "SpectralClustering::fit - construct affinity matrix");

  // Degree matrix
  MatrixXd D = W.rowwise().sum().asDiagonal();

  // Laplacian matrix
  MatrixXd L = D - W;

  Eigen::SelfAdjointEigenSolver<MatrixXd> es(n_samples);
  TIMING(es.compute(L), 1, "SpectralClustering::fit - compute eigen vectors");

  const MatrixXd& evs = es.eigenvectors();

  // embedded matrix (n_samples * k)
  MatrixXd embed(n_samples, m_n_clusters);

  for (int i = 0; i < m_n_clusters; ++i)
    embed.col(i) = evs.col(i);

  // run kmeans clustering on embed
  TIMING(KMeansBase<SpectralClustering>::fit(embed), 1,
      "SpectralClustering::fit - kmeans clustering");

  return *this;
}

} /* namespace clustering */
} /* namespace masc */


/* Copyright (c) 2015 Oleg Morozenkov
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include <boost/test/unit_test.hpp>

#include <test/math/Vector.h>

BOOST_AUTO_TEST_SUITE(tVector)

BOOST_AUTO_TEST_CASE(tvectorCalculateLengthSq) {
	Vector a = { 4, 4, 2 };
	BOOST_CHECK_EQUAL(vectorGetLengthSq(a), 36);
}

BOOST_AUTO_TEST_CASE(tvectorCalculateLength) {
	Vector a = { 4, 4, 2 };
	BOOST_CHECK_EQUAL(vectorGetLength(a), 6);
}

BOOST_AUTO_TEST_CASE(tvectorIsEqual) {
	Vector a = { 1, 2, 3 };
	Vector b = { 4, 5, 6 };
	BOOST_CHECK(!vectorIsEqual(a, b));
	BOOST_CHECK(vectorIsEqual(a, a));
}

BOOST_AUTO_TEST_CASE(tvectorGetOpposite) {
	Vector a = { 1, 2, 3 };
	Vector r = { -1, -2, -3 };
	BOOST_CHECK(vectorIsEqual(vectorGetOpposite(a), r));
}

BOOST_AUTO_TEST_CASE(tvectorSum) {
	Vector a = { 1, 2, 3 };
	Vector b = { 4, 5, 6 };
	Vector r = { 5, 7, 9 };
	BOOST_CHECK(vectorIsEqual(vectorSum(a, b), r));
}

BOOST_AUTO_TEST_CASE(tvectorSubstract) {
	Vector a = { 1, 2, 3 };
	Vector b = { 4, 5, 6 };
	Vector r = { -3, -3, -3 };
	BOOST_CHECK(vectorIsEqual(vectorSubstract(a, b), r));
}

BOOST_AUTO_TEST_CASE(tvectorMultiply) {
	Vector a = { 1, 2, 3 };
	Vector r = { 4, 8, 12 };
	BOOST_CHECK(vectorIsEqual(vectorMultiply(a, 4), r));
}

BOOST_AUTO_TEST_CASE(tvectorDivide) {
	Vector a = { 4, 8, 12 };
	Vector r = { 1, 2, 3 };
	BOOST_CHECK(vectorIsEqual(vectorDivide(a, 4), r));
}

BOOST_AUTO_TEST_CASE(tvectorDotProduct) {
	Vector a = { 1, 2, 3 };
	Vector b = { 4, 5, 6 };
	BOOST_CHECK_EQUAL(vectorDotProduct(a, b), 32);
}

BOOST_AUTO_TEST_CASE(tvectorCrossProduct) {
	Vector a = { 1, 2, 3 };
	Vector b = { 4, 5, 6 };
	Vector r = { -3, 6, -3 };
	BOOST_CHECK(vectorIsEqual(vectorCrossProduct(a, b), r));
}

BOOST_AUTO_TEST_CASE(tvectorNormalize) {
	Vector a = { 1, 2, 3 };
	BOOST_CHECK_EQUAL(vectorGetLength(vectorNormalize(a)), 1);
}

BOOST_AUTO_TEST_SUITE_END()