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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2017 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2016, 2017, 2018.
// Modifications copyright (c) 2016-2018, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

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

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_AREA_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_AREA_HPP

#include <boost/mpl/if.hpp>

//#include <boost/geometry/arithmetic/determinant.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/strategies/area.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace area
{

/*!
\brief Cartesian area calculation
\ingroup strategies
\details Calculates cartesian area using the trapezoidal rule
\tparam CalculationType \tparam_calculation

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

*/
template
<
    typename CalculationType = void
>
class cartesian
{
public :
    template <typename Geometry>
    struct result_type
        : strategy::area::detail::result_type
            <
                Geometry,
                CalculationType
            >
    {};

    template <typename Geometry>
    class state
    {
        friend class cartesian;

        typedef typename result_type<Geometry>::type return_type;

    public:
        inline state()
            : sum(0)
        {
            // Strategy supports only 2D areas
            assert_dimension<Geometry, 2>();
        }

    private:
        inline return_type area() const
        {
            return_type const two = 2;
            return sum / two;
        }

        return_type sum;
    };

    template <typename PointOfSegment, typename Geometry>
    static inline void apply(PointOfSegment const& p1,
                             PointOfSegment const& p2,
                             state<Geometry>& st)
    {
        typedef typename state<Geometry>::return_type return_type;

        // Below formulas are equivalent, however the two lower ones
        // suffer less from accuracy loss for great values of coordinates.
        // See: https://svn.boost.org/trac/boost/ticket/11928

        // SUM += x2 * y1 - x1 * y2;
        // state.sum += detail::determinant<return_type>(p2, p1);

        // SUM += (x2 - x1) * (y2 + y1)
        //state.sum += (return_type(get<0>(p2)) - return_type(get<0>(p1)))
        //           * (return_type(get<1>(p2)) + return_type(get<1>(p1)));

        // SUM += (x1 + x2) * (y1 - y2)
        st.sum += (return_type(get<0>(p1)) + return_type(get<0>(p2)))
                * (return_type(get<1>(p1)) - return_type(get<1>(p2)));
    }

    template <typename Geometry>
    static inline typename result_type<Geometry>::type
        result(state<Geometry>& st)
    {
        return st.area();
    }

};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{
    template <>
    struct default_strategy<cartesian_tag>
    {
        typedef strategy::area::cartesian<> type;
    };

} // namespace services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::area

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_AREA_HPP


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EULER_FUNCTIONS_SCALAR_ERF_HPP_INCLUDED
#define NT2_EULER_FUNCTIONS_SCALAR_ERF_HPP_INCLUDED

#include <nt2/euler/functions/erf.hpp>
#include <nt2/euler/functions/details/erf_kernel.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/twothird.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/exp.hpp>
#include <nt2/include/functions/scalar/is_ltz.hpp>
#include <nt2/include/functions/scalar/negif.hpp>
#include <nt2/include/functions/scalar/oneminus.hpp>
#include <nt2/include/functions/scalar/oneplus.hpp>
#include <nt2/include/functions/scalar/sqr.hpp>
#include <nt2/include/functions/scalar/sqrt.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>

#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <nt2/include/functions/scalar/is_nan.hpp>
#endif

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/functions/scalar/is_inf.hpp>
#include <nt2/include/functions/scalar/signnz.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( erf_, tag::cpu_
                            , (A0)
                            , (scalar_< double_<A0> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      #ifndef BOOST_SIMD_NO_INVALIDS
      if(is_nan(a0)) return a0;
      #endif
      A0 x =  nt2::abs(a0);
      A0 xx = nt2::sqr(x);
      if(x<= A0(0.0000000001))
        return a0*nt2::Two<A0>()/nt2::sqrt(Pi<A0>());
      else if (x<= A0(0.65))
      {
        return a0*details::erf_kernel<A0>::erf1(xx);
      }
      else if(x<= A0(2.2))
      {
        A0 z = oneminus(exp(-xx)*details::erf_kernel<A0>::erfc2(x));
        return negif(is_ltz(a0), z);
      }
      else if(x<= A0(6))
      {
        A0 z = nt2::oneminus(exp(-xx)*details::erf_kernel<A0>::erfc3(x));
        return nt2::negif(nt2::is_ltz(a0), z);
      }
      else
        return nt2::One<A0>();
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( erf_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      #ifndef BOOST_SIMD_NO_INVALIDS
      if(is_nan(a0)) return a0;
      #endif
      #ifndef BOOST_SIMD_NO_INFINITIES
      if (nt2::is_inf(a0)) return signnz(a0);
      #endif
      A0 x =  nt2::abs(a0);
      if (x < Twothird<A0>())
      {
        return a0*details::erf_kernel<A0>::erf1(sqr(x));
      }
      else
      {
       A0 z = x/oneplus(x)-A0(0.4);
        A0 r2 =   oneminus(exp(-sqr(x))*details::erf_kernel<A0>::erfc2(z));
        if (a0 < Zero<A0>()) r2 = -r2;
        return r2;
      }

    }
  };
} }

#endif


#include <iostream>

#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>

#include "Dynamics.h"

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

using namespace boost::numeric::odeint;

Dynamics::Dynamics() : PI(3.14159265359), rng_(new boost::mt19937()),
                       nd_(0, 1.0), var_nor(rng_, nd_),
                       process_noise_(0), measurement_noise_(0)
{
     for (int i = 0; i < 5; i++) {
          u_[i] = 0;
          x0_[i] = 0;
          state_[i] = 0;
     }

     rng_->seed((++seed_) + time(NULL));
}

std::vector<double> Dynamics::time_vector(double t0, double dt, double tend)
{
     std::vector<double> tt;
     for( double t = t0; t < tend ; t += dt ) {
          tt.push_back(t);
     }
     return tt;
}

void Dynamics::set_time(double t0, double dt, double tend)
{
     t0_ = t0;
     dt_ = dt;
     tend_ = tend;

     tt_.clear();
     for( double t = t0_ ; t < tend_ ; t += dt_ ) {
          tt_.push_back(t);
     }
}

void Dynamics::set_x0(state_5d_type x0)
{
     for(int i = 0; i < 5; i++) {
          x0_[i] = x0[i];
          state_[i] = x0[i];
          measurement_[i] = 0;
     }

     measurement_[0] = x0[0] + var_nor() * measurement_noise_;
     measurement_[1] = x0[1] + var_nor() * measurement_noise_;
}

