/* Copyright (C) 2010 Ion Torrent Systems, Inc. All Rights Reserved */
#include <Eigen/Dense>
#include <Eigen/LU>
#include "ZeromerMatDiff.h"
#include "IonH5Eigen.h"
#include "SampleStats.h"
#include "SampleQuantiles.h"

void ZeromerMatDiff::ShiftReference(int n_frames, size_t n_flow_wells, float shift,
                                    float *orig_data, float *shifted_data) {
  if (shift == 0.0f) {
    memcpy(shifted_data, orig_data, sizeof(float) * n_flow_wells * n_frames);
    return;
  }
  Eigen::Map<Eigen::MatrixXf, Eigen::Aligned> orig(orig_data, n_flow_wells, n_frames);
  Eigen::Map<Eigen::MatrixXf, Eigen::Aligned> shifted(shifted_data, n_flow_wells, n_frames);
  for (int frame_ix = 0; frame_ix < n_frames; frame_ix++) {
    int start_frame = floor(frame_ix + shift);
    int end_frame = ceil(frame_ix + shift);
    //    if (shift < 0) { std::swap(start_frame, end_frame); }
    if (start_frame >= 0 && end_frame < n_frames) {
      //interpolate...
      float mult = shift - floor(shift);
      shifted.col(frame_ix).array() = (orig.col(end_frame).array() - orig.col(start_frame).array()) * mult;
      shifted.col(frame_ix).array() += orig.col(start_frame).array();
    }
    else {
      // extrapolate backwards
      if (shift < 0) {
        // calculate slope
        shifted.col(frame_ix).array() = (orig.col(1).array() - orig.col(0).array());
        shifted.col(frame_ix).array() = orig.col(0).array() + shifted.col(frame_ix).array() * (frame_ix + shift);
      }
      // extrapolate forwards
      else if (shift > 0) {
        shifted.col(frame_ix).array() = (orig.col(n_frames - 1).array() - orig.col(n_frames - 2).array());
        shifted.col(frame_ix).array() = orig.col(n_frames - 1).array() + shifted.col(frame_ix).array() * (frame_ix + shift - (n_frames - 1));
      }
      else {
        assert(0);
      }
    }
  }
}

void ZeromerMatDiff::PredictZeromersSignal(const float *time, int n_frames,
                                           float *trace, float *ref, float *zeromer,
                                           size_t n_wells, size_t n_flows,
                                           size_t n_flow_wells,
                                           float taue_est, float *__restrict taub,
                                           const std::string &h5_dump) {
  Eigen::Map<Eigen::MatrixXf, Eigen::Aligned> trace_data(trace, n_flow_wells, n_frames);
  Eigen::Map<Eigen::MatrixXf, Eigen::Aligned> ref_data(ref, n_flow_wells, n_frames);
  Eigen::Map<Eigen::MatrixXf, Eigen::Aligned> zeromer_est(zeromer, n_flow_wells, n_frames);
  zeromer_est.setZero();
  Eigen::Map<Eigen::VectorXf, Eigen::Aligned> taub_v(taub, n_flow_wells);
  Eigen::VectorXf cdelta(n_flow_wells);
  cdelta.setZero();
  // column based operations vectorized by eigen for speed
  for (int f_ix = 1; f_ix < trace_data.cols(); f_ix++) {
    float dtime = time[f_ix] - time[f_ix -1];
    zeromer_est.col(f_ix).array() = ref_data.col(f_ix).array() * (taue_est + dtime);
    zeromer_est.col(f_ix) = zeromer_est.col(f_ix) + cdelta;
    zeromer_est.col(f_ix).array() = zeromer_est.col(f_ix).array() / (taub_v.array() + dtime);
    cdelta.array() += ref_data.col(f_ix).array() - zeromer_est.col(f_ix).array();
  }
  if (!h5_dump.empty()) {
    H5File h5(h5_dump);
    h5.Open(true);
    Eigen::MatrixXf trace_data_tmp = trace_data;
    Eigen::MatrixXf ref_data_tmp = ref_data;
    Eigen::MatrixXf zeromer_est_tmp = zeromer_est;
    H5Eigen::WriteMatrix(h5, "/traces", trace_data_tmp);
    H5Eigen::WriteMatrix(h5, "/ref", ref_data_tmp);
    H5Eigen::WriteMatrix(h5, "/shifted_ref", ref_data_tmp);
    H5Eigen::WriteMatrix(h5, "/zeromer", zeromer_est_tmp);
    h5.Close();
  }
  //  zeromer_est = trace_data - zeromer_est;
}

void ZeromerMatDiff::ZeromerMadError(const int *zero_flows, size_t n_zero_flows,
                                     float *signal_data,
                                     size_t n_wells, size_t n_flows,
                                     size_t n_flow_wells, size_t n_frames,
                                     float &mad) {
  // Calculate basics per well for integral, mad, max for each well/flow
  SampleStats<double> mad_mean;
  for (size_t z_ix = 0; z_ix < n_zero_flows; z_ix++) {
    size_t flow_ix = zero_flows[z_ix];
    for (size_t col_ix = 0; col_ix < n_frames; col_ix++) {
      float *__restrict signal_start = signal_data + n_wells * flow_ix + n_flow_wells * col_ix;
      float *__restrict signal_end = signal_start + n_wells;
      while (signal_start != signal_end) {
        mad_mean.AddValue(fabs(*signal_start));
        signal_start++;
      }
    }
  }
  mad = mad_mean.GetMean();
}

void ZeromerMatDiff::ZeromerSumSqErrorTrim(const int *zero_flows, size_t n_zero_flows,
                                           const char *bad_wells,
                                           float *signal_data, float *predict_data,
                                           size_t n_wells, size_t n_flows,
                                           size_t n_flow_wells, size_t n_frames,
                                           double &ssq) {
  double ssq_sum = 0;
  size_t bad_count = 0;
  float max_value = 1000.0f;
  for (size_t z_ix = 0; z_ix < n_zero_flows; z_ix++) {
    size_t flow_ix = zero_flows[z_ix];
    for (size_t col_ix = 0; col_ix < n_frames; col_ix++) {
      float *__restrict signal_start = signal_data + n_wells * flow_ix + n_flow_wells * col_ix;
      float *__restrict signal_end = signal_start + n_wells;
      float *__restrict predict_start = predict_data + n_wells * flow_ix + n_flow_wells * col_ix;
      const char *__restrict bad_start = bad_wells;
      while (signal_start != signal_end) {
        double value = *signal_start - *predict_start; //*signal_start * *signal_start;
        if (*bad_start == 0 && isfinite(value) && fabs(value) <= max_value) {
          ssq_sum += value * value;
        }
        else if (*bad_start == 0) {
          *predict_start = *signal_start + max_value;
          ssq_sum += max_value;
          bad_count++;
        }
        // levmar has it's own version of residual calculation so have to sub in the nan values
        if (!isfinite(value) || std::isnan(value) || value > max_value) {
          *predict_start = *signal_start + max_value;
        }
        bad_start++;
        signal_start++;
        predict_start++;
      }
    }
  }
  ssq = ssq_sum;
}

void ZeromerMatDiff::FitTauB(const int *zero_flows, size_t n_zero_flows,
                             const float *trace_data, const float *ref_data,
                             size_t n_wells, size_t n_flows, size_t n_flow_wells,
                             size_t n_frames, float taue_est, float *__restrict taub) {
  Eigen::Map<Eigen::VectorXf, Eigen::Aligned> taub_v(taub, n_flow_wells);
  Eigen::VectorXf vec_sum_x2(n_wells), vec_sum_xy(n_wells),
    vec_previous(n_wells), taub_sum(n_wells);
  taub_sum.setZero();
  taub_v.setZero();
  for (size_t flow_ix = 0; flow_ix < n_zero_flows; flow_ix++) {
    int z_ix = zero_flows[flow_ix];

    vec_sum_x2.setZero();
    vec_sum_xy.setZero();
    vec_previous.setZero();
    for (size_t frame_ix = 0; frame_ix < n_frames; frame_ix++) {
      size_t offset = frame_ix * n_flow_wells + n_wells * z_ix;
      const float * __restrict trace_ptr_start = trace_data + offset;
      const float * __restrict trace_ptr_end = trace_ptr_start + n_wells;
      const float * __restrict ref_ptr = ref_data + offset;
      float * __restrict previous_ptr = vec_previous.data();
      float * __restrict xx_ptr = vec_sum_x2.data();
      float * __restrict xy_ptr = vec_sum_xy.data();
      while (trace_ptr_start != trace_ptr_end) {
        float diff = *ref_ptr - *trace_ptr_start;
        float taues = *ref_ptr * taue_est;
        float y = *previous_ptr + diff + taues;
        *previous_ptr += diff;
        float x = *trace_ptr_start;
        *xx_ptr += x * x;
        *xy_ptr += x * y;

        trace_ptr_start++;
        ref_ptr++;
        previous_ptr++;
        xx_ptr++;
        xy_ptr++;
      }
    }
    float *__restrict tau_b_start = taub_sum.data();
    float *__restrict tau_b_end = tau_b_start + n_wells;
    float *__restrict xx_ptr = vec_sum_x2.data();
    float *__restrict xy_ptr = vec_sum_xy.data();
    while (tau_b_start != tau_b_end) {
      *tau_b_start++ += *xy_ptr++ / *xx_ptr++;
    }
  }

  /* for now same taub estimate per nuc, just copy for each flow. */
  for (size_t flow_ix = 0; flow_ix < n_flows; flow_ix++) {
    float *__restrict tau_b_start = taub + flow_ix * n_wells;
    float *__restrict tau_b_end = tau_b_start + n_wells;
    float *__restrict tau_b_sum_start = taub_sum.data();
    while (tau_b_start != tau_b_end) {
      *tau_b_start++ = *tau_b_sum_start++ / n_zero_flows;
    }
  }

}

void ZeromerMatDiff::FitTauBNuc(const int *zero_flows, size_t n_zero_flows,
                                const float *trace_data, const float *ref_data,
                                int *nuc_flows,
                                size_t n_wells, size_t n_flows, size_t n_flow_wells,
                                size_t n_frames, float taue_est, float *__restrict taub) {
  float nuc_weight_mult = .3;
  float combo_weight_mult = .7;

  Eigen::Map<Eigen::VectorXf, Eigen::Aligned> taub_v(taub, n_flow_wells);
  Eigen::VectorXf vec_sum_x2(n_wells), vec_sum_xy(n_wells),
    vec_previous(n_wells), taub_sum(n_wells);
  Eigen::MatrixXf taub_nuc(n_wells, 4);
  int nuc_counts[4] = {0,0,0,0};
  taub_nuc.setZero();
  taub_v.setZero();
  taub_sum.setZero();
  for (size_t flow_ix = 0; flow_ix < n_zero_flows; flow_ix++) {
    int z_ix = zero_flows[flow_ix];
    vec_sum_x2.setZero();
    vec_sum_xy.setZero();
    vec_previous.setZero();
    for (size_t frame_ix = 0; frame_ix < n_frames; frame_ix++) {
      size_t offset = frame_ix * n_flow_wells + n_wells * z_ix;
      const float * __restrict trace_ptr_start = trace_data + offset;
      const float * __restrict trace_ptr_end = trace_ptr_start + n_wells;
      const float * __restrict ref_ptr = ref_data + offset;
      float * __restrict previous_ptr = vec_previous.data();
      float * __restrict xx_ptr = vec_sum_x2.data();
      float * __restrict xy_ptr = vec_sum_xy.data();
      while (trace_ptr_start != trace_ptr_end) {
        float diff = *ref_ptr - *trace_ptr_start;
        float taues = *ref_ptr * taue_est;
        float y = *previous_ptr + diff + taues;
        *previous_ptr += diff;
        float x = *trace_ptr_start;
        *xx_ptr += x * x;
        *xy_ptr += x * y;

        trace_ptr_start++;
        ref_ptr++;
        previous_ptr++;
        xx_ptr++;
        xy_ptr++;
      }
    }
    int nuc_ix = nuc_flows[z_ix];
    nuc_counts[nuc_ix]++;
    float *__restrict tau_b_nuc_start = taub_nuc.col(nuc_ix).data();
    float *__restrict tau_b_start = taub_sum.data();
    float *__restrict tau_b_end = tau_b_start + n_wells;
    float *__restrict xx_ptr = vec_sum_x2.data();
    float *__restrict xy_ptr = vec_sum_xy.data();
    while (tau_b_start != tau_b_end) {
      float value = *xy_ptr++ / *xx_ptr++;
      if (!isfinite(value)) {
        value = 0;
      }
      *tau_b_start++ += value;
      *tau_b_nuc_start++ += value;
    }
  }

  /* for now same taub estimate per nuc, just copy for each flow. */
  for (size_t flow_ix = 0; flow_ix < n_flows; flow_ix++) {
    int nuc_ix = nuc_flows[flow_ix];
    float *__restrict tau_b_start = taub + flow_ix * n_wells;
    float *__restrict tau_b_end = tau_b_start + n_wells;
    float *__restrict tau_b_sum_start = taub_sum.data();
    float *__restrict tau_b_nuc_sum_start = taub_nuc.col(nuc_ix).data();
    // little awkward as we mult weight by number of observances but then divide out for average but more readable to keep calc
    float nuc_div = nuc_counts[nuc_ix];
    float nuc_weight = nuc_counts[nuc_ix] * nuc_weight_mult;
    float nuc_mult = nuc_div > 0.0f ? nuc_weight / nuc_div : 0.0f;
    float total_weight = nuc_weight + combo_weight_mult;
    float combo_mult = combo_weight_mult/n_zero_flows;
    while (tau_b_start != tau_b_end) {
      // weighted average of nuc specific and overall for this well taub
      *tau_b_start = ((*tau_b_sum_start * combo_mult) + (*tau_b_nuc_sum_start * nuc_mult)) / total_weight;
      tau_b_start++;
      tau_b_sum_start++;
      tau_b_nuc_sum_start++;
    }
  }
}


