/*
#include "gameoflifemodel.h"
#include <QFile>
#include <QTextStream>
#include <QRect>

GameOfLifeModel::GameOfLifeModel(QObject *parent)
    : QAbstractTableModel(parent)
{
    clear();
}

//! [modelsize]
int GameOfLifeModel::rowCount(const QModelIndex &parent) const
{
    if (parent.isValid())
        return 0;

    return height;
}

int GameOfLifeModel::columnCount(const QModelIndex &parent) const
{
    if (parent.isValid())
        return 0;

    return width;
}
//! [modelsize]

//! [read]
QVariant GameOfLifeModel::data(const QModelIndex &index, int role) const
{
    if (!index.isValid() || role != CellRole)
        return QVariant();

    return QVariant(m_currentState[cellIndex({index.column(), index.row()})]);
}
//! [read]

//! [write]
bool GameOfLifeModel::setData(const QModelIndex &index, const QVariant &value, int role)
{
    if (role != CellRole || data(index, role) == value)
        return false;

    m_currentState[cellIndex({index.column(), index.row()})] = value.toBool();
    emit dataChanged(index, index, {role});

    return true;
}
//! [write]

Qt::ItemFlags GameOfLifeModel::flags(const QModelIndex &index) const
{
    if (!index.isValid())
        return Qt::NoItemFlags;

    return Qt::ItemIsEditable;
}

//! [update]
void GameOfLifeModel::nextStep()
{
    StateContainer newValues;

    for (std::size_t i = 0; i < size; ++i) {
        bool currentState = m_currentState[i];

        int cellNeighborsCount = this->cellNeighborsCount(cellCoordinatesFromIndex(static_cast<int>(i)));

        newValues[i] = currentState == true
                ? cellNeighborsCount == 2 || cellNeighborsCount == 3
                : cellNeighborsCount == 3;
    }

    m_currentState = std::move(newValues);

    emit dataChanged(index(0, 0), index(height - 1, width - 1), {CellRole});
}
//! [update]

//! [loader]
bool GameOfLifeModel::loadFile(const QString &fileName)
{
    QFile file(fileName);
    if (!file.open(QIODevice::ReadOnly))
        return false;

    QTextStream in(&file);
    loadPattern(in.readAll());

    return true;
}

void GameOfLifeModel::loadPattern(const QString &plainText)
{
    clear();

    QStringList rows = plainText.split("\n");
    QSize patternSize(0, rows.count());
    for (QString row : rows) {
        if (row.size() > patternSize.width())
            patternSize.setWidth(row.size());
    }

    QPoint patternLocation((width - patternSize.width()) / 2, (height - patternSize.height()) / 2);

    for (int y = 0; y < patternSize.height(); ++y) {
        const QString line = rows[y];

        for (int x = 0; x < line.length(); ++x) {
            QPoint cellPosition(x + patternLocation.x(), y + patternLocation.y());
            m_currentState[cellIndex(cellPosition)] = line[x] == 'O';
        }
    }

    emit dataChanged(index(0, 0), index(height - 1, width - 1), {CellRole});
}
//! [loader]

void GameOfLifeModel::clear()
{
    m_currentState.fill(false);
    emit dataChanged(index(0, 0), index(height - 1, width - 1), {CellRole});
}

int GameOfLifeModel::cellNeighborsCount(const QPoint &cellCoordinates) const
{
    int count = 0;

    for (int x = -1; x <= 1; ++x) {
        for (int y = -1; y <= 1; ++y) {
            if (x == 0 && y == 0)
                continue;

            const QPoint neighborPosition { cellCoordinates.x() + x, cellCoordinates.y() + y };
            if (!areCellCoordinatesValid(neighborPosition))
                continue;

            if (m_currentState[cellIndex(neighborPosition)])
                ++count;

            if (count > 3)
                return count;
        }
    }

    return count;
}

bool GameOfLifeModel::areCellCoordinatesValid(const QPoint &coordinates)
{
    return QRect(0, 0, width, height).contains(coordinates);
}

QPoint GameOfLifeModel::cellCoordinatesFromIndex(int cellIndex)
{
    return {cellIndex % width, cellIndex / width};
}

std::size_t GameOfLifeModel::cellIndex(const QPoint &coordinates)
{
    return std::size_t(coordinates.y() * width + coordinates.x());
}
*/

#include "GameOfLifeModel.hpp"
#include <sstream>
#include <thread>
#include <algorithm>
#include <sstream>
#include <QFile>
#include <QUrl>
#include <QDir>
#include <iostream>
#include <fstream>
#include <regex>
#include <boost/lexical_cast.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include <boost/phoenix/bind/bind_function.hpp>
#include <boost/config/warning_disable.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/optional/optional_io.hpp>

GameOfLifeModel::GameOfLifeModel(const Map &map, QObject *parent)
    : QAbstractTableModel(parent), height(map.size()), width(map[0].size()), iteration(0)
{
    this->mMap = std::make_unique<Map>(this->height);
    this->mTmp_map = std::make_unique<Map>(this->height);

    for (int i = 0; i < height; i++) {
        (*this->mMap)[i] = std::vector<uint8_t>(width);
        (*this->mTmp_map)[i] = std::vector<uint8_t>(width);
    }

    for (int y = 0; y < height; y++) {
        for (int x = 0; x < width; x++) {
            if (map[y][x] == 1)
                setCell(x, y);
        }
    }
}

GameOfLifeModel::GameOfLifeModel(int height, int width, QObject *parent)
    : QAbstractTableModel(parent), height(height), width(width), iteration(0)
{
    this->mMap = std::make_unique<Map>(this->height);
    this->mTmp_map = std::make_unique<Map>(this->height);

    for (int i = 0; i < height; i++) {
        (*this->mMap)[i] = std::vector<uint8_t>(width);
        (*this->mTmp_map)[i] = std::vector<uint8_t>(width);
    }

    randomFillMap();
}

GameOfLifeModel::~GameOfLifeModel() {}

void GameOfLifeModel::printMap() {
    std::stringstream ss;

    ss << "Generation number = " << iteration << std::endl;
    for (int y = 0; y < height; y++) {
        ss << "[" << y << "]\t";
        for (int x = 0; x < width; x++) {
            if (((*mMap)[y][x] & 0x01) == 1)
                ss << "*";
            else
                ss << ".";
        }
        ss << std::endl;
    }

    std::cout << ss.str();
}

void GameOfLifeModel::liveOneGeneration() {
    int live_cells_cnt;

    // safe copy to mTmp_map
    for (int y = 0; y < height; y++)
        for (int x = 0; x < width; x++)
            (*mTmp_map)[y][x] = (*mMap)[y][x];

    this->iteration++;
    for (int y = 0; y < height; y++) {
        for (int x = 0; x < width; x++) {

            if ((*mTmp_map)[y][x] == 0)
                continue;

            // Get count of alive neightbour cells
            live_cells_cnt = (*mTmp_map)[y][x] >> 1;

            // Is alive ?
            if ((*mTmp_map)[y][x] & 0x01) {
                // Overpopulation or underpopulation - than die
                if (live_cells_cnt != 2 && live_cells_cnt != 3) {
                    clearCell(x, y);
                }
            } else if (live_cells_cnt == 3) {
                // Has 3 neighbours - than revive
                setCell(x, y);
            }
        }
    }
}

void GameOfLifeModel::liveNGeneration(int num_of_generations)
{
    while (num_of_generations > 0) {
        liveOneGeneration();
        num_of_generations--;
    }

    return;
}

Map_ptr GameOfLifeModel::liveNGeneration(int argc, char **argv,
                                    int num_of_generations,
                                    GOF_verbose_lvl verbose)
{
    Q_UNUSED(argc);
    Q_UNUSED(argv);
    Q_UNUSED(num_of_generations);
    Q_UNUSED(verbose);

    std::cout << "Unsported. Use liveNGeneration(int) instead." << std::endl;

    return nullptr;
}

int GameOfLifeModel::rowCount(const QModelIndex &parent) const
{
    Q_UNUSED(parent);
    return height;
}

int GameOfLifeModel::columnCount(const QModelIndex &parent) const
{
    Q_UNUSED(parent);
    return width;
}

QVariant GameOfLifeModel::data(const QModelIndex &index, int role) const
{
    if (!index.isValid() || role != Qt::DisplayRole)
        return QVariant();

    const auto y = static_cast<int>(index.row());
    const auto x = static_cast<int>(index.column());

    // std::cout << "data: x = " << x << ", y = " << y << std::endl;

    return QVariant((*mMap)[y][x]);
}

bool GameOfLifeModel::setData(const QModelIndex &index, const QVariant &value, int role)
{
    Q_UNUSED(value);
    if (!index.isValid() || role != Qt::DisplayRole)
        return false;

    const auto y = static_cast<int>(index.row());
    const auto x = static_cast<int>(index.column());

    if ((*mMap)[y][x] & 0x01)
        clearCell(x, y);
    else
        setCell(x, y);

    emit dataChanged(index, index, {role});

    return true;
}

void GameOfLifeModel::nextStep() {
    liveOneGeneration();
    emit dataChanged(index(0, 0), index(height - 1, width - 1), {CellRole});
}

bool GameOfLifeModel::parseFile(const std::string &fileName) {
    namespace qi     = boost::spirit::qi;
    namespace phx    = boost::phoenix;

    std::string     line;
    std::ifstream   file;
    std::regex      comment("#.*");
    std::regex      dimensions("height = [0-9]+, width = [0-9]+");
    std::regex      cell("(\\.|\\*)*");
    int             mapHeight, mapWidth;

    file = std::ifstream(fileName);

    if (!file.good()) {
        // throw CLI_InvalidFile();
        std::cout << "return InvalidFile " << std::endl;
        return false;
    }

    // skip empty lines
    while (std::getline(file, line)) {
        if (std::regex_match(line, comment) || line.empty())
            continue;
        else
            break;
    }

    // parse dimesions line
    auto begin = line.begin();
    auto end = line.end();
    bool ok = qi::phrase_parse
                    (
                        begin,
                        end,
                            "height = " >>
                            qi::int_[phx::ref(mapHeight) = qi::_1] >>
                            ", width = " >>
                            qi::int_[phx::ref(mapWidth) = qi::_1],
                        qi::space
                    );
    if (!ok) {
        // throw CLI_InvalidFile();
        std::cout << "return InvalidFile " << std::endl;
        return false;
    }

    // TODO: make it more beautiful
    // resize and clear map
    (*mMap).resize(mapHeight);
    for (auto &line : (*mMap)) {
        line.resize(mapWidth);
        std::fill(line.begin(), line.end(), 0);
    }
    // resize and clear Tmp_map
    (*mTmp_map).resize(mapHeight);
    for (auto &line : (*mTmp_map)) {
        line.resize(mapWidth);
        std::fill(line.begin(), line.end(), 0);
    }
    this->height = mapHeight;
    this->width = mapWidth;
    this->iteration = 0;

    // parse cells
    int y = 0;
    while (std::getline(file, line)) {
        if (std::regex_match(line, cell)) {
            for (unsigned x = 0; x < line.length(); x++) {
                if (line[x] == '*')
                    setCell(x, y);
            }
        } else {
            // throw CLI_InvalidMap();
            std::cout << "return InvalidMap " << std::endl;
            return false;
        }
        y++;
    }

    return true;
}

QString filterFileName(const QString &urlString) {
    const QUrl url(urlString);

    if (url.isLocalFile())
        return QDir::toNativeSeparators(url.toLocalFile());
    else
        return urlString;
}

bool GameOfLifeModel::loadFile(const QString &filePath) {
    auto res = parseFile(filePath.toStdString());

    emit dataChanged(index(0, 0), index(height - 1, width - 1), {CellRole});

    std::cout << "return res = " << res << std::endl;
    return res;
}

std::string GameOfLifeModel::prepareMap() {
    std::stringstream ss;

    ss << "height = " << height << ", width = " << width << std::endl;
    for (int y = 0; y < height; y++) {
        for (int x = 0; x < width; x++)
            ss << ((*mMap)[y][x] & 0x01 ? "*" : ".");
        ss << std::endl;
    }

    return ss.str();
}

bool GameOfLifeModel::saveToFile(const QString &filePath) {
    std::string mapStr = prepareMap();
    bool res = false;

    try {
        std::ofstream file(filePath.toStdString(), std::ofstream::out);
        file << mapStr;
        file.close();
        res = true;
    } catch (const char* msg) {
        std::cerr << "Error: " << msg << std::endl;
    }

    return res;
}

int GameOfLifeModel::getIteration()
{
    return iteration;
}