void Dynamics::compute_trajectory()
{
     headings_.clear();
     truth_points_.clear();

     if (model_ == cart || model_ == roomba) {
          state_5d_type x = {x0_[0], x0_[1], x0_[2], x0_[3], x0_[4]};
          runge_kutta4< state_5d_type > stepper;

          std::vector<double>::iterator it = tt_.begin();
          for (; it != tt_.end(); it++) {
               cv::Point2d truth, measured;

               truth = cv::Point2d(x[0], x[1]);

               measured.x = truth.x + var_nor() * measurement_noise_;
               measured.y = truth.y + var_nor() * measurement_noise_;

               headings_.push_back(x[2]*180.0/PI);
               truth_points_.push_back(truth);
               measured_points_.push_back(measured);

               state_5d_type state;
               state[0] = x[0];
               state[1] = x[1];
               state[2] = x[2];
               state[3] = 0;
               state[4] = 0;
               states_.push_back(state);

               if (model_ == cart) {
                    stepper.do_step(make_ode_wrapper( *this , &Dynamics::cart_model ), x , *it , dt_ );
               } else {
                    stepper.do_step(make_ode_wrapper( *this , &Dynamics::roomba_model ), x , *it , dt_ );
               }
          }
     } else if (model_ == constant_velocity) {
     }
}

void Dynamics::step_motion_model(double dt, double time)
{
     if (input_sequence_.size() > 0 && input_sequence_.front().time <= time) {
          cout << "Using input: " << endl;
          for (int i = 0; i < 5; i++) {
               u_[i] = input_sequence_.front().input[i];

               printf("%d : %f\n", i, u_[i]);
          }
          input_sequence_.erase(input_sequence_.begin());
     }

     if (model_ == cart || model_ == roomba) {
          if (model_ == cart) {
               stepper_.do_step(make_ode_wrapper( *this , &Dynamics::cart_model ), state_ , 0 , dt );
          } else {
               stepper_.do_step(make_ode_wrapper( *this , &Dynamics::roomba_model ), state_ , 0 , dt );
          }

          cv::Point2d truth, measured;
          truth = cv::Point2d(state_[0], state_[1]);

          measured.x = truth.x + var_nor() * measurement_noise_;
          measured.y = truth.y + var_nor() * measurement_noise_;

          measurement_[0] = measured.x;
          measurement_[1] = measured.y;
          measurement_[2] = 0;
          measurement_[3] = 0;
          measurement_[4] = 0;

          headings_.push_back(state_[2]*180.0/PI);
          truth_points_.push_back(truth);
          measured_points_.push_back(measured);

     } else if (model_ == constant_velocity) {
     }
}

void Dynamics::set_input(state_5d_type input)
{
     input_sequence_.clear();

     ControlInput control_input;
     control_input.time = 0;
     for(int i = 0; i < 5; i++) {
          control_input.input[i] = input[i];
     }

     input_sequence_.push_back(control_input);
}

void Dynamics::set_input(state_5d_type input, double time)
{
     ControlInput control_input;
     control_input.time = time;
     for(int i = 0; i < 5; i++) {
          control_input.input[i] = input[i];
     }

     input_sequence_.push_back(control_input);
}

void Dynamics::set_input_sequence(std::vector<ControlInput> &input_sequence)
{
     input_sequence_.clear();

     for (std::vector<ControlInput>::iterator it = input_sequence.begin();
          it != input_sequence.end(); it++) {

          ControlInput control_input;
          control_input.time = it->time;
          for(int i = 0; i < 5; i++) {
               control_input.input[i] = it->input[i];
          }
          input_sequence_.push_back(control_input);
     }
}

void Dynamics::cart_model(const state_5d_type &x , state_5d_type &dxdt , double t)
{
     /// 0 : x-position
     /// 1 : y-position
     /// 2 : theta

     //u_[0] = 2;
     //u_[1] = 3.14159265359/20;

     double u = u_[0];
     double u_theta = u_[1];
     double L = 3;

     dxdt[0] = u*cos(x[2]) + var_nor() * process_noise_;
     dxdt[1] = u*sin(x[2]) + var_nor() * process_noise_;
     dxdt[2] = u/L*tan(u_theta) + var_nor() * process_noise_;
     dxdt[3] = 0;
     dxdt[4] = 0;
}

void Dynamics::roomba_model(const state_5d_type &x , state_5d_type &dxdt , double t)
{
     /// 0 : x-position
     /// 1 : y-position
     /// 2 : theta

     double R = 1;
     double b = 0.5;

     double w_l = u_[0];
     double w_r = u_[1];

     dxdt[0] = R*cos(x[2])*(w_r + w_l)/2;
     dxdt[1] = R*sin(x[2])*(w_r + w_l)/2;
     dxdt[2] = R*(w_r-w_l)/(2*b);
     dxdt[3] = 0;
     dxdt[4] = 0;
}