/*
 * Copyright 2010, 2011, 2012
 * Nicolas Mansard,
 * François Bleibel,
 * Olivier Stasse,
 * Florent Lamiraux
 *
 * CNRS/AIST
 *
 */

#ifndef __SOT_KALMAN_H
#define __SOT_KALMAN_H

/* -------------------------------------------------------------------------- */
/* --- INCLUDE -------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */

#include <Eigen/LU>
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/constraint.hh>

/* -------------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------------ */
/* -------------------------------------------------------------------------- */

#if defined(WIN32)
#if defined(kalman_EXPORTS)
#define SOT_KALMAN_EXPORT __declspec(dllexport)
#else
#define SOT_KALMAN_EXPORT __declspec(dllimport)
#endif
#else
#define SOT_KALMAN_EXPORT
#endif

/* -------------------------------------------------------------------------- */
/* --- CLASSE --------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */

namespace dynamicgraph {
namespace sot {

class SOT_KALMAN_EXPORT Kalman : public Entity {
public:
  static const std::string CLASS_NAME;
  virtual const std::string &getClassName(void) const { return CLASS_NAME; }

protected:
  unsigned int size_state;
  unsigned int size_measure;
  double dt;

public:
  SignalPtr<Vector, int> measureSIN;         // y
  SignalPtr<Matrix, int> modelTransitionSIN; // F
  SignalPtr<Matrix, int> modelMeasureSIN;    // H
  SignalPtr<Matrix, int> noiseTransitionSIN; // Q
  SignalPtr<Matrix, int> noiseMeasureSIN;    // R

  SignalPtr<Vector, int> statePredictedSIN;            // x_{k|k-1}
  SignalPtr<Vector, int> observationPredictedSIN;      // y_pred = h (x_{k|k-1})
  SignalTimeDependent<Matrix, int> varianceUpdateSOUT; // P
  SignalTimeDependent<Vector, int> stateUpdateSOUT;    // X_est

  SignalTimeDependent<Matrix, int> gainSINTERN;       // K
  SignalTimeDependent<Matrix, int> innovationSINTERN; // S

public:
  virtual std::string getDocString() const {
    return "Implementation of extended Kalman filter     \n"
           "\n"
           "  Dynamics of the system:                    \n"
           "\n"
           "    x = f (x   , u   ) + w       (state)      \n"
           "     k      k-1   k-1     k-1                 \n"
           "\n"
           "    y = h (x ) + v               (observation)\n"
           "     k      k     k                           \n"
           "\n"
           "  Prediction:\n"
           "\n"
           "    ^          ^                       \n"
           "    x     = f (x       , u   )     (state) \n"
           "     k|k-1      k-1|k-1   k-1          \n"
           "\n"
           "                           T           \n"
           "    P     = F    P        F    + Q (covariance)\n"
           "     k|k-1   k-1  k-1|k-1  k-1         \n"
           "\n"
           "  with\n"
           "           \\                         \n"
           "           d f  ^                         \n"
           "    F    = --- (x       , u   )           \n"
           "     k-1   \\     k-1|k-1   k-1            \n"
           "           d x                         \n"
           "\n"
           "         \\                             \n"
           "         d h  ^                          \n"
           "    H  = --- (x       )                     \n"
           "     k   \\     k-1|k-1                      \n"
           "         d x                             \n"

           "  Update:\n"
           "\n"
           "                ^                            \n"
           "    z = y  - h (x     )             (innovation)\n"
           "     k   k       k|k-1                       \n"
           "                   T                          \n"
           "    S = H  P      H  + R            (innovation covariance)\n"
           "     k   k  k|k-1  k                          \n"
           "                T  -1                         \n"
           "    K = P      H  S                 (Kalman gain)\n"
           "     k   k|k-1  k  k                          \n"
           "    ^     ^                                   \n"
           "    x   = x      + K  z             (state)   \n"
           "     k|k   k|k-1    k  k                      \n"
           "\n"
           "    P   =(I - K  H ) P                        \n"
           "     k|k       k  k   k|k-1                   \n"
           "\n"
           "  Signals\n"
           "    - input(vector)::x_pred:  state prediction\n"
           "                                                         ^\n"
           "    - input(vector)::y_pred:  observation prediction: h (x     )\n"
           "                                                          k|k-1\n"
           "    - input(matrix)::F:       partial derivative wrt x of f\n"
           "    - input(vector)::y:       measure         \n"
           "    - input(matrix)::H:       partial derivative wrt x of h\n"
           "    - input(matrix)::Q:       variance of noise w\n"
           "                                                 k-1\n"
           "    - input(matrix)::R:       variance of noise v\n"
           "                                                 k\n"
           "    - output(matrix)::P_pred: variance of prediction\n"
           "                                               ^\n"
           "    - output(vector)::x_est:  state estimation x\n"
           "                                                k|k\n";
  }

protected:
  Matrix &computeVarianceUpdate(Matrix &P_k_k, const int &time);
  Vector &computeStateUpdate(Vector &x_est, const int &time);

  void setStateEstimation(const Vector &x0) {
    stateEstimation_ = x0;
    stateUpdateSOUT.recompute(0);
  }

  void setStateVariance(const Matrix &P0) {
    stateVariance_ = P0;
    varianceUpdateSOUT.recompute(0);
  }
  // Current state estimation
  // ^
  // x
  //  k-1|k-1
  Vector stateEstimation_;
  // Variance of current state estimation
  // P
  //  k-1|k-1
  Matrix stateVariance_;

  //                          ^
  // Innovation: z  = y  - H  x
  //              k    k    k  k|k-1
  Vector z_;

  // F    P
  //  k-1  k-1|k-1
  Matrix FP_;

  // Variance prediction
  // P
  //  k|k-1
  Matrix Pk_k_1_;

  // Innovation covariance
  Matrix S_;

  // Kalman Gain
  Matrix K_;

public:
  Kalman(const std::string &name);
  /* --- Entity --- */
  void display(std::ostream &os) const;
};

} // namespace sot
} // namespace dynamicgraph

/*!
  \file Kalman.h
  \brief  Extended kalman filter implementation
*/

#endif


//==================================================================================================
/**
  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_X86_AVX_SIMD_FUNCTION_NBTRUE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX_SIMD_FUNCTION_NBTRUE_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/popcnt.hpp>

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

   BOOST_DISPATCH_OVERLOAD( nbtrue_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::double_<A0>, bs::avx_>
                          )
   {
      BOOST_FORCEINLINE std::size_t operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        int r = _mm256_movemask_pd(is_nez(a0));
        return  popcnt(r);
      }
   };

   BOOST_DISPATCH_OVERLOAD( nbtrue_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::single_<A0>, bs::avx_>
                          )
   {
      BOOST_FORCEINLINE std::size_t operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        int r = _mm256_movemask_ps(is_nez(a0));
        return popcnt(r);
      }
   };

   BOOST_DISPATCH_OVERLOAD( nbtrue_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::integer_<A0>, bs::avx_>
                          )
   {
      BOOST_FORCEINLINE std::size_t operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        auto const s = slice(a0);
        return nbtrue(s[0]) + nbtrue(s[1]);
      }
   };
} } }

#endif


/*

Copyright (c) 2005-2021, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef VANLEEUWEN2009WNTSWATCELLCYCLEODESYSTEM_HPP_
#define VANLEEUWEN2009WNTSWATCELLCYCLEODESYSTEM_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/shared_ptr.hpp>

#include <cmath>
#include <iostream>

#include "AbstractOdeSystem.hpp"
#include "AbstractCellMutationState.hpp"
#include "MathsCustomFunctions.hpp"

/**
 * Represents the van Leeuwen et al. (2007) system of ODEs
 * [doi:10.1016/j.jtbi.2007.01.019]
 * coupled to the Swat et al. cell-cycle model equations.
 * [doi:10.1093/bioinformatics/bth110]
 *
 * The variables are
 *
 *   0. r = pRb
 *   1. e = E2F1 (This is the S-phase indicator)
 *   2. i = CycD (inactive)
 *   3. j = CycD (active)
 *   4. p = pRb-p
 *   5. D = APC destruction complex
 *   6. X = Axin
 *   7. Cu = Beta Cat marked for ubiquitination
 *   8. Co = Open form Beta Cat
 *   9. Cc = Closed form Beta Cat
 *   10. Mo = Open form Mutant Beta Cat
 *   11. Mc = Closed form Mutant Beta Cat
 *   12. A = Free Adhesion molecules
 *   13. Ca = BetaCat/Adhesion
 *   14. Ma = Mutant BetaCat/Adhesion
 *   15. T = free TCF
 *   16. Cot = Open BetaCat/TCF
 *   17. Cct = Closed BetaCat/TCF
 *   18. Mot = Open Mutant BetaCat/TCF
 *   19. Mct = Closed Mutant BetaCat/TCF
 *   20. Y = Wnt Target protein
 *   21. Wnt level
 */
class VanLeeuwen2009WntSwatCellCycleOdeSystem : public AbstractOdeSystem
{
private:

    /**
     * Parameters for the Swat et al. (2004) model
     */

    /** Dimensional parameter k_2. */
    double mk2d;
    /** Dimensional parameter k_3. */
    double mk3d;
    /** Dimensional parameter k_34. */
    double mk34d;
    /** Dimensional parameter k_2. */
    double mk43d;
    /** Dimensional parameter k_23. */
    double mk23d;
    /** Dimensional parameter a. */
    double mad;
    /** Dimensional parameter J_11. */
    double mJ11d;
    /** Dimensional parameter J_12. */
    double mJ12d;
    /** Dimensional parameter J_13. */
    double mJ13d;
    /** Dimensional parameter J_13. */
    double mJ61d;
    /** Dimensional parameter J_62. */
    double mJ62d;
    /** Dimensional parameter J_63. */
    double mJ63d;
    /** Dimensional parameter K_m1. */
    double mKm1d;
    /** Dimensional parameter k_p. */
    double mkpd;
    /** Dimensionless parameter phi_r. */
    double mphi_r;
    /** Dimensionless parameter phi_i. */
    double mphi_i;
    /** Dimensionless parameter phi_j. */
    double mphi_j;
    /** Dimensionless parameter phi_p. */
    double mphi_p;
    /** Dimensional parameter k_16. */
    double mk16d;
    /** Dimensional parameter k_61. */
    double mk61d;
    /** Dimensionless parameter phi_E2F1. */
    double mPhiE2F1;

    /**
     * Parameters for the Van Leeuwen et al. (2007) model
     */

    /** Dimensionless parameter s_A. */
    double mSa;
    /** Dimensionless parameter s_CA. */
    double mSca;
    /** Dimensionless parameter s_C. */
    double mSc;
    /** Dimensionless parameter s_CT. */
    double mSct;
    /** Dimensionless parameter s_D. */
    double mSd;
    /** Dimensionless parameter s_T. */
    double mSt;
    /** Dimensionless parameter s_X. */
    double mSx;
    /** Dimensionless parameter s_Y. */
    double mSy;
    /** Dimensionless parameter d_A. */
    double mDa;
    /** Dimensionless parameter d_CA. */
    double mDca;
    /** Dimensionless parameter d_C. */
    double mDc;
    /** Dimensionless parameter d_CT. */
    double mDct;
    /** Dimensionless parameter d_D. */
    double mDd;
    /** Dimensionless parameter d_Dx. */
    double mDdx;
    /** Dimensionless parameter d_T. */
    double mDt;
    /** Dimensionless parameter d_U. */
    double mDu;
    /** Dimensionless parameter d_X. */
    double mDx;
    /** Dimensionless parameter d_Y. */
    double mDy;
    /** Dimensionless parameter K_c. */
    double mKc;
    /** Dimensionless parameter K_D. */
    double mKd;
    /** Dimensionless parameter K_T. */
    double mKt;
    /** Dimensionless parameter p_c. */
    double mPc;
    /** Dimensionless parameter p_u. */
    double mPu;
    /** Dimensionless parameter xi_D. */
    double mXiD;
    /** Dimensionless parameter xi_Dx. */
    double mXiDx;
    /** Dimensionless parameter xi_X. */
    double mXiX;
    /** Dimensionless parameter xi_C. */
    double mXiC;