void GameOfLifeModel::_randomFillMap() {
    unsigned seed;
    int x, y;

    // Get seed; random if 0
    seed = (unsigned)time(NULL);
    srand(seed);

    // Randomly initialise cell map with ~50% on pixels
    for (int half_len = (height * width) / 2; half_len > 0; half_len--) {
        x = rand() % (width);
        y = rand() % (height);

        if (((*mMap)[y][x] & 0x01) == 0) {
            setCell(x, y);
        }
    }
}

void GameOfLifeModel::randomFillMap() {
    for (int i = 0; i < this->height; i++)
        std::fill((*mMap)[i].begin(), (*mMap)[i].end(), 0);
    _randomFillMap();
    iteration = 0;
    emit dataChanged(index(0, 0), index(height - 1, width - 1), {CellRole});
}

void GameOfLifeModel::setCell(int x, int y) {
    int x_left, x_right, y_up, y_down;

    x_left = (x == 0) ? width - 1 : x - 1;
    x_right = (x == width - 1) ? 0 : x + 1;
    y_up = (y == 0) ? height - 1 : y - 1;
    y_down = (y == height - 1) ? 0 : y + 1;

    (*mMap)[y][x] |= 0x01;

    (*mMap)[y_up  ][x_left ] += 0x02;
    (*mMap)[y_up  ][x      ] += 0x02;
    (*mMap)[y_up  ][x_right] += 0x02;
    (*mMap)[y     ][x_left ] += 0x02;
    (*mMap)[y     ][x_right] += 0x02;
    (*mMap)[y_down][x_left ] += 0x02;
    (*mMap)[y_down][x      ] += 0x02;
    (*mMap)[y_down][x_right] += 0x02;
}

void GameOfLifeModel::clearCell(int x, int y) {
    int x_left, x_right, y_up, y_down;

    x_left = (x == 0) ? width - 1 : x - 1;
    x_right = (x == width - 1) ? 0 : x + 1;
    y_up = (y == 0) ? height - 1 : y - 1;
    y_down = (y == height - 1) ? 0 : y + 1;

    (*mMap)[y][x] &= ~0x01;

    (*mMap)[y_up  ][x_left ] -= 0x02;
    (*mMap)[y_up  ][x      ] -= 0x02;
    (*mMap)[y_up  ][x_right] -= 0x02;
    (*mMap)[y     ][x_left ] -= 0x02;
    (*mMap)[y     ][x_right] -= 0x02;
    (*mMap)[y_down][x_left ] -= 0x02;
    (*mMap)[y_down][x      ] -= 0x02;
    (*mMap)[y_down][x_right] -= 0x02;
}

int GameOfLifeModel::getLiveNeighbours(int x, int y) {
    return ((*mMap)[y][x] >> 1);
}


// 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_TO_NLP_HPP_
#define SMOOTH__FEEDBACK__OCP_TO_NLP_HPP_

/**
 * @file
 * @brief Formulate optimal control problem as a nonlinear program
 */

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

#include "collocation/mesh.hpp"
#include "collocation/mesh_function.hpp"
#include "nlp.hpp"
#include "ocp.hpp"

namespace smooth::feedback {

// \cond
namespace detail {

/// @brief Variable and constraint structure of an OCP NLP
auto ocp_nlp_structure(const FlatOCPType auto & ocp, const MeshType auto & mesh)
{
  std::size_t N = mesh.N_colloc();

  // variable layout
  std::array<std::size_t, 4> var_len{
    1,                 // tf
    ocp.Nq,            // integrals
    ocp.Nx * (N + 1),  // states
    ocp.Nu * N,        // inputs
  };

  // constraint layout
  std::array<std::size_t, 4> con_len{
    ocp.Nx * N,   // derivatives
    ocp.Nq,       // other integrals
    ocp.Ncr * N,  // running constraints
    ocp.Nce,      // end constraints
  };

  std::array<std::size_t, 5> var_beg{0};
  std::partial_sum(var_len.begin(), var_len.end(), var_beg.begin() + 1);

  std::array<std::size_t, 5> con_beg{0};
  std::partial_sum(con_len.begin(), con_len.end(), con_beg.begin() + 1);

  return std::make_tuple(var_beg, var_len, con_beg, con_len);
}

/**
 * @brief NLP representing an OCP.
 *
 * @note This class allocates matrices internally and returns references to those.
 */
template<FlatOCPType Ocp, MeshType Mesh, diff::Type DT = diff::Type::Default>
class OCPNLP
{
private:
  static constexpr auto Nx = std::decay_t<Ocp>::Nx;
  static constexpr auto Nu = std::decay_t<Ocp>::Nu;
  static constexpr auto Nq = std::decay_t<Ocp>::Nq;

  Ocp ocp_;        // optimal control problem
  Mesh mesh_;      // discretization mesh
  std::size_t N_;  // number of collocation points in mesh

  std::size_t tfvar_B, qvar_B, xvar_B, uvar_B, n_;  // variable start indices
  std::size_t tfvar_L, qvar_L, xvar_L, uvar_L;      // variable lengths

  std::size_t dcon_B, qcon_B, crcon_B, cecon_B, m_;  // constraint start indices
  std::size_t dcon_L, qcon_L, crcon_L, cecon_L;      // constraint lengths

  // scaling
  double w_scaling_{1};

  // variable and constraint bounds
  Eigen::VectorXd xl_, xu_, gl_, gu_;

  // allocated return arguments
  Eigen::VectorXd g_;
  Eigen::SparseMatrix<double> df_dx_, dg_dx_, d2f_dx2_, d2g_dx2_;

  // allocated computation
  MeshValue<0> dyn_out0_, int_out0_, cr_out0_;
  MeshValue<1> dyn_out1_, int_out1_, cr_out1_;
  MeshValue<2> dyn_out2_, int_out2_, cr_out2_;

public:
  /// @brief Constructor
  template<typename OcpArg, typename MeshArg>
  OCPNLP(OcpArg && ocp, MeshArg && mesh)
      : ocp_(std::forward<OcpArg>(ocp)), mesh_(std::forward<MeshArg>(mesh)), N_(mesh_.N_colloc())
  {
    const auto [var_beg, var_len, con_beg, con_len] = detail::ocp_nlp_structure(ocp_, mesh_);

    tfvar_B = var_beg[0];
    qvar_B  = var_beg[1];
    xvar_B  = var_beg[2];
    uvar_B  = var_beg[3];
    n_      = var_beg[4];

    tfvar_L = var_len[0];
    qvar_L  = var_len[1];
    xvar_L  = var_len[2];
    uvar_L  = var_len[3];

    dcon_B  = con_beg[0];
    qcon_B  = con_beg[1];
    crcon_B = con_beg[2];
    cecon_B = con_beg[3];
    m_      = con_beg[4];

    dcon_L  = con_len[0];
    qcon_L  = con_len[1];
    crcon_L = con_len[2];
    cecon_L = con_len[3];

    // Mesh weight
    double max_weight = 1e-6;
    for (auto w : mesh_.all_weights()) { max_weight = std::max(max_weight, w); }
    w_scaling_ = 1. / max_weight;

    // VARIABLE BOUNDS

    xl_.setConstant(n_, -std::numeric_limits<double>::infinity());
    xl_.segment(tfvar_B, tfvar_L).setZero();  // tf lower bounded by zero
    xu_.setConstant(n_, std::numeric_limits<double>::infinity());

    // CONSTRAINT BOUNDS

    gl_.resize(m_);
    gu_.resize(m_);

    // derivative constraints are equalities
    gl_.segment(dcon_B, dcon_L).setZero();
    gu_.segment(dcon_B, dcon_L).setZero();

    // integral constraints are equalities
    gl_.segment(qcon_B, qcon_L).setZero();
    gu_.segment(qcon_B, qcon_L).setZero();

    // running constraints (scaled by quadrature weights)
    gl_.segment(crcon_B, crcon_L) = ocp.crl.replicate(N_, 1);
    gu_.segment(crcon_B, crcon_L) = ocp.cru.replicate(N_, 1);
    for (const auto & [i, w] : utils::zip(std::views::iota(0u, N_), mesh_.all_weights())) {
      gl_.segment(crcon_B + i * ocp_.Ncr, ocp_.Ncr) *= w_scaling_ * w;
      gu_.segment(crcon_B + i * ocp_.Ncr, ocp_.Ncr) *= w_scaling_ * w;
    }

    // end constraints
    gl_.segment(cecon_B, cecon_L) = ocp.cel;
    gu_.segment(cecon_B, cecon_L) = ocp.ceu;

    // allocate output args
    g_.setZero(m_);

    df_dx_.resize(1, n_);
    df_dx_.reserve(Eigen::VectorXi::Constant(n_, 1));

    d2f_dx2_.resize(n_, n_);
    dg_dx_.resize(m_, n_);
    d2g_dx2_.resize(n_, n_);

    /// @todo Allocate nnz's to speed up first call?
  }

  std::size_t n() const { return n_; }
  std::size_t m() const { return m_; }
  const Eigen::VectorXd & xl() const { return xl_; }
  const Eigen::VectorXd & xu() const { return xu_; }

  double f(const Eigen::Ref<const Eigen::VectorXd> x) const
  {
    assert(static_cast<std::size_t>(x.size()) == n_);

    const auto x0var_B = xvar_B;
    const auto xfvar_B = xvar_B + xvar_L - Nx;

    const double tf                    = x(tfvar_B);
    const Eigen::Vector<double, Nx> x0 = x.segment(x0var_B, Nx);
    const Eigen::Vector<double, Nx> xf = x.segment(xfvar_B, Nx);
    const Eigen::Vector<double, Nq> q  = x.segment(qvar_B, qvar_L);

    return ocp_.theta(tf, x0, xf, q);
  }

  const Eigen::SparseMatrix<double> & df_dx(const Eigen::Ref<const Eigen::VectorXd> x)
  {
    assert(static_cast<std::size_t>(x.size()) == n_);

    const auto x0var_B = xvar_B;
    const auto xfvar_B = xvar_B + xvar_L - Nx;

    const double tf                    = x(tfvar_B);
    const Eigen::Vector<double, Nx> x0 = x.segment(x0var_B, Nx);
    const Eigen::Vector<double, Nx> xf = x.segment(xfvar_B, Nx);
    const Eigen::Vector<double, Nq> q  = x.segment(qvar_B, qvar_L);

    const auto & [fval, dfval] = diff::dr<1, DT>(ocp_.theta, wrt(tf, x0, xf, q));

    set_zero(df_dx_);

    block_add(df_dx_, 0, tfvar_B, dfval.middleCols(0, 1));           // df / dtf
    block_add(df_dx_, 0, x0var_B, dfval.middleCols(1, Nx));          // df / dx0
    block_add(df_dx_, 0, xfvar_B, dfval.middleCols(1 + Nx, Nx));     // df / dxf
    block_add(df_dx_, 0, qvar_B, dfval.middleCols(1 + 2 * Nx, Nq));  // df / dq

    df_dx_.makeCompressed();
    return df_dx_;
  }

  const Eigen::SparseMatrix<double> & d2f_dx2(Eigen::Ref<const Eigen::VectorXd> x)
  {
    assert(static_cast<std::size_t>(x.size()) == n_);

    const auto x0var_B = xvar_B;
    const auto xfvar_B = xvar_B + xvar_L - Nx;

    const double tf                    = x(tfvar_B);
    const Eigen::Vector<double, Nx> x0 = x.segment(x0var_B, Nx);
    const Eigen::Vector<double, Nx> xf = x.segment(xfvar_B, Nx);
    const Eigen::Vector<double, Nq> q  = x.segment(qvar_B, qvar_L);

    const auto & [fval, dfval, d2fval] = diff::dr<2, DT>(ocp_.theta, wrt(tf, x0, xf, q));

    set_zero(d2f_dx2_);

    // clang-format off
    block_add(d2f_dx2_, tfvar_B, tfvar_B, d2fval.block(         0,          0,  1,  1), 1, true);  // tftf
    block_add(d2f_dx2_, tfvar_B, x0var_B, d2fval.block(         0,          1,  1, Nx), 1, true);  // tfx0
    block_add(d2f_dx2_, tfvar_B, xfvar_B, d2fval.block(         0,     1 + Nx,  1, Nx), 1, true);  // tfxf
    block_add(d2f_dx2_, tfvar_B,  qvar_B, d2fval.block(         0, 1 + 2 * Nx,  1, Nq), 1, true);  // tfq

    block_add(d2f_dx2_, x0var_B, x0var_B, d2fval.block(         1,          1, Nx, Nx), 1, true);  // x0x0
    block_add(d2f_dx2_, x0var_B, xfvar_B, d2fval.block(         1,     1 + Nx, Nx, Nx), 1, true);  // x0xf
    block_add(d2f_dx2_, x0var_B,  qvar_B, d2fval.block(         1, 1 + 2 * Nx, Nx, Nq), 1, true);  // x0q

    block_add(d2f_dx2_, xfvar_B, xfvar_B, d2fval.block(    1 + Nx,     1 + Nx, Nx, Nx), 1, true);  // xfxf
    block_add(d2f_dx2_, xfvar_B,  qvar_B, d2fval.block(    1 + Nx, 1 + 2 * Nx, Nx, Nq), 1, true);  // xfq

    block_add(d2f_dx2_,  qvar_B,  qvar_B, d2fval.block(1 + 2 * Nx, 1 + 2 * Nx, Nq, Nq), 1, true);  // qq
    // clang-format on

    d2f_dx2_.makeCompressed();
    return d2f_dx2_;
  }