//void Dynamics::aircraft_6DOF_model(const state_type &x , state_type &dxdt , double t)
//{
//     /// 0:  u     : surge velocity
//     /// 1:  v     : sway velocity
//     /// 2:  w     : heave velocity
//     /// 3:  p     : roll rate
//     /// 4:  q     : pitch rate
//     /// 5:  r     : yaw rate
//     /// 6:  xpos  : earth x-pos
//     /// 7:  ypos  : earth y-pos
//     /// 8:  zpos  : earth z-pos
//     /// 9:  phi   : roll angle
//     /// 10: theta : pitch angle
//     /// 11: psi   : yaw angle
//
//     // Get current state values
//     double u     = x[0];
//     double v     = x[1];
//     double w     = x[2];
//     double p     = x[3];
//     double q     = x[4];
//     double r     = x[5];
//     //double xpos  = x[6];
//     //double ypos  = x[7];
//     //double zpos  = x[8];
//     double phi   = (x[9]);
//     double theta = (x[10]);
//     double psi   = (x[11]);
//
//     // Precalculate trig functions
//     double c1 = cos(phi);
//     double c2 = cos(theta);
//     double c3 = cos(psi);
//     double s1 = sin(phi);
//     double s2 = sin(theta);
//     double s3 = sin(psi);
//     double t2 = tan(theta);
//
//     //double m = 4536; // mass
//     double m = 1;
//
//     // Thrust is in surge direction
//
//     // Force Inputs
//     double Fx = u_[0];
//     double Fy = 0;
//     double Fz = 0;
//     double Fk = u_[1]; // roll
//     double Fm = u_[2]; // pitch
//     double Fn = 0; // yaw
//
//     // For x-z symmetry:
//     // 1.) Ixy = Iyx = 0
//     // 2.) Iyz = Izy = 0
//     //double Ixx = 23;
//     //double Ixy = 0;
//     //double Ixz = 2.97;
//     //double Iyy = 15.13;
//     //double Iyz = 0;
//     //double Izz = 16.99;
//
//     double Ixx = 1;
//     double Ixy = 0;
//     double Ixz = 0;
//     double Iyy = 1;
//     double Iyz = 0;
//     double Izz = 1;
//
//     //double Ixx = 35926.5;
//     //double Ixy = 0;
//     //double Ixz = 3418.17;
//     //double Iyy = 33940.7;
//     //double Iyz = 0;
//     //double Izz = 67085.5;
//
//     // Calculate Translational Forces
//     double X = Fx - m*q*w + m*r*v;
//     double Y = Fy - m*r*u + m*p*w;
//     double Z = Fz - m*p*v + m*q*u;
//
//     // Calculate Rotational Forces
//     double K = Fk + Iyz*(pow(q,2)-pow(r,2)) + Ixz*p*q - Ixy*r*p + (Iyy-Izz)*q*r;
//     double M = Fm + Ixz*(pow(r,2)-pow(p,2)) + Ixy*q*r - Iyz*p*q + (Izz-Ixx)*r*p;
//     double N = Fn + Ixy*(pow(p,2)-pow(q,2)) + Iyz*r*p - Ixz*q*r + (Ixx-Iyy)*p*q;
//
//     // Calculate angles
//     double P = p + (q*s1 + r*c1)*t2;
//     double Q = q*c1 - r*s1;
//     double R = (q*s1 + r*c1)*1.0/c2;
//
//     // Calculate Velocities and Rates
//     // xdot(1:6) = inv(A)*[X Y Z K M N]'
//     Eigen::MatrixXd A, Forces, derivs;
//     A.resize(9,9); Forces.resize(9,1);
//
//     A << m, 0, 0,   0,    0,    0,  0, 0, 0,
//          0, m, 0,   0,    0,    0,  0, 0, 0,
//          0, 0, m,   0,    0,    0,  0, 0, 0,
//          0, 0, 0,  Ixx, -Ixy, -Ixz, 0, 0, 0,
//          0, 0, 0, -Ixy,  Iyy, -Iyz, 0, 0, 0,
//          0, 0, 0, -Ixz, -Iyz,  Izz, 0, 0, 0,
//          0, 0, 0,   0,    0,    0,  1, 0, 0,
//          0, 0, 0,   0,    0,    0,  0, 1, 0,
//          0, 0, 0,   0,    0,    0,  0, 0, 1;
//
//
//     Forces << X, Y, Z, K, M, N, P, Q, R;
//
//     derivs = A.inverse() * Forces;
//
//     dxdt[0] = derivs(0,0);
//     dxdt[1] = derivs(1,0);
//     dxdt[2] = derivs(2,0);
//     dxdt[3] = derivs(3,0);
//     dxdt[4] = derivs(4,0);
//     dxdt[5] = derivs(5,0);
//
//     // Calculate Positions
//     dxdt[6] = c3*c2*u + (c3*s2*s1-s3*c1)*v + (s3*s1+c3*c1*s2)*w;
//     dxdt[7] = s3*c2*u + (c1*c3+s1*s2*s3)*v + (c1*s2*s3-c3*s1)*w;
//     dxdt[8] = -s2*u + c2*s1*v + c1*c2*w;
//
//     // Calculate Angles
//     dxdt[9] = derivs(6,0);
//     dxdt[10] = derivs(7,0);
//     dxdt[11] = derivs(8,0);
//}


/***********************************************************************
 *                      Kuramoto oscillators                           *
 *             Copyright (c) 2014-2015 Alex Khrabrov                   *
 ***********************************************************************
 * This program is free software. It comes without any warranty, to    *
 * the extent permitted by applicable law. You can redistribute it     *
 * and/or modify it under the terms of the Do What The Fuck You Want   *
 * To Public License, Version 2, as published by Sam Hocevar. See      *
 * license text below.                                                 *
 ***********************************************************************
 *                                                                     *
 *            DO WHAT THE FUCK YOU WANT TO PUBLIC LICENSE              *
 *                    Version 2, December 2004                         *
 *                                                                     *
 * Copyright (C) 2004 Sam Hocevar <sam@hocevar.net>                    *
 *                                                                     *
 * Everyone is permitted to copy and distribute verbatim or modified   *
 * copies of this license document, and changing it is allowed as long *
 * as the name is changed.                                             *
 *                                                                     *
 *            DO WHAT THE FUCK YOU WANT TO PUBLIC LICENSE              *
 *   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION   *
 *                                                                     *
 *  0. You just DO WHAT THE FUCK YOU WANT TO.                          *
 *                                                                     *
 ***********************************************************************/

typedef double fp_type;

#define _USE_MATH_DEFINES
#include <cmath>
static const fp_type TWO_PI = 2.0 * M_PI;
#undef _USE_MATH_DEFINES

#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>
#include <map>
#include <random>
#include <algorithm>
#include <numeric>

#include <cassert>
#include <cstdint>
#include <cstdlib>

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#if defined(WIN32) || defined(_WIN32)
#define PATH_SEPARATOR "\\"
#else
#define PATH_SEPARATOR "/"
#endif

#ifdef _MSC_VER
#define FORCE_INLINE __forceinline
#else
#define FORCE_INLINE inline
#endif

//--------------------------------------------------------------------------------------------------

template<typename T>
FORCE_INLINE T mean(const std::vector<T> & v)
{
    const auto size = v.size();
    assert(size != 0);
    T sum(0);
    for(typename std::vector<T>::size_type i = 0; i < size; i++)
        sum += v[i];
    return sum / T(size);
}

static std::vector<fp_type> tmp_s;
static std::vector<fp_type> tmp_c;

template<typename T>
T calc_order_parameter(const std::vector<T> & phases)
{
    using namespace std;
    const auto size = phases.size();
    tmp_s.resize(size);
    tmp_c.resize(size);
    for(typename std::vector<T>::size_type i = 0; i < size; i++)
    {
        const T p = phases[i];
        tmp_s[i] = sin(p);
        tmp_c[i] = cos(p);
    }
    const T mean_sin = mean(tmp_s);
    const T mean_cos = mean(tmp_c);
    return sqrt(mean_cos * mean_cos + mean_sin * mean_sin);
}

template<typename T>
FORCE_INLINE T rand_range(T min, T max)
{
    assert(max > min);
    return ((T(rand()) / T(RAND_MAX)) * (max - min)) + min;
}

//--------------------------------------------------------------------------------------------------

template<typename T>
FORCE_INLINE T wrap_phase_1(T p)
{
    if(p > T(TWO_PI))
        p -= T(TWO_PI);
    else if(p < 0.0)
        p += T(TWO_PI);
    return p;
}

template<typename T>
FORCE_INLINE T wrap_phase_2(T p)
{
    while(p < 0.0 || p > T(TWO_PI))
    {
        if(p > T(TWO_PI))
            p -= T(TWO_PI);
        else if(p < 0.0)
            p += T(TWO_PI);
    }
}