    /** The mutation state of the cell. */
    boost::shared_ptr<AbstractCellMutationState> mpMutationState;

    /**
     * The hypothesis we are using
     *  = 1u for Van Leeuwen Hypothesis I
     *  = 2u for Van Leeuwen Hypothesis II
     */
    unsigned mHypothesis;

    /** The Wnt level (this affects the ODE system). */
    double mWntLevel;

    friend class boost::serialization::access;
    /**
     * Serialize the object and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractOdeSystem>(*this);
    }

public:

    /**
     * Constructor.
     *
     * @param hypothesis takes the value 1 or 2 and affects the ODE system.
     * @param wntLevel is a non-dimensional Wnt value between 0 and 1. This sets up the Wnt pathway in its steady state.
     * @param pMutationState cell mutation; some affect the ODE system
     * @param stateVariables optional initial conditions for state variables (only used in archiving)
     */
    VanLeeuwen2009WntSwatCellCycleOdeSystem(unsigned hypothesis,
                                            double wntLevel = 0.0,
                                            boost::shared_ptr<AbstractCellMutationState> pMutationState=boost::shared_ptr<AbstractCellMutationState>(),
                                            std::vector<double> stateVariables=std::vector<double>());

    /**
     * Destructor.
     */
    ~VanLeeuwen2009WntSwatCellCycleOdeSystem();

    /**
     * Initialise parameter values.
     */
    void Init();

    /**
     * Set the mutation state of the cell.
     *
     * This should be called by the relevant cell-cycle model before any solving
     * of the ODE system (as it is used to evaluate the Y derivatives).
     *
     * @param pMutationState the mutation state.
     */
    void SetMutationState(boost::shared_ptr<AbstractCellMutationState> pMutationState);

    /**
     * Called by the archive function on the Wnt cell-cycle model.
     *
     * @return #mpMutationState the mutation state of the cell.
     */
    const boost::shared_ptr<AbstractCellMutationState> GetMutationState() const;

    /**
     * Compute the RHS of the system of ODEs.
     *
     * Returns a vector representing the RHS of the ODEs at each time step, y' = [y1' ... yn'].
     * An ODE solver will call this function repeatedly to solve for y = [y1 ... yn].
     *
     * @param time used to evaluate the RHS.
     * @param rY value of the solution vector used to evaluate the RHS.
     * @param rDY filled in with the resulting derivatives (using van Leeuwen et al. (2007) system of equations)
     */
    void EvaluateYDerivatives(double time, const std::vector<double>& rY, std::vector<double>& rDY);

    /**
     * Calculate whether the conditions for the cell cycle to finish have been met.
     *
     * @param time at which to calculate whether the stopping event has occurred
     * @param rY value of the solution vector used to evaluate the RHS
     *
     * @return whether or not stopping conditions have been met
     */
    bool CalculateStoppingEvent(double time, const std::vector<double>& rY);

    /**
     * When using CVODE this function is called instead of CalculateStoppingEvent.
     * It allows the point at which rY[1] reaches 1 to be found to greater precision.
     *
     * @param time at which to calculate whether the stopping event has occurred
     * @param rY value of the solution vector used to evaluate the RHS
     *
     * @return How close we are to the root of the stopping condition
     */
    double CalculateRootFunction(double time, const std::vector<double>& rY);

    /**
     * @return #mWntLevel.
     */
    double GetWntLevel() const;

    /**
     * @return #mHypothesis.
     */
    unsigned GetHypothesis() const;
};

// Declare identifier for the serializer
#include "SerializationExportWrapper.hpp"
CHASTE_CLASS_EXPORT(VanLeeuwen2009WntSwatCellCycleOdeSystem)

namespace boost
{
namespace serialization
{
/**
 * Serialize information required to construct a VanLeeuwen2009WntSwatCellCycleOdeSystem.
 */
template<class Archive>
inline void save_construct_data(
    Archive & ar, const VanLeeuwen2009WntSwatCellCycleOdeSystem * t, const unsigned int file_version)
{
    // Save data required to construct instance
    const unsigned hypothesis = t->GetHypothesis();
    ar & hypothesis;

    const double wnt_level = t->GetWntLevel();
    ar & wnt_level;

    const boost::shared_ptr<AbstractCellMutationState> p_mutation_state = t->GetMutationState();
    ar & p_mutation_state;

    const std::vector<double>& state_variables = t->rGetConstStateVariables();
    ar & state_variables;
}

/**
 * De-serialize constructor parameters and initialise a VanLeeuwen2009WntSwatCellCycleOdeSystem.
 */
template<class Archive>
inline void load_construct_data(
    Archive & ar, VanLeeuwen2009WntSwatCellCycleOdeSystem * t, const unsigned int file_version)
{
    // Retrieve data from archive required to construct new instance
    unsigned hypothesis;
    ar & hypothesis;

    double wnt_level;
    ar & wnt_level;

    boost::shared_ptr<AbstractCellMutationState> p_mutation_state;
    ar & p_mutation_state;

    std::vector<double> state_variables;
    ar & state_variables;

    // Invoke inplace constructor to initialise instance
    ::new(t)VanLeeuwen2009WntSwatCellCycleOdeSystem(hypothesis, wnt_level, p_mutation_state, state_variables);
}
}
} // namespace ...

#endif /*VANLEEUWEN2009WNTSWATCELLCYCLEODESYSTEM_HPP_*/


/*
 * Copyright (C) 2014, Computing Systems Laboratory (CSLab), NTUA.
 * Copyright (C) 2014, Vasileios Karakasis
 * All rights reserved.
 *
 * This file is distributed under the BSD License. See LICENSE.txt for details.
 */

/**
 * \file Utility.hpp
 * \brief Several generic utility functions
 *
 * \author Computing Systems Laboratory (CSLab), NTUA
 * \date 2011–2014
 * \copyright This file is distributed under the BSD License. See LICENSE.txt
 * for details.
 */

#ifndef SPARSEX_INTERNALS_UTILITY_HPP
#define SPARSEX_INTERNALS_UTILITY_HPP

#include <boost/type_traits.hpp>

using namespace std;

namespace sparsex {
  namespace utilities {

    template<bool property>
    struct math_impl
    {
      template<typename T>
      static T do_gcd(T a, T b);

      template <typename T>
      static T do_abs(T a);

      template <typename T>
      static T do_iceil(T a, T b);
    };

    template<typename T>
    T gcd(T a, T b)
    {
      return math_impl<boost::is_integral<T>::value>::do_gcd(a, b);
    }

    // Integer ceiling
    template<typename T>
    T iceil(T a, T b)
    {
      return math_impl<boost::is_integral<T>::value>::do_iceil(a, b);
    }

    template<typename T>
    T abs(T a)
    {
      return math_impl<boost::is_arithmetic<T>::value>::do_abs(a);
    }

    template<typename T>
    T lcm(T a, T b)
    {
      return abs(a*b) / gcd(a, b);
    }

    template<>
    struct math_impl<true>
    {
      template<typename T>
      static T do_gcd(T a, T b)
      {
        while (b) {
	  T t = b;
	  b = a % b;
	  a = t;
        }

        return a;
      }

      template<typename T>
      static T do_iceil(T a, T b)
      {
        return a / b + (a % b != 0);
      }

      template<typename T>
      static T do_abs(T a)
      {
        return (a < 0) ? -a : a;
      }
    };

    template<bool is_pointer>
    struct iterator_checker
    {
      template<typename Iterator>
      static void do_check(const Iterator &) {}
    };

    template<typename Iterator>
    void check_iterator(const Iterator &i)
    {
      iterator_checker<boost::is_pointer<Iterator>::value>::do_check(i);
    }

    template<>
    struct iterator_checker<true>
    {
      template<typename Iterator>
      static void do_check(const Iterator &i)
      {
        if (!i)
	  throw invalid_argument("invalid iterator");
      }
    };

  } // end of namespace utilities
} // end of namespace sparsex

#endif  // SPARSEX_INTERNALS_UTILITY_HPP


#include "SMPSInput.hpp"
#include "ClpBALPInterface.hpp"
#include <boost/scoped_ptr.hpp>
#include <cstdlib>

using boost::scoped_ptr; // replace with unique_ptr for C++11
using namespace std;

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

	MPI_Init(&argc, &argv);

	int mype;
	MPI_Comm_rank(MPI_COMM_WORLD,&mype);

	if (argc != 2) {
		if (mype == 0) printf("Usage: %s [SMPS root name]\n",argv[0]);
		return 1;
	}

	string smpsrootname(argv[1]);

	SMPSInput input(smpsrootname+".cor",smpsrootname+".tim",smpsrootname+".sto");

	BAContext ctx(MPI_COMM_WORLD);
	ctx.initializeAssignment(input.nScenarios());

	ClpBALPInterface solver(input, ctx, ClpBALPInterface::useDual);
	if (argc == 5) {
		solver.loadStatus(argv[4]);
	}
	//solver.setDumpFrequency(5000,argv[3]);
	solver.setPrimalTolerance(1e-6);
	solver.setDualTolerance(1e-6);
	solver.go();

	if (argc >= 4 && argv[3][0] != '-') {
		if (mype == 0) printf("Writing solution\n");
		solver.writeStatus(argv[3]);
		if (mype == 0) printf("Finished writing solution\n");
	}

	MPI_Finalize();

	return 0;
}


//==================================================================================================
/*!
  @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_DETAIL_CONSTANT_MEDIUM_PI_HPP_INCLUDED
#define BOOST_SIMD_DETAIL_CONSTANT_MEDIUM_PI_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

/*

    @ingroup group-constant

    Constant used in trigonometric reductions

    @par Semantic:

    For type T:

    @code
    T r = Medium_pi<T>();
    @endcode

    is similar to:

    @code
    if T is double
      r = Pi<T>()*pow2(18);
    else if T is float
      r = Pi<T>()*pow2(6);
    else
      r =  201
    @endcode

    @return a value of type T

*/

namespace boost { namespace simd
{
  namespace tag
  {
    struct medium_pi_ : boost::dispatch::constant_value_<medium_pi_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,medium_pi_,boost::dispatch::constant_value_<medium_pi_>);
      BOOST_SIMD_REGISTER_CONSTANT(201, 0X43490FDB, 0X412921FB54442D18LL); //2^6/pi, //2^{18}/pi;
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag,medium_pi_);
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::medium_pi_,medium_pi);
  }

  template<typename T> BOOST_FORCEINLINE auto Medium_pi()
  BOOST_NOEXCEPT_DECLTYPE(detail::medium_pi( boost::dispatch::as_<T>{}))
  {
    return detail::medium_pi( boost::dispatch::as_<T>{} );
  }
} }

#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


#include <boost/numeric/odeint/stepper/generation/generation_dense_output_runge_kutta.hpp>


#include <Eigen/Core>
#include <aslam/common/pose-types.h>
#include <ceres-error-terms/parameterization/pose-param-jpl.h>
#include <ceres/ceres.h>
#include <eigen-checks/gtest.h>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <maplab-common/quaternion-math.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

#include <ceres-error-terms/common.h>
#include <ceres-error-terms/six-dof-block-pose-error-term-autodiff.h>

class SixDofBlockPoseErrorTerms : public ::testing::Test {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 protected:
  virtual void SetUp() {
    prior_position_A_ << 1, 2, 3;
    prior_position_B_ << 4, 5, 6;
    prior_orientation_A_.coeffs() << sqrt(2) / 2, 0, 0, sqrt(2) / 2;
    prior_orientation_B_.coeffs() << 0.15496688, -0.60676538, 0.30722298,
        0.71654385;
    prior_orientation_B_.coeffs().normalize();

    pose_A_ << prior_orientation_A_.coeffs(), prior_position_A_;
    pose_B_ << prior_orientation_B_.coeffs(), prior_position_B_;

    covariance_matrix_.setIdentity();
  }

  void addResidual(const aslam::Transformation& T_A_B);
  void solve();

  void fixA();
  void fixB();

  ceres::Problem problem_;
  ceres::Solver::Summary summary_;

  Eigen::Vector3d prior_position_A_;
  Eigen::Vector3d prior_position_B_;
  Eigen::Quaterniond prior_orientation_A_;
  Eigen::Quaterniond prior_orientation_B_;

  Eigen::Matrix<double, 7, 1> pose_A_;
  Eigen::Matrix<double, 7, 1> pose_B_;
  Eigen::Matrix<double, 6, 6> covariance_matrix_;
};