  const Eigen::VectorXd & g(const Eigen::Ref<const Eigen::VectorXd> x)
  {
    assert(static_cast<std::size_t>(x.size()) == n_);

    const auto x0var_B = xvar_B;
    const auto xfvar_B = xvar_B + xvar_L - Nx;

    const double t0                    = 0;
    const double tf                    = x(tfvar_B);
    const Eigen::Vector<double, Nx> x0 = x.segment(x0var_B, Nx);
    const Eigen::Vector<double, Nx> xf = x.segment(xfvar_B, Nx);
    const Eigen::Vector<double, Nq> q  = x.segment(qvar_B, qvar_L);

    const Eigen::Map<const Eigen::Matrix<double, Nx, -1>> X(x.data() + xvar_B, Nx, N_ + 1);
    const Eigen::Map<const Eigen::Matrix<double, Nu, -1>> U(x.data() + uvar_B, Nu, N_);

    mesh_dyn<0>(dyn_out0_, mesh_, ocp_.f, t0, tf, X.colwise(), U.colwise());
    mesh_integrate<0>(int_out0_, mesh_, ocp_.g, t0, tf, X.colwise(), U.colwise());
    mesh_eval<0>(cr_out0_, mesh_, ocp_.cr, t0, tf, X.colwise(), U.colwise(), true);

    g_.segment(dcon_B, dcon_L)   = w_scaling_ * dyn_out0_.F;
    g_.segment(qcon_B, qcon_L)   = w_scaling_ * (int_out0_.F - q);
    g_.segment(crcon_B, crcon_L) = w_scaling_ * cr_out0_.F;
    g_.segment(cecon_B, cecon_L) = ocp_.ce(tf, x0, xf, q);

    return g_;
  }
  const Eigen::VectorXd & gl() const { return gl_; }
  const Eigen::VectorXd & gu() const { return gu_; }
  const Eigen::SparseMatrix<double> & dg_dx(const Eigen::Ref<const Eigen::VectorXd> x)
  {
    assert(static_cast<std::size_t>(x.size()) == n_);

    const auto x0var_B = xvar_B;
    const auto xfvar_B = xvar_B + xvar_L - Nx;

    const double t0                    = 0;
    const double tf                    = x(tfvar_B);
    const Eigen::Vector<double, Nx> x0 = x.segment(x0var_B, Nx);
    const Eigen::Vector<double, Nx> xf = x.segment(xfvar_B, Nx);
    const Eigen::Vector<double, Nq> q  = x.segment(qvar_B, qvar_L);

    const Eigen::Map<const Eigen::Matrix<double, Nx, -1>> X(x.data() + xvar_B, Nx, N_ + 1);
    const Eigen::Map<const Eigen::Matrix<double, Nu, -1>> U(x.data() + uvar_B, Nu, N_);

    mesh_dyn<1, DT>(dyn_out1_, mesh_, ocp_.f, t0, tf, X.colwise(), U.colwise());
    mesh_integrate<1, DT>(int_out1_, mesh_, ocp_.g, t0, tf, X.colwise(), U.colwise());
    mesh_eval<1, DT>(cr_out1_, mesh_, ocp_.cr, t0, tf, X.colwise(), U.colwise(), true);
    const auto & [ceval, dceval] = diff::dr<1, DT>(ocp_.ce, wrt(tf, x0, xf, q));

    dyn_out1_.dF.makeCompressed();
    int_out1_.dF.makeCompressed();
    cr_out1_.dF.makeCompressed();

    set_zero(dg_dx_);

    // dynamics constraint
    block_add(dg_dx_, dcon_B, tfvar_B, dyn_out1_.dF.middleCols(1, 1), w_scaling_);
    block_add(dg_dx_, dcon_B, xvar_B, dyn_out1_.dF.middleCols(2, xvar_L), w_scaling_);
    block_add(dg_dx_, dcon_B, uvar_B, dyn_out1_.dF.middleCols(2 + xvar_L, uvar_L), w_scaling_);

    // integral constraint
    block_add(dg_dx_, qcon_B, tfvar_B, int_out1_.dF.middleCols(1, 1), w_scaling_);
    block_add(dg_dx_, qcon_B, xvar_B, int_out1_.dF.middleCols(2, xvar_L), w_scaling_);
    block_add(dg_dx_, qcon_B, uvar_B, int_out1_.dF.middleCols(2 + xvar_L, uvar_L), w_scaling_);
    block_add_identity(dg_dx_, qcon_B, qvar_B, qvar_L, -w_scaling_);

    // running constraint
    block_add(dg_dx_, crcon_B, tfvar_B, cr_out1_.dF.middleCols(1, 1), w_scaling_);
    block_add(dg_dx_, crcon_B, xvar_B, cr_out1_.dF.middleCols(2, xvar_L), w_scaling_);
    block_add(dg_dx_, crcon_B, uvar_B, cr_out1_.dF.middleCols(2 + xvar_L, uvar_L), w_scaling_);

    // end constraint
    block_add(dg_dx_, cecon_B, tfvar_B, dceval.middleCols(0, 1));
    block_add(dg_dx_, cecon_B, xvar_B, dceval.middleCols(1, Nx));
    block_add(dg_dx_, cecon_B, xvar_B + xvar_L - Nx, dceval.middleCols(1 + Nx, Nx));
    block_add(dg_dx_, cecon_B, qvar_B, dceval.middleCols(1 + 2 * Nx, Nq));

    dg_dx_.makeCompressed();
    return dg_dx_;
  }