//--------------------------------------------------------------------------------------------------

template<typename T>
FORCE_INLINE T kuramoto_classic(const T phi)
{
    return std::sin(phi);
}

template<int n, typename T>
FORCE_INLINE T kuramoto_n(const T phi)
{
    return std::sin(T(n) * phi);
}

template<typename T>
FORCE_INLINE T daido_original(const T phi)
{
    return std::sin(phi) +
        T(0.2) * std::cos(phi) -
        T(0.3) * std::sin(T(2.0) * phi) +
        T(0.6) * std::cos(T(2.0) * phi) +
        T(0.7) * std::sin(T(3.0) * phi) -
        T(0.4) * std::cos(T(3.0) * phi);
}

template<typename T>
FORCE_INLINE T coupling_1(const T phi)
{
    return std::sin(phi) + std::sin(T(3.0) * phi);
}

template<typename T>
FORCE_INLINE T coupling_2(const T phi)
{
    return std::sin(phi) +
           std::sin(T(3.0) * phi) +
           std::sin(T(6.0) * phi);
}

template<typename T>
FORCE_INLINE T coupling_3(const T phi)
{
    return std::sin(phi) + std::cos(T(3.0) * phi);
}

template<typename T>
FORCE_INLINE T coupling_4(const T phi)
{
    return std::cos(phi) + std::cos(T(3.0) * phi);
}

//--------------------------------------------------------------------------------------------------

template<typename T>
void write_vector(std::ostream & os, const std::vector<T> & vec)
{
    for(const auto x : vec)
        os << x << ' ';
}

template<typename T>
void dump_state
(
    const std::string & out_file_name,
    const std::vector<T> & phase,
    const std::vector<T> & vel,
    const unsigned int step,
    const unsigned int N,
    const T r,
    const T mean_phase
)
{
    using namespace std;
    ofstream out(out_file_name, ofstream::trunc);
    out << step << ' ' << N << ' ' << r << ' ' << mean_phase << '\n';
    write_vector(out, phase);
    out << '\n';
    write_vector(out, vel);
}

template<typename T, T(*H)(T)>
void run_simulation
(
    const bool quiet,                    // if true produce no output to console

    const unsigned int N,                // number of oscillators
    const unsigned int N_steps,          // simulations steps

    const T dt,                          // time step

    std::vector<T> phase,                // initial phases
    const std::vector<T> freq,           // oscillator's frequencies
    const std::vector<T> k,              // coupling matrix

    const std::string & name,            // current working set name

    const T noise,                       // noise amplitude

    const T forcing_strength,            // when != 0 all oscillators are forced by external field
    const T forcing_freq,                // external forcing frequency

    const unsigned int dump_interval,    // how often to dump frame snapshots

    const bool r_history_enabled,        // write r values to file
    const bool mean_history_enabled,     // write mean values to file
    const bool mean_vel_history_enabled, // write mean velocity values to file

    const bool freq_modulation_enabled,  // is oscillator frequency modulation enabled
    const std::vector<T> freq_ampl,      // frequency modulation amplitude
    const std::vector<T> freq_freq,      // frequency modulation frequency
    const std::vector<T> freq_offset,    // frequency modulation offset

    const bool k_modulation_enabled,     // is coupling coefficient modulation enabled
    const std::vector<T> k_ampl,         // coupling coefficient modulation amplitude
    const std::vector<T> k_freq,         // coupling coefficient modulation frequency
    const std::vector<T> k_offset,       // coupling coefficient modulation offset

    const std::string & out_dir_name,      // MUST end with directory separator (/ or \)
    const std::string & r_file_path,       // if specified r history will be written to this file
    const std::string & mean_file_path,    // if specified mean phase will be written to this file
    const std::string & mean_vel_file_path // if specified mean velocity will be written to this file
)
{
    using namespace std;

    assert(N > 0 && dt > 0 && phase.size() == N && freq.size() == N && k.size() == N*N);
    assert(freq_modulation_enabled ? (freq_ampl.size() == N && freq_freq.size() == N && freq_offset.size() == N) : true);
    assert(k_modulation_enabled ? (k_ampl.size() == N && k_freq.size() == N*N && k_offset.size() == N*N) : true);

    const T two_dt = T(2.0) * dt;

    const bool add_noise = (noise != 0.0);
    const bool forcing_enabled = (forcing_strength != 0.0);
    const bool dump_enabled = (dump_interval != 0);

    // collect global statistics during simulation
    vector<T> r_hist(N_steps);
    vector<T> mean_hist(N_steps);
    vector<T> mean_vel_hist(N_steps);

    vector<T> phase_old = phase;
    vector<T> phase_old_old = phase;

    vector<T> vel(N);

    unsigned int dump_counter = 0;

    for(unsigned int step = 0; step < N_steps; step++)
    {
        const T t = T(step) * dt;
        for(unsigned int i = 0; i < N; i++)
        {
            T f;
            if(!freq_modulation_enabled)
                f = freq[i];
            else
                f = freq[i] + freq_ampl[i] * sin(T(TWO_PI) * freq_freq[i] * t + freq_offset[i]);

            // NOTE: k[i*N + j] can be interpreted as k[i][j]
            if(!k_modulation_enabled)
            {
                const unsigned int offset = i * N;
                const T phase_old_i = phase_old[i];
                for(unsigned int j = 0; j < N; j++)
                    f += k[offset + j] * H(phase_old[j] - phase_old_i);
            }
            else
            {
                const unsigned int offset = i * N;
                const T phase_old_i = phase_old[i];
                for(unsigned int j = 0; j < N; j++)
                {
                    const unsigned int idx = offset + j;
                    f += (k[idx] + k_ampl[i] * sin(T(TWO_PI) * k_freq[idx] * t + k_offset[idx]))
                        * H(phase_old[j] - phase_old_i);
                }
            }

            if(forcing_enabled)
                f += forcing_strength * sin(forcing_freq * t - phase_old[i]);

            if(add_noise)
                f += rand_range<T>(-noise, noise);

            // calculate new phase
            const T p = phase_old[i] + f * dt;
            phase[i] = wrap_phase_1(p);
        }

        for(unsigned int i = 0; i < N; i++)
            vel[i] = (phase[i] - phase_old_old[i]) / two_dt;

        const T r = calc_order_parameter(phase);
        r_hist[step] = r;

        const T mean_phase = mean(phase);
        mean_hist[step] = mean_phase;

        mean_vel_hist[step] = mean(vel);

        if(dump_enabled)
        {
            if(dump_counter == 0)
            {
                dump_counter = dump_interval;
                const string out_file_name(out_dir_name + to_string(step) + ".txt");
                if(!quiet)
                    cout << "Saving at step: " << step << endl;
                dump_state(out_file_name, phase, vel, step, N, r, mean_phase);
            }
            dump_counter--;
        }
        else if(!(step % 100))
            if(!quiet)
                cout << "Step: " << step << endl;

        phase_old_old = phase_old;
        phase_old = phase;
    }

    if(r_history_enabled)
    {
        ofstream r_out(r_file_path, ofstream::trunc);
        write_vector(r_out, r_hist);
    }

    if(mean_history_enabled)
    {
        ofstream mean_out(mean_file_path, ofstream::trunc);
        write_vector(mean_out, mean_hist);
    }

    if(mean_vel_history_enabled)
    {
        ofstream mean_vel_out(mean_vel_file_path, ofstream::trunc);
        write_vector(mean_vel_out, mean_vel_hist);
    }
}

