//==================================================================================================
/*!
  @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_ARCH_COMMON_SIMD_FUNCTION_LOG2_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_LOG2_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/function/musl.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>
#include <cmath>
#include <boost/simd/function/simd/any.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/fms.hpp>
#include <boost/simd/function/ifrexp.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/if_nan_else.hpp>
#include <boost/simd/function/ilog2.hpp>
#include <boost/simd/function/is_lez.hpp>
#include <boost/simd/function/is_ngez.hpp>
#include <boost/simd/function/musl.hpp>
#include <boost/simd/function/plain.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/tofloat.hpp>

#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/invlog_2.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/constant/sqrt_2o_2.hpp>

#include <boost/simd/detail/constant/invlog_2hi.hpp>
#include <boost/simd/detail/constant/invlog_2lo.hpp>

#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD_IF ( log2_
                          , (typename A0,typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::single_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 const& a0) const BOOST_NOEXCEPT
    {
      return musl_(log2)(a0);
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF ( log2_
                          , (typename A0,typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::double_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 const& a0) const BOOST_NOEXCEPT
    {
      return musl_(log2)(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log2_
                          , (typename A0,typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::arithmetic_<A0>, X >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 const& a0) const BOOST_NOEXCEPT
    {
      return bitwise_cast<A0>(bs::ilog2(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log2_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::musl_tag
                          , bs::pack_< bd::single_<A0>, X>
                          )
  {
    /* origin: FreeBSD /usr/src/lib/msun/src/e_log2f.c */
    /*
     * ====================================================
     * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
     *
     * Developed at SunPro, a Sun Microsystems, Inc. business.
     * Permission to use, copy, modify, and distribute this
     * software is freely granted, provided that this notice
     * is preserved.
     * ====================================================
     */
    BOOST_FORCEINLINE A0 operator() (const musl_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      A0 x =  a0;
      iA0 k(0);
      auto isnez = is_nez(a0);
#ifndef BOOST_SIMD_NO_DENORMALS
      auto test = is_less(a0, Smallestposval<A0>())&&isnez;
      if (any(test))
      {
        k = if_minus(test, k, iA0(25));
        x = if_else(test, x*A0(33554432ul), x);
      }
#endif
      uiA0 ix = bitwise_cast<uiA0>(x);
      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      ix += 0x3f800000 - 0x3f3504f3;
      k += bitwise_cast<iA0>(ix>>23) - 0x7f;
      ix = (ix&0x007fffff) + 0x3f3504f3;
      x =  bitwise_cast<A0>(ix);
      A0 f = dec(x);
      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3eccce13, 0x3e789e26>(w);
      A0 t2= z*horn<A0, 0x3f2aaaaa, 0x3e91e9ee>(w);
      A0 R = t2 + t1;
      A0 hfsq = Half<A0>()*sqr(f);

      A0 dk = tofloat(k);
      A0 r =   fma(fms(s, hfsq+R, hfsq)+f, Invlog_2<A0>(), dk);
      // The original algorithm does some extra calculation in place of the return line
      // to get extra precision but this is uneeded for float as the exhaustive test shows
      // a 0.5 ulp maximal error on the full range.
      // Moreover all log2(exp2(i)) i =  1..31 are flint
      // I leave the code here in case an exotic proc will not play the game.
      //       A0  hi = f - hfsq;
      //       hi =  bitwise_and(hi, uiA0(0xfffff000ul));
      //       A0  lo = fma(s, hfsq+R, f - hi - hfsq);
      //       A0 r = (lo+hi)*detail::Invlog_2lo<A0>() + lo*detail::Invlog_2hi<A0>() + hi*detail::Invlog_2hi<A0>() + k;

#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ngez(a0), zz);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log2_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::musl_tag
                             , bs::pack_< bd::double_<A0>, X>
                             )
  {
    BOOST_FORCEINLINE A0 operator() (const musl_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
    /* origin: FreeBSD /usr/src/lib/msun/src/e_log2f.c */
    /*
     * ====================================================
     * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
     *
     * Developed at SunPro, a Sun Microsystems, Inc. business.
     * Permission to use, copy, modify, and distribute this
     * software is freely granted, provided that this notice
     * is preserved.
     * ====================================================
     */
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      A0 x =  a0;
      uiA0 hx = bitwise_cast<uiA0>(x) >> 32;
      iA0 k(0);
      auto isnez = is_nez(a0);

#ifndef BOOST_SIMD_NO_DENORMALS
      auto test = is_less(a0, Smallestposval<A0>())&&isnez;
      if (any(test))
      {
        k = if_minus(test, k, iA0(54));
        x = if_else(test, x*A0(18014398509481984ull), x);
      }
#endif
      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      hx += 0x3ff00000 - 0x3fe6a09e;
      k += bitwise_cast<iA0>(hx>>20) - 0x3ff;
      hx = (hx&0x000fffff) + 0x3fe6a09e;
      x = bitwise_cast<A0>(hx<<32 | (bitwise_and(0xffffffffull, bitwise_cast<uiA0>(x))));

      A0 f = dec(x);
      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3fd999999997fa04ll, 0x3fcc71c51d8e78afll, 0x3fc39a09d078c69fll > (w);
      A0 t2= z*horn<A0, 0x3fe5555555555593ll, 0x3fd2492494229359ll
                      , 0x3fc7466496cb03dell, 0x3fc2f112df3e5244ll> (w);
      A0 R = t2 + t1;
      A0 hfsq = Half<A0>()*sqr(f);
//        return -(hfsq-(s*(hfsq+R))-f)*Invlog_2<A0>()+dk;  // fast ?

      /*
       * f-hfsq must (for args near 1) be evaluated in extra precision
       * to avoid a large cancellation when x is near sqrt(2) or 1/sqrt(2).
       * This is fairly efficient since f-hfsq only depends on f, so can
       * be evaluated in parallel with R.  Not combining hfsq with R also
       * keeps R small (though not as small as a true `lo' term would be),
       * so that extra precision is not needed for terms involving R.
       *
       * Compiler bugs involving extra precision used to break Dekker's
       * theorem for spitting f-hfsq as hi+lo, unless double_t was used
       * or the multi-precision calculations were avoided when double_t
       * has extra precision.  These problems are now automatically
       * avoided as a side effect of the optimization of combining the
       * Dekker splitting step with the clear-low-bits step.
       *
       * y must (for args near sqrt(2) and 1/sqrt(2)) be added in extra
       * precision to avoid a very large cancellation when x is very near
       * these values.  Unlike the above cancellations, this problem is
       * specific to base 2.  It is strange that adding +-1 is so much
       * harder than adding +-ln2 or +-log10_2.
       *
       * This uses Dekker's theorem to normalize y+val_hi, so the
       * compiler bugs are back in some configurations, sigh.  And I
       * don't want to used double_t to avoid them, since that gives a
       * pessimization and the support for avoiding the pessimization
       * is not yet available.
       *
       * The multi-precision calculations for the multiplications are
       * routine.
       */

      /* hi+lo = f - hfsq + s*(hfsq+R) ~ log(1+f) */
      A0  hi = f - hfsq;
      hi =  bitwise_and(hi, (Allbits<uiA0>() << 32));
      A0 lo = fma(s, hfsq+R, f - hi - hfsq);

      A0 val_hi = hi*Invlog_2hi<A0>();
      A0 val_lo = fma(lo+hi, Invlog_2lo<A0>(), lo*Invlog_2hi<A0>());

      A0 dk = tofloat(k);
      A0 w1 = dk + val_hi;
      val_lo += (dk - w1) + val_hi;
      val_hi = w1;
      A0 r =  val_lo + val_hi;
#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ngez(a0), zz);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log2_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::plain_tag
                             , bs::pack_< bd::single_<A0>, X>
                             )
  {
    /* origin: FreeBSD /usr/src/lib/msun/src/e_log2f.c */
    /*
     * ====================================================
     * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
     *
     * Developed at SunPro, a Sun Microsystems, Inc. business.
     * Permission to use, copy, modify, and distribute this
     * software is freely granted, provided that this notice
     * is preserved.
     * ====================================================
     */
    BOOST_FORCEINLINE A0 operator() (const plain_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      using iA0 = bd::as_integer_t<A0,   signed>;
      A0 x =  a0;
      iA0 k(0);
      auto isnez = is_nez(a0);
#ifndef BOOST_SIMD_NO_DENORMALS
      auto test = is_less(a0, Smallestposval<A0>())&&isnez;
      if (any(test))
      {
        k = if_minus(test, k, iA0(25));
        x = if_else(test, x*A0(33554432ul), x);
      }
#endif
      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      iA0 kk;
      std::tie(x, kk) = ifrexp(x);
      A0 x_lt_sqrthf = genmask(Sqrt_2o_2<A0>() > x);
      k += kk + bitwise_cast<iA0>(x_lt_sqrthf);
      A0 f = dec(x+bitwise_and(x, x_lt_sqrthf));
      A0 dk = tofloat(k);

      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3eccce13, 0x3e789e26>(w);
      A0 t2= z*horn<A0, 0x3f2aaaaa, 0x3e91e9ee>(w);
      A0 R = t2 + t1;
      A0 hfsq = Half<A0>()*sqr(f);

      A0 r =   fma(fms(s, hfsq+R, hfsq)+f, Invlog_2<A0>(), dk);
      // The original algorithm does some extra calculation in place of the return line
      // to get extra precision but this is uneeded for float as the exhaustive test shows
      // a 0.5 ulp maximal error on the full range.
      // Moreover all log2(exp2(i)) i =  1..31 are flint
      // I leave the code here in case an exotic proc will not play the game.
      //       A0  hi = f - hfsq;
      //       hi =  bitwise_and(hi, uiA0(0xfffff000ul));
      //       A0  lo = fma(s, hfsq+R, f - hi - hfsq);
      //       A0 r = (lo+hi)*Invlog_2lo<A0>() + lo*Invlog_2hi<A0>() + hi*Invlog_2hi<A0>() + k;

#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ngez(a0), zz);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( log2_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::plain_tag
                             , bs::pack_< bd::double_<A0>, X>
                             )
  {
    BOOST_FORCEINLINE A0 operator() (const plain_tag &, const A0& a0) const BOOST_NOEXCEPT
    {
      /* origin: FreeBSD /usr/src/lib/msun/src/e_log2f.c */
      /*
       * ====================================================
       * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
       *
       * Developed at SunPro, a Sun Microsystems, Inc. business.
       * Permission to use, copy, modify, and distribute this
       * software is freely granted, provided that this notice
       * is preserved.
       * ====================================================
       */
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      A0 x =  a0;
      uiA0 hx = bitwise_cast<uiA0>(x) >> 32;
      iA0 k(0);
      auto isnez = is_nez(a0);

#ifndef BOOST_SIMD_NO_DENORMALS
      auto test = is_less(a0, Smallestposval<A0>())&&isnez;
      if (any(test))
      {
        k = if_minus(test, k, iA0(54));
        x = if_else(test, x*A0(18014398509481984ull), x);
      }
#endif
      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      iA0 kk;
      std::tie(x, kk) = ifrexp(x);
      A0 x_lt_sqrthf = genmask(Sqrt_2o_2<A0>() > x);
      k += kk + bitwise_cast<iA0>(x_lt_sqrthf);
      A0 f = dec(x+bitwise_and(x, x_lt_sqrthf));
      A0 dk = tofloat(k);

      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3fd999999997fa04ll, 0x3fcc71c51d8e78afll, 0x3fc39a09d078c69fll > (w);
      A0 t2= z*horn<A0, 0x3fe5555555555593ll, 0x3fd2492494229359ll
        , 0x3fc7466496cb03dell, 0x3fc2f112df3e5244ll> (w);
      A0 R = t2 + t1;
      A0 hfsq = Half<A0>()*sqr(f);
//        return -(hfsq-(s*(hfsq+R))-f)*Invlog_2<A0>()+dk;  // fast ?

      /*
       * f-hfsq must (for args near 1) be evaluated in extra precision
       * to avoid a large cancellation when x is near sqrt(2) or 1/sqrt(2).
       * This is fairly efficient since f-hfsq only depends on f, so can
       * be evaluated in parallel with R.  Not combining hfsq with R also
       * keeps R small (though not as small as a true `lo' term would be),
       * so that extra precision is not needed for terms involving R.
       *
       * Compiler bugs involving extra precision used to break Dekker's
       * theorem for spitting f-hfsq as hi+lo, unless double_t was used
       * or the multi-precision calculations were avoided when double_t
       * has extra precision.  These problems are now automatically
       * avoided as a side effect of the optimization of combining the
       * Dekker splitting step with the clear-low-bits step.
       *
       * y must (for args near sqrt(2) and 1/sqrt(2)) be added in extra
       * precision to avoid a very large cancellation when x is very near
       * these values.  Unlike the above cancellations, this problem is
       * specific to base 2.  It is strange that adding +-1 is so much
       * harder than adding +-ln2 or +-log10_2.
       *
       * This uses Dekker's theorem to normalize y+val_hi, so the
       * compiler bugs are back in some configurations, sigh.  And I
       * don't want to used double_t to avoid them, since that gives a
       * pessimization and the support for avoiding the pessimization
       * is not yet available.
       *
       * The multi-precision calculations for the multiplications are
       * routine.
       */

      /* hi+lo = f - hfsq + s*(hfsq+R) ~ log(1+f) */
      A0  hi = f - hfsq;
      hi =  bitwise_and(hi, (Allbits<uiA0>() << 32));
      A0 lo = fma(s, hfsq+R, f - hi - hfsq);

      A0 val_hi = hi*Invlog_2hi<A0>();
      A0 val_lo = fma(lo+hi, Invlog_2lo<A0>(), lo*Invlog_2hi<A0>());

      A0 w1 = dk + val_hi;
      val_lo += (dk - w1) + val_hi;
      val_hi = w1;
      A0 r =  val_lo + val_hi;
#ifndef BOOST_SIMD_NO_INFINITIES
      A0 zz = if_else(isnez, if_else(a0 == Inf<A0>(), Inf<A0>(), r), Minf<A0>());
#else
      A0 zz = if_else(isnez, r, Minf<A0>());
#endif
      return if_nan_else(is_ngez(a0), zz);
    }
  };

} } }