void SixDofBlockPoseErrorTerms::addResidual(
    const aslam::Transformation& T_A_B) {
  ceres::LocalParameterization* pose_parameterization =
      new ceres_error_terms::JplPoseParameterization;

  ceres::CostFunction* relative_pose_cost = new ceres::AutoDiffCostFunction<
      ceres_error_terms::SixDoFBlockPoseErrorTerm,
      ceres_error_terms::SixDoFBlockPoseErrorTerm::residualBlockSize,
      ceres_error_terms::poseblocks::kPoseSize,
      ceres_error_terms::poseblocks::kPoseSize>(
      new ceres_error_terms::SixDoFBlockPoseErrorTerm(
          T_A_B, covariance_matrix_));

  problem_.AddResidualBlock(
      relative_pose_cost, NULL, pose_A_.data(), pose_B_.data());

  problem_.SetParameterization(pose_A_.data(), pose_parameterization);
  problem_.SetParameterization(pose_B_.data(), pose_parameterization);
}

void SixDofBlockPoseErrorTerms::fixA() {
  problem_.SetParameterBlockConstant(pose_A_.data());
}

void SixDofBlockPoseErrorTerms::fixB() {
  problem_.SetParameterBlockConstant(pose_B_.data());
}

void SixDofBlockPoseErrorTerms::solve() {
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = false;
  options.parameter_tolerance = 1e-14;
  options.gradient_tolerance = 1e-14;
  options.function_tolerance = 1e-14;
  options.max_num_iterations = 30u;
  LOG(INFO) << "Solving...";
  ceres::Solve(options, &problem_, &summary_);

  LOG(INFO) << summary_.BriefReport() << std::endl;
  LOG(INFO) << summary_.message << std::endl;
}

TEST_F(SixDofBlockPoseErrorTerms, TestPosePriorErrorTermZeroCost) {
  aslam::Transformation T_G_A(prior_orientation_A_, prior_position_A_);
  aslam::Transformation T_G_B(prior_orientation_B_, prior_position_B_);

  addResidual(T_G_A.inverse() * T_G_B);
  fixA();
  solve();

  EXPECT_NEAR(summary_.initial_cost, 0.0, 1e-12);
  EXPECT_NEAR(summary_.final_cost, 0.0, 1e-12);
  EXPECT_EQ(summary_.iterations.size(), 1u);
}

TEST_F(SixDofBlockPoseErrorTerms, TestPosePriorErrorTermIdentityPoseA) {
  aslam::Transformation T_G_A(prior_orientation_A_, prior_position_A_);

  addResidual(aslam::Transformation());
  fixA();
  solve();

  aslam::Transformation T_G_B_est(
      Eigen::Quaterniond(pose_B_.head(4).data()), pose_B_.tail(3));

  EXPECT_NEAR(summary_.final_cost, 0.0, 1e-12);
  EXPECT_TRUE(
      EIGEN_MATRIX_NEAR(
          T_G_B_est.getTransformationMatrix(), T_G_A.getTransformationMatrix(),
          1e-8));
}

TEST_F(SixDofBlockPoseErrorTerms, TestPosePriorErrorTermIdentityPoseB) {
  aslam::Transformation T_G_B(prior_orientation_B_, prior_position_B_);

  addResidual(aslam::Transformation());
  fixB();
  solve();

  aslam::Transformation T_G_A_est(
      Eigen::Quaterniond(pose_A_.head(4).data()), pose_A_.tail(3));

  EXPECT_NEAR(summary_.final_cost, 0.0, 1e-12);
  EXPECT_TRUE(
      EIGEN_MATRIX_NEAR(
          T_G_A_est.getTransformationMatrix(), T_G_B.getTransformationMatrix(),
          1e-8));
}

TEST_F(SixDofBlockPoseErrorTerms, TestPosePriorErrorTermNonIdentity) {
  aslam::Transformation T_G_A(prior_orientation_A_, prior_position_A_);

  Eigen::Vector3d q_A_B_axis_angle_vec;
  q_A_B_axis_angle_vec << 0.2, -1.2, 0.4;
  Eigen::Vector3d q_A_B_axis = q_A_B_axis_angle_vec;
  q_A_B_axis.normalize();
  aslam::AngleAxis q_A_B_angle_axis(q_A_B_axis_angle_vec.norm(), q_A_B_axis);
  aslam::Quaternion q_A_B(q_A_B_angle_axis);
  Eigen::Vector3d p_A_B;
  p_A_B << 12.4, 423.3, -341.423;
  aslam::Transformation T_A_B(q_A_B, p_A_B);

  addResidual(T_A_B);
  fixA();
  solve();

  aslam::Transformation T_G_B_est(
      Eigen::Quaterniond(pose_B_.head(4).data()), pose_B_.tail(3));

  EXPECT_NEAR(summary_.final_cost, 0.0, 1e-12);
  EXPECT_TRUE(
      EIGEN_MATRIX_NEAR(
          T_G_B_est.getTransformationMatrix(),
          (T_G_A * T_A_B).getTransformationMatrix(), 1e-8));
}

MAPLAB_UNITTEST_ENTRYPOINT


#include "NeuralNet.hpp"
#include <string>
#include <fstream>
#include <iostream>
#include <boost/range/irange.hpp>
#include <typeinfo>

int main() {
    std::cout << "Hello World" << std::endl;

    // test softmax forward - OK
    // ActivationSoftmax activation_softmax;
    // Eigen::MatrixXd test_in(2, 3);
    // Eigen::MatrixXd test_out;
    // test_in.row(0) << 1, 2, 3;
    // test_in.row(1) << 4, 5, 6;
    // // std::cout << "test_in" << test_in << std::endl;
    // activation_softmax.forward(test_in);
    // test_out = activation_softmax.output;
    // std::cout << "test_out" << "\n" << test_out << std::endl;

    // test softmax backward - OK
    // ActivationSoftmax activation_softmax;
    // Eigen::MatrixXd test_in(2, 3);
    // Eigen::MatrixXd output(2, 3);
    // Eigen::MatrixXd test_out;
    // test_in.row(0) << 1, 2, 3;
    // test_in.row(1) << 4, 5, 6;
    // std::cout << "test_in" << "\n" << test_in << std::endl;
    // output.row(0) << 1, 2, 3;
    // output.row(1) << 4, 5, 6;
    // activation_softmax.output = output;
    // std::cout << "activation_softmax.output" << "\n" << activation_softmax.output << std::endl;
    // activation_softmax.backward(test_in);
    // test_out = activation_softmax.dinputs;
    // std::cout << "test_out" << "\n" << test_out << std::endl;

    // test dense backward - OK
    // LayerDense dense1(2, 3);
    // Eigen::MatrixXd test_in(2, 3);
    // Eigen::MatrixXd test_out;
    // test_in.row(0) << 1, 2, 3;
    // test_in.row(1) << 4, 5, 6;
    // Eigen::MatrixXd inputs(2, 3);
    // inputs.row(0) << 1, 2, 3;
    // inputs.row(1) << 4, 5, 6;
    // dense1.inputs = inputs;
    // dense1.backward(test_in);
    // test_out = dense1.dweights;
    // std::cout << "test_out" << "\n" << test_out << std::endl;

    // test dense forward - OK
    // LayerDense dense1(2, 3);
    // Eigen::MatrixXd test_in(2, 3);
    // Eigen::MatrixXd test_out;
    // test_in.row(0) << 1, 2, 3;
    // test_in.row(1) << 4, 5, 6;
    // std::cout << "dense1.biases is of size " << dense1.biases.rows() << "x" << dense1.biases.cols() << std::endl;
    // Eigen::MatrixXd weights(2, 3);
    // weights.row(0) << 1.0, -1.0, 0.5;
    // weights.row(1) << 0.5, -0.1, 2.0;
    // dense1.weights = weights;
    // dense1.forward(test_in.transpose());
    // test_out = dense1.output;
    // std::cout << "dense1.weights" << "\n" << dense1.weights << std::endl;
    // std::cout << "test_out" << "\n" << test_out << std::endl;

    // test relu backward - OK
    // ActivationRelu activation_relu;
    // Eigen::MatrixXd test_in(2, 3);
    // Eigen::MatrixXd test_out;
    // test_in.row(0) << 1, 2, 3;
    // test_in.row(1) << 4.1, -5, 6;
    // Eigen::MatrixXd inputs(2, 3);
    // inputs.row(0) << 1, 2, 0;
    // inputs.row(1) << 0, 5, -6;
    // activation_relu.inputs = inputs;
    // activation_relu.backward(test_in);
    // test_out = activation_relu.dinputs;
    // std::cout << "test_out" << "\n" << test_out << std::endl;

    // test relu forward - OK
    // ActivationRelu activation_relu;
    // Eigen::MatrixXd test_in(2, 3);
    // Eigen::MatrixXd test_out;
    // test_in.row(0) << 1, 2, -3;
    // test_in.row(1) << 4.1, -5, 6;
    // activation_relu.forward(test_in);
    // test_out = activation_relu.output;
    // std::cout << "test_out" << "\n" << test_out << std::endl;

    // test loss forward - OK
    // CrossEntropyLoss loss_categorical_crossentropy;
    // Eigen::MatrixXd y_pred(4, 3);
    // Eigen::VectorXd y_true(4);
    // Eigen::MatrixXd test_out;
    // y_pred.row(0) << 0.8, 0.1, 0.1;
    // y_pred.row(1) << 0.4, 0.6, 0.9;
    // y_pred.row(2) << 0.6, 0.1, 0.3;
    // y_pred.row(3) << 0.5, 0.4, 0.1;
    // std::cout << "y_pred" << "\n" << y_pred << std::endl;
    // y_true << 2, 2, 0, 1;
    // std::cout << "y_true" << "\n" << y_true << std::endl;
    // test_out = loss_categorical_crossentropy.forward(y_pred, y_true);
    // std::cout << "test_out" << "\n" << test_out << std::endl;
    // std::cout << "test_out" << "\n" << test_out.rows()<< "x" << test_out.cols() << std::endl;

    // test loss calculate - OK
    // CrossEntropyLoss loss_categorical_crossentropy;
    // Eigen::MatrixXd y_pred(4, 3);
    // Eigen::VectorXd y_true(4);
    // double test_out;
    // y_pred.row(0) << 0.8, 0.1, 0.1;
    // y_pred.row(1) << 0.4, 0.6, 0.9;
    // y_pred.row(2) << 0.6, 0.1, 0.3;
    // y_pred.row(3) << 0.5, 0.4, 0.1;
    // std::cout << "y_pred" << "\n" << y_pred << std::endl;
    // y_true << 2, 2, 0, 1;
    // std::cout << "y_true" << "\n" << y_true << std::endl;
    // test_out = loss_categorical_crossentropy.calculate(y_pred, y_true);
    // std::cout << "test_out" << "\n" << test_out << std::endl;

    // test loss backward - OK
    // CrossEntropyLoss loss_categorical_crossentropy;
    // Eigen::MatrixXd dvalues(4, 3);
    // Eigen::VectorXd y_true(4);
    // Eigen::MatrixXd test_out;
    // dvalues.row(0) << 0.8, 0.1, 0.1;
    // dvalues.row(1) << 0.4, 0.6, 0.9;
    // dvalues.row(2) << 0.6, 0.1, 0.3;
    // dvalues.row(3) << 0.5, 0.4, 0.1;
    // std::cout << "dvalues" << dvalues.rows()<< "x" << dvalues.cols() << std::endl;
    // std::cout << "dvalues" << "\n" << dvalues << std::endl;
    // y_true << 2, 2, 0, 1;
    // std::cout << "y_true" << "\n" << y_true << std::endl;
    // loss_categorical_crossentropy.backward(dvalues, y_true);
    // test_out = loss_categorical_crossentropy.dinputs;
    // std::cout << "test_out" << "\n" << test_out << std::endl;

    // test optimizer pre_update_params - OK
    // StochasticGradientDescent optimizer_SGD(1.0, 1e-3, 0.9);
    // double test_out;
    // optimizer_SGD.iterations = 500;
    // optimizer_SGD.pre_update_params(1.0);
    // test_out = optimizer_SGD.learning_rate;
    // std::cout << "test_out" << "\n" << test_out << std::endl;

    // test optimizer post_update_params - OK
    // StochasticGradientDescent optimizer_SGD(1.0, 1e-3, 0.9);
    // double test_out;
    // optimizer_SGD.iterations = 500;
    // optimizer_SGD.post_update_params();
    // test_out = optimizer_SGD.iterations;
    // std::cout << "test_out" << "\n" << test_out << std::endl;

    // test optimizer update_params - OK
    // LayerDense dense1(2, 3);
    // Eigen::MatrixXd test_in(2, 3);
    // Eigen::MatrixXd test_out_w;
    // Eigen::VectorXd test_out_b;
    // test_in.row(0) << 1, 2, 3;
    // test_in.row(1) << 4, 5, 6;

    // Eigen::MatrixXd weights(2, 3);
    // weights.row(0) << 1.0, -1.0, 0.5;
    // weights.row(1) << 0.5, -0.1, 2.0;
    // dense1.weights = weights;

    // Eigen::MatrixXd dweights(2, 3);
    // dweights.row(0) << 0.1, -0.1, 0.5;
    // dweights.row(1) << 0.7, -0.2, 1.1;
    // dense1.dweights = dweights;

    // Eigen::VectorXd biases(3);
    // biases << 0.02, -0.3, 0.56;
    // dense1.biases = biases;

    // Eigen::VectorXd dbiases(3);
    // dbiases << 1.3, -1.6, 0.44;
    // dense1.dbiases = dbiases;

    // StochasticGradientDescent optimizer_SGD(1.0, 1e-3, 0.9);
    // Eigen::MatrixXd test_out;
    // optimizer_SGD.iterations = 500;
    // optimizer_SGD.update_params(dense1);
    // test_out_w = dense1.weights;
    // std::cout << "test_out_w" << "\n" << test_out_w << std::endl;
    // test_out_b = dense1.biases;
    // std::cout << "test_out_b" << "\n" << test_out_b << std::endl;

}