void read_preset(std::ifstream & input,
                 unsigned int & N,
                 std::vector<double> & freq,
                 std::vector<double> & phase,
                 std::vector<double> & k)
{
    using namespace std;

    uint32_t N_;
    input.read(reinterpret_cast<char*>(&N_), sizeof(N_));
    N = N_;

    freq.resize(N);
    phase.resize(N);
    k.resize(N*N);

    input.read(reinterpret_cast<char*>(freq.data()), N*sizeof(double));
    input.read(reinterpret_cast<char*>(phase.data()), N*sizeof(double));
    input.read(reinterpret_cast<char*>(k.data()), N*N*sizeof(double));
}

void read_freq_modulation_data(std::ifstream & input,
                               unsigned int N,
                               std::vector<double> & freq_ampl,
                               std::vector<double> & freq_freq,
                               std::vector<double> & freq_offset)
{
    using namespace std;

    freq_ampl.resize(N);
    freq_freq.resize(N);
    freq_offset.resize(N);

    input.read(reinterpret_cast<char*>(freq_ampl.data()), N*sizeof(double));
    input.read(reinterpret_cast<char*>(freq_freq.data()), N*sizeof(double));
    input.read(reinterpret_cast<char*>(freq_offset.data()), N*sizeof(double));
}

void read_k_modulation_data(std::ifstream & input,
                            unsigned int N,
                            std::vector<double> & k_ampl,
                            std::vector<double> & k_freq,
                            std::vector<double> & k_offset)
{
    using namespace std;

    k_ampl.resize(N);
    k_freq.resize(N*N);
    k_offset.resize(N*N);

    input.read(reinterpret_cast<char*>(k_ampl.data()), N*sizeof(double));
    input.read(reinterpret_cast<char*>(k_freq.data()), N*N*sizeof(double));
    input.read(reinterpret_cast<char*>(k_offset.data()), N*N*sizeof(double));
}

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

    ios_base::sync_with_stdio(false);

    try
    {
        namespace po = boost::program_options;
        po::options_description desc("Options");
        desc.add_options()
            ("help,h", "print help message")
            ("quiet,q", "produce no output to console")

            ("preset,p", po::value<std::string>()->required(), "preset name")
            ("steps,s", po::value<unsigned int>()->default_value(1000), "simulation steps")
            ("dump-interval,di", po::value<unsigned int>()->default_value(100), "data dump interval")
            ("dt", po::value<double>()->default_value(0.01), "simulation time step")

            ("noise", po::value<double>()->default_value(0.0), "noise level")

            ("coupling,c", po::value<std::string>()->default_value("kuramoto"), "coupling type")

            ("forcing-strength,fs", po::value<double>()->default_value(0.0), "forcing strength")
            ("forcing-freq,ff", po::value<double>()->default_value(0.0), "forcing frequency")

            ("enable-k-modulation", "enable coupling coefficient modulation")
            ("enable-freq-modulation", "enable frequency modulation")

            ("r-file", po::value<std::string>()->implicit_value(""), "r file path")
            ("mean-phase-file", po::value<std::string>()->implicit_value(""), "mean phase file path")
            ("mean-vel-file", po::value<std::string>()->implicit_value(""), "mean velocity file path");

        po::variables_map vm;

        try
        {
            po::store(po::parse_command_line(argc, argv, desc), vm);

            if(vm.count("help"))
            {
                std::cout << std::endl
                          << "kuramoto_simulation - versatile Kuramoto simulation program" << std::endl
                          << std::endl
                          << desc << std::endl;
                return EXIT_SUCCESS;
            }
        }
        catch(const po::error & e)
        {
            std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
            std::cerr << desc << std::endl;
            return EXIT_FAILURE;
        }

        if(!vm.count("preset"))
        {
            std::cerr << "Preset name is required!" << std::endl
                      << "Hint: use -h option to display help." << std::endl;
            return EXIT_FAILURE;
        }

        const bool quiet = vm.count("quiet");

        const string preset_name = vm["preset"].as<std::string>();

        const unsigned int N_steps = vm["steps"].as<unsigned int>();
        unsigned int dump_interval = vm["dump-interval"].as<unsigned int>();
        const fp_type dt = vm["dt"].as<double>();
        const string coupling_type = vm["coupling"].as<std::string>();

        const fp_type noise = vm["noise"].as<double>();

        const fp_type forcing_strength = vm["forcing-strength"].as<double>();
        const fp_type forcing_freq = vm["forcing-freq"].as<double>();

        const bool freq_modulation_enabled = vm.count("enable-freq-modulation");
        const bool k_modulation_enabled = vm.count("enable-k-modulation");

        unsigned int N;
        vector<fp_type> freq;
        vector<fp_type> phase;
        vector<fp_type> k;

        // read preset data
        {
            ifstream input(preset_name + ".preset", ifstream::in | ifstream::binary);

            if(!input)
            {
                cerr << "Unable to open input file: " << preset_name << ".preset" << endl;
                return EXIT_FAILURE;
            }

            input.exceptions(ifstream::failbit | ifstream::badbit);

            read_preset(input, N, freq, phase, k);
        }

        vector<fp_type> freq_ampl;
        vector<fp_type> freq_freq;
        vector<fp_type> freq_offset;

        vector<fp_type> k_ampl;
        vector<fp_type> k_freq;
        vector<fp_type> k_offset;

        if(freq_modulation_enabled)
        {
            if(!quiet)
                cout << "freq_modulation_enabled" << endl;
            ifstream input_s(preset_name + ".fm.preset", ifstream::in | ifstream::binary);
            if (!input_s)
            {
                cerr << "Unable to open frequency modulation data file: " << preset_name << ".fm.preset" << endl;
                return EXIT_FAILURE;
            }
            input_s.exceptions(ifstream::failbit | ifstream::badbit);
            read_freq_modulation_data(input_s, N, freq_ampl, freq_freq, freq_offset);
        }

        if(k_modulation_enabled)
        {
            if(!quiet)
                cout << "k_modulation_enabled" << endl;
            ifstream input_s(preset_name + ".km.preset", ifstream::in | ifstream::binary);
            if (!input_s)
            {
                cerr << "Unable to open k modulation data file: " << preset_name << ".km.preset" << endl;
                return EXIT_FAILURE;
            }
            input_s.exceptions(ifstream::failbit | ifstream::badbit);
            read_k_modulation_data(input_s, N, k_ampl, k_freq, k_offset);
        }

        if(!quiet)
            std::cout << "Data loaded." << std::endl;

        const bool r_history_enabled = vm.count("r-file");
        const bool mean_history_enabled = vm.count("mean-phase-file");
        const bool mean_vel_history_enabled = vm.count("mean-vel-file");

        string r_file_path;
        string mean_file_path;
        string mean_vel_file_path;

        string out_dir_name;

        // initialize dump directory
        if(dump_interval > 0)
        {
            out_dir_name = "dump_" + preset_name + PATH_SEPARATOR + "steps" + PATH_SEPARATOR;
            // std::cout << "Creating steps dir: " << out_dir_name << std::endl;
            boost::filesystem::create_directories(out_dir_name);
        }

        if(r_history_enabled)
        {
            r_file_path = vm["r-file"].as<std::string>();
            if(r_file_path.empty())
                r_file_path = out_dir_name + "r.txt";
            const string path_str = boost::filesystem::path(r_file_path).parent_path().string();
            if(!path_str.empty())
                boost::filesystem::create_directories(path_str);
        }

        if(mean_history_enabled)
        {
            mean_file_path = vm["mean-phase-file"].as<std::string>();
            if(mean_file_path.empty())
                mean_file_path = out_dir_name + "mean.txt";
            const string path_str = boost::filesystem::path(mean_file_path).parent_path().string();
            if(!path_str.empty())
                boost::filesystem::create_directories(path_str);
        }

        if(mean_vel_history_enabled)
        {
            mean_vel_file_path = vm["mean-vel-file"].as<std::string>();
            if(mean_vel_file_path.empty())
                mean_vel_file_path = out_dir_name + "mean_vel.txt";
            const string path_str = boost::filesystem::path(mean_vel_file_path).parent_path().string();
            if(!path_str.empty())
                boost::filesystem::create_directories(path_str);
        }

        //std::cout << "r_file_path: " << r_file_path << std::endl;
        //std::cout << "mean_file_path: " << mean_file_path << std::endl;
        //std::cout << "mean_vel_file_path: " << mean_vel_file_path << std::endl;

#define RUN_SIMUL(coupling)                  \
        run_simulation<fp_type, coupling>(   \
            quiet,                           \
            N, N_steps, dt,                  \
            phase, freq, k, preset_name,     \
            noise,                           \
            forcing_strength, forcing_freq,  \
            dump_interval,                   \
            r_history_enabled,               \
            mean_history_enabled,            \
            mean_vel_history_enabled,        \
            freq_modulation_enabled,         \
            freq_ampl,                       \
            freq_freq,                       \
            freq_offset,                     \
            k_modulation_enabled,            \
            k_ampl,                          \
            k_freq,                          \
            k_offset,                        \
            out_dir_name,                    \
            r_file_path,                     \
            mean_file_path,                  \
            mean_vel_file_path               \
        )

        if(coupling_type == "kuramoto")
            RUN_SIMUL(kuramoto_classic);
        else if(coupling_type == "kuramoto_2")
            RUN_SIMUL(kuramoto_n<2>);
    	else if(coupling_type == "kuramoto_3")
    		RUN_SIMUL(kuramoto_n<3>);
    	else if(coupling_type == "kuramoto_4")
    		RUN_SIMUL(kuramoto_n<4>);
    	else if(coupling_type == "kuramoto_5")
    		RUN_SIMUL(kuramoto_n<5>);
    	else if(coupling_type == "kuramoto_6")
    		RUN_SIMUL(kuramoto_n<6>);
    	else if(coupling_type == "kuramoto_7")
    		RUN_SIMUL(kuramoto_n<7>);
    	else if(coupling_type == "kuramoto_8")
    		RUN_SIMUL(kuramoto_n<8>);
    	else if(coupling_type == "kuramoto_9")
    		RUN_SIMUL(kuramoto_n<9>);
    	else if(coupling_type == "kuramoto_10")
    		RUN_SIMUL(kuramoto_n<10>);
    	else if(coupling_type == "daido")
            RUN_SIMUL(daido_original);
        else if(coupling_type == "coupling_1")
            RUN_SIMUL(coupling_1);
        else if(coupling_type == "coupling_2")
            RUN_SIMUL(coupling_2);
        else if(coupling_type == "coupling_3")
            RUN_SIMUL(coupling_3);
        else if(coupling_type == "coupling_4")
            RUN_SIMUL(coupling_4);
        else
        {
            std::cerr << "Unknown coupling type: " << coupling_type << std::endl;
            return EXIT_FAILURE;
        }
    }
    catch(std::exception& e)
    {
        std::cerr << "Unhandled Exception reached the top of main: "
                  << e.what() << ", application will now exit." << std::endl;
        return EXIT_FAILURE;
    }

    return EXIT_SUCCESS;
}