  const Eigen::SparseMatrix<double> &
  d2g_dx2(const Eigen::Ref<const Eigen::VectorXd> x, const Eigen::Ref<const Eigen::VectorXd> lambda)
  {
    assert(static_cast<std::size_t>(x.size()) == n_);
    assert(static_cast<std::size_t>(lambda.size()) == m_);

    const auto x0var_B = xvar_B;
    const auto xfvar_B = xvar_B + xvar_L - Nx;

    const double t0                    = 0;
    const double tf                    = x(tfvar_B);
    const Eigen::Vector<double, Nx> x0 = x.segment(x0var_B, Nx);
    const Eigen::Vector<double, Nx> xf = x.segment(xfvar_B, Nx);
    const Eigen::Vector<double, Nq> q  = x.segment(qvar_B, qvar_L);

    const Eigen::Map<const Eigen::Matrix<double, Nx, -1>> X(x.data() + xvar_B, Nx, N_ + 1);
    const Eigen::Map<const Eigen::Matrix<double, Nu, -1>> U(x.data() + uvar_B, Nu, N_);

    dyn_out2_.lambda = lambda.segment(dcon_B, dcon_L);
    int_out2_.lambda = lambda.segment(qcon_B, qcon_L);
    cr_out2_.lambda  = lambda.segment(crcon_B, crcon_L);
    mesh_dyn<2, DT>(dyn_out2_, mesh_, ocp_.f, t0, tf, X.colwise(), U.colwise());
    mesh_integrate<2, DT>(int_out2_, mesh_, ocp_.g, t0, tf, X.colwise(), U.colwise());
    mesh_eval<2, DT>(cr_out2_, mesh_, ocp_.cr, t0, tf, X.colwise(), U.colwise(), true);
    const auto & [ceval, dceval, d2ceval] = diff::dr<2, DT>(ocp_.ce, wrt(tf, x0, xf, q));

    dyn_out2_.dF.makeCompressed();
    int_out2_.dF.makeCompressed();
    cr_out2_.dF.makeCompressed();

    dyn_out2_.d2F.makeCompressed();
    int_out2_.d2F.makeCompressed();
    cr_out2_.d2F.makeCompressed();

    set_zero(d2g_dx2_);

    // clang-format off
    block_add(d2g_dx2_, tfvar_B, tfvar_B, dyn_out2_.d2F.block(1, 1, 1, 1), w_scaling_, true);      // tftf
    block_add(d2g_dx2_, tfvar_B, tfvar_B, int_out2_.d2F.block(1, 1, 1, 1), w_scaling_, true);      // tftf
    block_add(d2g_dx2_, tfvar_B, tfvar_B,  cr_out2_.d2F.block(1, 1, 1, 1), w_scaling_, true);      // tftf

    block_add(d2g_dx2_, tfvar_B, xvar_B, dyn_out2_.d2F.block(1, 2, 1, xvar_L), w_scaling_, true);  // tfx
    block_add(d2g_dx2_, tfvar_B, xvar_B, int_out2_.d2F.block(1, 2, 1, xvar_L), w_scaling_, true);  // tfx
    block_add(d2g_dx2_, tfvar_B, xvar_B,  cr_out2_.d2F.block(1, 2, 1, xvar_L), w_scaling_, true);  // tfx

    block_add(d2g_dx2_, tfvar_B, uvar_B, dyn_out2_.d2F.block(1, 2 + xvar_L, 1, uvar_L), w_scaling_, true);  // tfu
    block_add(d2g_dx2_, tfvar_B, uvar_B, int_out2_.d2F.block(1, 2 + xvar_L, 1, uvar_L), w_scaling_, true);  // tfu
    block_add(d2g_dx2_, tfvar_B, uvar_B,  cr_out2_.d2F.block(1, 2 + xvar_L, 1, uvar_L), w_scaling_, true);  // tfu

    block_add(d2g_dx2_, xvar_B, xvar_B, dyn_out2_.d2F.block(2,          2, xvar_L, xvar_L), w_scaling_, true);  // xx
    block_add(d2g_dx2_, xvar_B, xvar_B, int_out2_.d2F.block(2,          2, xvar_L, xvar_L), w_scaling_, true);  // xx
    block_add(d2g_dx2_, xvar_B, xvar_B,  cr_out2_.d2F.block(2,          2, xvar_L, xvar_L), w_scaling_, true);  // xx

    block_add(d2g_dx2_, xvar_B, uvar_B, dyn_out2_.d2F.block(2, 2 + xvar_L, xvar_L, uvar_L), w_scaling_, true);  // xu
    block_add(d2g_dx2_, xvar_B, uvar_B, int_out2_.d2F.block(2, 2 + xvar_L, xvar_L, uvar_L), w_scaling_, true);  // xu
    block_add(d2g_dx2_, xvar_B, uvar_B,  cr_out2_.d2F.block(2, 2 + xvar_L, xvar_L, uvar_L), w_scaling_, true);  // xu

    block_add(d2g_dx2_, uvar_B, uvar_B, dyn_out2_.d2F.block(2 + xvar_L, 2 + xvar_L, uvar_L, uvar_L), w_scaling_, true);  // uu
    block_add(d2g_dx2_, uvar_B, uvar_B, int_out2_.d2F.block(2 + xvar_L, 2 + xvar_L, uvar_L, uvar_L), w_scaling_, true);  // uu
    block_add(d2g_dx2_, uvar_B, uvar_B,  cr_out2_.d2F.block(2 + xvar_L, 2 + xvar_L, uvar_L, uvar_L), w_scaling_, true);  // uu
    // clang-format on

    for (auto j = 0u; j < ocp_.Nce; ++j) {
      const auto b0 = (1 + 2 * Nx + ocp_.Nq) * j;
      // clang-format off
      block_add(d2g_dx2_, tfvar_B, tfvar_B, d2ceval.block(         0, b0 +          0,  1,  1), lambda(cecon_B + j), true);  // tftf
      block_add(d2g_dx2_, tfvar_B, x0var_B, d2ceval.block(         0, b0 +          1,  1, Nx), lambda(cecon_B + j), true);  // tfx0
      block_add(d2g_dx2_, tfvar_B, xfvar_B, d2ceval.block(         0, b0 +     1 + Nx,  1, Nx), lambda(cecon_B + j), true);  // tfxf
      block_add(d2g_dx2_, tfvar_B,  qvar_B, d2ceval.block(         0, b0 + 1 + 2 * Nx,  1, Nq), lambda(cecon_B + j), true);  // tfq

      block_add(d2g_dx2_, x0var_B, x0var_B, d2ceval.block(         1, b0 +          1, Nx, Nx), lambda(cecon_B + j), true);  // x0x0
      block_add(d2g_dx2_, x0var_B, xfvar_B, d2ceval.block(         1, b0 +     1 + Nx, Nx, Nx), lambda(cecon_B + j), true);  // x0xf
      block_add(d2g_dx2_, x0var_B,  qvar_B, d2ceval.block(         1, b0 + 1 + 2 * Nx, Nx, Nq), lambda(cecon_B + j), true);  // x0q

      block_add(d2g_dx2_, xfvar_B, xfvar_B, d2ceval.block(    1 + Nx, b0 +     1 + Nx, Nx, Nx), lambda(cecon_B + j), true);  // xfxf
      block_add(d2g_dx2_, xfvar_B,  qvar_B, d2ceval.block(    1 + Nx, b0 + 1 + 2 * Nx, Nx, Nq), lambda(cecon_B + j), true);  // xfq

      block_add(d2g_dx2_,  qvar_B,  qvar_B, d2ceval.block(1 + 2 * Nx, b0 + 1 + 2 * Nx, Nq, Nq), lambda(cecon_B + j), true);  // qq
      // clang-format on
    }

    d2g_dx2_.makeCompressed();
    return d2g_dx2_;
  }
};

}  // namespace detail
// \endcond

/**
 * @brief Formulate an OCP as a NLP using collocation on a Mesh.
 *
 * @param ocp Optimal control problem definition
 * @param mesh collocation point structure
 * @return encoding of ocp as a nonlinear program
 *
 * @see ocpsol_to_nlpsol(), nlpsol_to_ocpsol()
 */
template<diff::Type DT = diff::Type::Default>
auto ocp_to_nlp(FlatOCPType auto && ocp, MeshType auto && mesh)
  -> detail::OCPNLP<std::decay_t<decltype(ocp)>, std::decay_t<decltype(mesh)>, DT>
{
  return detail::OCPNLP<std::decay_t<decltype(ocp)>, std::decay_t<decltype(mesh)>, DT>(
    std::forward<decltype(ocp)>(ocp), std::forward<decltype(mesh)>(mesh));
}

/**
 * @brief Convert nonlinear program solution to ocp solution
 */
auto nlpsol_to_ocpsol(
  const FlatOCPType auto & ocp, const MeshType auto & mesh, const NLPSolution & nlp_sol)
{
  using ocp_t = std::decay_t<decltype(ocp)>;

  static constexpr auto Nx  = ocp_t::Nx;
  static constexpr auto Nu  = ocp_t::Nu;
  static constexpr auto Nq  = ocp_t::Nq;
  static constexpr auto Ncr = ocp_t::Ncr;

  const std::size_t N                             = mesh.N_colloc();
  const auto [var_beg, var_len, con_beg, con_len] = detail::ocp_nlp_structure(ocp, mesh);

  const auto [tfvar_B, qvar_B, xvar_B, uvar_B, n] = var_beg;
  const auto [tfvar_L, qvar_L, xvar_L, uvar_L]    = var_len;

  const auto [dcon_B, qcon_B, crcon_B, cecon_B, m] = con_beg;
  const auto [dcon_L, qcon_L, crcon_L, cecon_L]    = con_len;

  const double t0 = 0;
  const double tf = nlp_sol.x(tfvar_B);

  const Eigen::Vector<double, Nq> Q = nlp_sol.x.segment(qvar_B, qvar_L);

  // state vector has a value at the endpoint

  Eigen::MatrixXd X(ocp.Nx, N + 1);
  X = nlp_sol.x.segment(xvar_B, xvar_L).reshaped(ocp.Nx, xvar_L / ocp.Nx);

  auto xfun =
    [t0 = t0, tf = tf, mesh = mesh, X = std::move(X)](double t) -> Eigen::Vector<double, Nx> {
    return mesh.template eval<Eigen::Vector<double, Nx>>(
      (t - t0) / (tf - t0), X.colwise(), 0, true);
  };

  // for these we repeat last point since there are no values for endpoint

  Eigen::MatrixXd U(ocp.Nu, N);
  U = nlp_sol.x.segment(uvar_B, uvar_L).reshaped(ocp.Nu, uvar_L / ocp.Nu);

  auto ufun =
    [t0 = t0, tf = tf, mesh = mesh, U = std::move(U)](double t) -> Eigen::Vector<double, Nu> {
    return mesh.template eval<Eigen::Vector<double, Nu>>(
      (t - t0) / (tf - t0), U.colwise(), 0, false);
  };

  Eigen::MatrixXd Ldyn(ocp.Nx, N);
  Ldyn = nlp_sol.lambda.segment(dcon_B, dcon_L).reshaped(ocp.Nx, dcon_L / ocp.Nx);

  auto ldfun =
    [t0 = t0, tf = tf, mesh = mesh, Ldyn = std::move(Ldyn)](double t) -> Eigen::Vector<double, Nx> {
    return mesh.template eval<Eigen::Vector<double, Nx>>(
      (t - t0) / (tf - t0), Ldyn.colwise(), 0, false);
  };

  Eigen::MatrixXd Lcr(ocp.Ncr, N);
  Lcr = nlp_sol.lambda.segment(crcon_B, crcon_L).reshaped(ocp.Ncr, crcon_L / ocp.Ncr);

  auto lcrfun =
    [t0 = t0, tf = tf, mesh = mesh, Lcr = std::move(Lcr)](double t) -> Eigen::Vector<double, Ncr> {
    return mesh.template eval<Eigen::Vector<double, Ncr>>(
      (t - t0) / (tf - t0), Lcr.colwise(), 0, false);
  };

  return OCPSolution<typename ocp_t::X, typename ocp_t::U, ocp_t::Nq, ocp_t::Ncr, ocp_t::Nce>{
    .t0         = t0,
    .tf         = tf,
    .Q          = std::move(Q),
    .u          = std::move(ufun),
    .x          = std::move(xfun),
    .lambda_q   = nlp_sol.lambda.segment(qcon_B, qcon_L),
    .lambda_ce  = nlp_sol.lambda.segment(cecon_B, cecon_L),
    .lambda_dyn = std::move(ldfun),
    .lambda_cr  = std::move(lcrfun),
  };
}

/**
 * @brief Convert ocp solution to nonlinear program solution
 *
 * @note Allocates memory for return type.
 */
NLPSolution
ocpsol_to_nlpsol(const FlatOCPType auto & ocp, const MeshType auto & mesh, const auto & ocpsol)
{
  const auto N = mesh.N_colloc();

  const auto [var_beg, var_len, con_beg, con_len] = detail::ocp_nlp_structure(ocp, mesh);

  const auto [tfvar_B, qvar_B, xvar_B, uvar_B, n] = var_beg;
  const auto [tfvar_L, qvar_L, xvar_L, uvar_L]    = var_len;

  const auto [dcon_B, qcon_B, crcon_B, cecon_B, m] = con_beg;
  const auto [dcon_L, qcon_L, crcon_L, cecon_L]    = con_len;

  const double t0 = 0;
  const double tf = ocpsol.tf;

  Eigen::VectorXd x(n), lambda(m);

  x(tfvar_B)                = ocpsol.tf;
  x.segment(qvar_B, qvar_L) = ocpsol.Q;

  lambda.segment(qcon_B, qcon_L)   = ocpsol.lambda_q;
  lambda.segment(cecon_B, cecon_L) = ocpsol.lambda_ce;

  for (const auto & [i, tau] : utils::zip(std::views::iota(0u), mesh.all_nodes())) {
    x.segment(xvar_B + i * ocp.Nx, ocp.Nx) = ocpsol.x(t0 + tau * (tf - t0));
    if (i < N) {
      x.segment(uvar_B + i * ocp.Nu, ocp.Nu)         = ocpsol.u(t0 + tau * (tf - t0));
      lambda.segment(dcon_B + i * ocp.Nx, ocp.Nx)    = ocpsol.lambda_dyn(t0 + tau * (tf - t0));
      lambda.segment(crcon_B + i * ocp.Ncr, ocp.Ncr) = ocpsol.lambda_cr(t0 + tau * (tf - t0));
    }
  }

  return {
    .status = NLPSolution::Status::Unknown,
    .x      = std::move(x),
    .zl     = Eigen::VectorXd::Zero(n),
    .zu     = Eigen::VectorXd::Zero(n),
    .lambda = std::move(lambda),
  };
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__OCP_TO_NLP_HPP_


/*
 [auto_generated]
 test/std_array.cpp

 [begin_description]
 Checks if odeint compiles fine with the std::array using the array algebra
 [end_description]

 Copyright 2009-2014 Karsten Ahnert
 Copyright 2009-2014 Mario Mulansky

 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)
 */

#define BOOST_TEST_MODULE odeint_std_array

#include <array>
#include <boost/numeric/odeint.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/test/unit_test.hpp>

using namespace boost::unit_test;

typedef std::array<double, 3> state_type;

void rhs(const state_type &x, state_type &dxdt, const double t)
{
}

BOOST_AUTO_TEST_SUITE( unwrap_reference_test )

BOOST_AUTO_TEST_CASE( test_case )
{
    state_type x = {0.0, 0.0, 0.0};

    typedef boost::numeric::odeint::runge_kutta4<state_type> stepper_type;
// check if array algebra is selected, but only if odeint detects c++11
#ifdef BOOST_NUMERIC_ODEINT_CXX11
    BOOST_STATIC_ASSERT(( boost::is_same< stepper_type::algebra_type ,
                          boost::numeric::odeint::array_algebra >::value ));
#endif
    stepper_type stepper1;
    stepper1.do_step(rhs, x, 0.0, 0.1);

    boost::numeric::odeint::runge_kutta4<
        state_type, double, state_type, double,
        boost::numeric::odeint::array_algebra > stepper;
    stepper.do_step(rhs, x, 0.0, 0.1);

}

BOOST_AUTO_TEST_SUITE_END()


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "main.h"
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/QR>

template<typename BoxType> void alignedbox(const BoxType& _box)
{
  /* this test covers the following files:
     AlignedBox.h
  */

  const int dim = _box.dim();
  typedef typename BoxType::Scalar Scalar;
  typedef typename NumTraits<Scalar>::Real RealScalar;
  typedef Matrix<Scalar, BoxType::AmbientDimAtCompileTime, 1> VectorType;

  VectorType p0 = VectorType::Random(dim);
  VectorType p1 = VectorType::Random(dim);
  RealScalar s1 = ei_random<RealScalar>(0,1);

  BoxType b0(dim);
  BoxType b1(VectorType::Random(dim),VectorType::Random(dim));
  BoxType b2;

  b0.extend(p0);
  b0.extend(p1);
  VERIFY(b0.contains(p0*s1+(Scalar(1)-s1)*p1));
  VERIFY(!b0.contains(p0 + (1+s1)*(p1-p0)));

  (b2 = b0).extend(b1);
  VERIFY(b2.contains(b0));
  VERIFY(b2.contains(b1));
  VERIFY_IS_APPROX(b2.clamp(b0), b0);

  // casting
  const int Dim = BoxType::AmbientDimAtCompileTime;
  typedef typename GetDifferentType<Scalar>::type OtherScalar;
  AlignedBox<OtherScalar,Dim> hp1f = b0.template cast<OtherScalar>();
  VERIFY_IS_APPROX(hp1f.template cast<Scalar>(),b0);
  AlignedBox<Scalar,Dim> hp1d = b0.template cast<Scalar>();
  VERIFY_IS_APPROX(hp1d.template cast<Scalar>(),b0);
}

void test_eigen2_alignedbox()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( alignedbox(AlignedBox<float,2>()) );
    CALL_SUBTEST_2( alignedbox(AlignedBox<float,3>()) );
    CALL_SUBTEST_3( alignedbox(AlignedBox<double,4>()) );
  }
}


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

#include <boost/simd/include/functor.hpp>
#include <boost/simd/sdk/constant/register.hpp>
#include <boost/simd/sdk/constant/constant.hpp>