#endif


/*
 * @Description: Kalman filter based localization on GNSS-INS-Sim
 * @Author: Ge Yao
 * @Date: 2020-11-12 15:14:07
 */
#ifndef LIDAR_LOCALIZATION_FILTERING_GNSS_INS_SIM_FILTERING_HPP_
#define LIDAR_LOCALIZATION_FILTERING_GNSS_INS_SIM_FILTERING_HPP_

#include <string>
#include <deque>
#include <unordered_map>

#include <Eigen/Dense>

#include <yaml-cpp/yaml.h>

#include "lidar_localization/EKFStd.h"

#include "lidar_localization/sensor_data/imu_data.hpp"
#include "lidar_localization/sensor_data/pos_vel_mag_data.hpp"
#include "lidar_localization/sensor_data/pose_data.hpp"

#include "lidar_localization/models/kalman_filter/kalman_filter.hpp"

namespace lidar_localization {

class GNSSINSSimFiltering {
  public:
    GNSSINSSimFiltering();

    bool Init(
      const Eigen::Matrix4f& init_pose,
      const Eigen::Vector3f &init_vel,
      const IMUData &init_imu_data
    );

    bool Update(
      const IMUData &imu_data
    );
    bool Correct(
      const IMUData &imu_data,
      const PosVelMagData &pos_vel_mag_data
    );

    // getters:
    bool HasInited() const { return has_inited_; }

    double GetTime(void) { return kalman_filter_ptr_->GetTime(); }
    Eigen::Matrix4f GetPose(void) { return current_pose_; }
    Eigen::Vector3f GetVel(void) { return current_vel_; }
    void GetOdometry(Eigen::Matrix4f &pose, Eigen::Vector3f &vel);
    void GetStandardDeviation(EKFStd &kf_std_msg);
    void SaveObservabilityAnalysis(void);

  private:
    bool InitWithConfig(void);
    bool InitFusion(const YAML::Node& config_node);

    // init pose setter:
    bool SetInitGNSS(const Eigen::Matrix4f& init_pose);
    bool SetInitPose(const Eigen::Matrix4f& init_pose);

  private:
    bool has_inited_ = false;

    // Kalman filter:
    struct {
      std::string FUSION_METHOD;

      std::unordered_map<std::string, KalmanFilter::MeasurementType> FUSION_STRATEGY_ID;
      KalmanFilter::MeasurementType FUSION_STRATEGY;
    } CONFIG;
    std::shared_ptr<KalmanFilter> kalman_filter_ptr_;
    KalmanFilter::Measurement current_measurement_;

    Eigen::Matrix4f current_gnss_pose_ = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f init_pose_ = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f current_pose_ = Eigen::Matrix4f::Identity();
    Eigen::Vector3f current_vel_ = Eigen::Vector3f::Zero();
    KalmanFilter::Cov current_cov_;
};

} // namespace lidar_localization

#endif // LIDAR_LOCALIZATION_FILTERING_GNSS_INS_SIM_FILTERING_HPP_


#define EIGEN_DEFAULT_DENSE_INDEX_TYPE int32_t

#include "../repos/stl_reader/stl_reader.h"
#include <array>
#include <deque>
#include <chrono>
#include <random>
#include <fstream>
#include <iostream>
#include <Eigen/Dense>

using Triangle = std::array<Eigen::Vector3f, 3u>;
using CudaTriangle = Eigen::Vector3f*;
using CudaConstTriangle = Eigen::Vector3f const*;
using Triangles = std::deque<Triangle>;

constexpr float   cgEpsilon        = 0.00001f;       // TODO consider if uniform epsilon suits all needs.
constexpr uint32_t cgSignumZero     = 0u;
constexpr uint32_t cgSignumPlus     = 1u;
constexpr uint32_t cgSignumMinus    = 2u;
constexpr uint32_t cgSignumShift0   = 0u;
constexpr uint32_t cgSignumShift1   = 2u;
constexpr uint32_t cgSignumShift2   = 4u;
constexpr uint32_t cgSignumAllZero  = (cgSignumZero  << cgSignumShift0) | (cgSignumZero  << cgSignumShift1) | (cgSignumZero  << cgSignumShift2);
constexpr uint32_t cgSignumAllPlus  = (cgSignumPlus  << cgSignumShift0) | (cgSignumPlus  << cgSignumShift1) | (cgSignumPlus  << cgSignumShift2);
constexpr uint32_t cgSignumAllMinus = (cgSignumMinus << cgSignumShift0) | (cgSignumMinus << cgSignumShift1) | (cgSignumMinus << cgSignumShift2);

constexpr uint32_t cgSignumSelect0a = (cgSignumPlus  << cgSignumShift0) | (cgSignumMinus << cgSignumShift1) | (cgSignumMinus << cgSignumShift2);
constexpr uint32_t cgSignumSelect0b = (cgSignumMinus << cgSignumShift0) | (cgSignumPlus  << cgSignumShift1) | (cgSignumPlus  << cgSignumShift2);
constexpr uint32_t cgSignumSelect0c = (cgSignumZero  << cgSignumShift0) | (cgSignumPlus  << cgSignumShift1) | (cgSignumPlus  << cgSignumShift2);
constexpr uint32_t cgSignumSelect0d = (cgSignumZero  << cgSignumShift0) | (cgSignumMinus << cgSignumShift1) | (cgSignumMinus << cgSignumShift2);
constexpr uint32_t cgSignumSelect0e = (cgSignumPlus  << cgSignumShift0) | (cgSignumZero  << cgSignumShift1) | (cgSignumZero  << cgSignumShift2);
constexpr uint32_t cgSignumSelect0f = (cgSignumMinus << cgSignumShift0) | (cgSignumZero  << cgSignumShift1) | (cgSignumZero  << cgSignumShift2);
constexpr uint32_t cgSignumSelect0g = (cgSignumZero  << cgSignumShift0) | (cgSignumPlus  << cgSignumShift1) | (cgSignumMinus << cgSignumShift2);
constexpr uint32_t cgSignumSelect0h = (cgSignumZero  << cgSignumShift0) | (cgSignumMinus << cgSignumShift1) | (cgSignumPlus  << cgSignumShift2);

constexpr uint32_t cgSignumSelect1a = (cgSignumPlus  << cgSignumShift1) | (cgSignumMinus << cgSignumShift0) | (cgSignumMinus << cgSignumShift2);
constexpr uint32_t cgSignumSelect1b = (cgSignumMinus << cgSignumShift1) | (cgSignumPlus  << cgSignumShift0) | (cgSignumPlus  << cgSignumShift2);
constexpr uint32_t cgSignumSelect1c = (cgSignumZero  << cgSignumShift1) | (cgSignumPlus  << cgSignumShift0) | (cgSignumPlus  << cgSignumShift2);
constexpr uint32_t cgSignumSelect1d = (cgSignumZero  << cgSignumShift1) | (cgSignumMinus << cgSignumShift0) | (cgSignumMinus << cgSignumShift2);
constexpr uint32_t cgSignumSelect1e = (cgSignumPlus  << cgSignumShift1) | (cgSignumZero  << cgSignumShift0) | (cgSignumZero  << cgSignumShift2);
constexpr uint32_t cgSignumSelect1f = (cgSignumMinus << cgSignumShift1) | (cgSignumZero  << cgSignumShift0) | (cgSignumZero  << cgSignumShift2);
constexpr uint32_t cgSignumSelect1g = (cgSignumZero  << cgSignumShift1) | (cgSignumPlus  << cgSignumShift0) | (cgSignumMinus << cgSignumShift2);
constexpr uint32_t cgSignumSelect1h = (cgSignumZero  << cgSignumShift1) | (cgSignumMinus << cgSignumShift0) | (cgSignumPlus  << cgSignumShift2);

// Otherwise select 2, no need for checking and thus no constants.

constexpr uint32_t cgSignumCircumferenceA = (cgSignumZero << cgSignumShift0) | (cgSignumPlus << cgSignumShift1) | (cgSignumPlus << cgSignumShift2);
constexpr uint32_t cgSignumCircumferenceB = (cgSignumZero << cgSignumShift0) | (cgSignumZero << cgSignumShift1) | (cgSignumPlus << cgSignumShift2);
constexpr uint32_t cgSignumCircumferenceC = (cgSignumPlus << cgSignumShift0) | (cgSignumZero << cgSignumShift1) | (cgSignumPlus << cgSignumShift2);
constexpr uint32_t cgSignumCircumferenceD = (cgSignumPlus << cgSignumShift0) | (cgSignumZero << cgSignumShift1) | (cgSignumZero << cgSignumShift2);
constexpr uint32_t cgSignumCircumferenceE = (cgSignumPlus << cgSignumShift0) | (cgSignumPlus << cgSignumShift1) | (cgSignumZero << cgSignumShift2);
constexpr uint32_t cgSignumCircumferenceF = (cgSignumZero << cgSignumShift0) | (cgSignumPlus << cgSignumShift1) | (cgSignumZero << cgSignumShift2);