#include <catch2/catch.hpp>

#include <Toucan/LinAlg.h>

#include <iostream>
#include <cmath>

#include <Eigen/Core>
#include <Eigen/Geometry>

template<typename scalar_type> using EigenVector2 = Eigen::Matrix<scalar_type, 2, 1>;
template<typename scalar_type> using EigenVector3 = Eigen::Matrix<scalar_type, 3, 1>;
template<typename scalar_type> using EigenVector4 = Eigen::Matrix<scalar_type, 4, 1>;

// Toucan-Eigen matrix equality operator
template<typename scalar_type, int rows, int columns>
bool operator==(const Toucan::Matrix<scalar_type, rows, columns>& toucan_matrix, const Eigen::Matrix<scalar_type, rows, columns>& eigen_matrix) {

	for (int row_index = 0; row_index < rows; ++row_index) {
		for (int column_index = 0; column_index < columns; ++column_index) {
			if (toucan_matrix(row_index, column_index) != eigen_matrix(row_index, column_index)) {
				return false;
			}
		}
	}

	return true;
}

// Toucan-Eigen matrix inequality operator
template<typename scalar_type, int rows, int columns>
bool operator!=(const Toucan::Matrix<scalar_type, rows, columns>& toucan_matrix, const Eigen::Matrix<scalar_type, rows, columns>& eigen_matrix) {

	for (int row_index = 0; row_index < rows; ++row_index) {
		for (int column_index = 0; column_index < columns; ++column_index) {
			if (toucan_matrix(row_index, column_index) != eigen_matrix(row_index, column_index)) {
				return true;
			}
		}
	}

	return false;
}

TEMPLATE_TEST_CASE("Matrix static constructors", "[matrix][static_constructor]", float, double, int) {

	SECTION("Zero") {
		Toucan::Matrix4<TestType> matrix_zero_toucan = Toucan::Matrix4<TestType>::Zero();
		Eigen::Matrix<TestType, 4, 4> matrix_zero_eigen = Eigen::Matrix<TestType, 4, 4>::Zero();
		REQUIRE((matrix_zero_toucan == matrix_zero_eigen));
	}

	SECTION("Ones") {
		Toucan::Matrix4<TestType> matrix_ones_toucan = Toucan::Matrix4<TestType>::Ones();
		Eigen::Matrix<TestType, 4, 4> matrix_ones_eigen = Eigen::Matrix<TestType, 4, 4>::Ones();

		REQUIRE((matrix_ones_toucan == matrix_ones_eigen));
	}

	SECTION("Identity") {
		Toucan::Matrix4<TestType> matrix_identity_toucan = Toucan::Matrix4<TestType>::Identity();
		Eigen::Matrix<TestType, 4, 4> matrix_identity_eigen = Eigen::Matrix<TestType, 4, 4>::Identity();

		REQUIRE((matrix_identity_toucan == matrix_identity_eigen));
	}

	SECTION("UnitX") {
		Toucan::Vector2<TestType> v2 = Toucan::Vector2<TestType>::UnitX();
		REQUIRE(v2.x() == Approx(1));
		REQUIRE(v2.y() == Approx(0));

		Toucan::Vector3<TestType> v3 = Toucan::Vector3<TestType>::UnitX();
		REQUIRE(v3.x() == Approx(1));
		REQUIRE(v3.y() == Approx(0));
		REQUIRE(v3.z() == Approx(0));
	}

	SECTION("UnitX RowVector") {
		Toucan::RowVector2<TestType> v2 = Toucan::RowVector2<TestType>::UnitX();
		REQUIRE(v2.x() == Approx(1));
		REQUIRE(v2.y() == Approx(0));

		Toucan::RowVector3<TestType> v3 = Toucan::RowVector3<TestType>::UnitX();
		REQUIRE(v3.x() == Approx(1));
		REQUIRE(v3.y() == Approx(0));
		REQUIRE(v3.z() == Approx(0));
	}

	SECTION("UnitY") {
		Toucan::Vector2<TestType> v2 = Toucan::Vector2<TestType>::UnitY();
		REQUIRE(v2.x() == Approx(0));
		REQUIRE(v2.y() == Approx(1));

		Toucan::Vector3<TestType> v3 = Toucan::Vector3<TestType>::UnitY();
		REQUIRE(v3.x() == Approx(0));
		REQUIRE(v3.y() == Approx(1));
		REQUIRE(v3.z() == Approx(0));
	}

	SECTION("UnitY RowVector") {
		Toucan::RowVector2<TestType> v2 = Toucan::RowVector2<TestType>::UnitY();
		REQUIRE(v2.x() == Approx(0));
		REQUIRE(v2.y() == Approx(1));

		Toucan::RowVector3<TestType> v3 = Toucan::RowVector3<TestType>::UnitY();
		REQUIRE(v3.x() == Approx(0));
		REQUIRE(v3.y() == Approx(1));
		REQUIRE(v3.z() == Approx(0));
	}

	SECTION("UnitZ") {
		Toucan::Vector3<TestType> v3 = Toucan::Vector3<TestType>::UnitZ();
		REQUIRE(v3.x() == Approx(0));
		REQUIRE(v3.y() == Approx(0));
		REQUIRE(v3.z() == Approx(1));
	}

	SECTION("UnitZ RowVector") {
		Toucan::RowVector3<TestType> v3 = Toucan::RowVector3<TestType>::UnitZ();
		REQUIRE(v3.x() == Approx(0));
		REQUIRE(v3.y() == Approx(0));
		REQUIRE(v3.z() == Approx(1));
	}

	SECTION("UnitN") {
		Toucan::Matrix<TestType, 32, 1> v = Toucan::Matrix<TestType, 32, 1>::UnitN(10);
		for (int index = 0; index < 32; ++index) {
			if (index == 10) {
				REQUIRE(v(index) == Approx(1));
			} else {
				REQUIRE(v(index) == Approx(0));
			}
		}
	}

	SECTION("UnitN RowVector") {
		Toucan::Matrix<TestType, 1, 32> v = Toucan::Matrix<TestType, 1, 32>::UnitN(10);
		for (int index = 0; index < 32; ++index) {
			if (index == 10) {
				REQUIRE(v(index) == Approx(1));
			} else {
				REQUIRE(v(index) == Approx(0));
			}
		}
	}
}

TEMPLATE_TEST_CASE("Matrix default constructor", "[matrix][constructor]", float, double, int) {
	Toucan::Vector3<TestType> v3;
	REQUIRE(v3.x() == Approx(0));
	REQUIRE(v3.y() == Approx(0));
	REQUIRE(v3.z() == Approx(0));
}

TEMPLATE_TEST_CASE("Matrix constructors", "[matrix][constructor]", float, double, int) {

	Toucan::Matrix3<TestType> matrix_toucan(
		TestType(1), TestType(2), TestType(3),
		TestType(4), TestType(5), TestType(6),
		TestType(7), TestType(8), TestType(9)
	);
	Eigen::Matrix<TestType, 3, 3> matrix_eigen;
	matrix_eigen << TestType(1), TestType(2), TestType(3),
	                TestType(4), TestType(5), TestType(6),
	                TestType(7), TestType(8), TestType(9);

	REQUIRE((matrix_toucan == matrix_eigen));

	// TODO(Matias): Check that a invalid number of parameters to a Toucan Matrix constructor fails at compile time.
}

TEMPLATE_TEST_CASE("Matrix constructors 2", "[matrix][constructor]", float, double, int) {
	Toucan::Matrix<TestType, 2, 2> m2_zero(
			TestType(0), TestType(0),
			TestType(0), TestType(0)
	);

	Toucan::Matrix<TestType, 2, 2> m2_ones(
			TestType(1), TestType(1),
			TestType(1), TestType(1)
	);

	Toucan::Matrix<TestType, 2, 2> m2_identity(
			TestType(1), TestType(0),
			TestType(0), TestType(1)
	);

	SECTION("Zero") {
		REQUIRE(Toucan::Matrix<TestType, 2, 2>::Zero() == m2_zero);
		REQUIRE_FALSE(Toucan::Matrix<TestType, 2, 2>::Zero() != m2_zero);

		REQUIRE_FALSE(Toucan::Matrix<TestType, 2, 2>::Zero() == m2_ones);
		REQUIRE(Toucan::Matrix<TestType, 2, 2>::Zero() != m2_ones);

		REQUIRE_FALSE(Toucan::Matrix<TestType, 2, 2>::Zero() == m2_identity);
		REQUIRE(Toucan::Matrix<TestType, 2, 2>::Zero() != m2_identity);
	}
	SECTION("Ones") {
		REQUIRE_FALSE(Toucan::Matrix<TestType, 2, 2>::Ones() == m2_zero);
		REQUIRE(Toucan::Matrix<TestType, 2, 2>::Ones() != m2_zero);

		REQUIRE(Toucan::Matrix<TestType, 2, 2>::Ones() == m2_ones);
		REQUIRE_FALSE(Toucan::Matrix<TestType, 2, 2>::Ones() != m2_ones);

		REQUIRE_FALSE(Toucan::Matrix<TestType, 2, 2>::Ones() == m2_identity);
		REQUIRE(Toucan::Matrix<TestType, 2, 2>::Ones() != m2_identity);
	}
	SECTION("Identity") {
		REQUIRE_FALSE(Toucan::Matrix<TestType, 2, 2>::Identity() == m2_zero);
		REQUIRE(Toucan::Matrix<TestType, 2, 2>::Identity() != m2_zero);

		REQUIRE_FALSE(Toucan::Matrix<TestType, 2, 2>::Identity() == m2_ones);
		REQUIRE(Toucan::Matrix<TestType, 2, 2>::Identity() != m2_ones);

		REQUIRE(Toucan::Matrix<TestType, 2, 2>::Identity() == m2_identity);
		REQUIRE_FALSE(Toucan::Matrix<TestType, 2, 2>::Identity() != m2_identity);

	}
}

TEMPLATE_TEST_CASE("Matrix shape", "[matrix][shape]", float, double, int) {
	Toucan::Vector2<TestType> v2;
	STATIC_REQUIRE(v2.number_of_rows() == 2);
	STATIC_REQUIRE(v2.number_of_columns() == 1);
	STATIC_REQUIRE(v2.number_of_elements() == 2);

	Toucan::Vector3<TestType> v3;
	STATIC_REQUIRE(v3.number_of_rows() == 3);
	STATIC_REQUIRE(v3.number_of_columns() == 1);
	STATIC_REQUIRE(v3.number_of_elements() == 3);

	Toucan::Vector4<TestType> v4;
	STATIC_REQUIRE(v4.number_of_rows() == 4);
	STATIC_REQUIRE(v4.number_of_columns() == 1);
	STATIC_REQUIRE(v4.number_of_elements() == 4);

	Toucan::RowVector2<TestType> v2_row;
	STATIC_REQUIRE(v2_row.number_of_rows() == 1);
	STATIC_REQUIRE(v2_row.number_of_columns() == 2);
	STATIC_REQUIRE(v2_row.number_of_elements() == 2);

	Toucan::RowVector3<TestType> v3_row;
	STATIC_REQUIRE(v3_row.number_of_rows() == 1);
	STATIC_REQUIRE(v3_row.number_of_columns() == 3);
	STATIC_REQUIRE(v3_row.number_of_elements() == 3);

	Toucan::RowVector4<TestType> v4_row;
	STATIC_REQUIRE(v4_row.number_of_rows() == 1);
	STATIC_REQUIRE(v4_row.number_of_columns() == 4);
	STATIC_REQUIRE(v4_row.number_of_elements() == 4);

	Toucan::Matrix<TestType, 4, 8> m1;
	STATIC_REQUIRE(m1.number_of_rows() == 4);
	STATIC_REQUIRE(m1.number_of_columns() == 8);
	STATIC_REQUIRE(m1.number_of_elements() == 32);
}