/*!
 * \ingroup boost_simd_constant
 * \defgroup boost_simd_constant_nan Nan
 *
 * \par Description
 * Constant Nan not a number for floating point value.
 * \par
 * All floating point "nan" generated by nt2 are quiet nan with all bits at 1
 * \par
 * For integer types the constant value is zero, the rationale being the C case.
 * \par
 * The value of this constant is type dependant. This means that for different
 * types it does not represent the same mathematical number.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/nan.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::nan_(A0)>::type
 *     Nan();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Nan
 *
 * \return type T value
 *
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
     * \brief Define the tag Nan of functor Nan
     *        in namespace boost::simd::tag for toolbox boost.simd.constant
    **/
    #ifdef BOOST_SIMD_NO_NANS
    BOOST_SIMD_CONSTANT_REGISTER( Nan, double, 0
                                , 0x0, 0x0ULL
                                );

    #else
    BOOST_SIMD_CONSTANT_REGISTER( Nan, double, 0
                                , 0xFFFFFFFF, 0xFFFFFFFFFFFFFFFFULL
                                );
    #endif
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Nan, Nan)
} }

#include <boost/simd/sdk/constant/common.hpp>

#endif


//
//  cvxCalib3d.hpp
//  RGB_RF
//
//  Created by jimmy on 2016-06-12.
//  Copyright © 2016 jimmy. All rights reserved.
//

#ifndef cvxCalib3d_cpp
#define cvxCalib3d_cpp

#include <stdio.h>
#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/core/core_c.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#include <Eigen/Dense>

using std::vector;

class CvxCalib3D
{
public:
    // rigid transform from source point (src) to destination point (dst)
    // affine: 3 x 4 matrix
    static void rigidTransform(const vector<cv::Point3d> & src,
                               const cv::Mat & affine,
                               vector<cv::Point3d> & dst);

    // estimate a 3 x 4 rigid transfrom from src to dst
    static void KabschTransform(const vector<cv::Point3d> & src,
                                const vector<cv::Point3d> & dst,
                                cv::Mat & affine);

    // estimate a 3 x 4 rigid transfrom from src to dst
    static Eigen::Affine3d KabschTransform(const vector<Eigen::Vector3d> & src, const vector<Eigen::Vector3d> & dst);

    // calculate locations in camera coordinates and world coordinates using
    // camera pose (e.g., ground truth) and depth image
    // camera_depth_img: CV_64FC1
    // camera_to_world_pose: 4x4 CV_64FC1
    // calibration_matrix: 3x3 CV_64FC1
    // depth_factor: e.g. 1000.0 for MS 7 scenes
    // camera_xyz: output camera coordinate location, CV_64FC3
    // mask: output CV_8UC1 0 -- > invalid, 1 --> valid
    // return: CV_64FC3 , x y z in world coordinate
    static cv::Mat cameraDepthToWorldCoordinate(const cv::Mat & camera_depth_img,
                                                const cv::Mat & camera_to_world_pose,
                                                const cv::Mat & calibration_matrix,
                                                const double depth_factor,
                                                const double min_depth,
                                                const double max_depth,
                                                cv::Mat & camera_coordinate,
                                                cv::Mat & mask);

};

#endif /* cvxCalib3d_cpp */


//==================================================================================================
/*
  Copyright 2017 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)
*/
//==================================================================================================
//! [segmented_range_t]
#include <boost/simd/range/segmented_range.hpp>
#include <boost/simd/literal.hpp>
#include <iostream>
#include <vector>

int main()
{
  using namespace boost::simd::literal;

  std::vector<float> data{1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18};

  boost::simd::segmented_range_t<float*,4> r = boost::simd::segmented_range(&data[0], &data[0]+18,4_c);

  for(float v : r.head) std::cout << v << " ";
  std::cout << "\n";

  for(boost::simd::pack<float,4> v : r.body) std::cout << v << " ";
  std::cout << "\n";

  for(float v : r.tail) std::cout << v << " ";
  std::cout << "\n";

  return 0;
}
//! [segmented_range_t]


#include "MemoryManagerTree.h"
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/detail/bitscan.hpp>
#include <iostream>
#include <string>

//static boost::uint64_t g_numLeaves = 0;

void MemoryManagerTree::SetupTree(const int depth, void *node) {
	MemoryManagerInnerNode * const innerNode = (MemoryManagerInnerNode*)node;
	innerNode->m_bitMask = UINT64_MAX;
	if (depth > 1) { //inner node

		MemoryManagerInnerNode * const childInnerNodes = (MemoryManagerInnerNode*)malloc(64 * sizeof(MemoryManagerInnerNode));
		innerNode->m_childNodes = childInnerNodes;
		for (unsigned int i = 0; i < 64; ++i) {
			SetupTree(depth - 1, &childInnerNodes[i]);
		}
	}
	else { //depth == 1 so setup leaf node
		MemoryManagerLeafNode * const childLeafNodes = (MemoryManagerLeafNode*)malloc(64 * sizeof(MemoryManagerLeafNode));
		//g_numLeaves += 64;
		innerNode->m_childNodes = childLeafNodes;
		for (unsigned int i = 0; i < 64; ++i) {
			childLeafNodes[i].m_bitMask = UINT64_MAX;
		}
	}

}

void MemoryManagerTree::SetupTree() {
	SetupTree(MAX_TREE_DEPTH, &m_rootNode);
}

void MemoryManagerTree::FreeTree(const int depth, void *node) {
	MemoryManagerInnerNode * const innerNode = (MemoryManagerInnerNode*)node;
	if (depth > 1) { //inner node
		MemoryManagerInnerNode * const childInnerNodes = (MemoryManagerInnerNode*)innerNode->m_childNodes;
		for (unsigned int i = 0; i < 64; ++i) {
			FreeTree(depth - 1, &childInnerNodes[i]);
		}
		free(childInnerNodes);
	}
	else { //depth == 1 so free leaf node
		MemoryManagerLeafNode * const childLeafNodes = (MemoryManagerLeafNode*)innerNode->m_childNodes;
		//g_numLeaves -= 64;
		free(childLeafNodes);
	}

}

void MemoryManagerTree::FreeTree() {
	FreeTree(MAX_TREE_DEPTH, &m_rootNode);
}

void MemoryManagerTree::GetAndSetFirstFreeSegmentId(const int depth, void *node, boost::uint32_t * segmentId) {

	MemoryManagerInnerNode * const innerNode = (MemoryManagerInnerNode*)node;
	if (depth > 1) { // this is an inner node that has children that are also inner nodes
		if (innerNode->m_bitMask != 0) { //bitmask not zero means space available
			MemoryManagerInnerNode * const childInnerNodes = (MemoryManagerInnerNode*)innerNode->m_childNodes;
			const unsigned int firstFreeIndex = boost::multiprecision::detail::find_lsb<boost::uint64_t>(innerNode->m_bitMask);
			*segmentId += firstFreeIndex * (64 << ((depth - 1) * 6)); // 64^depth
			//std::cout << "d" << depth << " i" << firstFreeIndex << " ";
			MemoryManagerInnerNode * const targetChildInnerNode = &childInnerNodes[firstFreeIndex];
			GetAndSetFirstFreeSegmentId(depth - 1, targetChildInnerNode, segmentId);
			if (targetChildInnerNode->m_bitMask == 0) { //target child now full, so mark the parent's bit full (0)
				const boost::uint64_t mask64 = (((boost::uint64_t)1) << firstFreeIndex);
				innerNode->m_bitMask &= ~mask64;
			}
		}
	}
	else { //depth == 1 , this is an inner node that has children that are leaf nodes
		MemoryManagerLeafNode * const childLeafNodes = (MemoryManagerLeafNode*)innerNode->m_childNodes;
		const unsigned int firstFreeIndexInnerNode = boost::multiprecision::detail::find_lsb<boost::uint64_t>(innerNode->m_bitMask);
		*segmentId += firstFreeIndexInnerNode * 64; // 64^1 where 1 is depth
		//std::cout << "d" << depth << " i" << firstFreeIndexInnerNode << " ";
		MemoryManagerLeafNode * const targetChildLeafNode = &childLeafNodes[firstFreeIndexInnerNode];
		const unsigned int firstFreeIndexLeaf = boost::multiprecision::detail::find_lsb<boost::uint64_t>(targetChildLeafNode->m_bitMask);
		*segmentId += firstFreeIndexLeaf;
		//std::cout << "d0 i" << firstFreeIndexLeaf << " ";
		const boost::uint64_t mask64InnerNode = (((boost::uint64_t)1) << firstFreeIndexInnerNode);
		const boost::uint64_t mask64LeafNode = (((boost::uint64_t)1) << firstFreeIndexLeaf);
		targetChildLeafNode->m_bitMask &= ~mask64LeafNode;
		if (targetChildLeafNode->m_bitMask == 0) { //target child now full, so mark the parent's bit full (0)
			innerNode->m_bitMask &= ~mask64InnerNode;
		}
	}

}

boost::uint32_t MemoryManagerTree::GetAndSetFirstFreeSegmentId() {
	if (m_rootNode.m_bitMask == 0) return UINT32_MAX; //bitmask of zero means full
	boost::uint32_t segmentId = 0;
	GetAndSetFirstFreeSegmentId(MAX_TREE_DEPTH, &m_rootNode, &segmentId);
	return segmentId;
}

void MemoryManagerTree::FreeSegmentId(const int depth, void *node, boost::uint32_t segmentId, bool *success) {

	MemoryManagerInnerNode * const innerNode = (MemoryManagerInnerNode*)node;
	if (depth > 1) { // this is an inner node that has children that are also inner nodes

		const unsigned int index = (segmentId >> (depth * 6)) & 63;
		const boost::uint64_t mask64 = (((boost::uint64_t)1) << index);
		MemoryManagerInnerNode * const childInnerNodes = (MemoryManagerInnerNode*)innerNode->m_childNodes;
		MemoryManagerInnerNode * const targetChildInnerNode = &childInnerNodes[index];
		FreeSegmentId(depth - 1, targetChildInnerNode, segmentId, success);
		//target child is definitely not full, so mark the parent's bit not full (1)
		//0=full, 1=empty or partially filled (not full)
		innerNode->m_bitMask |= mask64; //set bit to 1 (not full)
	}
	else { //depth == 1 , this is an inner node that has children that are leaf nodes
		MemoryManagerLeafNode * const childLeafNodes = (MemoryManagerLeafNode*)innerNode->m_childNodes;
		const unsigned int indexInnerNode = (segmentId >> 6) & 63; //depth=1
		MemoryManagerLeafNode * const targetChildLeafNode = &childLeafNodes[indexInnerNode];
		const unsigned int indexLeafNode = segmentId & 63; //depth=0
		const boost::uint64_t mask64LeafNode = (((boost::uint64_t)1) << indexLeafNode);
		if (targetChildLeafNode->m_bitMask & mask64LeafNode) { //leaf bit is already 1 (empty)
			*success = false; //error
		}
		else { //leaf bit 0 so delete it by setting it to 1
			targetChildLeafNode->m_bitMask |= mask64LeafNode;
			//target child is now definitely not full, so mark the parent's bit not full (1)
			//0=full, 1=empty or partially filled (not full)
			const boost::uint64_t mask64InnerNode = (((boost::uint64_t)1) << indexInnerNode);
			innerNode->m_bitMask |= mask64InnerNode; //set bit to 1 (not full)

		}
	}

}

bool MemoryManagerTree::FreeSegmentId(boost::uint32_t segmentId) {
	bool success = true;
	FreeSegmentId(MAX_TREE_DEPTH, &m_rootNode, segmentId, &success);
	return success;
}


#include <boost/scoped_ptr.hpp>
#include <iostream>
#include <queue>

template<typename T>
class TreeNode {
public:
  TreeNode(const T& n, TreeNode* left = NULL, TreeNode* right = NULL)
    : mValue(n),
      mLeft(left),
      mRight(right) {}

  T getValue() const {
    return mValue;
  }

  TreeNode* left() const {
    return mLeft.get();
  }

  TreeNode* right() const {
    return mRight.get();
  }

  void preorderTraverse() const {
    std::cout << " " << getValue();
    if(mLeft)  { mLeft->preorderTraverse();  }
    if(mRight) { mRight->preorderTraverse(); }
  }

  void inorderTraverse() const {
    if(mLeft)  { mLeft->inorderTraverse();  }
    std::cout << " " << getValue();
    if(mRight) { mRight->inorderTraverse(); }
  }

  void postorderTraverse() const {
    if(mLeft)  { mLeft->postorderTraverse();  }
    if(mRight) { mRight->postorderTraverse(); }
    std::cout << " " << getValue();
  }

  void levelorderTraverse() const {
    std::queue<const TreeNode*> q;
    q.push(this);

    while(!q.empty()) {
      const TreeNode* n = q.front();
      q.pop();
      std::cout << " " << n->getValue();

      if(n->left())  { q.push(n->left());  }
      if(n->right()) { q.push(n->right()); }
    }
  }

protected:
  T mValue;
  boost::scoped_ptr<TreeNode> mLeft;
  boost::scoped_ptr<TreeNode> mRight;

private:
  TreeNode();
};