constexpr uint32_t calculateSignum(float const distances[3]) noexcept {
  uint32_t result = 0u;
  for(int32_t i = 0; i < 3; ++i) {
    int32_t tmp = cgSignumZero;
    if(distances[i] > cgEpsilon) {
      tmp = cgSignumPlus;
    }
    else if(distances[i] < -cgEpsilon) {
      tmp = cgSignumMinus;
    }
    else { // nothing to do
    }
    result |= tmp << (cgSignumShift1 * i);
  }
  return result;
}

void calculateNormals(Eigen::Vector2f const aShape[3], Eigen::Vector2f aNormals[3]) noexcept {
  Eigen::Vector2f side = aShape[1] - aShape[0];
  aNormals[0](0) = -side(1);
  aNormals[0](1) = side(0);
  float correction = 1.0f;
  if(aNormals[0].dot(aShape[2]) < 0.0f) {
    correction = -1.0f;                     // They shall point towards the interior.
    aNormals[0] *= correction;
  }
  else { // nothing to do
  }
  side = aShape[2] - aShape[1];
  aNormals[1](0) = -side(1) * correction;
  aNormals[1](1) = side(0) * correction;
  side = aShape[0] - aShape[2];
  aNormals[2](0) = -side(1) * correction;
  aNormals[2](1) = side(0) * correction;
}

bool doesTouchOther(uint32_t const aSignums) noexcept {
  bool result = (aSignums == cgSignumAllPlus
  || aSignums == cgSignumCircumferenceA
  || aSignums == cgSignumCircumferenceB
  || aSignums == cgSignumCircumferenceC
  || aSignums == cgSignumCircumferenceD
  || aSignums == cgSignumCircumferenceE
  || aSignums == cgSignumCircumferenceF);
if(result) std::cout << "coplanar circumference or interior\n";
  return result;
}

bool checkCornerOnPerimeterAndInterior(Eigen::Vector2f aShape1[3], Eigen::Vector2f aShape2[3]) noexcept {
  Eigen::Vector2f normals1[3]; // i : i->(i+1)%3
  Eigen::Vector2f normals2[3];
  calculateNormals(aShape1, normals1); // Normal vectors point to the center.
  calculateNormals(aShape2, normals2);
  bool result = false;
  for(int32_t indexCorner = 0; indexCorner < 3; ++indexCorner) {
    float distancesCornerFromEachSideOfShape1[3];
    float distancesCornerFromEachSideOfShape2[3];
    for(int32_t indexSide = 0; indexSide < 3; ++indexSide) {
      distancesCornerFromEachSideOfShape2[indexSide] = normals2[indexSide].dot(aShape1[indexCorner] - aShape2[indexSide]);
      distancesCornerFromEachSideOfShape1[indexSide] = normals1[indexSide].dot(aShape2[indexCorner] - aShape1[indexSide]);
    }
    result = result || doesTouchOther(calculateSignum(distancesCornerFromEachSideOfShape2));
    result = result || doesTouchOther(calculateSignum(distancesCornerFromEachSideOfShape1));   // True if one corner is on the sides, corners or interior of the other triangle.
    // Don't break out since it won't use on CUDA.
  }
  return result;
}

bool checkTrueIntersecitonOfSides(Eigen::Vector2f aShape1[3], Eigen::Vector2f aShape2[3]) noexcept {
  bool result = false;
  for(int32_t indexSide1 = 0; indexSide1 < 3; ++indexSide1) {
    for(int32_t indexSide2 = 0; indexSide2 < 3; ++indexSide2) {
      Eigen::Vector2f &side1a = aShape1[indexSide1];
      Eigen::Vector2f &side1b = aShape1[(indexSide1 + 1) % 3];
      Eigen::Vector2f &side2a = aShape2[indexSide2];
      Eigen::Vector2f &side2b = aShape2[(indexSide2 + 1) % 3];
      // Manually solve linear EQ to make sure we have as few branches as possible.
      uint32_t nonzeroAindex = (fabs(side1a(0) - side1b(0)) > cgEpsilon ? 0 : 1);
      float a = side1a(nonzeroAindex) - side1b(nonzeroAindex);
      float b = side2b(nonzeroAindex) - side2a(nonzeroAindex);
      float c = side1a(1 - nonzeroAindex) - side1b(1 - nonzeroAindex);
      float d = side2b(1 - nonzeroAindex) - side2a(1 - nonzeroAindex);
      float determinant = a * d - b * c;
      if(fabs(determinant) > cgEpsilon) {
        float k = side1a(nonzeroAindex) - side2a(nonzeroAindex);
        float l = side1a(1 - nonzeroAindex) - side2a(1 - nonzeroAindex);
        float v = (l * a - c * k) / determinant;
        float u = (k - b * v) / a;
if(u >= 0.0f && u <= 1.0f && v >= 0.0f && v <= 1.0f) std::cout << u << ' ' << v << " coplanar sides intersect\n";
        result = result || (u >= 0.0f && u <= 1.0f && v >= 0.0f && v <= 1.0f);  // The intersection is inside of both sides.
      }
      else { // nothing to do, because the lines are parallel but can't touch each other.
      }
    }
  }
  return result;
}

bool hasCommonPoint(CudaConstTriangle const aShape1, CudaConstTriangle const aShape2, Eigen::Vector3f const &aShape1normal, Eigen::Vector3f const &aShape2normal) noexcept { // coplanar
  Eigen::Vector3f normal;
  if(aShape1normal.dot(aShape2normal) > 0.0f) {
    normal = aShape1normal + aShape2normal;
  }
  else {
    normal = aShape1normal - aShape2normal;
  }
  int32_t indexX = 1;
  int32_t indexY = 2;
  float abs1 = fabs(normal(1)); // Looking for the biggest projection. TODO fabsf for CUDA
  if(abs1 > fabs(normal(0))) {
    indexX = 0;
  }
  else { // nothing to do
  }
  if(fabs(normal(2)) > abs1) {
    indexX = 0;
    indexY = 1;
  }
  else { // nothing to do
  }
  Eigen::Vector2f shape1[3];
  Eigen::Vector2f shape2[3];
  for(int32_t i = 0; i < 3; ++i) {
    shape1[i](0) = aShape1[i](indexX);  // Project 3D triangle to axis-parallel plane.
    shape1[i](1) = aShape1[i](indexY);
    shape2[i](0) = aShape2[i](indexX);
    shape2[i](1) = aShape2[i](indexY);
  }
  bool result = checkCornerOnPerimeterAndInterior(shape1, shape2);
  if(!result) { // Common point may only occur now when sides truly intersect each other.
    result = checkTrueIntersecitonOfSides(shape1, shape2);
  }
  else { // nothing to do
  }
  return result;
}

void calculateIntersectionParameter(
  CudaConstTriangle const aShape
, Eigen::Vector3f const &aIntersectionVector
, float const aDistanceCornerNfromOtherPlane[3]
, uint32_t const aSignumShapeFromOtherPlane
, float &aIntersectionParameterA
, float &aIntersectionParameterB) noexcept {
  int32_t indexCommon, indexA, indexB;
  if(aSignumShapeFromOtherPlane == cgSignumSelect0a
  || aSignumShapeFromOtherPlane == cgSignumSelect0b
  || aSignumShapeFromOtherPlane == cgSignumSelect0c
  || aSignumShapeFromOtherPlane == cgSignumSelect0d
  || aSignumShapeFromOtherPlane == cgSignumSelect0e
  || aSignumShapeFromOtherPlane == cgSignumSelect0f
  || aSignumShapeFromOtherPlane == cgSignumSelect0g
  || aSignumShapeFromOtherPlane == cgSignumSelect0h) {
    indexCommon = 0; indexA = 1; indexB = 2;
  }
  else if(aSignumShapeFromOtherPlane == cgSignumSelect1a
  || aSignumShapeFromOtherPlane == cgSignumSelect1b
  || aSignumShapeFromOtherPlane == cgSignumSelect1c
  || aSignumShapeFromOtherPlane == cgSignumSelect1d
  || aSignumShapeFromOtherPlane == cgSignumSelect1e
  || aSignumShapeFromOtherPlane == cgSignumSelect1f
  || aSignumShapeFromOtherPlane == cgSignumSelect1g
  || aSignumShapeFromOtherPlane == cgSignumSelect1h) {
    indexCommon = 1; indexA = 0; indexB = 2;
  }
  else {
    indexCommon = 2; indexA = 1; indexB = 0;
  }
  float vertexProjections[3];
  for(int32_t i = 0; i < 3; ++i) {
    vertexProjections[i] = aIntersectionVector.dot(aShape[i]);
  }
  aIntersectionParameterA =
   vertexProjections[indexA]
 + (vertexProjections[indexCommon] - vertexProjections[indexA])
 * aDistanceCornerNfromOtherPlane[indexA]
 / (aDistanceCornerNfromOtherPlane[indexA] - aDistanceCornerNfromOtherPlane[indexCommon]);
  aIntersectionParameterB =
   vertexProjections[indexB]
 + (vertexProjections[indexCommon] - vertexProjections[indexB])
 * aDistanceCornerNfromOtherPlane[indexB]
 / (aDistanceCornerNfromOtherPlane[indexB] - aDistanceCornerNfromOtherPlane[indexCommon]);
}

bool hasCommonPoint(CudaConstTriangle const aShape1, CudaConstTriangle const aShape2) noexcept { // Entry point for common point check.
  bool result = false;
  Eigen::Vector3f shape1normal = (aShape1[1] - aShape1[0]).cross(aShape1[2] - aShape1[0]);
  Eigen::Vector3f shape2normal = (aShape2[1] - aShape2[0]).cross(aShape2[2] - aShape2[0]);
  shape1normal.normalize();
  shape2normal.normalize();
  float distanceCornerNofShape1FromPlane2[3];
  float distanceCornerNofShape2FromPlane1[3];
  for(int32_t i = 0; i < 3; ++i) {
    distanceCornerNofShape1FromPlane2[i] = shape2normal.dot(aShape1[i] - aShape2[0]);
    distanceCornerNofShape2FromPlane1[i] = shape1normal.dot(aShape2[i] - aShape1[0]);
  }
  uint32_t signumShape1FromPlane2 = calculateSignum(distanceCornerNofShape1FromPlane2); // These contain info about relation of each point and the other plane.
  uint32_t signumShape2FromPlane1 = calculateSignum(distanceCornerNofShape2FromPlane1);
  if(signumShape1FromPlane2 == cgSignumAllPlus || signumShape1FromPlane2 == cgSignumAllMinus || signumShape2FromPlane1 == cgSignumAllPlus || signumShape2FromPlane1 == cgSignumAllMinus) {
    // Nothing to do: one triangle is completely on the one side of the other's plane
  }
  else {
    Eigen::Vector3f intersectionVector = shape1normal.cross(shape2normal);
    if(intersectionVector.norm() > cgEpsilon && signumShape1FromPlane2 != cgSignumAllZero && signumShape2FromPlane1 != cgSignumAllZero) { // Real intersection, planes are not identical, and both triangles touch the common line.
      intersectionVector.normalize();
      float intersectionParameterAshape1;
      float intersectionParameterBshape1;
      float intersectionParameterAshape2;
      float intersectionParameterBshape2;
      calculateIntersectionParameter(aShape1, intersectionVector, distanceCornerNofShape1FromPlane2, signumShape1FromPlane2, intersectionParameterAshape1, intersectionParameterBshape1); // The two parameters will contain the locations of the touching point.
      calculateIntersectionParameter(aShape2, intersectionVector, distanceCornerNofShape2FromPlane1, signumShape2FromPlane1, intersectionParameterAshape2, intersectionParameterBshape2);
      if(intersectionParameterAshape1 > intersectionParameterBshape1) {
        std::swap(intersectionParameterAshape1, intersectionParameterBshape1);
      }
      else { // nothing to do
      }
      if(intersectionParameterAshape2 > intersectionParameterBshape2) {
        std::swap(intersectionParameterAshape2, intersectionParameterBshape2);
      }
      else { // nothing to do
      }
      if(intersectionParameterAshape1 - cgEpsilon <= intersectionParameterAshape2 && intersectionParameterAshape2 <= intersectionParameterBshape1 + cgEpsilon // Epsilons make possible to check for triangles with corner-corner or corner-edge touch.
      || intersectionParameterAshape1 - cgEpsilon <= intersectionParameterBshape2 && intersectionParameterBshape2 <= intersectionParameterBshape1 + cgEpsilon
      || intersectionParameterAshape2 - cgEpsilon <= intersectionParameterAshape1 && intersectionParameterAshape1 <= intersectionParameterBshape2 + cgEpsilon
      || intersectionParameterAshape2 - cgEpsilon <= intersectionParameterBshape1 && intersectionParameterBshape1 <= intersectionParameterBshape2 + cgEpsilon) {
std::cout << "on intersecting line\n";
        result = true;
      }
      else { // nothing to do
      }
    }
    else {
      result = hasCommonPoint(aShape1, aShape2, shape1normal, shape2normal); // Coplanar triangles
    }
  }
  return result;
}