// Copyright Louis Dionne 2015
// Distributed under the Boost Software License, Version 1.0.

#include <boost/mpl/less.hpp>
#include <boost/mpl/min_element.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/sizeof.hpp>
#include <boost/mpl/vector.hpp>

#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <type_traits>

template <int n>
struct storage { char weight[n]; };

namespace then {
using namespace boost::mpl;

// sample(smallest_type-then)
template <typename ...T>
struct smallest
  : deref<
    typename min_element<
      vector<T...>, less<sizeof_<_1>, sizeof_<_2>>
    >::type
  >
{ };

template <typename ...T>
using smallest_t = typename smallest<T...>::type;

static_assert(std::is_same<
  smallest_t<char, long, long double>, char
>::value, "");
// end-sample

static_assert(std::is_same<
  smallest_t<storage<3>, storage<1>, storage<2>>,
  storage<1>
>::value, "");
}

namespace now {
using namespace boost::hana;

// sample(smallest_type-now)
template <typename ...T>
auto smallest = minimum(tuple_t<T...>, [](auto t, auto u) {
  return sizeof_(t) < sizeof_(u);
});

template <typename ...T>
using smallest_t = typename decltype(smallest<T...>)::type;

static_assert(std::is_same<
  smallest_t<char, long, long double>, char
>::value, "");
// end-sample

static_assert(std::is_same<
  smallest_t<storage<3>, storage<1>, storage<2>>,
  storage<1>
>::value, "");
}