int main() {
  TreeNode<int> root(1,
    new TreeNode<int>(2,
      new TreeNode<int>(4,
        new TreeNode<int>(7)),
      new TreeNode<int>(5)),
    new TreeNode<int>(3,
      new TreeNode<int>(6,
        new TreeNode<int>(8),
        new TreeNode<int>(9))));

  std::cout << "preorder:   ";
  root.preorderTraverse();
  std::cout << std::endl;

  std::cout << "inorder:    ";
  root.inorderTraverse();
  std::cout << std::endl;

  std::cout << "postorder:  ";
  root.postorderTraverse();
  std::cout << std::endl;

  std::cout << "level-order:";
  root.levelorderTraverse();
  std::cout << std::endl;

  return 0;
}


#ifndef TESTDEATHCLOCKSRNMODEL_HPP_
#define TESTDEATHCLOCKSRNMODEL_HPP_

#include <cxxtest/TestSuite.h>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

#include <fstream>

#include "AbstractCellBasedTestSuite.hpp"
#include "WildTypeCellMutationState.hpp"
#include "DifferentiatedCellProliferativeType.hpp"
#include "TransitCellProliferativeType.hpp"
#include "UniformG1GenerationalCellCycleModel.hpp"
#include "UniformCellCycleModel.hpp"
#include "SmartPointers.hpp"
#include "DeathClockSrnModel.hpp"

// This test is always run sequentially (never in parallel)
#include "FakePetscSetup.hpp"

class TestDeathClockSrnModel : public AbstractCellBasedTestSuite
{
    public:

        void TestDeathClockSrnBaseRate()
        {
            TS_ASSERT_THROWS_NOTHING(DeathClockSrnModel srn_model);

            DeathClockSrnModel* p_srn_model = new DeathClockSrnModel();

            // Create a vector of initial conditions
            std::vector<double> starter_conditions;
            starter_conditions.push_back(0.5);
            p_srn_model->SetInitialConditions(starter_conditions);

            UniformG1GenerationalCellCycleModel* p_cc_model = new
                UniformG1GenerationalCellCycleModel();

            MAKE_PTR(WildTypeCellMutationState, p_healthy_state);
            MAKE_PTR(DifferentiatedCellProliferativeType, p_diff_type);

            CellPtr p_cell(new Cell(p_healthy_state, p_cc_model, p_srn_model,
                        false, CellPropertyCollection()));
            p_cell->SetCellProliferativeType(p_diff_type);
            p_cell->InitialiseCellCycleModel();
            p_cell->InitialiseSrnModel();

            // Now updated to initial conditions
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 0.5, 1e-4);

            // Set base rate to 3.0
            p_srn_model->SetBaseRate(3.0);
            TS_ASSERT_DELTA(p_srn_model->GetBaseRate(), 3.0, 1e-4);

            // Set number of neighbours to 0.0
            p_cell->GetCellData()->SetItem("number of neighbours", 0.0);
            p_cell->GetCellData()->SetItem("number of neighbours g2", 0.0);
            p_cell->GetCellData()->SetItem("number of neighbours apoptosis", 0.0);

            // Set shared edge length to 0.0
            p_cell->GetCellData()->SetItem("shared edge length", 0.0);
            p_cell->GetCellData()->SetItem("shared edge length g2", 0.0);
            p_cell->GetCellData()->SetItem("shared edge length apoptosis", 0.0);

            // Now update the SRN
            SimulationTime* p_simulation_time = SimulationTime::Instance();
            unsigned num_steps = 100;
            double end_time = 10.0;
            p_simulation_time->SetEndTimeAndNumberOfTimeSteps(end_time, num_steps);

            while (p_simulation_time->GetTime() < end_time)
            {
                p_simulation_time->IncrementTimeOneStep();
                p_srn_model->SimulateToCurrentTime();
            }