Eigen::Matrix3f randomTransform() {
  std::default_random_engine generator;
  generator.seed((std::chrono::high_resolution_clock::now() - std::chrono::high_resolution_clock::time_point::min()).count());
  std::uniform_real_distribution<float> distribution(0.0f, 1.0f);
  Eigen::Matrix3f result;
  for(int32_t i = 0; i < 9; ++i) {
    result(i / 3, i % 3) = distribution(generator);
  }
  return result;
}

Triangles readTriangles(char const * const aFilename, Eigen::Matrix3f const &aTransform) {
  Triangles result;
  stl_reader::StlMesh<float, int32_t> mesh(aFilename);
  for(int32_t indexTriangle = 0; indexTriangle < mesh.num_tris(); ++indexTriangle) {
      Triangle triangle;
      for(int32_t indexCorner = 0; indexCorner < 3; ++indexCorner) {
          float const * const coords = mesh.tri_corner_coords(indexTriangle, indexCorner);
          Eigen::Vector3f in;
          for(int32_t i = 0; i < 3; ++i) {
            in(i) = coords[i];
          }
          triangle[indexCorner] = aTransform * in;
      }
      result.push_back(triangle);
  }
  return result;
}

void writeTriangles(Triangles const &aTriangles, char const * const aFilename) {
  std::ofstream out(aFilename);
  out << "solid Exported from Blender-2.82 (sub 7)\n";
  for(auto const & triangle : aTriangles) {
//    Eigen::Vector3f normal = (triangle[1] - triangle[0]).cross(aShape2[2] - aShape2[0]);
//  shape1normal.normalize();
    out << "facet normal 0.000000 0.000000 0.000000\nouter loop\n";
    for(auto const & vertex : triangle) {
      out << "vertex " << vertex(0) << ' ' << vertex(1) << ' ' << vertex(2) << '\n';
    }
    out << "endloop\nendfacet\n";
  }
  out << "endsolid Exported from Blender-2.82 (sub 7)\n";
}

void check(Triangles const &aTriangles) {
  for(int32_t i = 0; i < aTriangles.size(); ++i) {
    for(int32_t j = i + 1; j < aTriangles.size(); ++j) {
      if(hasCommonPoint(aTriangles[i].data(), aTriangles[j].data())) {
        std::cout << "Has common point: " << i << ' ' << j << "\n\n";
      }
      else { // nothing to do
      }
    }
  }
}

int main(int argc, char **argv) {
  int ret = 0;
  if(argc < 2) {
    std::cerr << "Usage: " << argv[0] << " <filenameIn> [filenameOut]\n";
    ret = 1;
  }
  else {
    try {
      auto transform = randomTransform();
      auto triangles = readTriangles(argv[1], transform);
      if(argc >= 3) {
        writeTriangles(triangles, argv[2]);
      }
      else { // nothing to do
      }
      check(triangles);
    }
    catch(std::exception &e) {
      ret = 2;
    }
  }
  return ret;
}


#include "stdafx.h"

#include "problem.hpp"
#include "utility.hpp"

#include <fstream>
#include <regex>
#include <utility>
#include <unordered_map>
#include <unordered_set>
#include <numeric>
#include <boost/algorithm/string.hpp>

struct advent_2017_15 : problem
{
	advent_2017_15() noexcept : problem(2017, 15) {
	}

protected:
	struct generator
	{
		std::string name;
		uint64_t seed;
		uint64_t factor;
		uint64_t current;

		generator(const std::string& name_, uint64_t seed_, uint64_t factor_) : name(name_), seed(seed_), factor(factor_), current(seed_) {
		}

		uint16_t generate(uint64_t divisor) noexcept {
			for(;;) {
				const uint64_t product = current * factor;
				current = product % 2'147'483'647ui64;
				if((current % divisor) == 0ui64) {
					return current & 0xffff;
				}
			}
		}

		void reset() noexcept {
			current = seed;
		}
	};

	std::unique_ptr<generator> a;
	std::unique_ptr<generator> b;