TEMPLATE_TEST_CASE("Matrix norms", "[matrix][norm]", float, double) {
	Toucan::Vector2<TestType> v2_ones = Toucan::Vector2<TestType>::Ones();
	REQUIRE(v2_ones.norm() == Approx(std::sqrt(2)));

	Toucan::Vector3<TestType> v3_ones = Toucan::Vector3<TestType>::Ones();
	REQUIRE(v3_ones.norm() == Approx(std::sqrt(3)));

	Toucan::Matrix<TestType, 2, 2> m2_ones = Toucan::Matrix<TestType, 2, 2>::Ones();
	REQUIRE(m2_ones.norm() == Approx(std::sqrt(4)));

	Toucan::Vector4<TestType> v4(TestType(1), TestType(2), TestType(3), TestType(4));
	const TestType v4_squared_sum = 1+4+9+16;
	const TestType v4_sum = std::sqrt(v4_squared_sum);
	REQUIRE(v4.norm() == Approx(std::sqrt(v4_squared_sum)));

	Toucan::Vector4<TestType> v4_normalized = v4.normalized();
	REQUIRE(v4_normalized(0) == Approx(1.0/v4_sum));
	REQUIRE(v4_normalized(1) == Approx(2.0/v4_sum));
	REQUIRE(v4_normalized(2) == Approx(3.0/v4_sum));
	REQUIRE(v4_normalized(3) == Approx(4.0/v4_sum));

	v4.normalize();
	REQUIRE(v4(0) == Approx(1.0/v4_sum));
	REQUIRE(v4(1) == Approx(2.0/v4_sum));
	REQUIRE(v4(2) == Approx(3.0/v4_sum));
	REQUIRE(v4(3) == Approx(4.0/v4_sum));
}

TEMPLATE_TEST_CASE("Matrix squared norms", "[matrix][norms]", float, double, int) {
	Toucan::Vector2<TestType> v2_ones = Toucan::Vector2<TestType>::Ones();
	REQUIRE(v2_ones.squared_norm() == Approx(2));

	Toucan::Vector3<TestType> v3_ones = Toucan::Vector3<TestType>::Ones();
	REQUIRE(v3_ones.squared_norm() == Approx(3));

	Toucan::Matrix<TestType, 2, 2> m2_ones = Toucan::Matrix<TestType, 2, 2>::Ones();
	REQUIRE(m2_ones.squared_norm() == Approx(4));

	Toucan::Vector4<TestType> v4(TestType(1), TestType(2), TestType(3), TestType(4));
	const TestType v4_squared_sum = 1+4+9+16;
	REQUIRE(v4.squared_norm() == Approx(v4_squared_sum));
}

TEMPLATE_TEST_CASE("Matrix math functions", "[matrix][math]", float, double, int) {

	SECTION("Trace") {
		Toucan::Matrix<TestType, 4, 4> m4(
				TestType(1), TestType(2), TestType(3), TestType(4),
				TestType(5), TestType(6), TestType(7), TestType(8),
				TestType(9), TestType(10), TestType(11), TestType(12),
				TestType(13), TestType(14), TestType(15), TestType(16)
		);

		REQUIRE(m4.trace() == Approx(1+6+11+16));
	}

	Toucan::Vector3<TestType> v1(TestType(1), TestType(2), TestType(3));
	Toucan::Vector3<TestType> v2(TestType(4), TestType(5), TestType(6));

	SECTION("Dot product") {
		TestType dot_product = v1.dot_product(v2);
		REQUIRE(dot_product == Approx(32));
	}

	SECTION("Cross product") {
		Toucan::Vector3<TestType> cross_product = v1.cross_product(v2);
		REQUIRE(cross_product.x() == Approx(-3));
		REQUIRE(cross_product.y() == Approx(6));
		REQUIRE(cross_product.z() == Approx(-3));
	}
}

TEMPLATE_TEST_CASE("Vector accessors", "[matrix][accessor]", float, double, int) {
	Toucan::Vector2<TestType> v2;
	v2.x() = TestType(1);
	v2.y() = TestType(2);

	REQUIRE(v2.x() == TestType(1));
	REQUIRE(v2.y() == TestType(2));

	Toucan::Vector3<TestType> v3;
	v3.x() = TestType(1);
	v3.y() = TestType(2);
	v3.z() = TestType(3);

	REQUIRE(v3.x() == TestType(1));
	REQUIRE(v3.y() == TestType(2));
	REQUIRE(v3.z() == TestType(3));
}

TEMPLATE_TEST_CASE("Vector const accessors", "[matrix][accessor]", float, double, int) {
	const Toucan::Vector2<TestType> v2(TestType(1), TestType(2));

	REQUIRE(v2.x() == TestType(1));
	REQUIRE(v2.y() == TestType(2));

	const Toucan::Vector3<TestType> v3(TestType(1), TestType(2), TestType(3));

	REQUIRE(v3.x() == TestType(1));
	REQUIRE(v3.y() == TestType(2));
	REQUIRE(v3.z() == TestType(3));
}

TEMPLATE_TEST_CASE("Row Vector accessors", "[matrix][accessor]", float, double, int) {
	Toucan::RowVector2<TestType> v2;
	v2.x() = TestType(1);
	v2.y() = TestType(2);

	REQUIRE(v2.x() == TestType(1));
	REQUIRE(v2.y() == TestType(2));

	Toucan::RowVector3<TestType> v3;
	v3.x() = TestType(1);
	v3.y() = TestType(2);
	v3.z() = TestType(3);

	REQUIRE(v3.x() == TestType(1));
	REQUIRE(v3.y() == TestType(2));
	REQUIRE(v3.z() == TestType(3));
}

TEMPLATE_TEST_CASE("Row Vector const accessors", "[matrix][accessor]", float, double, int) {
	const Toucan::RowVector2<TestType> v2(TestType(1), TestType(2));

	REQUIRE(v2.x() == TestType(1));
	REQUIRE(v2.y() == TestType(2));

	const Toucan::RowVector3<TestType> v3(TestType(1), TestType(2), TestType(3));

	REQUIRE(v3.x() == TestType(1));
	REQUIRE(v3.y() == TestType(2));
	REQUIRE(v3.z() == TestType(3));
}

TEMPLATE_TEST_CASE("Rotation matrices", "[matrix]", float, double) {
	constexpr TestType angle_1 = TestType(7)*M_PI/TestType(3);
	constexpr TestType angle_2 = TestType(5)*M_PI/TestType(3);
	constexpr TestType angle_3 = TestType(4)*M_PI/TestType(3);

	const Eigen::Matrix<TestType, 3, 3> r1_eigen =
			(Eigen::AngleAxis<TestType>(angle_1, EigenVector3<TestType>::UnitX()) *
			 Eigen::AngleAxis<TestType>(angle_2, EigenVector3<TestType>::UnitY()) *
			 Eigen::AngleAxis<TestType>(angle_3, EigenVector3<TestType>::UnitZ())).toRotationMatrix();

	const EigenVector3<TestType> v1_eigen(TestType(1), TestType(2), TestType(3));
	const EigenVector3<TestType> v1_eigen_transformed = r1_eigen * v1_eigen;

	const Toucan::Matrix3<TestType> r1_toucan =
			Toucan::create_3d_rotation_matrix_x(angle_1) *
			Toucan::create_3d_rotation_matrix_y(angle_2) *
			Toucan::create_3d_rotation_matrix_z(angle_3);

	const Toucan::Vector3<TestType> v1_toucan(TestType(1), TestType(2), TestType(3));
	const Toucan::Vector3<TestType> v1_toucan_transformed = r1_toucan * v1_toucan;

	REQUIRE(v1_toucan_transformed.x() == Approx(v1_eigen_transformed.x()));
	REQUIRE(v1_toucan_transformed.y() == Approx(v1_eigen_transformed.y()));
	REQUIRE(v1_toucan_transformed.z() == Approx(v1_eigen_transformed.z()));
}

TEMPLATE_TEST_CASE("Diagonal Matrix constructor", "[matrix]", float, double, int) {
	const Toucan::DiagonalMatrix3<TestType> d1;
	REQUIRE(d1(0) == Approx(0));
	REQUIRE(d1(1) == Approx(0));
	REQUIRE(d1(2) == Approx(0));

	const Toucan::DiagonalMatrix3<TestType> d2(TestType(1), TestType(2), TestType(3));
	REQUIRE(d2(0) == Approx(1));
	REQUIRE(d2(1) == Approx(2));
	REQUIRE(d2(2) == Approx(3));
}

TEMPLATE_TEST_CASE("Diagonal Matrix shape functions", "[matrix]", float, double, int) {
	const Toucan::DiagonalMatrix2<TestType> d2;
	REQUIRE(d2.number_of_rows() == 2);
	REQUIRE(d2.number_of_columns() == 2);
	REQUIRE(d2.number_of_elements() == 2*2);

	const Toucan::DiagonalMatrix3<TestType> d3;
	REQUIRE(d3.number_of_rows() == 3);
	REQUIRE(d3.number_of_columns() == 3);
	REQUIRE(d3.number_of_elements() == 3*3);

	const Toucan::DiagonalMatrix4<TestType> d4;
	REQUIRE(d4.number_of_rows() == 4);
	REQUIRE(d4.number_of_columns() == 4);
	REQUIRE(d4.number_of_elements() == 4*4);
}

TEMPLATE_TEST_CASE("Diagonal Matrix get diagonal functions", "[matrix]", float, double, int) {
	//const Toucan::DiagonalMatrix2<TestType> d2(TestType(1), TestType(2));
}

TEMPLATE_TEST_CASE("Quaternion default constructor", "[quaternion][constructor]", float, double) {
	const Toucan::Quaternion<TestType> q;
	REQUIRE(q.w == Approx(1));
	REQUIRE(q.x == Approx(0));
	REQUIRE(q.y == Approx(0));
	REQUIRE(q.z == Approx(0));
}

TEMPLATE_TEST_CASE("Quaternion 4 scalar constructor", "[quaternion][constructor]", float, double) {
	const Toucan::Quaternion<TestType> q(TestType(1), TestType(2), TestType(3), TestType(4));
	REQUIRE(q.w == Approx(1));
	REQUIRE(q.x == Approx(2));
	REQUIRE(q.y == Approx(3));
	REQUIRE(q.z == Approx(4));
}

TEMPLATE_TEST_CASE("Quaternion parameter vector constructor", "[quaternion][constructor]", float, double) {
	const Toucan::Vector4<TestType> v4(TestType(1), TestType(2), TestType(3), TestType(4));
	const Toucan::Quaternion<TestType> q(v4);
	REQUIRE(q.w == Approx(1));
	REQUIRE(q.x == Approx(2));
	REQUIRE(q.y == Approx(3));
	REQUIRE(q.z == Approx(4));
}

TEMPLATE_TEST_CASE("Quaternion axis angle constructor", "[quaternion][constructor]", float, double) {
	const Toucan::Vector3<TestType> axis1(TestType(0.506979), TestType(0.253490), TestType(0.823842));
	const TestType angle1 = 1.845;

	const Toucan::Quaternion<TestType> q1(axis1, angle1);
	REQUIRE(q1.w == Approx(0.6038293));
	REQUIRE(q1.x == Approx(0.4041198));
	REQUIRE(q1.y == Approx(0.2020603));
	REQUIRE(q1.z == Approx(0.6566956));

	const Toucan::Vector3<TestType> axis2(TestType(0.857921), TestType(-0.509391), TestType(-0.067025));
	const TestType angle2 = -2.271;

	const Toucan::Quaternion<TestType> q2(axis2, angle2);
	REQUIRE(q2.w == Approx(0.4216791));
	REQUIRE(q2.x == Approx(-0.7779157));
	REQUIRE(q2.y == Approx(0.4618878));
	REQUIRE(q2.z == Approx(0.0607746));
}

TEMPLATE_TEST_CASE("Quaternion rotation matrix constructor", "[quaternion][constructor]", float, double) {
	const Toucan::Matrix3<TestType> rotation_matrix1(
			TestType(0.0558452), TestType(-0.6297508), TestType(0.7747872),
			TestType(0.9563771), TestType(-0.1891237), TestType(-0.2226545),
			TestType(0.2867475), TestType(0.7534229), TestType(0.5917177)
	);

	const Toucan::Quaternion<TestType> q1(rotation_matrix1);
	REQUIRE(q1.w == Approx(0.6038293));
	REQUIRE(q1.x == Approx(0.4041198));
	REQUIRE(q1.y == Approx(0.2020603));
	REQUIRE(q1.z == Approx(0.6566956));

	const Toucan::Matrix3<TestType> rotation_matrix2(
			TestType(0.5659322), TestType(-0.7698743), TestType(0.2949819),
			TestType(-0.6673648), TestType(-0.2176927), TestType(0.7122037),
			TestType(-0.4840919), TestType(-0.5999195), TestType(-0.6369864)
	);

	const Toucan::Quaternion<TestType> q2(rotation_matrix2);
	REQUIRE(q2.w == Approx(0.4216791));
	REQUIRE(q2.x == Approx(-0.7779157));
	REQUIRE(q2.y == Approx(0.4618878));
	REQUIRE(q2.z == Approx(0.0607746));
}