int main() { }


/**
 * @author J Howard Johnson
 * @file sigprune_fet.cc
 * @brief Calculate Fisher's exact test significance levels
 *
 *
 * Technologies langagieres interactives / Interactive Language Technologies
 * Inst. de technologie de l'information / Institute for Information Technology
 * Conseil national de recherches Canada / National Research Council Canada
 * Copyright 2006-2011, Sa Majeste la Reine du Chef du Canada /
 * Copyright 2006-2011, Her Majesty in Right of Canada
 */

#include <iostream>
#include <fstream>
#include <cmath>
#include <boost/math/special_functions/gamma.hpp>

#include "file_utils.h"
#include "arg_reader.h"
#include "printCopyright.h"

using namespace Portage;
using namespace std;

static char help_message[] = "\n\
sigprune_fet [options] [INFILE [OUTFILE]]\n\
\n\
  Copy INFILE to OUTFILE (default stdin to stdout)\n\
  adding 3 fields to the beginning:\n\
     (1) sort key for log p level\n\
     (2) log p level\n\
     (3) flag\n\
         0 : anti-linked (p_level > 0.50)\n\
         1 : linked with log p level > alpha\n\
         2 : linked with log p level = alpha (1-1-1's)\n\
         3 : linked with log p level < alpha\n\
\n\
Options:\n\
\n\
  -v    Write progress reports to cerr.\n\
  -c    Don't cap anti-linked p_levels at 0.5. Using this\n\
        flag ensures monotonic scores as joint frequency decreases while\n\
        marginals are held constant.\n\
  -l    Minimum flag value to output (for pruning)\n\
  -n NL Copy only first NL lines [0 = all]\n\
";

// globals

static bool verbose = false;
static bool no_cap = false;
static Uint num_lines = 0;
static Uint flag_threshold = 0;
static string infile( "-" );
static string outfile( "-" );

// arg processing

void getArgs( int argc, char* argv[] )
{
   const char* switches[] = { "v", "c", "n:", "l:" };
   ArgReader arg_reader( ARRAY_SIZE( switches ), switches, 0, 2, help_message );
   arg_reader.read( argc - 1, argv + 1 );

   arg_reader.testAndSet( "v", verbose );
   arg_reader.testAndSet( "c", no_cap );
   arg_reader.testAndSet( "n", num_lines );
   arg_reader.testAndSet( "l", flag_threshold );

   arg_reader.testAndSet( 0, "infile", infile );
   arg_reader.testAndSet( 1, "outfile", outfile );
}

typedef long double quad;

quad lnfact( double x )
{
   return lgamma( (quad) x + 1.0 );
}

double ln_hyper( double C_xy, double C_x, double C_y, double nn )
{
   quad result =
      - lnfact( C_xy )
      + lnfact( C_x )
      - lnfact( C_x - C_xy )
      + lnfact( C_y )
      - lnfact( C_y - C_xy )
      + lnfact( nn - C_x )
      - lnfact( nn - C_x - C_y + C_xy )
      + lnfact( nn - C_y )
      - lnfact( nn );
   return result;
}

double lnsum( double ln_x, double ln_y )
{
   if ( ln_x < ln_y ) swap( ln_x, ln_y );
   double del = ln_y - ln_x;
   double eps = exp( del );
   if ( del < 10.0 ) return ln_x + log( 1.0 + eps );
   double last_result = 0.0;
   double result = ln_x;
   double power = eps;
   double i = 1.0;
   while ( result != last_result ) {
      last_result = result;
      result = last_result + power / i;
      power *= -eps;
      ++i;
   }
   return result;
//   return ln_x + log( exp( ln_y - ln_x ) + 1.0 );
}

double ln_p_value( double C_xy, double C_x, double C_y, double nn )
{
   double ln_p = ln_hyper( C_xy, C_x, C_y, nn );
   double result = ln_p;
   double C_xY = C_x - C_xy;
   double C_Xy = C_y - C_xy;
   double C_XY = nn - C_x - C_y + C_xy;
   double C_xy_lim = ( ( C_x < C_y ) ? C_x : C_y );
   double last_result = 0.0;
   for ( ++C_xy;
         C_xy <= C_xy_lim && result != last_result;
         ++C_xy ) {
      last_result = result;
      ln_p +=   ( log( C_Xy-- ) + log( C_xY-- ) )
              - ( log( C_xy   ) + log( ++C_XY ) );
      result = lnsum( result, ln_p );
   }
   return result;
}

string write_b64( double i )
{
   string result;
   int nd = 0;
   int digit;
   int d[ 20 ];
   if ( i > 0 ) {
      while ( nd < 20 && i > 0 ) {
         digit = fmod( i, 64.0 );
         d[ nd++ ] = digit;
         i = ( i - digit ) / 64.0;
      }
      result.push_back( 'P' + nd );
      while ( nd > 0 ) {
         result.push_back( '0' + d[ --nd ] );
      }
   } else {
      i = -i;
      while ( nd < 20 && i > 0 ) {
         digit = fmod( i, 64.0 );
         d[ nd++ ] = digit;
         i = ( i - digit ) / 64.0;
      }
      result.push_back( 'O' - nd );
      while ( nd > 0 ) {
         result.push_back( 'o' - d[ --nd ] );
      }
   }
   return result;
}