	void prepare_input(std::ifstream& fin) override {
		std::regex pattern(R"(Generator ([[:upper:]]) starts with ([[:digit:]]*))");
		std::string line;
		std::getline(fin, line);
		std::smatch m;
		std::regex_search(line, m, pattern);
		a = std::make_unique<generator>(m[1], std::stoul(m[2]), 16'807);
		std::getline(fin, line);
		std::regex_search(line, m, pattern);
		b = std::make_unique<generator>(m[1], std::stoul(m[2]), 48'271);
	}

	std::string part_1() override {
		std::size_t pairs = 0;
		for(std::size_t i = 0; i < 40'000'000; ++i) {
			pairs += a->generate(1) == b->generate(1);
		}
		return std::to_string(pairs);
	}

	void tidy_up() noexcept override {
		a->reset();
		b->reset();
	}

	std::string part_2() override {
		std::size_t pairs = 0;
		for(std::size_t i = 0; i < 5'000'000; ++i) {
			pairs += a->generate(4) == b->generate(8);
		}
		return std::to_string(pairs);
	}
};

REGISTER_SOLVER(2017, 15);


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// 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.

#ifndef SMOOTH__FEEDBACK__OCP_HPP_
#define SMOOTH__FEEDBACK__OCP_HPP_

/**
 * @file
 * @brief Optimal control problem definition.
 */

#include <Eigen/Core>
#include <smooth/diff.hpp>
#include <smooth/lie_group.hpp>

#include <iostream>

#include "traits.hpp"

namespace smooth::feedback {

// \cond
// Forward declaration
template<LieGroup _X, Manifold _U, int _Nq, int _Ncr, int _Nce>
struct OCPSolution;
// \endcond

/**
 * @brief Optimal control problem definition
 * @tparam _X state space
 * @tparam _U input space
 *
 * Problem is defined on the interval \f$ t \in [0, t_f] \f$.
 * \f[
 * \begin{cases}
 *  \min              & \theta(t_f, x_0, x_f, q)                                         \\
 *  \text{s.t.}       & x(0) = x_0                                                       \\
 *                    & x(t_f) = x_f                                                     \\
 *                    & \dot x(t) = f(t, x(t), u(t))                                     \\
 *                    & q = \int_{0}^{t_f} g(t, x(t), u(t)) \mathrm{d}t                  \\
 *                    & c_{rl} \leq c_r(t, x(t), u(t)) \leq c_{ru} \quad t \in [0, t_f]  \\
 *                    & c_{el} \leq c_e(t_f, x_0, x_f, q) \leq c_{eu}
 * \end{cases}
 * \f]
 *
 * The optimal control problem depends on arbitrary functions \f$ \theta, f, g, c_r, c_e \f$.
 * The type of those functions are template pararamters in this structure.
 *
 * @note To enable automatic differentiation \f$ \theta, f, g, c_r, c_e \f$ must be templated over
 * the scalar type.
 */
template<LieGroup _X, Manifold _U, typename Theta, typename F, typename G, typename CR, typename CE>
struct OCP
{
  /// @brief State space
  using X = _X;
  /// @brief Input space
  using U = _U;

  /// @brief State space dimension
  static constexpr int Nx = Dof<X>;
  /// @brief Input space dimension
  static constexpr int Nu = Dof<U>;
  /// @brief Number of integrals
  static constexpr int Nq = std::invoke_result_t<G, double, X, U>::SizeAtCompileTime;
  /// @brief Number of running constraints
  static constexpr int Ncr = std::invoke_result_t<CR, double, X, U>::SizeAtCompileTime;
  /// @brief Number of end constraints
  static constexpr int Nce =
    std::invoke_result_t<CE, double, X, X, Eigen::Matrix<double, Nq, 1>>::SizeAtCompileTime;

  /// @brief Solution type corresponding to this problem
  using Solution = OCPSolution<X, U, Nq, Ncr, Nce>;

  static_assert(Nx > 0, "Static size required");
  static_assert(Nu > 0, "Static size required");
  static_assert(Nq > 0, "Static size required");
  static_assert(Ncr > 0, "Static size required");
  static_assert(Nce > 0, "Static size required");

  /// @brief Objective function \f$ \theta : R \times X \times X \times R^{n_q} \rightarrow R \f$
  Theta theta;

  /// @brief System dynamics \f$ f : R \times X \times U \rightarrow Tangent<X> \f$
  F f;
  /// @brief Integrals \f$ g : R \times X \times U \rightarrow R^{n_q} \f$
  G g;

  /// @brief Running constraint \f$ c_r : R \times X \times U \rightarrow R^{n_{cr}} \f$
  CR cr;
  /// @brief Running constraint lower bound \f$ c_{rl} \in R^{n_{cr}} \f$
  Eigen::Vector<double, Ncr> crl = Eigen::Vector<double, Ncr>::Zero();
  /// @brief Running constraint upper bound \f$ c_{ru} \in R^{n_{cr}} \f$
  Eigen::Vector<double, Ncr> cru = Eigen::Vector<double, Ncr>::Zero();

  /// @brief End constraint \f$ c_e : R \times X \times X \times R^{n_q} \rightarrow R^{n_{ce}} \f$
  CE ce;
  /// @brief End constraint lower bound \f$ c_{el} \in R^{n_{ce}} \f$
  Eigen::Vector<double, Nce> cel = Eigen::Vector<double, Nce>::Zero();
  /// @brief End constraint upper bound \f$ c_{eu} \in R^{n_{ce}} \f$
  Eigen::Vector<double, Nce> ceu = Eigen::Vector<double, Nce>::Zero();
};

/// @brief Concept that is true for OCP specializations
template<typename T>
concept OCPType = traits::is_specialization_of_v<std::decay_t<T>, OCP>;

/// @brief Concept that is true for FlatOCP specializations
template<typename T>
concept FlatOCPType = OCPType<T> &&(smooth::traits::RnType<typename std::decay_t<T>::X> &&
                                      smooth::traits::RnType<typename std::decay_t<T>::U>);

/**
 * @brief Solution to OCP.
 */
template<LieGroup _X, Manifold _U, int _Nq, int _Ncr, int _Nce>
struct OCPSolution
{
  /// @brief State space
  using X = _X;
  /// @brief Input space
  using U = _U;

  /// @brief Number of integrals
  static constexpr int Nq = _Nq;
  /// @brief Number of running constraints
  static constexpr int Ncr = _Ncr;
  /// @brief Number of end constraints
  static constexpr int Nce = _Nce;

  ///@{
  /// @brief Initial and final time
  double t0, tf;
  //}@

  /// @brief Integral values
  Eigen::Vector<double, Nq> Q{};

  ///@{
  /// @brief Callable functions for state and input
  std::function<U(double)> u;
  std::function<X(double)> x;
  //}@

  /// @brief Multipliers for integral constraints
  Eigen::Vector<double, Nq> lambda_q{};

  /// @brief Multipliers for endpoint constraints
  Eigen::Vector<double, Nce> lambda_ce{};

  /// @brief Multipliers for dynamics equality constraint
  std::function<Eigen::Vector<double, Dof<X>>(double)> lambda_dyn{};

  /// @brief Multipliers for active running constraints
  std::function<Eigen::Vector<double, Ncr>(double)> lambda_cr{};
};

/**
 * @brief Test analytic derivatives for an OCP problem.
 *
 * @tparam DT differentiation method to compare against.
 *
 * @param ocp problem to test derivatives for
 * @param num_trials number of random points to test
 *
 * @todo Make it possible to test a subset of derivatives
 */
template<diff::Type DT = diff::Type::Numerical>
bool test_ocp_derivatives(OCPType auto & ocp, uint32_t num_trials = 1, double eps = 1e-4)
{
  using OCP = std::decay_t<decltype(ocp)>;

  using X = typename OCP::X;
  using U = typename OCP::U;
  using Q = Eigen::Vector<double, OCP::Nq>;

  if (!diff::detail::diffable_order1<decltype(ocp.theta), std::tuple<double, X, X, Q>>) {
    std::cout << "no jacobian for theta\n";
  }
  if (!diff::detail::diffable_order2<decltype(ocp.theta), std::tuple<double, X, X, Q>>) {
    std::cout << "no hessian for theta\n";
  }
  if (!diff::detail::diffable_order1<decltype(ocp.f), std::tuple<double, X, U>>) {
    std::cout << "no jacobian for f\n";
  }
  if (!diff::detail::diffable_order2<decltype(ocp.f), std::tuple<double, X, U>>) {
    std::cout << "no hessian for f\n";
  }
  if (!diff::detail::diffable_order1<decltype(ocp.g), std::tuple<double, X, U>>) {
    std::cout << "no jacobian for g\n";
  }
  if (!diff::detail::diffable_order2<decltype(ocp.g), std::tuple<double, X, U>>) {
    std::cout << "no hessian for g\n";
  }
  if (!diff::detail::diffable_order1<decltype(ocp.cr), std::tuple<double, X, U>>) {
    std::cout << "no jacobian for cr\n";
  }
  if (!diff::detail::diffable_order2<decltype(ocp.cr), std::tuple<double, X, U>>) {
    std::cout << "no hessian for cr\n";
  }
  if (!diff::detail::diffable_order1<decltype(ocp.ce), std::tuple<double, X, X, Q>>) {
    std::cout << "no jacobian for ce\n";
  }
  if (!diff::detail::diffable_order2<decltype(ocp.ce), std::tuple<double, X, X, Q>>) {
    std::cout << "no hessian for ce\n";
  }

  const auto cmp = [&eps](const auto & m1, const auto & m2) {
    return (
      // clang-format off
      (m1.cols() == m2.cols())
      && (m1.rows() == m2.rows())
      && (
          m1.isApprox(m2, 1e-4) ||
          Eigen::MatrixXd(m1 - m2).cwiseAbs().maxCoeff() < eps
      )
      // clang-format on
    );
  };

  bool success = true;

  for (auto trial = 0u; trial < num_trials; ++trial) {
    // endpt parameters
    const double tf                        = 1 + static_cast<double>(std::rand()) / RAND_MAX;
    const X x0                             = Random<X>();
    const X xf                             = Random<X>();
    const Eigen::Vector<double, OCP::Nq> q = Eigen::Vector<double, OCP::Nq>::Random();
    const double t                         = 1 + static_cast<double>(std::rand()) / RAND_MAX;
    const X x                              = Random<X>();
    const U u                              = Random<U>();

    // theta
    if constexpr (diff::detail::diffable_order1<decltype(ocp.theta), std::tuple<double, X, X, Q>>) {
      const auto [f_def, df_def] = diff::dr<1, diff::Type::Analytic>(ocp.theta, wrt(tf, x0, xf, q));
      const auto [f_num, df_num] = diff::dr<1, DT>(ocp.theta, wrt(tf, x0, xf, q));

      if (!cmp(df_def, df_num)) {
        std::cout << "Error in 1st derivative of theta: got\n"
                  << Eigen::MatrixXd(df_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(df_num) << '\n';
        success = false;
      };
    }
    if constexpr (diff::detail::diffable_order2<decltype(ocp.theta), std::tuple<double, X, X, Q>>) {
      const auto [f_def, df_def, d2f_def] =
        diff::dr<2, diff::Type::Analytic>(ocp.theta, wrt(tf, x0, xf, q));
      const auto [f_num, df_num, d2f_num] = diff::dr<2, DT>(ocp.theta, wrt(tf, x0, xf, q));

      if (!cmp(d2f_def, d2f_num)) {
        std::cout << "Error in 2nd derivative of theta: got\n"
                  << Eigen::MatrixXd(d2f_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(d2f_num) << '\n';
        success = false;
      };
    }

    // end constraints
    if constexpr (diff::detail::diffable_order1<decltype(ocp.ce), std::tuple<double, X, X, Q>>) {
      const auto [f_def, df_def] = diff::dr<1, diff::Type::Analytic>(ocp.ce, wrt(tf, x0, xf, q));
      const auto [f_num, df_num] = diff::dr<1, DT>(ocp.ce, wrt(tf, x0, xf, q));

      if (!cmp(df_def, df_num)) {
        std::cout << "Error in 1st derivative of ce: got\n"
                  << Eigen::MatrixXd(df_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(df_num) << '\n';
        success = false;
      };
    }
    if constexpr (diff::detail::diffable_order2<decltype(ocp.ce), std::tuple<double, X, X, Q>>) {
      const auto [f_def, df_def, d2f_def] =
        diff::dr<2, diff::Type::Analytic>(ocp.ce, wrt(tf, x0, xf, q));
      const auto [f_num, df_num, d2f_num] = diff::dr<2, DT>(ocp.ce, wrt(tf, x0, xf, q));

      if (!cmp(d2f_def, d2f_num)) {
        std::cout << "Error in 2nd derivative of ce: got\n"
                  << Eigen::MatrixXd(d2f_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(d2f_num) << '\n';
        success = false;
      };
    }

    // dynamics
    if constexpr (diff::detail::diffable_order1<decltype(ocp.f), std::tuple<double, X, U>>) {
      const auto [f_def, df_def] = diff::dr<1, diff::Type::Analytic>(ocp.f, wrt(t, x, u));
      const auto [f_num, df_num] = diff::dr<1, DT>(ocp.f, wrt(t, x, u));
      if (!cmp(df_def, df_num)) {
        std::cout << "Error in 1st derivative of f: got\n"
                  << Eigen::MatrixXd(df_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(df_num) << '\n';
        success = false;
      };
    }
    if constexpr (diff::detail::diffable_order2<decltype(ocp.f), std::tuple<double, X, U>>) {
      const auto [f_def, df_def, d2f_def] = diff::dr<2, diff::Type::Analytic>(ocp.f, wrt(t, x, u));
      const auto [f_num, df_num, d2f_num] = diff::dr<2, DT>(ocp.f, wrt(t, x, u));
      if (!cmp(d2f_def, d2f_num)) {
        std::cout << "Error in 2nd derivative of f: got\n"
                  << Eigen::MatrixXd(d2f_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(d2f_num) << '\n';
        success = false;
      };
    }

    // integrand
    if constexpr (diff::detail::diffable_order1<decltype(ocp.g), std::tuple<double, X, U>>) {
      const auto [f_def, df_def] = diff::dr<1, diff::Type::Analytic>(ocp.g, wrt(t, x, u));
      const auto [f_num, df_num] = diff::dr<1, DT>(ocp.g, wrt(t, x, u));
      if (!cmp(df_def, df_num)) {
        std::cout << "Error in 1st derivative of g: got\n"
                  << Eigen::MatrixXd(df_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(df_num) << '\n';
        success = false;
      };
    }
    if constexpr (diff::detail::diffable_order2<decltype(ocp.g), std::tuple<double, X, U>>) {
      const auto [f_def, df_def, d2f_def] = diff::dr<2, diff::Type::Analytic>(ocp.g, wrt(t, x, u));
      const auto [f_num, df_num, d2f_num] = diff::dr<2, DT>(ocp.g, wrt(t, x, u));
      if (!cmp(d2f_def, d2f_num)) {
        std::cout << "Error in 2nd derivative of g: got\n"
                  << Eigen::MatrixXd(d2f_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(d2f_num) << '\n';
        success = false;
      };
    }

    // running constraints
    if constexpr (diff::detail::diffable_order1<decltype(ocp.cr), std::tuple<double, X, U>>) {
      const auto [f_def, df_def] = diff::dr<1, diff::Type::Analytic>(ocp.cr, wrt(t, x, u));
      const auto [f_num, df_num] = diff::dr<1, DT>(ocp.cr, wrt(t, x, u));
      if (!cmp(df_def, df_num)) {
        std::cout << "Error in 1st derivative of cr: got\n"
                  << Eigen::MatrixXd(df_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(df_num) << '\n';
        success = false;
      };
    }
    if constexpr (diff::detail::diffable_order2<decltype(ocp.cr), std::tuple<double, X, U>>) {
      const auto [f_def, df_def, d2f_def] = diff::dr<2, diff::Type::Analytic>(ocp.cr, wrt(t, x, u));
      const auto [f_num, df_num, d2f_num] = diff::dr<2, DT>(ocp.cr, wrt(t, x, u));
      if (!cmp(d2f_def, d2f_num)) {
        std::cout << "Error in 2nd derivative of cr: got\n"
                  << Eigen::MatrixXd(d2f_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(d2f_num) << '\n';
        success = false;
      };
    }
  }

  return success;
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__OCP_HPP_


#include <cstdio>
#include <cstdlib>

#include <iostream>

#include <armadillo>

using namespace std;
using namespace arma;

int main(int argc, char* argv[]) {
  if (argc < 3) {
    cerr << "Usage: " << argv[0] << " num-particles rand-seed\n";
    return EXIT_FAILURE;
  }

  const size_t num_particles = atoi(argv[1]);

  const ulong rand_seed = strtoul(argv[2], nullptr, 10);
  clog << "Using random seed: " << rand_seed << "\n";
  srand(rand_seed);

  const double sigma = pow(2.0, 1.0 / 6.0);
  mat::fixed<3, 3> L = 40.0 * sigma * eye(3, 3);
  L.save("cell.mat", arma::csv_ascii);

  mat x = randu<mat>(3, num_particles);
  mat r = L * x;
  r.save("positions.mat", arma::csv_ascii);

  vec q = 2e1 * randu<vec>(num_particles);
  // vec q = ones<vec>(num_particles);
  for (size_t n = 0; n < num_particles; ++n) if (n % 3 != 0) q[n] = 0.0;
  q.save("charges.mat", arma::csv_ascii);

  return EXIT_SUCCESS;
}


#define NPY_NO_DEPRECATED_API NPY_1_11_API_VERSION

#include <iostream>
#include <stdint.h>
#include <alloca.h>
#include <omp.h>
#include <Eigen/Core>
#include <Python.h>
#include <numpy/ndarrayobject.h>

using namespace Eigen;
using namespace std;

static double extract_double(PyArrayObject *arr, int i) {
    return ((double*) PyArray_DATA(arr))[i];
}

static double extract_double_2d(PyArrayObject *arr, int i, int j) {
    return ((double*) PyArray_DATA(arr))[i * PyArray_DIM(arr, 1) + j];
}

static double extract_int64_t(PyArrayObject *arr, int i) {
    return ((int64_t*) PyArray_DATA(arr))[i];
}

void capsule_cleanup(PyObject *capsule) {
    void *memory = PyCapsule_GetPointer(capsule, NULL);
    delete memory;
}

static PyObject* run(PyObject * module, PyObject * args) {
    //TODO: check if parameters are null.
    //TODO: check that dicts have the required members.
    //TODO: check that all parameters have the right sizes.
    //TODO: i'm not sending any error messages.
    import_array();

    PyObject *data_ptr = NULL, *model_ptr = NULL, *fwd_msgs = NULL;
    PyArrayObject *alldata = NULL, *allresources = NULL, *starts = NULL, *lengths = NULL, *learns = NULL, *forgets = NULL, *guesses = NULL, *slips = NULL, *forward_messages = NULL;
    double prior;
    int parallel;

    if (!PyArg_ParseTuple(args, "OOOi", &data_ptr, &model_ptr, &fwd_msgs, &parallel)) {
        PyErr_SetString(PyExc_ValueError, "Error parsing arguments.");
        return NULL;
    }

    if (!parallel)
        omp_set_num_threads(1);

    int DTYPE = PyArray_ObjectType(fwd_msgs, NPY_FLOAT);
    forward_messages = (PyArrayObject *)PyArray_FROM_OTF(fwd_msgs, DTYPE, NPY_ARRAY_IN_ARRAY);

    char* DM_NAMES[] = {"data", "resources", "starts", "lengths", "learns", "forgets", "guesses", "slips"};
    PyArrayObject** DM_PTRS[] = {&alldata, &allresources, &starts, &lengths, &learns, &forgets, &guesses, &slips};
    for (int i = 0; i < 8; i++) {
        PyObject *dp = PyDict_GetItemString(i < 4 ? data_ptr : model_ptr, DM_NAMES[i]);
        DTYPE = PyArray_ObjectType(dp, (i < 4 ? (i < 1 ? NPY_INT : NPY_INT64) : NPY_FLOAT)); // hack to force correct type
        *DM_PTRS[i] = (PyArrayObject *)PyArray_FROM_OTF(dp, DTYPE, NPY_ARRAY_IN_ARRAY);
    }
    prior = PyFloat_AsDouble(PyDict_GetItemString(model_ptr, "prior"));

    int bigT = (int) PyArray_DIM(alldata, 1), num_subparts = (int) PyArray_DIM(alldata, 0);
    int len_allresources = (int) PyArray_DIM(allresources, 0);
    int num_sequences = (int) PyArray_DIM(starts, 0);
    int len_lengths = (int) PyArray_DIM(lengths, 0);
    int num_resources = (int) PyArray_DIM(learns, 0);

    Array2d initial_distn;
    initial_distn << 1-prior, prior;

    MatrixXd As(2,2*num_resources);
    for (int n=0; n<num_resources; n++) {
        double learn = extract_double(learns, n);
        double forget = extract_double(forgets, n);
        As.col(2*n) << 1-learn, learn;
        As.col(2*n+1) << forget, 1-forget;
    }

    // forward messages
    //numpy::ndarray all_forward_messages = extract<numpy::ndarray>(forward_messages);
    double * forward_messages_temp = new double[2*bigT];
    for (int i=0; i<2; i++) {
        for (int j=0; j<bigT; j++){
            forward_messages_temp[i* bigT +j] = extract_double_2d(forward_messages, i, j);
        }
    }

    //// outputs

    double* all_predictions = new double[2 * bigT];
    Map<Array2Xd,Aligned> predictions(all_predictions,2,bigT);

    /* COMPUTATION */

    #pragma omp parallel for
    for (int sequence_index=0; sequence_index < num_sequences; sequence_index++) {
        // NOTE: -1 because Matlab indexing starts at 1
        int64_t sequence_start = extract_int64_t(starts, sequence_index) - 1;
        int64_t T = extract_int64_t(lengths, sequence_index);

        //int16_t *resources = allresources + sequence_start;
        Map<MatrixXd, Aligned> forward_messages(forward_messages_temp + 2*sequence_start,2,T);
        Map<MatrixXd, Aligned> predictions(all_predictions + 2*sequence_start,2,T);

        predictions.col(0) = initial_distn;
        for (int t=0; t<T-1; t++) {
            int64_t resources_temp = extract_int64_t(allresources, sequence_start + t);
            predictions.col(t+1) = As.block(0,2*(resources_temp-1),2,2) * forward_messages.col(t);
        }
    }

    npy_intp dims[] = {2, bigT};
    PyObject *all_predictions_arr = (PyObject *) PyArray_SimpleNewFromData(2, dims, NPY_DOUBLE, all_predictions);
    PyObject *capsule = PyCapsule_New(all_predictions, NULL, capsule_cleanup);
    PyArray_SetBaseObject((PyArrayObject *) all_predictions_arr, capsule);

    for (int i = 0; i < 8; i++)
        Py_XDECREF(*DM_PTRS[i]);
    Py_XDECREF(forward_messages);

    return(all_predictions_arr);
}

static PyMethodDef predict_onestep_states_Methods[] = {
    {"run",  run, METH_VARARGS,
     "Generates predictions for BKT model"},
    {NULL, NULL, 0, NULL}        /* Sentinel */
};

static struct PyModuleDef predict_onestep_states_module = {
   PyModuleDef_HEAD_INIT,
   "predict_onestep_states",   /* name of module */
   NULL, /* module documentation, may be NULL */
   -1,       /* size of per-interpreter state of the module,
                or -1 if the module keeps state in global variables. */
   predict_onestep_states_Methods
};

PyMODINIT_FUNC PyInit_predict_onestep_states() {
    return PyModule_Create(&predict_onestep_states_module);
}


#ifndef BOOST_SAFE_NUMERICS_INTERVAL_HPP
#define BOOST_SAFE_NUMERICS_INTERVAL_HPP

//  Copyright (c) 2012 Robert Ramey
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <limits>
#include <cassert>
#include <type_traits>
#include <initializer_list>
#include <algorithm> // minmax, min, max

#include <boost/logic/tribool.hpp>

#include "utility.hpp" // log

#include "concept/integer.hpp"

// from stack overflow
// http://stackoverflow.com/questions/23815138/implementing-variadic-min-max-functions

namespace boost {
namespace safe_numerics {

template<typename R>
struct interval {
    const R l;
    const R u;

    template<typename T>
    constexpr interval(const T & lower, const T & upper) :
        l(lower),
        u(upper)
    {
        // assert(static_cast<bool>(l <= u));
    }
    template<typename T>
    constexpr interval(const std::pair<T, T> & p) :
        l(p.first),
        u(p.second)
    {}
    template<class T>
    constexpr interval(const interval<T> & rhs) :
        l(rhs.l),
        u(rhs.u)
    {}
    constexpr interval() :
        l(std::numeric_limits<R>::min()),
        u(std::numeric_limits<R>::max())
    {}
    // return true if this interval contains the given point
    constexpr tribool includes(const R & t) const {
        return l <= t && t <= u;
    }
    // if this interval contains every point found in some other inteval t
    //  return true
    // otherwise
    //  return false or indeterminate
    constexpr tribool includes(const interval<R> & t) const {
        return u >= t.u && l <= t.l;
    }

    // return true if this interval contains the given point
    constexpr tribool excludes(const R & t) const {
        return t < l || t > u;
    }
    // if this interval excludes every point found in some other inteval t
    //  return true
    // otherwise
    //  return false or indeterminate
    constexpr tribool excludes(const interval<R> & t) const {
        return t.u < l || u < t.l;
    }

};

template<class R>
constexpr inline interval<R> make_interval(){
    return interval<R>();
}
template<class R>
constexpr  inline interval<R> make_interval(const R &){
    return interval<R>();
}

// account for the fact that for floats and doubles
// the most negative value is called "lowest" rather
// than min
template<>
constexpr inline interval<float>::interval() :
    l(std::numeric_limits<float>::lowest()),
    u(std::numeric_limits<float>::max())
{}
template<>
constexpr inline interval<double>::interval() :
    l(std::numeric_limits<double>::lowest()),
    u(std::numeric_limits<double>::max())
{}

template<typename T>
constexpr inline interval<T> operator+(const interval<T> & t, const interval<T> & u){
    // adapted from https://en.wikipedia.org/wiki/Interval_arithmetic
    return {t.l + u.l, t.u + u.u};
}

template<typename T>
constexpr inline interval<T> operator-(const interval<T> & t, const interval<T> & u){
    // adapted from https://en.wikipedia.org/wiki/Interval_arithmetic
    return {t.l - u.u, t.u - u.l};
}

template<typename T>
constexpr inline interval<T> operator*(const interval<T> & t, const interval<T> & u){
    // adapted from https://en.wikipedia.org/wiki/Interval_arithmetic
    return utility::minmax<T>(
        std::initializer_list<T> {
            t.l * u.l,
            t.l * u.u,
            t.u * u.l,
            t.u * u.u
        }
    );
}

// interval division
// note: presumes 0 is not included in the range of the denominator
template<typename T>
constexpr inline interval<T> operator/(const interval<T> & t, const interval<T> & u){
    assert(static_cast<bool>(u.excludes(T(0))));
    return utility::minmax<T>(
        std::initializer_list<T> {
            t.l / u.l,
            t.l / u.u,
            t.u / u.l,
            t.u / u.u
        }
    );
}

// modulus of two intervals.  This will give a new range of for the modulus.
// note: presumes 0 is not included in the range of the denominator
template<typename T>
constexpr inline interval<T> operator%(const interval<T> & t, const interval<T> & u){
    assert(static_cast<bool>(u.excludes(T(0))));
    return utility::minmax<T>(
        std::initializer_list<T> {
            t.l % u.l,
            t.l % u.u,
            t.u % u.l,
            t.u % u.u
        }
    );
}

template<typename T>
constexpr inline interval<T> operator<<(const interval<T> & t, const interval<T> & u){
    static_assert(
        boost::safe_numerics::Integer<T>::value,
        "left shift only defined for integral type"
    );
    //return interval<T>{t.l << u.l, t.u << u.u};
    return utility::minmax<T>(
        std::initializer_list<T> {
            t.l << u.l,
            t.l << u.u,
            t.u << u.l,
            t.u << u.u
        }
    );
}

template<typename T>
constexpr inline interval<T> operator>>(const interval<T> & t, const interval<T> & u){
    static_assert(
        boost::safe_numerics::Integer<T>::value,
        "right shift only defined for integral type"
    );
    //return interval<T>{t.l >> u.u, t.u >> u.l};
    return utility::minmax<T>(
        std::initializer_list<T> {
            t.l >> u.l,
            t.l >> u.u,
            t.u >> u.l,
            t.u >> u.u
        }
    );
}

// union of two intervals
template<typename T>
constexpr interval<T> operator|(const interval<T> & t, const interval<T> & u){
    const T & rl = std::min(t.l, u.l);
    const T & ru = std::max(t.u, u.u);
    return interval<T>(rl, ru);
}

// intersection of two intervals
template<typename T>
constexpr inline interval<T> operator&(const interval<T> & t, const interval<T> & u){
    const T & rl = std::max(t.l, u.l);
    const T & ru = std::min(t.u, u.u);
    return interval<T>(rl, ru);
}

// determine whether two intervals intersect
template<typename T>
constexpr inline boost::logic::tribool intersect(const interval<T> & t, const interval<T> & u){
    return t.u >= u.l || t.l <= u.u;
}

template<typename T>
constexpr inline boost::logic::tribool operator<(
    const interval<T> & t,
    const interval<T> & u
){
    return
        // if every element in t is less than every element in u
        t.u < u.l ? boost::logic::tribool(true):
        // if every element in t is greater than every element in u
        t.l > u.u ? boost::logic::tribool(false):
        // otherwise some element(s) in t are greater than some element in u
        boost::logic::indeterminate
    ;
}

template<typename T>
constexpr inline boost::logic::tribool operator>(
    const interval<T> & t,
    const interval<T> & u
){
    return
        // if every element in t is greater than every element in u
        t.l > u.u ? boost::logic::tribool(true) :
        // if every element in t is less than every element in u
        t.u < u.l ? boost::logic::tribool(false) :
        // otherwise some element(s) in t are greater than some element in u
        boost::logic::indeterminate
    ;
}

template<typename T>
constexpr inline bool operator==(
    const interval<T> & t,
    const interval<T> & u
){
    // intervals have the same limits
    return t.l == u.l && t.u == u.u;
}

template<typename T>
constexpr inline bool operator!=(
    const interval<T> & t,
    const interval<T> & u
){
    return ! (t == u);
}

template<typename T>
constexpr inline boost::logic::tribool operator<=(
    const interval<T> & t,
    const interval<T> & u
){
    return ! (t > u);
}

template<typename T>
constexpr inline boost::logic::tribool operator>=(
    const interval<T> & t,
    const interval<T> & u
){
    return ! (t < u);
}

} // safe_numerics
} // boost

#include <iosfwd>

namespace std {

template<typename CharT, typename Traits, typename T>
inline std::basic_ostream<CharT, Traits> &
operator<<(
    std::basic_ostream<CharT, Traits> & os,
    const boost::safe_numerics::interval<T> & i
){
    return os << '[' << i.l << ',' << i.u << ']';
}
template<typename CharT, typename Traits>
inline std::basic_ostream<CharT, Traits> &
operator<<(
    std::basic_ostream<CharT, Traits> & os,
    const boost::safe_numerics::interval<unsigned char> & i
){
    os << "[" << (unsigned)i.l << "," << (unsigned)i.u << "]";
    return os;
}

template<typename CharT, typename Traits>
inline std::basic_ostream<CharT, Traits> &
operator<<(
    std::basic_ostream<CharT, Traits> & os,
    const boost::safe_numerics::interval<signed char> & i
){
    os << "[" << (int)i.l << "," << (int)i.u << "]";
    return os;
}

} // std

#endif // BOOST_SAFE_NUMERICS_INTERVAL_HPP


#ifndef LINEAR_SOLVERS_HPP
#define LINEAR_SOLVERS_HPP

#include "compressed_sensing.hpp"
#include <boost/shared_ptr.hpp>

namespace Pecos {

void normalise_columns( RealMatrix &A, RealVector &result );

class LinearSolver
{
protected:

  RealVector residualTols_;

  int maxIters_;

  int verbosity_;

  bool normaliseInputs_;

  Real solverTol_;

  Real conjugateGradientTol_;

  int numPrimaryEqs_;

public:

  LinearSolver() :
    maxIters_(std::numeric_limits<int>::max()), verbosity_(0),
    normaliseInputs_(false), solverTol_(1.e-6), conjugateGradientTol_(-1.),
    numPrimaryEqs_( -1 )
  { residualTols_.size( 1 ); };

  ~LinearSolver()
  {
    clear();
  };

  void clear()
  {
    maxIters_ = std::numeric_limits<int>::max();
    verbosity_ = 0; solverTol_ = 1.e-6; conjugateGradientTol_ = -1.;
    numPrimaryEqs_ = -1; residualTols_.size( 1 );
  }

  /**
   * \brief Find a regularized solution to AX = B
   */
  virtual void solve( RealMatrix &A, RealMatrix &B, RealMatrix &result_0,
		      RealMatrix &result_1 )
  {
    std::string msg = "solve() Has not been implemented for ";
    msg += "this class.";
    throw( std::runtime_error( msg ) );
  };

  virtual void solve_using_points( RealMatrix &build_points, RealMatrix &B,
				   RealMatrix &result_0,
				   RealMatrix &result_1 )
  {
    std::string msg = "solve_using_points() Has not been implemented for ";
    msg += "this class.";
    throw( std::runtime_error( msg ) );
  };

  void set_verbosity( int verbosity )
  {
    verbosity_ = verbosity;
  };

  void set_residual_tolerance( Real tolerance )
  {
    RealVector residual_tols( 1, false );
    residual_tols[0] = tolerance;
    set_residual_tolerances( residual_tols );
  };

  void set_residual_tolerances( const RealVector &residual_tols )
  {
    residualTols_.sizeUninitialized( residual_tols.length() );
    residualTols_.assign( residual_tols );
  };

  void set_solver_tolerance( Real tolerance )
  {
    solverTol_  = tolerance;
  };

  void set_conjugate_gradient_tolerance( Real tolerance )
  {
    conjugateGradientTol_  = tolerance;
  };

  void set_num_primary_equations( int num_primary_eqs )
  {
    numPrimaryEqs_  = num_primary_eqs;
  };

  Real get_residual_tolerance()
  {
    return residualTols_[0];
  };

  void get_residual_tolerances( RealVector &result_0 )
  {
    result_0.sizeUninitialized( residualTols_.length() );
    result_0.assign( residualTols_ );
  };

  void set_max_iters( int max_iters )
  {
    maxIters_ = max_iters;
  };

  void set_normalise_inputs( bool normalise )
  {
    normaliseInputs_ = normalise;
  };

  void copy( const LinearSolver &source )
  {
    set_residual_tolerances( source.residualTols_ );
    set_max_iters( source.maxIters_ );
    set_verbosity( source.verbosity_ );
  };

  void normalise_columns( RealMatrix &A, RealVector &result )
  {
    int M = A.numRows(), N = A.numCols();
    result.sizeUninitialized( N );
    for ( int i = 0; i < N; i++ )
      {
	RealVector col( Teuchos::View, A[i], M );
	result[i] = col.normFrobenius();
	col.scale(1./result[i]);
      }
  };

  void adjust_coefficients( const RealVector &normalisation_factors,
			    RealMatrix &coefficients )
  {
    int num_coeff = coefficients.numRows(), num_qoi = coefficients.numCols();
    for ( int i = 0; i < num_qoi; i++ )
      {
	for ( int j = 0; j < num_coeff; j++ )
	  coefficients(j,i) /= normalisation_factors[j];
      }
  };

  /**
   * For adaptive methods such as orthogonal least interpolation
   * it is sometimes necessary to reconstruct the Matrix A of Ax=b
   * for instance when performing cross validation
   */
  virtual void build_matrix( const RealMatrix &build_points,
			     RealMatrix &result_0 )
  {
    std::string msg = "linear_solver::build_matrix() Not implemented.";
    throw( std::runtime_error( msg ) );
  }
};

class BPSolver : public LinearSolver
{
protected:

public:

  BPSolver(){};

  ~BPSolver(){};

  /**
   * \brief Find the solution min ||x||_0 such that |AX = B||_2 == 0
   */
  void solve( RealMatrix &A, RealMatrix &B, RealMatrix &result_0,
	      RealMatrix &result_1 )
  {
    if ( B.numCols() != 1 )
      throw( std::runtime_error(" BPSolver::solve() B must be a vector") );

    RealVector b( Teuchos::View, B[0], B.numRows() );
    RealMatrix A_copy( A );

    RealVector column_norms;
    if ( normaliseInputs_ )
      normalise_columns( A_copy, column_norms );

    BP_primal_dual_interior_point_method( A_copy, b,
					  result_0,
					  solverTol_,
					  conjugateGradientTol_,
					  verbosity_ );

    if ( normaliseInputs_ )
      adjust_coefficients( column_norms, result_0 );

    result_1.shapeUninitialized( 2, 1 );
    result_1(0,0) = 0.;
    int num_non_zeros = 0;
    for ( int i = 0; i < result_0.numRows(); i++ )
      if ( std::abs( result_0(i,0) ) > std::numeric_limits<double>::epsilon() )
	num_non_zeros++;
    result_1(1,0) = num_non_zeros;
  };
};

class BPDNSolver : public LinearSolver
{
public:

  BPDNSolver(){};

  ~BPDNSolver(){};

  /**
   * \brief Find the solution min ||x||_0 such that |AX = B||_2 < eps
   */
  void solve( RealMatrix &A, RealMatrix &B, RealMatrix &result_0,
	      RealMatrix &result_1 )
  {
    if ( residualTols_.length() <= 0 )
      throw( std::runtime_error(" BPDNSolver::solve() set residual tols") );

    if ( B.numCols() != 1 )
      throw( std::runtime_error(" BPDNSolver::solve() B must be a vector") );

    RealVector b( Teuchos::View, B[0], B.numRows() );
    RealMatrix A_copy( A );

    RealVector column_norms;
    if ( normaliseInputs_ )
      normalise_columns( A_copy, column_norms );

    result_0.shapeUninitialized( A.numCols(), residualTols_.length() );
    result_1.shapeUninitialized( 2, residualTols_.length() );
    for ( int j = 0; j < residualTols_.length(); j++ )
      {
	RealMatrix x;
	BPDN_log_barrier_interior_point_method( A_copy, b,
						x,
						residualTols_[j],
						solverTol_,
						conjugateGradientTol_,
						verbosity_ );

	if ( normaliseInputs_ )
	  adjust_coefficients( column_norms, x );

	result_1(0,j) = residualTols_[j];
	int num_non_zeros = 0;
	for ( int i = 0; i < result_0.numRows(); i++ )
	  {
	    if ( std::abs( x(i,0) ) > std::numeric_limits<double>::epsilon() )
	      num_non_zeros++;
	    result_0(i,j) = x(i,0);
	  }
	result_1(1,j) = num_non_zeros;
      }
  };
};

class OMPSolver : public LinearSolver
{
protected:
  IntVector ordering_; // enforce a set of columns to be chosen first

public:

  OMPSolver(){};

  ~OMPSolver(){};

  /**
   * \brief Find the solution min ||x||_0 such that |AX = B||_2 < eps
   */
  void solve( RealMatrix &A, RealMatrix &B, RealMatrix &result_0,
	      RealMatrix &result_1 )
  {
    if ( B.numCols() != 1 )
      throw( std::runtime_error(" OMPSolver::solve() B must be a vector") );

    RealVector b( Teuchos::View, B[0], B.numRows() );
    RealMatrix A_copy( A );

    RealVector column_norms;
    if ( normaliseInputs_ )
      normalise_columns( A_copy, column_norms );

    orthogonal_matching_pursuit( A_copy, b, result_0, result_1,
				 residualTols_[0], maxIters_, verbosity_,
				 ordering_ );

    if ( normaliseInputs_ )
      adjust_coefficients( column_norms, result_0 );
  };

  void set_ordering( IntVector &ordering )
  {
    ordering_.resize( ordering.length() );
    ordering_.assign( ordering );
  };
};

class LARSSolver : public LinearSolver
{
private:

  int solver_;

  Real delta_;

public:
  LARSSolver() : solver_( LEAST_ANGLE_REGRESSION ), delta_( 0.0 ){};

  ~LARSSolver(){clear();};

  void clear()
  {
    LinearSolver::clear();
    solver_ = LEAST_ANGLE_REGRESSION; delta_ = 0.0;
  };

  void set_sub_solver( int solver_id )
  {
    if ( ( solver_id != LASSO_REGRESSION ) &&
	 ( solver_id != LEAST_ANGLE_REGRESSION ) )
      {
	std::stringstream msg;
	msg << "set_sub_solver() solver id must be either: " << LASSO_REGRESSION
	    << " or " << LEAST_ANGLE_REGRESSION << "\n";
	throw( std::runtime_error( msg.str() ) );
      }
    solver_ = solver_id;
  };

  void set_delta( Real delta )
  {
    delta_ = delta;
  }

  /**
   * \brief Find the solution min ||x||_0 such that |AX = B||_2 < eps
   */
  void solve( RealMatrix &A, RealMatrix &B, RealMatrix &result_0,
	      RealMatrix &result_1 )
  {
    if ( B.numCols() != 1 )
      throw( std::runtime_error(" LARSSolver::solve() B must be a vector") );

    RealVector b( Teuchos::View, B[0], B.numRows() );
    RealMatrix A_copy( A );

    RealVector column_norms;
    if ( normaliseInputs_ )
      normalise_columns( A_copy, column_norms );

    least_angle_regression( A_copy, b, result_0, result_1,
			    residualTols_[0], solver_, delta_,
			    maxIters_, verbosity_ );

    if ( normaliseInputs_ )
      adjust_coefficients( column_norms, result_0 );
  };
};

class COSAMPSolver : public LinearSolver
{
private:
  int sparsity_;

public:
  COSAMPSolver() : sparsity_( 0 ) {};

  ~COSAMPSolver(){clear();};

  void clear()
  {
    LinearSolver::clear();
    sparsity_ = 0;
  };

  void set_sparsity( int sparsity )
  {
    sparsity_ = sparsity;
  }

  /**
   * \brief Find the solution min ||x||_0 such that |AX = B||_2 < eps
   */
  void solve( RealMatrix &A, RealMatrix &B, RealMatrix &result_0,
	      RealMatrix &result_1 )
  {
    if ( B.numCols() != 1 )
      throw( std::runtime_error(" COSAMPSolver::solve() B must be a vector") );

    RealVector b( Teuchos::View, B[0], B.numRows() );
    RealMatrix A_copy( A );

    RealVector column_norms;
    if ( normaliseInputs_ )
      normalise_columns( A_copy, column_norms );

    cosamp( A_copy, b, result_0, result_1, sparsity_,
	    maxIters_, verbosity_ );

    if ( normaliseInputs_ )
      adjust_coefficients( column_norms, result_0 );
  };
};

class LSQSolver : public LinearSolver
{
public:
  LSQSolver(){};

  ~LSQSolver(){};

  /**
   * \brief Find the solution min ||x||_0 such that |AX = B||_2 < eps
   */
  void solve( RealMatrix &A, RealMatrix &B, RealMatrix &result_0,
	      RealMatrix &result_1 )
  {
    if ( B.numCols() != 1 )
      throw( std::runtime_error("LSQSolver::solve() B must be a vector") );

   if ( A.numRows() < A.numCols() )
     std::cout << "LSQSolver::solve() Warning A is under-determined. " <<
       "M = " << A.numRows() << " N = " << A.numCols() <<
       ". Returning minimum norm solution\n";

    RealVector b( Teuchos::View, B[0], B.numRows() );
    RealMatrix A_copy( A );

    RealVector column_norms;
    if ( normaliseInputs_ )
      normalise_columns( A_copy, column_norms );

    RealVector singular_values;
    int rank(0);
    svd_solve( A_copy, b, result_0, singular_values, rank,
	       solverTol_ );

    result_1.shapeUninitialized( 2, 1 );
    RealVector residual( b );
    residual.multiply( Teuchos::NO_TRANS, Teuchos::NO_TRANS,
		       -1.0, A_copy, result_0, 1.0 );
    result_1(0,0) = residual.normFrobenius();
    int num_non_zeros = 0;
    for ( int i = 0; i < result_0.numRows(); i++ )
      {
	if ( std::abs( result_0(i,0) ) > std::numeric_limits<double>::epsilon() )
	  num_non_zeros++;
      }
    result_1(1,0) = num_non_zeros;

    if ( normaliseInputs_ )
      adjust_coefficients( column_norms, result_0 );
  };
};

class EqualityConstrainedLSQSolver : public LinearSolver
{

public:
  EqualityConstrainedLSQSolver(){};

  ~EqualityConstrainedLSQSolver(){};

  /**
   * \brief Find the solution min ||x||_0 such that |AX = B||_2 < eps
   */
  void solve( RealMatrix &A, RealMatrix &B, RealMatrix &result_0,
	      RealMatrix &result_1 )
  {
    if ( B.numCols() != 1 )
      throw( std::runtime_error(" EqualityConstrainedLSQSolver::solve() B must be a vector") );

    if ( numPrimaryEqs_ <= 0 )
      throw( std::runtime_error(" EqualityConstrainedLSQSolver::solve() set num primary equations") );

    if ( numPrimaryEqs_ > A.numCols() )
      throw( std::runtime_error(" EqualityConstrainedLSQSolver::solve() num primary equations is larger than the number of columns in A") );

    if ( A.numRows() < A.numCols() )
      throw( std::runtime_error(" EqualityConstrainedLSQSolver::solve() A is underdetermined") );

    RealMatrix A_copy( A );

    RealVector column_norms;
    if ( normaliseInputs_ )
      normalise_columns( A_copy, column_norms );

    RealMatrix C_eq( Teuchos::View, A_copy, numPrimaryEqs_,
		     A_copy.numCols(), 0, 0 );
    RealMatrix A_eq( Teuchos::View, A_copy,
		     A_copy.numRows() - numPrimaryEqs_,
		     A_copy.numCols(), numPrimaryEqs_, 0 );
    RealVector d_eq( Teuchos::View, B.values(), numPrimaryEqs_ );
    RealVector b_eq( Teuchos::View,
		     B.values() + numPrimaryEqs_,
		     B.numRows() - numPrimaryEqs_ );
    equality_constrained_least_squares_solve( A_eq, b_eq, C_eq, d_eq,
					      result_0 );

    result_1.shapeUninitialized( 2, 1 );
    RealVector residual( b_eq );
    residual.multiply( Teuchos::NO_TRANS, Teuchos::NO_TRANS,
		       -1.0, A_eq, result_0, 1.0 );
    result_1(0,0) = residual.normFrobenius();
    int num_non_zeros = 0;
    for ( int i = 0; i < result_0.numRows(); i++ )
      {
	if ( std::abs( result_0(i,0) ) > std::numeric_limits<double>::epsilon() )
	  num_non_zeros++;
      }
    result_1(1,0) = num_non_zeros;

    if ( normaliseInputs_ )
      adjust_coefficients( column_norms, result_0 );
  };
};

typedef boost::shared_ptr<LinearSolver> LinearSolver_ptr;

/**
 * \brief Specify a set of options for using the CompressedSensingTool
 */
class CompressedSensingOptions
{
public:
  //solverType solver; //!< Specify which regression solver to use. See solverType
  short solver; //!< Specify which regression solver to use: see pecos_global_defs
  Real solverTolerance; //!< Specify the internal tolerance of the solver
  Real epsilon;         //!< Specify the residual tolerance of the solver
  Real delta;           //!< Specify the regularization parameter value
  int maxNumIterations; //!< Specify the maximum number of solver iterations
  bool standardizeInputs;  //!< Specify if the inputs need to be standardized
  bool storeHistory;       //<! Specify if the solution history should be stored
  Real conjugateGradientsTolerance; //<! Specify wether to use conjugate gradients internally to solve newton step in BP and BPDN.  If < 0 cholesky factorization will be used.
  int verbosity;           //!< The verbosity level. 0: off, 1: warnings on,  2: all print statements on.
  int numFunctionSamples; //!< The number of function samples used to construct A and B. Used when A contains gradient information. If zero then numFunctionSamples = A.numRows()

public:

  CompressedSensingOptions() :
    solver( DEFAULT_LEAST_SQ_REGRESSION ), solverTolerance( -1. ),
    epsilon( 0.0 ), delta( 0.0 ),
    maxNumIterations( std::numeric_limits<int>::max() ),
    standardizeInputs( false ), storeHistory( false ),
    conjugateGradientsTolerance( -1 ), verbosity( 0 ), numFunctionSamples( 0 )
  {};

  ~CompressedSensingOptions(){};

  void print()
  {
    std::cout << "Solver: " << solver << "\n";
    std::cout << "Solver Tolerance: " << solverTolerance << "\n";
    std::cout << "Epsilon: " << epsilon << "\n";
    std::cout << "Delta: " << delta << "\n";
    std::cout << "MaxNumIterations: " << maxNumIterations << "\n";
    std::cout << "StandardizeInputs: " << standardizeInputs << "\n";
    std::cout << "StoreHistory: " << storeHistory << "\n";
    std::cout << "Verbosity: " << verbosity << "\n";
  };

  CompressedSensingOptions& operator=(const CompressedSensingOptions& source )
  {
    if(this == &source)
      return (*this);
    solver = source.solver;
    solverTolerance = source.solverTolerance;
    epsilon = source.epsilon;
    delta = source.delta;
    maxNumIterations = source.maxNumIterations;
    standardizeInputs = source.standardizeInputs;
    storeHistory = source.storeHistory;
    conjugateGradientsTolerance = source.conjugateGradientsTolerance;
    verbosity = source.verbosity;
    numFunctionSamples = source.numFunctionSamples;
    return (*this);
  }

  CompressedSensingOptions( const CompressedSensingOptions& source )
  {
    solver = source.solver;
    solverTolerance = source.solverTolerance;
    epsilon = source.epsilon;
    delta = source.delta;
    maxNumIterations = source.maxNumIterations;
    standardizeInputs = source.standardizeInputs;
    storeHistory = source.storeHistory;
    conjugateGradientsTolerance = source.conjugateGradientsTolerance;
    verbosity = source.verbosity;
    numFunctionSamples = source.numFunctionSamples;
  }
};

typedef std::vector< std::vector<CompressedSensingOptions> > CompressedSensingOptionsList;

/**
 * \class CompressedSensingTool
 * \brief Tool that implements a number of popular compressed sensing algorithms
 */
class CompressedSensingTool
{
protected:
  LinearSolver_ptr linearSolver_;

public:

  /// Default constructor
  CompressedSensingTool()
  {
    std::cout.precision( std::numeric_limits<Real>::digits10 );
    std::cout.setf( std::ios::scientific );
  };

  // Deconstructor
  ~CompressedSensingTool(){};

  //! @name Interface tools.
  //@{

  /**
   * \brief Wrapper to call any of the compressed sensing methods
   *
   * \param A ( M x N ) matrix of the linear system AX=B
   *
   * \param B ( M x num_rhs ) matrix of the linear system AX=B
   *
   * \param solutions (output) vector containing multiple solutions
   * to AX=B. Each entry in solutions is a ( N x num_rhs ) matrix
   * opts.solver=LS will return only one solution whilst methods such
   * as OMP, LARS, and LASSO will return a history of solutions
   *
   * \param opts specifies the method options
   *
   * \param opts_list specifies the method options that can be used to
   * reproduce all the solutions found.
   */
  void solve( RealMatrix &A,
	      RealMatrix &B,
	      RealMatrixArray &solutions,
	      CompressedSensingOptions &opts,
	      CompressedSensingOptionsList &opts_list );

  void set_linear_solver( CompressedSensingOptions &opts );

  LinearSolver_ptr get_linear_solver();
};

} // namespace Pecos

#endif //LINEAR_SOLVERS_HPP