            // Test evolved as expected
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 30.5, 1e-4);
        }

        void TestDeathClockSrnNeighbours()
        {
            TS_ASSERT_THROWS_NOTHING(DeathClockSrnModel srn_model);

            DeathClockSrnModel* p_srn_model = new DeathClockSrnModel();

            // Create a vector of initial conditions
            std::vector<double> starter_conditions;
            starter_conditions.push_back(0.5);
            p_srn_model->SetInitialConditions(starter_conditions);

            UniformG1GenerationalCellCycleModel* p_cc_model = new
                UniformG1GenerationalCellCycleModel();

            MAKE_PTR(WildTypeCellMutationState, p_healthy_state);
            MAKE_PTR(DifferentiatedCellProliferativeType, p_diff_type);

            CellPtr p_cell(new Cell(p_healthy_state, p_cc_model, p_srn_model,
                        false, CellPropertyCollection()));
            p_cell->SetCellProliferativeType(p_diff_type);
            p_cell->InitialiseCellCycleModel();
            p_cell->InitialiseSrnModel();

            // Now updated to initial conditions
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 0.5, 1e-4);

            // Set neighbours constant to 3.0
            p_srn_model->SetNeighboursConstant(3.0);
            TS_ASSERT_DELTA(p_srn_model->GetNeighboursConstant(), 3.0, 1e-4);

            // Set number of neighbours artifically to 2.0
            p_cell->GetCellData()->SetItem("number of neighbours", 2);
            p_cell->GetCellData()->SetItem("number of neighbours g2", 0.0);
            p_cell->GetCellData()->SetItem("number of neighbours apoptosis", 0.0);

            // Set shared edge length to 0.0
            p_cell->GetCellData()->SetItem("shared edge length", 0.0);
            p_cell->GetCellData()->SetItem("shared edge length g2", 0.0);
            p_cell->GetCellData()->SetItem("shared edge length apoptosis", 0.0);

            // Now update the SRN
            SimulationTime* p_simulation_time = SimulationTime::Instance();
            unsigned num_steps = 100;
            double end_time = 10.0;
            p_simulation_time->SetEndTimeAndNumberOfTimeSteps(end_time, num_steps);

            while (p_simulation_time->GetTime() < end_time)
            {
                p_simulation_time->IncrementTimeOneStep();
                p_srn_model->SimulateToCurrentTime();
            }

            // Test evolved as expected
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 60.5, 1e-4);
        }

        void TestDeathClockSrnNeighboursG2()
        {
            TS_ASSERT_THROWS_NOTHING(DeathClockSrnModel srn_model);

            DeathClockSrnModel* p_srn_model = new DeathClockSrnModel();

            // Create a vector of initial conditions
            std::vector<double> starter_conditions;
            starter_conditions.push_back(0.5);
            p_srn_model->SetInitialConditions(starter_conditions);

            UniformG1GenerationalCellCycleModel* p_cc_model = new
                UniformG1GenerationalCellCycleModel();

            MAKE_PTR(WildTypeCellMutationState, p_healthy_state);
            MAKE_PTR(DifferentiatedCellProliferativeType, p_diff_type);

            CellPtr p_cell(new Cell(p_healthy_state, p_cc_model, p_srn_model,
                        false, CellPropertyCollection()));
            p_cell->SetCellProliferativeType(p_diff_type);
            p_cell->InitialiseCellCycleModel();
            p_cell->InitialiseSrnModel();

            // Now updated to initial conditions
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 0.5, 1e-4);

            // Set neighbours g2 constant to 3.0
            p_srn_model->SetNeighboursG2Constant(3.0);
            TS_ASSERT_DELTA(p_srn_model->GetNeighboursG2Constant(), 3.0, 1e-4);

            // Set number of neighbours G2 artifically to 2.0
            p_cell->GetCellData()->SetItem("number of neighbours", 0.0);
            p_cell->GetCellData()->SetItem("number of neighbours g2", 2.0);
            p_cell->GetCellData()->SetItem("number of neighbours apoptosis", 0.0);

            // Set shared edge length to 0.0
            p_cell->GetCellData()->SetItem("shared edge length", 0.0);
            p_cell->GetCellData()->SetItem("shared edge length g2", 0.0);
            p_cell->GetCellData()->SetItem("shared edge length apoptosis", 0.0);

            // Now update the SRN
            SimulationTime* p_simulation_time = SimulationTime::Instance();
            unsigned num_steps = 100;
            double end_time = 10.0;
            p_simulation_time->SetEndTimeAndNumberOfTimeSteps(end_time, num_steps);

            while (p_simulation_time->GetTime() < end_time)
            {
                p_simulation_time->IncrementTimeOneStep();
                p_srn_model->SimulateToCurrentTime();
            }

            // Test evolved as expected
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 60.5, 1e-4);
        }

        void TestDeathClockSrnNeighboursApoptosis()
        {
            TS_ASSERT_THROWS_NOTHING(DeathClockSrnModel srn_model);

            DeathClockSrnModel* p_srn_model = new DeathClockSrnModel();

            // Create a vector of initial conditions
            std::vector<double> starter_conditions;
            starter_conditions.push_back(0.5);
            p_srn_model->SetInitialConditions(starter_conditions);

            UniformG1GenerationalCellCycleModel* p_cc_model = new
                UniformG1GenerationalCellCycleModel();

            MAKE_PTR(WildTypeCellMutationState, p_healthy_state);
            MAKE_PTR(DifferentiatedCellProliferativeType, p_diff_type);

            CellPtr p_cell(new Cell(p_healthy_state, p_cc_model, p_srn_model,
                        false, CellPropertyCollection()));
            p_cell->SetCellProliferativeType(p_diff_type);
            p_cell->InitialiseCellCycleModel();
            p_cell->InitialiseSrnModel();

            // Now updated to initial conditions
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 0.5, 1e-4);

            // Set neighbours apoptosis constant to 3.0
            p_srn_model->SetNeighboursApoptosisConstant(3.0);
            TS_ASSERT_DELTA(p_srn_model->GetNeighboursApoptosisConstant(), 3.0, 1e-4);

            // Set number of neighbours apoptosis artifically to 2.0
            p_cell->GetCellData()->SetItem("number of neighbours", 0.0);
            p_cell->GetCellData()->SetItem("number of neighbours g2", 0.0);
            p_cell->GetCellData()->SetItem("number of neighbours apoptosis", 2.0);

            // Set shared edge length to 0.0
            p_cell->GetCellData()->SetItem("shared edge length", 0.0);
            p_cell->GetCellData()->SetItem("shared edge length g2", 0.0);
            p_cell->GetCellData()->SetItem("shared edge length apoptosis", 0.0);

            // Now update the SRN
            SimulationTime* p_simulation_time = SimulationTime::Instance();
            unsigned num_steps = 100;
            double end_time = 10.0;
            p_simulation_time->SetEndTimeAndNumberOfTimeSteps(end_time, num_steps);

            while (p_simulation_time->GetTime() < end_time)
            {
                p_simulation_time->IncrementTimeOneStep();
                p_srn_model->SimulateToCurrentTime();
            }

            // Test evolved as expected
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 60.5, 1e-4);
        }

        void TestDeathClockSrnNormalisedNeighboursG2()
        {
            TS_ASSERT_THROWS_NOTHING(DeathClockSrnModel srn_model);

            DeathClockSrnModel* p_srn_model = new DeathClockSrnModel();

            // Create a vector of initial conditions
            std::vector<double> starter_conditions;
            starter_conditions.push_back(0.5);
            p_srn_model->SetInitialConditions(starter_conditions);

            UniformG1GenerationalCellCycleModel* p_cc_model = new
                UniformG1GenerationalCellCycleModel();

            MAKE_PTR(WildTypeCellMutationState, p_healthy_state);
            MAKE_PTR(DifferentiatedCellProliferativeType, p_diff_type);

            CellPtr p_cell(new Cell(p_healthy_state, p_cc_model, p_srn_model,
                        false, CellPropertyCollection()));
            p_cell->SetCellProliferativeType(p_diff_type);
            p_cell->InitialiseCellCycleModel();
            p_cell->InitialiseSrnModel();

            // Now updated to initial conditions
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 0.5, 1e-4);

            // Set normalised neighbours g2 constant to 3.0
            p_srn_model->SetNormalisedNeighboursG2Constant(3.0);
            TS_ASSERT_DELTA(p_srn_model->GetNormalisedNeighboursG2Constant(), 3.0, 1e-4);

            // Set number of neighbours G2 artifically to 2.0
            p_cell->GetCellData()->SetItem("number of neighbours", 5.0);
            p_cell->GetCellData()->SetItem("number of neighbours g2", 2.0);
            p_cell->GetCellData()->SetItem("number of neighbours apoptosis", 0.0);

            // Set shared edge length to 0.0
            p_cell->GetCellData()->SetItem("shared edge length", 0.0);
            p_cell->GetCellData()->SetItem("shared edge length g2", 0.0);
            p_cell->GetCellData()->SetItem("shared edge length apoptosis", 0.0);

            // Now update the SRN
            SimulationTime* p_simulation_time = SimulationTime::Instance();
            unsigned num_steps = 100;
            double end_time = 10.0;
            p_simulation_time->SetEndTimeAndNumberOfTimeSteps(end_time, num_steps);

            while (p_simulation_time->GetTime() < end_time)
            {
                p_simulation_time->IncrementTimeOneStep();
                p_srn_model->SimulateToCurrentTime();
            }

            // Test evolved as expected
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 12.5, 1e-4);
        }

        void TestDeathClockSrnNormalisedNeighboursApoptosis()
        {
            TS_ASSERT_THROWS_NOTHING(DeathClockSrnModel srn_model);

            DeathClockSrnModel* p_srn_model = new DeathClockSrnModel();

            // Create a vector of initial conditions
            std::vector<double> starter_conditions;
            starter_conditions.push_back(0.5);
            p_srn_model->SetInitialConditions(starter_conditions);

            UniformG1GenerationalCellCycleModel* p_cc_model = new
                UniformG1GenerationalCellCycleModel();

            MAKE_PTR(WildTypeCellMutationState, p_healthy_state);
            MAKE_PTR(DifferentiatedCellProliferativeType, p_diff_type);

            CellPtr p_cell(new Cell(p_healthy_state, p_cc_model, p_srn_model,
                        false, CellPropertyCollection()));
            p_cell->SetCellProliferativeType(p_diff_type);
            p_cell->InitialiseCellCycleModel();
            p_cell->InitialiseSrnModel();

            // Now updated to initial conditions
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 0.5, 1e-4);

            // Set normalised neighbours apoptosis constant to 3.0
            p_srn_model->SetNormalisedNeighboursApoptosisConstant(3.0);
            TS_ASSERT_DELTA(p_srn_model->GetNormalisedNeighboursApoptosisConstant(), 3.0, 1e-4);

            // Set number of neighbours apoptosis artifically to 2.0
            p_cell->GetCellData()->SetItem("number of neighbours", 5.0);
            p_cell->GetCellData()->SetItem("number of neighbours g2", 0.0);
            p_cell->GetCellData()->SetItem("number of neighbours apoptosis", 2.0);

            // Set shared edge length to 0.0
            p_cell->GetCellData()->SetItem("shared edge length", 0.0);
            p_cell->GetCellData()->SetItem("shared edge length g2", 0.0);
            p_cell->GetCellData()->SetItem("shared edge length apoptosis", 0.0);

            // Now update the SRN
            SimulationTime* p_simulation_time = SimulationTime::Instance();
            unsigned num_steps = 100;
            double end_time = 10.0;
            p_simulation_time->SetEndTimeAndNumberOfTimeSteps(end_time, num_steps);

            while (p_simulation_time->GetTime() < end_time)
            {
                p_simulation_time->IncrementTimeOneStep();
                p_srn_model->SimulateToCurrentTime();
            }

            // Test evolved as expected
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 12.5, 1e-4);
        }

        void TestDeathClockSrnSharedEdge()
        {
            TS_ASSERT_THROWS_NOTHING(DeathClockSrnModel srn_model);

            DeathClockSrnModel* p_srn_model = new DeathClockSrnModel();

            // Create a vector of initial conditions
            std::vector<double> starter_conditions;
            starter_conditions.push_back(0.5);
            p_srn_model->SetInitialConditions(starter_conditions);

            UniformG1GenerationalCellCycleModel* p_cc_model = new
                UniformG1GenerationalCellCycleModel();

            MAKE_PTR(WildTypeCellMutationState, p_healthy_state);
            MAKE_PTR(DifferentiatedCellProliferativeType, p_diff_type);

            CellPtr p_cell(new Cell(p_healthy_state, p_cc_model, p_srn_model,
                        false, CellPropertyCollection()));
            p_cell->SetCellProliferativeType(p_diff_type);
            p_cell->InitialiseCellCycleModel();
            p_cell->InitialiseSrnModel();

            // Now updated to initial conditions
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 0.5, 1e-4);

            // Set shared edge constant to 3.0
            p_srn_model->SetSharedEdgeConstant(3.0);
            TS_ASSERT_DELTA(p_srn_model->GetSharedEdgeConstant(), 3.0, 1e-4);

            // Set number of neighbours to 0
            p_cell->GetCellData()->SetItem("number of neighbours", 0.0);
            p_cell->GetCellData()->SetItem("number of neighbours g2", 0.0);
            p_cell->GetCellData()->SetItem("number of neighbours apoptosis", 0.0);

            // Set shared edge length to artificially to 2.0
            p_cell->GetCellData()->SetItem("shared edge length", 2.5);
            p_cell->GetCellData()->SetItem("shared edge length g2", 0.0);
            p_cell->GetCellData()->SetItem("shared edge length apoptosis", 0.0);

            // Now update the SRN
            SimulationTime* p_simulation_time = SimulationTime::Instance();
            unsigned num_steps = 100;
            double end_time = 10.0;
            p_simulation_time->SetEndTimeAndNumberOfTimeSteps(end_time, num_steps);

            while (p_simulation_time->GetTime() < end_time)
            {
                p_simulation_time->IncrementTimeOneStep();
                p_srn_model->SimulateToCurrentTime();
            }

            // Test evolved as expected
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 75.5, 1e-4);
        }

        void TestDeathClockSrnSharedEdgeG2()
        {
            TS_ASSERT_THROWS_NOTHING(DeathClockSrnModel srn_model);

            DeathClockSrnModel* p_srn_model = new DeathClockSrnModel();

            // Create a vector of initial conditions
            std::vector<double> starter_conditions;
            starter_conditions.push_back(0.5);
            p_srn_model->SetInitialConditions(starter_conditions);

            UniformG1GenerationalCellCycleModel* p_cc_model = new
                UniformG1GenerationalCellCycleModel();

            MAKE_PTR(WildTypeCellMutationState, p_healthy_state);
            MAKE_PTR(DifferentiatedCellProliferativeType, p_diff_type);

            CellPtr p_cell(new Cell(p_healthy_state, p_cc_model, p_srn_model,
                        false, CellPropertyCollection()));
            p_cell->SetCellProliferativeType(p_diff_type);
            p_cell->InitialiseCellCycleModel();
            p_cell->InitialiseSrnModel();

            // Now updated to initial conditions
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 0.5, 1e-4);

            // Set shared edge g2 constant to 3.0
            p_srn_model->SetSharedEdgeG2Constant(3.0);
            TS_ASSERT_DELTA(p_srn_model->GetSharedEdgeG2Constant(), 3.0, 1e-4);

            // Set number of neighbours G2 to 0.0
            p_cell->GetCellData()->SetItem("number of neighbours", 0.0);
            p_cell->GetCellData()->SetItem("number of neighbours g2", 0.0);
            p_cell->GetCellData()->SetItem("number of neighbours apoptosis", 0.0);

            // Set shared edge length G2 to 2.0
            p_cell->GetCellData()->SetItem("shared edge length", 0.0);
            p_cell->GetCellData()->SetItem("shared edge length g2", 2.5);
            p_cell->GetCellData()->SetItem("shared edge length apoptosis", 0.0);

            // Now update the SRN
            SimulationTime* p_simulation_time = SimulationTime::Instance();
            unsigned num_steps = 100;
            double end_time = 10.0;
            p_simulation_time->SetEndTimeAndNumberOfTimeSteps(end_time, num_steps);

            while (p_simulation_time->GetTime() < end_time)
            {
                p_simulation_time->IncrementTimeOneStep();
                p_srn_model->SimulateToCurrentTime();
            }

            // Test evolved as expected
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 75.5, 1e-4);
        }

        void TestDeathClockSrnSharedEdgeApoptosis()
        {
            TS_ASSERT_THROWS_NOTHING(DeathClockSrnModel srn_model);

            DeathClockSrnModel* p_srn_model = new DeathClockSrnModel();

            // Create a vector of initial conditions
            std::vector<double> starter_conditions;
            starter_conditions.push_back(0.5);
            p_srn_model->SetInitialConditions(starter_conditions);

            UniformG1GenerationalCellCycleModel* p_cc_model = new
                UniformG1GenerationalCellCycleModel();

            MAKE_PTR(WildTypeCellMutationState, p_healthy_state);
            MAKE_PTR(DifferentiatedCellProliferativeType, p_diff_type);

            CellPtr p_cell(new Cell(p_healthy_state, p_cc_model, p_srn_model,
                        false, CellPropertyCollection()));
            p_cell->SetCellProliferativeType(p_diff_type);
            p_cell->InitialiseCellCycleModel();
            p_cell->InitialiseSrnModel();

            // Now updated to initial conditions
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 0.5, 1e-4);

            // Set shared edge apoptosis constant to 3.0
            p_srn_model->SetSharedEdgeApoptosisConstant(3.0);
            TS_ASSERT_DELTA(p_srn_model->GetSharedEdgeApoptosisConstant(), 3.0, 1e-4);

            // Set number of neighbours apoptosis to 0.0
            p_cell->GetCellData()->SetItem("number of neighbours", 0.0);
            p_cell->GetCellData()->SetItem("number of neighbours g2", 0.0);
            p_cell->GetCellData()->SetItem("number of neighbours apoptosis", 0.0);

            // Set shared edge length apoptosis to 0.0
            p_cell->GetCellData()->SetItem("shared edge length", 0.0);
            p_cell->GetCellData()->SetItem("shared edge length g2", 0.0);
            p_cell->GetCellData()->SetItem("shared edge length apoptosis", 2.5);

            // Now update the SRN
            SimulationTime* p_simulation_time = SimulationTime::Instance();
            unsigned num_steps = 100;
            double end_time = 10.0;
            p_simulation_time->SetEndTimeAndNumberOfTimeSteps(end_time, num_steps);

            while (p_simulation_time->GetTime() < end_time)
            {
                p_simulation_time->IncrementTimeOneStep();
                p_srn_model->SimulateToCurrentTime();
            }

            // Test evolved as expected
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 75.5, 1e-4);
        }

        void TestDeathClockSrnNormalisedSharedEdgeG2()
        {
            TS_ASSERT_THROWS_NOTHING(DeathClockSrnModel srn_model);

            DeathClockSrnModel* p_srn_model = new DeathClockSrnModel();

            // Create a vector of initial conditions
            std::vector<double> starter_conditions;
            starter_conditions.push_back(0.5);
            p_srn_model->SetInitialConditions(starter_conditions);

            UniformG1GenerationalCellCycleModel* p_cc_model = new
                UniformG1GenerationalCellCycleModel();

            MAKE_PTR(WildTypeCellMutationState, p_healthy_state);
            MAKE_PTR(DifferentiatedCellProliferativeType, p_diff_type);

            CellPtr p_cell(new Cell(p_healthy_state, p_cc_model, p_srn_model,
                        false, CellPropertyCollection()));
            p_cell->SetCellProliferativeType(p_diff_type);
            p_cell->InitialiseCellCycleModel();
            p_cell->InitialiseSrnModel();

            // Now updated to initial conditions
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 0.5, 1e-4);

            // Set normalised shared edge g2 constant to 3.0
            p_srn_model->SetNormalisedSharedEdgeG2Constant(3.0);
            TS_ASSERT_DELTA(p_srn_model->GetNormalisedSharedEdgeG2Constant(), 3.0, 1e-4);

            // Set number of neighbours G2 to 0.0
            p_cell->GetCellData()->SetItem("number of neighbours", 0.0);
            p_cell->GetCellData()->SetItem("number of neighbours g2", 0.0);
            p_cell->GetCellData()->SetItem("number of neighbours apoptosis", 0.0);

            // Set shared edge length G2 to 2.0
            p_cell->GetCellData()->SetItem("shared edge length", 5.0);
            p_cell->GetCellData()->SetItem("shared edge length g2", 2.5);
            p_cell->GetCellData()->SetItem("shared edge length apoptosis", 0.0);

            // Now update the SRN
            SimulationTime* p_simulation_time = SimulationTime::Instance();
            unsigned num_steps = 100;
            double end_time = 10.0;
            p_simulation_time->SetEndTimeAndNumberOfTimeSteps(end_time, num_steps);

            while (p_simulation_time->GetTime() < end_time)
            {
                p_simulation_time->IncrementTimeOneStep();
                p_srn_model->SimulateToCurrentTime();
            }

            // Test evolved as expected
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 15.5, 1e-4);
        }

        void TestDeathClockSrnNormalisedSharedEdgeApoptosis()
        {
            TS_ASSERT_THROWS_NOTHING(DeathClockSrnModel srn_model);

            DeathClockSrnModel* p_srn_model = new DeathClockSrnModel();

            // Create a vector of initial conditions
            std::vector<double> starter_conditions;
            starter_conditions.push_back(0.5);
            p_srn_model->SetInitialConditions(starter_conditions);

            UniformG1GenerationalCellCycleModel* p_cc_model = new
                UniformG1GenerationalCellCycleModel();

            MAKE_PTR(WildTypeCellMutationState, p_healthy_state);
            MAKE_PTR(DifferentiatedCellProliferativeType, p_diff_type);

            CellPtr p_cell(new Cell(p_healthy_state, p_cc_model, p_srn_model,
                        false, CellPropertyCollection()));
            p_cell->SetCellProliferativeType(p_diff_type);
            p_cell->InitialiseCellCycleModel();
            p_cell->InitialiseSrnModel();

            // Now updated to initial conditions
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 0.5, 1e-4);

            // Set normalised shared edge apoptosis constant to 3.0
            p_srn_model->SetNormalisedSharedEdgeApoptosisConstant(3.0);
            TS_ASSERT_DELTA(p_srn_model->GetNormalisedSharedEdgeApoptosisConstant(), 3.0, 1e-4);

            // Set number of neighbours apoptosis to 0.0
            p_cell->GetCellData()->SetItem("number of neighbours", 0.0);
            p_cell->GetCellData()->SetItem("number of neighbours g2", 0.0);
            p_cell->GetCellData()->SetItem("number of neighbours apoptosis", 0.0);

            // Set shared edge length to 0.0
            p_cell->GetCellData()->SetItem("shared edge length", 5.0);
            p_cell->GetCellData()->SetItem("shared edge length g2", 0.0);
            p_cell->GetCellData()->SetItem("shared edge length apoptosis", 2.5);

            // Now update the SRN
            SimulationTime* p_simulation_time = SimulationTime::Instance();
            unsigned num_steps = 100;
            double end_time = 10.0;
            p_simulation_time->SetEndTimeAndNumberOfTimeSteps(end_time, num_steps);

            while (p_simulation_time->GetTime() < end_time)
            {
                p_simulation_time->IncrementTimeOneStep();
                p_srn_model->SimulateToCurrentTime();
            }

            // Test evolved as expected
            TS_ASSERT_DELTA(p_srn_model->GetTau(), 15.5, 1e-4);
        }

        void TestDeathClockSrnCreateCopy()
        {
            // Test with DeathClockSrnModel
            DeathClockSrnModel* p_model= new DeathClockSrnModel;

            // Set ODE system
            std::vector<double> state_variables;
            state_variables.push_back(2.0);
            p_model->SetOdeSystem(new DeathClockOdeSystem(state_variables));

            p_model->SetInitialConditions(state_variables);

            // Create a copy
            DeathClockSrnModel* p_model2 = static_cast<DeathClockSrnModel*>
                (p_model->CreateSrnModel());

            // Check correct initializations
            TS_ASSERT_EQUALS(p_model2->GetTau(), 2.0);

            // Destroy models
            delete p_model;
            delete p_model2;
        }

        void TestArchiveDeathClockSrnModel()
        {
            OutputFileHandler handler("archive", false);
            std::string archive_filename = handler.GetOutputDirectoryFullPath()
                + "death_clock_srn.arch";

            // Create an output archive
            {
                SimulationTime* p_simulation_time = SimulationTime::Instance();
                p_simulation_time->SetEndTimeAndNumberOfTimeSteps(2.0, 4);

                UniformCellCycleModel* p_cc_model = new UniformCellCycleModel();

                // As usual, we archive via a pointer to the most abstract class possible
                AbstractSrnModel* p_srn_model = new DeathClockSrnModel;

                MAKE_PTR(WildTypeCellMutationState, p_healthy_state);
                MAKE_PTR(TransitCellProliferativeType, p_transit_type);

                // We must create a cell to be able to initialise the cell srn model's ODE system
                CellPtr p_cell(new Cell(p_healthy_state, p_cc_model, p_srn_model));
                p_cell->SetCellProliferativeType(p_transit_type);
                p_cell->InitialiseCellCycleModel();
                p_cell->InitialiseSrnModel();
                p_cell->SetBirthTime(0.0);

                static_cast<DeathClockSrnModel*>(p_srn_model)->SetBaseRate(0.5);

                static_cast<DeathClockSrnModel*>(p_srn_model)->SetNeighboursConstant(0.6);
                static_cast<DeathClockSrnModel*>(p_srn_model)->SetNeighboursG2Constant(0.7);
                static_cast<DeathClockSrnModel*>(p_srn_model)->SetNeighboursApoptosisConstant(0.8);
                static_cast<DeathClockSrnModel*>(p_srn_model)->SetNormalisedNeighboursG2Constant(0.9);
                static_cast<DeathClockSrnModel*>(p_srn_model)->SetNormalisedNeighboursApoptosisConstant(1.0);

                static_cast<DeathClockSrnModel*>(p_srn_model)->SetSharedEdgeConstant(1.6);
                static_cast<DeathClockSrnModel*>(p_srn_model)->SetSharedEdgeG2Constant(1.7);
                static_cast<DeathClockSrnModel*>(p_srn_model)->SetSharedEdgeApoptosisConstant(1.8);
                static_cast<DeathClockSrnModel*>(p_srn_model)->SetNormalisedSharedEdgeG2Constant(1.9);
                static_cast<DeathClockSrnModel*>(p_srn_model)->SetNormalisedSharedEdgeApoptosisConstant(2.0);

                std::ofstream ofs(archive_filename.c_str());
                boost::archive::text_oarchive output_arch(ofs);

                output_arch << p_srn_model;

                // Note that here, deletion of the cell-cycle model and srn is handled by the cell destructor
                SimulationTime::Destroy();
            }

            {
                // We must set SimulationTime::mStartTime here to avoid tripping an assertion
                SimulationTime::Instance()->SetStartTime(0.0);

                AbstractSrnModel* p_srn_model;

                std::ifstream ifs(archive_filename.c_str(), std::ios::binary);
                boost::archive::text_iarchive input_arch(ifs);

                input_arch >> p_srn_model;

                TS_ASSERT_DELTA(static_cast<DeathClockSrnModel*>(p_srn_model)->GetBaseRate(), 0.5, 1e-12);

                TS_ASSERT_DELTA(static_cast<DeathClockSrnModel*>(p_srn_model)->GetNeighboursConstant(), 0.6, 1e-12);
                TS_ASSERT_DELTA(static_cast<DeathClockSrnModel*>(p_srn_model)->GetNeighboursG2Constant(), 0.7, 1e-12);
                TS_ASSERT_DELTA(static_cast<DeathClockSrnModel*>(p_srn_model)->GetNeighboursApoptosisConstant(), 0.8, 1e-12);
                TS_ASSERT_DELTA(static_cast<DeathClockSrnModel*>(p_srn_model)->GetNormalisedNeighboursG2Constant(), 0.9, 1e-12);
                TS_ASSERT_DELTA(static_cast<DeathClockSrnModel*>(p_srn_model)->GetNormalisedNeighboursApoptosisConstant(), 1.0, 1e-12);

                TS_ASSERT_DELTA(static_cast<DeathClockSrnModel*>(p_srn_model)->GetSharedEdgeConstant(), 1.6, 1e-12);
                TS_ASSERT_DELTA(static_cast<DeathClockSrnModel*>(p_srn_model)->GetSharedEdgeG2Constant(), 1.7, 1e-12);
                TS_ASSERT_DELTA(static_cast<DeathClockSrnModel*>(p_srn_model)->GetSharedEdgeApoptosisConstant(), 1.8, 1e-12);
                TS_ASSERT_DELTA(static_cast<DeathClockSrnModel*>(p_srn_model)->GetNormalisedSharedEdgeG2Constant(), 1.9, 1e-12);
                TS_ASSERT_DELTA(static_cast<DeathClockSrnModel*>(p_srn_model)->GetNormalisedSharedEdgeApoptosisConstant(), 2.0, 1e-12);

                delete p_srn_model;
            }
        }
};