TEMPLATE_TEST_CASE("Quaternion operators", "[quaternion][operator]", float, double) {
	const Toucan::Quaternion<TestType> q1(0.6038293, 0.4041198, 0.2020603, 0.6566956);
	const Toucan::Quaternion<TestType> q2(0.4216791, -0.7779157, 0.4618878, 0.0607746);

	constexpr TestType eps = 1e-4;

	SECTION("Quaternion-Quaternion composition") {
		const Toucan::Quaternion<TestType> q_1_2 = q1*q2;
		REQUIRE(q_1_2.w == Approx(0.435754).epsilon(eps));
		REQUIRE(q_1_2.x == Approx(-0.590359).epsilon(eps));
		REQUIRE(q_1_2.y == Approx(-0.17130).epsilon(eps));
		REQUIRE(q_1_2.z == Approx(0.657456).epsilon(eps));

		const Toucan::Quaternion<TestType> q_2_1 = q2*q1;
		REQUIRE(q_2_1.w == Approx(0.435754).epsilon(eps));
		REQUIRE(q_2_1.x == Approx(-0.00827987).epsilon(eps));
		REQUIRE(q_2_1.y == Approx(0.89952).epsilon(eps));
		REQUIRE(q_2_1.z == Approx(-0.0302316).epsilon(eps));
	}

	SECTION("Quaternion-Point composition") {
		const Eigen::Quaternion<TestType> q1_eig(q2.w, q2.x, q2.y, q2.z);

		const Toucan::Vector3<TestType> p(TestType(8.91), TestType(-42.8), TestType(0.25));

		const Toucan::Vector3<TestType> p1 = q1 * p;
		REQUIRE(p1.x() == Approx(27.6446).epsilon(eps));
		REQUIRE(p1.y() == Approx(16.5602).epsilon(eps));
		REQUIRE(p1.z() == Approx(-29.5437).epsilon(eps));

		const Toucan::Vector3<TestType> p2 = q2 * p;
		REQUIRE(p2.x() == Approx(38.0668).epsilon(eps));
		REQUIRE(p2.y() == Approx(3.54908).epsilon(eps));
		REQUIRE(p2.z() == Approx(21.204).epsilon(eps));
	}
}

TEMPLATE_TEST_CASE("Scaled Transform 2D inverse", "", float, double) {
	Toucan::ScaledTransform2D<TestType> t(
			TestType(0.123),
			Toucan::Vector2<TestType>(TestType(1.0), TestType(2.0)),
			Toucan::Vector2<TestType>(TestType(0.5), TestType(2.0))
	);

	SECTION("3x3 Transformation matrix") {
		const Toucan::Matrix3<TestType> t_matrix = t.transformation_matrix();
		const Toucan::Matrix3<TestType> t_matrix_inverse = t.transformation_matrix_inverse();
		const Toucan::Matrix3<TestType> identity_1 = t_matrix * t_matrix_inverse;
		const Toucan::Matrix3<TestType> identity_2 = t_matrix_inverse * t_matrix;

		REQUIRE(identity_1(0, 0) == Approx(1));
		REQUIRE(identity_1(0, 1) == Approx(0));
		REQUIRE(identity_1(0, 2) == Approx(0));
		REQUIRE(identity_1(1, 0) == Approx(0));
		REQUIRE(identity_1(1, 1) == Approx(1));
		REQUIRE(identity_1(1, 2) == Approx(0));
		REQUIRE(identity_1(2, 0) == Approx(0));
		REQUIRE(identity_1(2, 1) == Approx(0));
		REQUIRE(identity_1(2, 2) == Approx(1));

		REQUIRE(identity_2(0, 0) == Approx(1));
		REQUIRE(identity_2(0, 1) == Approx(0));
		REQUIRE(identity_2(0, 2) == Approx(0));
		REQUIRE(identity_2(1, 0) == Approx(0));
		REQUIRE(identity_2(1, 1) == Approx(1));
		REQUIRE(identity_2(1, 2) == Approx(0));
		REQUIRE(identity_2(2, 0) == Approx(0));
		REQUIRE(identity_2(2, 1) == Approx(0));
		REQUIRE(identity_2(2, 2) == Approx(1));
	}

	SECTION("4x4 Transformation matrix") {
		const Toucan::Matrix4<TestType> t_matrix = t.transformation_matrix_3d();
		const Toucan::Matrix4<TestType> t_matrix_inverse = t.transformation_matrix_inverse_3d();
		const Toucan::Matrix4<TestType> identity_1 = t_matrix * t_matrix_inverse;
		const Toucan::Matrix4<TestType> identity_2 = t_matrix_inverse * t_matrix;

		REQUIRE(identity_1(0, 0) == Approx(1));
		REQUIRE(identity_1(0, 1) == Approx(0));
		REQUIRE(identity_1(0, 2) == Approx(0));
		REQUIRE(identity_1(0, 3) == Approx(0));
		REQUIRE(identity_1(1, 0) == Approx(0));
		REQUIRE(identity_1(1, 1) == Approx(1));
		REQUIRE(identity_1(1, 2) == Approx(0));
		REQUIRE(identity_1(1, 3) == Approx(0));
		REQUIRE(identity_1(2, 0) == Approx(0));
		REQUIRE(identity_1(2, 1) == Approx(0));
		REQUIRE(identity_1(2, 2) == Approx(1));
		REQUIRE(identity_1(2, 3) == Approx(0));
		REQUIRE(identity_1(3, 0) == Approx(0));
		REQUIRE(identity_1(3, 1) == Approx(0));
		REQUIRE(identity_1(3, 2) == Approx(0));
		REQUIRE(identity_1(3, 3) == Approx(1));

		REQUIRE(identity_2(0, 0) == Approx(1));
		REQUIRE(identity_2(0, 1) == Approx(0));
		REQUIRE(identity_2(0, 2) == Approx(0));
		REQUIRE(identity_2(0, 3) == Approx(0));
		REQUIRE(identity_2(1, 0) == Approx(0));
		REQUIRE(identity_2(1, 1) == Approx(1));
		REQUIRE(identity_2(1, 2) == Approx(0));
		REQUIRE(identity_2(1, 3) == Approx(0));
		REQUIRE(identity_2(2, 0) == Approx(0));
		REQUIRE(identity_2(2, 1) == Approx(0));
		REQUIRE(identity_2(2, 2) == Approx(1));
		REQUIRE(identity_2(2, 3) == Approx(0));
		REQUIRE(identity_2(3, 0) == Approx(0));
		REQUIRE(identity_2(3, 1) == Approx(0));
		REQUIRE(identity_2(3, 2) == Approx(0));
		REQUIRE(identity_2(3, 3) == Approx(1));
	}

}
TEMPLATE_TEST_CASE("Scaled Transform 3D inverse", "", float, double) {
	Toucan::ScaledTransform3D<TestType> t(
			Toucan::Quaternion<TestType>(Toucan::Vector3<TestType>(TestType(1), TestType(2), TestType(3)).normalized(), TestType(2*M_PI/3)),
			Toucan::Vector3<TestType>(TestType(1.0), TestType(2.0), TestType(3.0)),
			Toucan::Vector3<TestType>(TestType(0.5), TestType(2.0), TestType(3.25))
	);

	const Toucan::Matrix4<TestType> t_matrix = t.transformation_matrix();
	const Toucan::Matrix4<TestType> t_matrix_inverse = t.transformation_matrix_inverse();
	const Toucan::Matrix4<TestType> identity_1 = t_matrix * t_matrix_inverse;
	const Toucan::Matrix4<TestType> identity_2 = t_matrix_inverse * t_matrix;

	REQUIRE(identity_1(0, 0) == Approx(1));
	REQUIRE(identity_1(0, 1) == Approx(0));
	REQUIRE(identity_1(0, 2) == Approx(0));
	REQUIRE(identity_1(0, 3) == Approx(0));
	REQUIRE(identity_1(1, 0) == Approx(0));
	REQUIRE(identity_1(1, 1) == Approx(1));
	REQUIRE(identity_1(1, 2) == Approx(0));
	REQUIRE(identity_1(1, 3) == Approx(0));
	REQUIRE(identity_1(2, 0) == Approx(0));
	REQUIRE(identity_1(2, 1) == Approx(0));
	REQUIRE(identity_1(2, 2) == Approx(1));
	REQUIRE(identity_1(2, 3) == Approx(0));
	REQUIRE(identity_1(3, 0) == Approx(0));
	REQUIRE(identity_1(3, 1) == Approx(0));
	REQUIRE(identity_1(3, 2) == Approx(0));
	REQUIRE(identity_1(3, 3) == Approx(1));

	REQUIRE(identity_2(0, 0) == Approx(1));
	REQUIRE(identity_2(0, 1) == Approx(0));
	REQUIRE(identity_2(0, 2) == Approx(0));
	REQUIRE(identity_2(0, 3) == Approx(0));
	REQUIRE(identity_2(1, 0) == Approx(0));
	REQUIRE(identity_2(1, 1) == Approx(1));
	REQUIRE(identity_2(1, 2) == Approx(0));
	REQUIRE(identity_2(1, 3) == Approx(0));
	REQUIRE(identity_2(2, 0) == Approx(0));
	REQUIRE(identity_2(2, 1) == Approx(0));
	REQUIRE(identity_2(2, 2) == Approx(1));
	REQUIRE(identity_2(2, 3) == Approx(0));
	REQUIRE(identity_2(3, 0) == Approx(0));
	REQUIRE(identity_2(3, 1) == Approx(0));
	REQUIRE(identity_2(3, 2) == Approx(0));
	REQUIRE(identity_2(3, 3) == Approx(1));

}


#ifndef __GLM_LINK_H__
#define __GLM_LINK_H__

#include <armadillo>

class glm_link
{
public:
    /**
     * Constructor.
     *
     * @param name The name of link function.
     */
    glm_link(const std::string &name)
        : m_name( name )
    {
    }

    /**
     * Destructor.
     */
    virtual ~glm_link(){ };

    /**
     * Returns the name of this link function.
     *
     * @return the name of this link function.
     */
    std::string get_name() const
    {
        return m_name;
    }

    /**
     * Compute eta from the mean value parameter (the link function).
     *
     * @param mu The mean value parameter.
     *
     * @return The linearized parameter eta.
     */
    virtual arma::vec eta(const arma::vec &mu) const = 0;

    /**
     * The derivative of mu with respect to eta. It is often good to
     * use the relationship dmu/deta = (deta/dmu)^-1.
     *
     * @param mu The mean value parameter.
     *
     * @return The derivative of mu with respect to eta.
     */
    virtual arma::vec mu_eta(const arma::vec &mu) const = 0;

    /**
     * Compute the mean value parameter from the linearized parameter.
     *
     * @param eta The linearized parameter.
     *
     * @return The mean value parameter.
     */
    virtual arma::vec mu(const arma::vec &eta) const = 0;

private:
    std::string m_name;
};

/**
 * Creates a link function from the given name.
 *
 * The following links are avaiable:
 * - "identity" g(mu) = mu
 * - "log" g(mu) = log(mu)
 * - "logc" g(mu) = log(1-mu)
 * - "logit" g(mu) = log(mu/(1-mu))
 * - "odds" g(mu) = mu/(1-mu)
 *
 * @param link_name The name of the link function.
 *
 * @return The link if found, or null otherwise.
 */
glm_link *make_link(const std::string &link_name);

#endif /* End of __GLM_LINK_H__ */


#ifdef MEX
// Force header only version
#ifdef IGL_STATIC_LIBRARY
#undef IGL_STATIC_LIBRARY
#endif
#include <mex.h>

#include <igl/matlab/mexErrMsgTxt.h>
#include <igl/STR.h>
#undef assert
#define assert( isOK ) ( (isOK) ? (void)0 : (void) mexErrMsgTxt(C_STR(__FILE__<<":"<<__LINE__<<": failed assertion `"<<#isOK<<"'"<<std::endl) ) )
#include <igl/matlab/MexStream.h>
#include <igl/matlab/prepare_lhs.h>

#include <igl/xml/serialize_xml.h>
#include <Eigen/Core>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <wordexp.h>
#include <iostream>

// http://www.alecjacobson.com/weblog/?p=4477
typedef CGAL::Epeck::FT EScalar;
typedef Eigen::Matrix<EScalar,Eigen::Dynamic,Eigen::Dynamic> MatrixXE;
namespace igl
{
  namespace xml
  {
    namespace serialization_xml
    {
      template <> inline void serialize(
        const MatrixXE & obj,
        tinyxml2::XMLDocument* doc,
        tinyxml2::XMLElement* element,
        const std::string& name)
      {
        const std::function<std::string(const MatrixXE::Scalar &) > to_string =
          [](const MatrixXE::Scalar & v)->std::string
          {
            return
              STR(CGAL::exact(v));
          };
        serialize(obj,name,to_string,doc,element);
      }
      template <> inline void deserialize(
        MatrixXE & obj,
        const tinyxml2::XMLDocument* doc,
        const tinyxml2::XMLElement* element,
        const std::string& name)
      {
        const std::function<void(const std::string &,MatrixXE::Scalar &)> &
          from_string =
          [](const std::string & s, MatrixXE::Scalar & v)
          {
            std::stringstream(s)>>v;
          };
        deserialize(doc,element,name,from_string,obj);
      }
    }
  }
}