int main( int argc, char* argv[] )
{
   printCopyright(2006, "sigprune_fet");
   getArgs( argc, argv );

   iSafeMagicStream istr( infile );
   oSafeMagicStream ostr( outfile );

   Uint lineno = 0;
   string line;
   string line_copy;

   ssize_t read;

   int i;
   vector< char * > field;

   while (    getline( istr, line )
           && (    lineno++ < num_lines
                || num_lines == 0 ) ) {
      line_copy = line;
      read = line.length();
      field.resize( 0 );
      field.push_back( &line[ 0 ] );
      for ( i = 0; i < read; ++i ) {
         if (    line[ i ] == '\t'
              || line[ i ] == '\n' ) {
            line[ i ] = '\0';
            field.push_back( &line[ i + 1 ] );
         }
      }
      double C_fr_en = strtod( field[ 1 ], 0 );
      double C_fr    = strtod( field[ 2 ], 0 );
      double C_en    = strtod( field[ 3 ], 0 );
      double nn      = strtod( field[ 4 ], 0 );
      if (    0.0 > C_fr_en
           || C_fr_en > C_fr
           || C_fr_en > C_en
           || C_fr > nn
           || C_en > nn ) {
         error(ETFatal, "Illegal contingency table\n%d : %d : %d : %d : %d", lineno, C_fr_en, C_fr, C_en, nn);
      }
      double minus_ln_nn = -log( nn );
      Uint flag;
      double p_value;

// Anti-linked
      if ( C_fr_en * nn < C_fr * C_en ) {
         flag = 0;
         p_value = no_cap ? ln_p_value( C_fr_en, C_fr, C_en, nn ) : -log( 2.0 );
      }

// 1-1-1 's
      else if ( C_fr_en == 1.0 && C_fr == 1.0 && C_en == 1.0 ) {
         flag = 2;
         p_value = minus_ln_nn;
      }

// Associated less strongly than 1-1-1's
      else {
         p_value = ln_p_value( C_fr_en, C_fr, C_en, nn );
         if ( p_value > minus_ln_nn ) {
            flag = 1;
         }

// Associated more strongly than 1-1-1's
         else {
            flag = 3;
         }
      }

      if ( flag >= flag_threshold ) {
         double s_p_value = floor( p_value * 1.0e8 + 0.5 );
         ostr << write_b64( s_p_value ) << '\t';
         ostr << setprecision( 14 );
         ostr << s_p_value * 1.0e-8 << '\t';
         ostr << flag << '\t';
         ostr << line_copy << '\n';
      }
   }
}


/*=============================================================================
Copyright 2020 Syed Ali Hasan <alihasan9922@gmail.com>

Distributed under the Boost Software License, Version 1.0. (See accompanying
file License.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

#ifndef BOOST_ASTRONOMY_HORIZON_COORD_HPP
#define BOOST_ASTRONOMY_HORIZON_COORD_HPP

#include <iostream>
#include <boost/static_assert.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/units/get_dimension.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/dimensionless.hpp>
#include <boost/units/physical_dimensions/plane_angle.hpp>
#include <boost/astronomy/coordinate/coord_sys/coord_sys.hpp>

/**
 * The Horizon Coordinates, Altitude and Azimuth, of an object
 * in the sky are referred to the plane of the observer’s horizon
 *
 * Azimuth
 * THe Azimuth, is in the range of 0◦ to 360◦ and indicates how far an object
 * in the sky is from the north as measured along an observer’s horizon.
 *
 * Altitude
 * The Altitude, represented by the symbol h, and ranges from −90◦ to +90◦.
 * Positive altitudes indicate objects above the horizon while negative
 * altitudes indicate objects below the horizon.
 *
**/

namespace boost { namespace astronomy { namespace coordinate {

namespace bu = boost::units;
namespace bg = boost::geometry;

template
<
    typename CoordinateType = double,
    typename AltitudeQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>,
    typename AzimuthQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>
>
struct horizon_coord : public coord_sys
    <2, bg::cs::spherical<bg::radian>, CoordinateType>
{
  ///@cond INTERNAL
  BOOST_STATIC_ASSERT_MSG(
      ((std::is_same<typename bu::get_dimension<AltitudeQuantity>::type,
          bu::plane_angle_dimension>::value) &&
       (std::is_same<typename bu::get_dimension<AzimuthQuantity>::type,
           bu::plane_angle_dimension>::value)),
      "Altitude and Azimuth must be of plane angle type");
  BOOST_STATIC_ASSERT_MSG((std::is_floating_point<CoordinateType>::value),
                          "CoordinateType must be a floating-point type");
  ///@endcond
public:
    typedef AltitudeQuantity quantity1;
    typedef AzimuthQuantity quantity2;

    //Default constructor
    horizon_coord() {}

    horizon_coord
    (
        AltitudeQuantity const &Altitude,
        AzimuthQuantity const &Azimuth
    )
    {
        this->set_altitude_azimuth(Altitude, Azimuth);
    }

    //Create tuple of Altitude and Azimuth
    std::tuple<AltitudeQuantity, AzimuthQuantity> get_altitude_azimuth() const
    {
        return std::make_tuple(this->get_altitude(), this->get_azimuth());
    }

    //Get Altitude
    AltitudeQuantity get_altitude() const
    {
        return static_cast<AltitudeQuantity>
            (
                bu::quantity<bu::si::plane_angle, CoordinateType>::from_value
                        (bg::get<0>(this->point))
            );
    }

    //Get Azimuth
    AzimuthQuantity get_azimuth() const
    {
        return static_cast<AzimuthQuantity>
            (
                bu::quantity<bu::si::plane_angle, CoordinateType>::from_value
                        (bg::get<1>(this->point))
            );
    }

    //Set value of Altitude and Azimuth
    void set_altitude_azimuth
    (
        AltitudeQuantity const &Altitude,
        AzimuthQuantity const &Azimuth
    )
    {
        this->set_altitude(Altitude);
        this->set_azimuth(Azimuth);
    }

    //Set Altitude
    void set_altitude(AltitudeQuantity const &Altitude)
    {
        bg::set<0>
            (
                this->point,
                static_cast<bu::quantity<bu::si::plane_angle, CoordinateType>>(Altitude).value()
            );
    }

    //Set Azimuth
    void set_azimuth(AzimuthQuantity const &Azimuth)
    {
        bg::set<1>
            (
                this->point,
                static_cast<bu::quantity<bu::si::plane_angle, CoordinateType>>(Azimuth).value()
            );
    }

}; //horizon_coord

//Make Horizon Coordinate
template
<
    typename CoordinateType,
    template<typename Unit1, typename CoordinateType_> class AltitudeQuantity,
    template<typename Unit2, typename CoordinateType_> class AzimuthQuantity,
    typename Unit1,
    typename Unit2
>
horizon_coord
<
    CoordinateType,
    AltitudeQuantity<Unit1, CoordinateType>,
    AzimuthQuantity<Unit2, CoordinateType>
> make_horizon_coord
(
    AltitudeQuantity<Unit1, CoordinateType> const &Altitude,
    AzimuthQuantity<Unit2, CoordinateType> const &Azimuth
)
{
    return horizon_coord
        <
            CoordinateType,
            AltitudeQuantity<Unit1, CoordinateType>,
            AzimuthQuantity<Unit2, CoordinateType>
        > (Altitude, Azimuth);
}

//Print Horizon Coordinates
template
<
    typename CoordinateType,
    class AltitudeQuantity,
    class AzimuthQuantity
>
std::ostream &operator << (std::ostream &out, horizon_coord
        <CoordinateType, AltitudeQuantity, AzimuthQuantity> const &point) {
    out << "Horizon Coordinate (Altitude: "
        << point.get_altitude() << ", Azimuth: "
        << point.get_azimuth() << ")";

    return out;
}

}}}

#endif //BOOST_ASTRONOMY_HORIZON_COORD_HPP