#endif // TESTDEATHCLOCKSRNMODEL_HPP_


// -------------- test the visual odometry -------------
#include <fstream>
#include <boost/timer.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/viz.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "myslam/config.h"
#include "myslam/visual_odometry.h"

int main ( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"usage: run_vo parameter_file"<<endl;
        return 1;
    }

    myslam::Config::setParameterFile ( argv[1] );
    myslam::VisualOdometry::Ptr vo ( new myslam::VisualOdometry );

    string dataset_dir = myslam::Config::get<string> ( "dataset_dir" );
    cout<<"dataset: "<<dataset_dir<<endl;
    ifstream fin ( dataset_dir+"/associate.txt" );
    if ( !fin )
    {
        cout<<"please generate the associate file called associate.txt!"<<endl;
        return 1;
    }

    vector<string> rgb_files, depth_files;
    vector<double> rgb_times, depth_times;
    while ( !fin.eof() )
    {
        string rgb_time, rgb_file, depth_time, depth_file;
        fin>>rgb_time>>rgb_file>>depth_time>>depth_file;
        rgb_times.push_back ( atof ( rgb_time.c_str() ) );
        depth_times.push_back ( atof ( depth_time.c_str() ) );
        rgb_files.push_back ( dataset_dir+"/"+rgb_file );
        depth_files.push_back ( dataset_dir+"/"+depth_file );

        if ( fin.good() == false )
            break;
    }

    myslam::Camera::Ptr camera ( new myslam::Camera );

    // visualization
    cv::viz::Viz3d vis ( "Visual Odometry" );
    cv::viz::WCoordinateSystem world_coor ( 1.0 ), camera_coor ( 0.5 );
    cv::Point3d cam_pos ( 0, -1.0, -1.0 ), cam_focal_point ( 0,0,0 ), cam_y_dir ( 0,1,0 );
    cv::Affine3d cam_pose = cv::viz::makeCameraPose ( cam_pos, cam_focal_point, cam_y_dir );
    vis.setViewerPose ( cam_pose );

    world_coor.setRenderingProperty ( cv::viz::LINE_WIDTH, 2.0 );
    camera_coor.setRenderingProperty ( cv::viz::LINE_WIDTH, 1.0 );
    vis.showWidget ( "World", world_coor );
    vis.showWidget ( "Camera", camera_coor );

    cout<<"read total "<<rgb_files.size() <<" entries"<<endl;
    for ( int i=0; i<rgb_files.size(); i++ )
    {
        cout<<"****** loop "<<i<<" ******"<<endl;
        Mat color = cv::imread ( rgb_files[i] );
        Mat depth = cv::imread ( depth_files[i], -1 );
        if ( color.data==nullptr || depth.data==nullptr )
            break;
        myslam::Frame::Ptr pFrame = myslam::Frame::createFrame();
        pFrame->camera_ = camera;
        pFrame->color_ = color;
        pFrame->depth_ = depth;
        pFrame->time_stamp_ = rgb_times[i];

        boost::timer timer;
        vo->addFrame ( pFrame );
        cout<<"VO costs time: "<<timer.elapsed() <<endl;

        if ( vo->state_ == myslam::VisualOdometry::LOST )
            break;
        SE3 Twc = pFrame->T_c_w_.inverse();

        // show the map and the camera pose
        cv::Affine3d M (
            cv::Affine3d::Mat3 (
                Twc.rotation_matrix() ( 0,0 ), Twc.rotation_matrix() ( 0,1 ), Twc.rotation_matrix() ( 0,2 ),
                Twc.rotation_matrix() ( 1,0 ), Twc.rotation_matrix() ( 1,1 ), Twc.rotation_matrix() ( 1,2 ),
                Twc.rotation_matrix() ( 2,0 ), Twc.rotation_matrix() ( 2,1 ), Twc.rotation_matrix() ( 2,2 )
            ),
            cv::Affine3d::Vec3 (
                Twc.translation() ( 0,0 ), Twc.translation() ( 1,0 ), Twc.translation() ( 2,0 )
            )
        );

        Mat img_show = color.clone();
        for ( auto& pt:vo->map_->map_points_ )
        {
            myslam::MapPoint::Ptr p = pt.second;
            Vector2d pixel = pFrame->camera_->world2pixel ( p->pos_, pFrame->T_c_w_ );
            cv::circle ( img_show, cv::Point2f ( pixel ( 0,0 ),pixel ( 1,0 ) ), 5, cv::Scalar ( 0,255,0 ), 2 );
        }

        cv::imshow ( "image", img_show );
        cv::waitKey ( 1 );
        vis.setWidgetPose ( "Camera", M );
        vis.spinOnce ( 1, false );
        cout<<endl;
    }

    return 0;
}