void mexFunction(
  int nlhs, mxArray *plhs[],
  int nrhs, const mxArray *prhs[])
{
  using namespace std;
  using namespace Eigen;
  using namespace igl;
  using namespace igl::matlab;
  igl::matlab::MexStream mout;
  std::streambuf *outbuf = cout.rdbuf(&mout);

  mexErrMsgTxt(nrhs >= 1, "The number of input arguments must be >=1.");
  mexErrMsgTxt(mxIsChar(prhs[0]),"File name should be string");
  string filename;
  {
    wordexp_t exp_result;
    wordexp(mxArrayToString(prhs[0]), &exp_result, 0);
    filename = exp_result.we_wordv[0];
  }

  MatrixXE V;
  MatrixXi F;

  // Read mesh
  igl::xml::deserialize_xml(V,"vertices",filename);
  igl::xml::deserialize_xml(F,"faces",    filename);

  plhs[0] = mxCreateDoubleMatrix(V.rows(),V.cols(), mxREAL);
  double * Vp = mxGetPr(plhs[0]);
  switch(nlhs)
  {
    default:
    {
      mexErrMsgTxt(false,"Too many output parameters.");
    }
    case 2:
    {
      prepare_lhs_index(F,plhs+1);
      // fall through
    }
    case 1:
    {
      const int m = V.rows();
      const int n = V.cols();
      plhs[0] = mxCreateDoubleMatrix(m,n, mxREAL);
      double * Vp = mxGetPr(plhs[0]);
      for(int i = 0;i<m;i++)
      {
        for(int j = 0;j<n;j++)
        {
          Vp[i+m*j] = CGAL::to_double(V(i,j));
        }
      }
      // fall through
    }
    case 0: break;
  }

}
#endif


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate two to the number of mantissa bits.

    @par Header <boost/simd/constant/twotonmb.hpp>

    @par Semantic:

    @code
    T r = Twotonmb<T>();
    @endcode

    is similar to:

    @code
    T r = pow(2, Nbmantissabits<T>());
    @endcode

    @return The Twotonmb constant for the proper type
  **/
  template<typename T> T Twotonmb();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant twotonmb.

      @return The Twotonmb constant for the proper type
    **/
    Value Twotonmb();
  }
} }
#endif

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

#endif


#pragma once

#include <complex>
#include <mpreal.h>
#include <Eigen/MPRealSupport>
#include <Eigen/CXX11/Tensor>

#include "piecewise_polynomial.hpp"
#include "detail/gauss_legendre.hpp"
#include "detail/legendre_polynomials.hpp"

namespace irlib {
    inline bool python_runtime_check(bool b, const std::string& message) {
#ifdef SWIGPYTHON
        if (!b) {
            throw std::runtime_error(message);
        }
#endif
        return b;
    }

    namespace statistics {
        enum statistics_type {
            BOSONIC = 0,
            FERMIONIC = 1
        };
    }

    using std::abs;
    using std::sqrt;
    using std::pow;
    using std::cosh;
    using std::sinh;
    using std::asin;
    using std::acos;
    using std::atan;

    using mpfr::mpreal;
    using mpfr::abs;
    using mpfr::sqrt;
    using mpfr::pow;
    using mpfr::cosh;
    using mpfr::sinh;
    using mpfr::asin;
    using mpfr::acos;
    using mpfr::atan;

    //using boost::multiprecision::float128;

    namespace detail {
        template<typename S> inline S sqrt(const S& s);
        template<> inline double sqrt<double>(const double& s) {return std::sqrt(s);};
        template<> inline long double sqrt<long double>(const long double& s) {return std::sqrt(s);};
        template<> inline mpfr::mpreal sqrt<mpreal>(const mpreal& s) {return mpfr::sqrt(s);};

        template<typename S> inline S pow(const S& s, const S& p);
        template<> inline double pow<double>(const double& s, const double& p) {return std::pow(s, p);};
        template<> inline long double pow<long double>(const long double& s, const long double& p) {return std::pow(s, p);};
        template<> inline mpfr::mpreal pow<mpreal>(const mpreal& s, const mpreal& p) {return mpfr::pow(s, p);};
    }

    using pp_type = piecewise_polynomial<mpreal,mpreal>;

    using MatrixXmp = Eigen::Matrix<mpfr::mpreal,Eigen::Dynamic,Eigen::Dynamic>;
    using MatrixXc = Eigen::Matrix<std::complex<double>,Eigen::Dynamic,Eigen::Dynamic>;

    template<typename ScalarType>
    inline void ir_set_default_prec(mp_prec_t prec);

    template<typename ScalarType>
    inline mp_prec_t ir_get_default_prec();

    template<>
    inline
    void
    ir_set_default_prec<mpfr::mpreal>(mp_prec_t prec) {
        mpfr::mpreal::set_default_prec(prec);
    }

    template<>
    inline void ir_set_default_prec<double>(mp_prec_t prec) {}

    template<>
    inline void ir_set_default_prec<long double>(mp_prec_t prec) {}

    template<>
    inline mp_prec_t ir_get_default_prec<mpfr::mpreal>() {
        return mpfr::mpreal::get_default_prec();
    }

    template<>
    inline mp_prec_t ir_get_default_prec<double>() {
        return 15;
    }

    template<>
    inline mp_prec_t ir_get_default_prec<long double>() {
        return 20;//extended double?
    }

    inline mp_prec_t ir_digits2bits(mp_prec_t prec) {
        return mpfr::digits2bits(prec);
    }

    inline std::complex<double>
    to_dcomplex(const std::complex<mpreal>& mp) {
        return std::complex<double>(
                static_cast<double>(mp.real()), static_cast<double>(mp.imag())
        );
    }

}


/*
 * Taken from http://www.andrew.cmu.edu/user/vanhoeve/mdd/ with the notice:
 * "The software can be freely used but comes with no warranty"
 * --------------------------------------------------------
 * Ordering class
 * --------------------------------------------------------
 */

#ifndef ORDERING_HPP_
#define ORDERING_HPP_

#include <cassert>
#include <cstdio>
#include "instance.hpp"
#include "bdd.hpp"
#include <vector>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

struct IntComparator {
	vector<int> &v;
	IntComparator(vector<int> &_v) : v(_v) { }
	bool operator()(int i, int j) {
		return (v[i] > v[j]);
	}
};

using namespace std;

enum OrderType {
  MinState, RandMinState, MaximalPath, Random, CutVertexGen, CutVertex, Fixed,
  MinDegree
};

// Class representing a general ordering
struct IS_Ordering {

	IndepSetInst   *inst;
	char           name[256];
	OrderType	   order_type;

	IS_Ordering(IndepSetInst* _inst, OrderType _order_type) : inst(_inst), order_type(_order_type) { }

	// returns vertex corresponding to particular layer
	virtual int vertex_in_layer(BDD* bdd, int layer) = 0;
};

// Vertex that it is in the least number of states
struct MinInState : IS_Ordering {

	vector<bool> avail_v;   // available vertices

	MinInState(IndepSetInst *_inst) : IS_Ordering(_inst, MinState) {
		avail_v.resize(inst->graph->n_vertices, true);
		sprintf(name, "min_in_state");
	}

	int vertex_in_layer(BDD* bdd, int layer);
};

// Vertex that it is in the least number of states - randomized!!
struct RandomizedMinInState : IS_Ordering {

	vector<bool> avail_v;   // available vertices
	double prob;			// probability

	boost::random::mt19937 gen;

	RandomizedMinInState(IndepSetInst *_inst, double _prob) : IS_Ordering(_inst, RandMinState), prob(_prob) {
		avail_v.resize(inst->graph->n_vertices, true);
		sprintf(name, "rand_min");

		gen.seed(inst->graph->n_vertices + inst->graph->n_edges);
	}

	int vertex_in_layer(BDD* bdd, int layer);
};

// Maximal Path Decomposition
struct MaximalPathDecomp : IS_Ordering {

	vector<int> v_in_layer;   // vertex at each layer

	MaximalPathDecomp(IndepSetInst *_inst) : IS_Ordering(_inst, MaximalPath) {
		sprintf(name, "maxpath");
		construct_ordering();
	}

	int vertex_in_layer(BDD* bdd, int layer) {
		assert( layer >= 0 && layer < inst->graph->n_vertices);
		return v_in_layer[layer];
	}

private:
	void construct_ordering();
};

// Minimum degree ordering
struct MinDegreeOrdering : IS_Ordering {

  vector<int> v_in_layer;   // vertex at each layer

  MinDegreeOrdering(IndepSetInst *_inst) : IS_Ordering(_inst, MinDegree) {
    sprintf(name, "mindegree");
    construct_ordering();
  }

  int vertex_in_layer(BDD* bdd, int layer) {
    assert( layer >= 0 && layer < inst->graph->n_vertices);
    return v_in_layer[layer];
  }

private:
  void construct_ordering();
};

// Random ordering
struct RandomOrdering : IS_Ordering {

	vector<int> v_in_layer;   // vertex at each layer
	RandomOrdering(IndepSetInst *_inst) : IS_Ordering(_inst, Random) {
		sprintf(name, "random");
		v_in_layer.resize(inst->graph->n_vertices);
		construct_ordering();
	}

	int vertex_in_layer(BDD* bdd, int layer) {
		assert( layer >= 0 && layer < inst->graph->n_vertices);
		return v_in_layer[layer];
	}

private:
	void construct_ordering();
};

// FixedOrdering: read from a file
struct FixedOrdering : IS_Ordering {

	vector<int> v_in_layer;   // vertex at each layer
	FixedOrdering(IndepSetInst *_inst, char* filename) : IS_Ordering(_inst, Fixed) {

		sprintf(name, "fixed");
		v_in_layer.resize(inst->graph->n_vertices);
		read_ordering(filename);
	}

	int vertex_in_layer(BDD* bdd, int layer) {
		assert( layer >= 0 && layer < inst->graph->n_vertices);
		return v_in_layer[layer];
	}

private:
	void read_ordering(char* filename);
};

// FixedOrdering: read from a file
struct FixedOrderingFromSeq : IS_Ordering {

	vector<int> v_in_layer;   // vertex at each layer
	FixedOrderingFromSeq(IndepSetInst *_inst, vector<int> action_list) : IS_Ordering(_inst, Fixed) {

		sprintf(name, "fixed");
		v_in_layer.resize(inst->graph->n_vertices);

		int i = 0;
		for(auto& a : action_list) {
			v_in_layer[i] = inst->node_mapping.at(a);
			i++;
		}
	}

	int vertex_in_layer(BDD* bdd, int layer) {
		assert( layer >= 0 && layer < inst->graph->n_vertices);
		return v_in_layer[layer];
	}

};

// FixedOrdering: read from a file
struct OnlineOrdering : IS_Ordering {

	vector<int> v_in_layer;   // vertex at each layer
	OnlineOrdering(IndepSetInst *_inst) : IS_Ordering(_inst, Fixed) {
		sprintf(name, "online");
		v_in_layer.resize(inst->graph->n_vertices);

	}

	int vertex_in_layer(BDD* bdd, int layer) {
		assert( layer >= 0 && layer < inst->graph->n_vertices);
		return v_in_layer[layer];
	}

};

// Cut vertex decomposition
struct CutVertexDecompositionGeneralGraph : IS_Ordering {

	vector<int> v_in_layer;      // vertex at each layer
	bool** original_adj_matrix;

	CutVertexDecompositionGeneralGraph(IndepSetInst *_inst) : IS_Ordering(_inst, CutVertexGen) {
		sprintf(name, "cut-vertex-gen");
		v_in_layer.resize(inst->graph->n_vertices);

		restrict_graph();
		construct_ordering();
		regenerate_graph();
	}

	int vertex_in_layer(BDD* bdd, int layer) {
		assert( layer >= 0 && layer < inst->graph->n_vertices);
		return v_in_layer[layer];
	}

private:
	void        restrict_graph();
	void        regenerate_graph();
	void        construct_ordering();
	void        identify_components(vector< vector<int> > &comps, vector<bool> &is_in_graph);
	vector<int> find_ordering(vector<bool> is_in_graph);
};

// Cut vertex decomposition
struct CutVertexDecomposition : IS_Ordering {

	vector<int> v_in_layer;   // vertex at each layer

	CutVertexDecomposition(IndepSetInst *_inst) : IS_Ordering(_inst, CutVertex) {
		sprintf(name, "cut-vertex");
		v_in_layer.resize(inst->graph->n_vertices);
		construct_ordering();
	}

	int vertex_in_layer(BDD* bdd, int layer) {
		assert( layer >= 0 && layer < inst->graph->n_vertices);
		return v_in_layer[layer];
	}

private:
	void        construct_ordering();
	void        identify_components(vector< vector<int> > &comps, vector<bool> &is_in_graph);
	vector<int> find_ordering(vector<bool> is_in_graph);
};

#endif