//               Copyright Marek Dalewski 2017
// Distributed under the Boost Software License, Version 1.0.
//        (See accompanying file LICENSE.md or copy at
//           http://www.boost.org/LICENSE_1_0.txt)

#include <commander/pack_position.hpp>

#include <boost/test/unit_test.hpp>

#include <commander/algorithm.hpp>

#include <algorithm>
#include <iterator>
#include <type_traits>
#include <vector>

#define PP_MSG(pack) "{" << (pack).is_first << ',' << (pack).exact << ',' << (pack).is_last << "}"
#define PP_OP_PP_MSG(lhs, op, rhs) PP_MSG(lhs) << " " << (op) << " " << PP_MSG(rhs)

template <typename It, typename Callback, typename... Indexes>
void each_except(It begin, It end, Callback callback, Indexes... indexes)
{
    size_t pos = size_t(-1);
    std::for_each(begin, end, [&](const auto& v) {
        if (!commander::compare_disjunction(++pos, size_t(-1), size_t(indexes)...))
            callback(v);
    });
}

template <typename It, typename Callback, typename... Indexes>
void each_of(It begin, It end, Callback callback, Indexes... indexes)
{
    size_t pos = size_t(-1);
    std::for_each(begin, end, [&](const auto& v) {
        if (commander::compare_disjunction(++pos, size_t(-1), size_t(indexes)...))
            callback(v);
    });
}

BOOST_AUTO_TEST_CASE( pack_position )
{
    namespace com = commander;

    std::vector<com::pack_position<size_t>> pos;

    pos.push_back(com::pack_position<size_t> {false, 0, false});
    pos.push_back(com::pack_position<size_t> {false, 0, true });
    pos.push_back(com::pack_position<size_t> {true,  0, false});
    pos.push_back(com::pack_position<size_t> {true,  0, true });

    pos.push_back(com::pack_position<size_t> {false, 1, false});
    pos.push_back(com::pack_position<size_t> {false, 1, true });
    pos.push_back(com::pack_position<size_t> {true,  1, false});
    pos.push_back(com::pack_position<size_t> {true,  1, true });

    pos.push_back(com::pack_position<size_t> {false, 2, false});
    pos.push_back(com::pack_position<size_t> {false, 2, true });
    pos.push_back(com::pack_position<size_t> {true,  2, false});
    pos.push_back(com::pack_position<size_t> {true,  2, true });

    pos.push_back(com::pack_position<size_t> {false, 3, false});
    pos.push_back(com::pack_position<size_t> {false, 3, true });
    pos.push_back(com::pack_position<size_t> {true,  3, false});
    pos.push_back(com::pack_position<size_t> {true,  3, true });

    {
        static_assert(std::is_same_v<int, com::pack_position<int>::size_type>);
        static_assert(std::is_same_v<size_t, com::pack_position<size_t>::size_type>);
    }

    {
        BOOST_CHECK(pos.at( 0).is_first == false); BOOST_CHECK(pos.at( 0).exact == 0); BOOST_CHECK(pos.at( 0).is_last == false);
        BOOST_CHECK(pos.at( 1).is_first == false); BOOST_CHECK(pos.at( 1).exact == 0); BOOST_CHECK(pos.at( 1).is_last == true);
        BOOST_CHECK(pos.at( 2).is_first == true);  BOOST_CHECK(pos.at( 2).exact == 0); BOOST_CHECK(pos.at( 2).is_last == false);
        BOOST_CHECK(pos.at( 3).is_first == true);  BOOST_CHECK(pos.at( 3).exact == 0); BOOST_CHECK(pos.at( 3).is_last == true);
        BOOST_CHECK(pos.at( 4).is_first == false); BOOST_CHECK(pos.at( 4).exact == 1); BOOST_CHECK(pos.at( 4).is_last == false);
        BOOST_CHECK(pos.at( 5).is_first == false); BOOST_CHECK(pos.at( 5).exact == 1); BOOST_CHECK(pos.at( 5).is_last == true);
        BOOST_CHECK(pos.at( 6).is_first == true);  BOOST_CHECK(pos.at( 6).exact == 1); BOOST_CHECK(pos.at( 6).is_last == false);
        BOOST_CHECK(pos.at( 7).is_first == true);  BOOST_CHECK(pos.at( 7).exact == 1); BOOST_CHECK(pos.at( 7).is_last == true);
        BOOST_CHECK(pos.at( 8).is_first == false); BOOST_CHECK(pos.at( 8).exact == 2); BOOST_CHECK(pos.at( 8).is_last == false);
        BOOST_CHECK(pos.at( 9).is_first == false); BOOST_CHECK(pos.at( 9).exact == 2); BOOST_CHECK(pos.at( 9).is_last == true);
        BOOST_CHECK(pos.at(10).is_first == true);  BOOST_CHECK(pos.at(10).exact == 2); BOOST_CHECK(pos.at(10).is_last == false);
        BOOST_CHECK(pos.at(11).is_first == true);  BOOST_CHECK(pos.at(11).exact == 2); BOOST_CHECK(pos.at(11).is_last == true);
        BOOST_CHECK(pos.at(12).is_first == false); BOOST_CHECK(pos.at(12).exact == 3); BOOST_CHECK(pos.at(12).is_last == false);
        BOOST_CHECK(pos.at(13).is_first == false); BOOST_CHECK(pos.at(13).exact == 3); BOOST_CHECK(pos.at(13).is_last == true);
        BOOST_CHECK(pos.at(14).is_first == true);  BOOST_CHECK(pos.at(14).exact == 3); BOOST_CHECK(pos.at(14).is_last == false);
        BOOST_CHECK(pos.at(15).is_first == true);  BOOST_CHECK(pos.at(15).exact == 3); BOOST_CHECK(pos.at(15).is_last == true);
    }

    {
        for (size_t cur_idx = 0; cur_idx < 4; ++cur_idx) {
            const auto& cur = pos.at(cur_idx);
            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur == p) == true,  PP_OP_PP_MSG(cur, "==", p)); }, cur_idx);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur == p) == false, PP_OP_PP_MSG(cur, "==", p)); }, cur_idx);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur != p) == false, PP_OP_PP_MSG(cur, "!=", p)); }, cur_idx);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur != p) == true,  PP_OP_PP_MSG(cur, "!=", p)); }, cur_idx);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur <  p) == false, PP_OP_PP_MSG(cur, "<",  p)); }, 0, 1, 2, 3);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur <  p) == true,  PP_OP_PP_MSG(cur, "<",  p)); }, 0, 1, 2, 3);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur <= p) == false, PP_OP_PP_MSG(cur, "<=", p)); });
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur <= p) == true,  PP_OP_PP_MSG(cur, "<=", p)); });

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur >  p) == true,  PP_OP_PP_MSG(cur, ">",  p)); });
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur >  p) == false, PP_OP_PP_MSG(cur, ">",  p)); });

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur >= p) == true,  PP_OP_PP_MSG(cur, ">=", p)); }, 0, 1, 2, 3);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur >= p) == false, PP_OP_PP_MSG(cur, ">=", p)); }, 0, 1, 2, 3);
        }
    }

    {
        for (size_t cur_idx = 4; cur_idx < 8; ++cur_idx) {
            const auto& cur = pos.at(cur_idx);
            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur == p) == true,  PP_OP_PP_MSG(cur, "==", p)); }, cur_idx);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur == p) == false, PP_OP_PP_MSG(cur, "==", p)); }, cur_idx);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur != p) == false, PP_OP_PP_MSG(cur, "!=", p)); }, cur_idx);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur != p) == true,  PP_OP_PP_MSG(cur, "!=", p)); }, cur_idx);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur <  p) == false, PP_OP_PP_MSG(cur, "<",  p)); }, 0, 1, 2, 3, 4, 5, 6, 7);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur <  p) == true,  PP_OP_PP_MSG(cur, "<",  p)); }, 0, 1, 2, 3, 4, 5, 6, 7);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur <= p) == false, PP_OP_PP_MSG(cur, "<=", p)); }, 0, 1, 2, 3);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur <= p) == true,  PP_OP_PP_MSG(cur, "<=", p)); }, 0, 1, 2, 3);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur >  p) == true,  PP_OP_PP_MSG(cur, ">",  p)); }, 0, 1, 2, 3);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur >  p) == false, PP_OP_PP_MSG(cur, ">",  p)); }, 0, 1, 2, 3);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur >= p) == true,  PP_OP_PP_MSG(cur, ">=", p)); }, 0, 1, 2, 3, 4, 5, 6, 7);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur >= p) == false, PP_OP_PP_MSG(cur, ">=", p)); }, 0, 1, 2, 3, 4, 5, 6, 7);
        }
    }

    {
        for (size_t cur_idx = 8; cur_idx < 12; ++cur_idx) {
            const auto& cur = pos.at(cur_idx);
            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur == p) == true,  PP_OP_PP_MSG(cur, "==", p)); }, cur_idx);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur == p) == false, PP_OP_PP_MSG(cur, "==", p)); }, cur_idx);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur != p) == false, PP_OP_PP_MSG(cur, "!=", p)); }, cur_idx);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur != p) == true,  PP_OP_PP_MSG(cur, "!=", p)); }, cur_idx);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur <  p) == false, PP_OP_PP_MSG(cur, "<",  p)); }, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur <  p) == true,  PP_OP_PP_MSG(cur, "<",  p)); }, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur <= p) == false, PP_OP_PP_MSG(cur, "<=", p)); }, 0, 1, 2, 3, 4, 5, 6, 7);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur <= p) == true,  PP_OP_PP_MSG(cur, "<=", p)); }, 0, 1, 2, 3, 4, 5, 6, 7);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur >  p) == true,  PP_OP_PP_MSG(cur, ">",  p)); }, 0, 1, 2, 3, 4, 5, 6, 7);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur >  p) == false, PP_OP_PP_MSG(cur, ">",  p)); }, 0, 1, 2, 3, 4, 5, 6, 7);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur >= p) == true,  PP_OP_PP_MSG(cur, ">=", p)); }, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur >= p) == false, PP_OP_PP_MSG(cur, ">=", p)); }, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11);
        }
    }

    {
        for (size_t cur_idx = 12; cur_idx < 16; ++cur_idx) {
            const auto& cur = pos.at(cur_idx);
            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur == p) == true,  PP_OP_PP_MSG(cur, "==", p)); }, cur_idx);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur == p) == false, PP_OP_PP_MSG(cur, "==", p)); }, cur_idx);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur != p) == false, PP_OP_PP_MSG(cur, "!=", p)); }, cur_idx);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur != p) == true,  PP_OP_PP_MSG(cur, "!=", p)); }, cur_idx);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur <  p) == false, PP_OP_PP_MSG(cur, "<",  p)); }, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur <  p) == true,  PP_OP_PP_MSG(cur, "<",  p)); }, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur <= p) == false, PP_OP_PP_MSG(cur, "<=", p)); }, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur <= p) == true,  PP_OP_PP_MSG(cur, "<=", p)); }, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur >  p) == true,  PP_OP_PP_MSG(cur, ">",  p)); }, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur >  p) == false, PP_OP_PP_MSG(cur, ">",  p)); }, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11);

            each_of    (pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur >= p) == true,  PP_OP_PP_MSG(cur, ">=", p)); }, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15);
            each_except(pos.begin(), pos.end(), [&](const auto& p) { BOOST_CHECK_MESSAGE((cur >= p) == false, PP_OP_PP_MSG(cur, ">=", p)); }, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15);
        }
    }
}


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

#include "test_1F0.hpp"

#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

BOOST_AUTO_TEST_CASE( test_main )
{
#ifndef BOOST_MATH_BUGGY_LARGE_FLOAT_CONSTANTS
   test_spots(0.0F);
#endif
   test_spots(0.0);
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_spots(0.0L);
#ifndef BOOST_MATH_NO_REAL_CONCEPT_TESTS
   test_spots(boost::math::concepts::real_concept(0.1));
#endif
#endif
   test_spots(boost::multiprecision::cpp_bin_float_quad());
   test_spots(boost::multiprecision::cpp_dec_float_50());
}


#pragma once

#include <utils.hpp>
#include <polygon.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/reverse_graph.hpp>
#include <boost/graph/graphviz.hpp>

using namespace boost;

using BoostGraph = adjacency_list<vecS, vecS, bidirectionalS, urquhart::Polygon>;
using BoostRGraph = reverse_graph<BoostGraph>;
using BoostVertexT = graph_traits<BoostGraph>::vertex_descriptor;
using BoostEdgeT = graph_traits<BoostGraph>::edge_descriptor;
using BoostEdgeIter = graph_traits<BoostGraph>::out_edge_iterator;

class BFSVisitor : public boost::default_bfs_visitor {
    public:
        BFSVisitor(std::vector<BoostVertexT>& _visited) : visited(_visited){}
        void discover_vertex(BoostVertexT s, const BoostGraph &g) { if (s != 0) visited.push_back(s); }
        void discover_vertex(BoostVertexT s, const BoostRGraph &g) { if (s != 0) visited.push_back(s); }
        std::vector<BoostVertexT>& visited;
};

namespace urquhart {

    class Tree {
        public:
            explicit Tree(const std::vector<Polygon>& polygons);
            void view();
            // Merges two polygons of the tree. If i or j have more than 3 vertices, they are
            // disconnected from the main structure and their children are connected to the new polygon.
            // They are not deleted to keep the incremental indices (this could be improved to reduce memory consumption).
            BoostVertexT merge_op(BoostVertexT i, BoostVertexT j, const Polygon& data);
            Polygon get_vertex(const BoostVertexT v);
            // Gets the parent of a polygon
            BoostVertexT get_ancestor(const BoostVertexT v);
            // Gets the children of a vertex. This function reverses the graph and does a search,
            // TODO: this could be simplified by getting the in_edges of v which should be always 1
            std::vector<BoostVertexT> get_children(const BoostVertexT v);
            // Does a BFSearch from the root
            std::vector<BoostVertexT> traverse();
            // Does a BFSearch from v
            std::vector<BoostVertexT> traverse(const BoostVertexT v);
            BoostGraph graph;
            BoostVertexT root;
    };
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2014 Zuse Institute Berlin                                 */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <numeric>

#include <boost/timer/timer.hpp>

#include <dune/common/fmatrix.hh>
#include "dune/grid/config.h"

#include "linalg/threadedMatrix.hh"

using namespace Kaskade;

// Create a NxN lower triangular matrix with 1x2 entries
// as a mockup of the divergence discretization in Stokes problems
template <class Index>
void divergence(Index const N)
{
  std::cout << "running divergence with N=" << N << "\n";

  // First, sparsity pattern creation. We proceed row by row.
  NumaCRSPatternCreator<Index> creator(N,N,false);

  boost::timer::cpu_timer timer;
  std::vector<std::vector<Index>> cols(N), rows(N);
  for (Index k=0; k<cols.size(); ++k)
  {
    cols[k].resize(k+1);
    rows[k].resize(1);
    std::iota(begin(cols[k]),end(cols[k]),0);
    rows[k][0] = k;
  }
  std::cerr << "en bloc filling of indices: " << timer.format() << "\n";
  timer.start();
  creator.addElements(rows,cols);
  std::cerr << "en bloc adding elements:    " << timer.format() << "\n";

  // Next, create a sparsity pattern from the creator and a matrix.
  timer.start();
  creator.balance();
  std::cerr << "balance:                    " << timer.format() << "\n";
  timer.start();
  std::shared_ptr<NumaCRSPattern<Index>> pattern(new NumaCRSPattern<Index>(creator));
  std::cerr << "pattern creation:           " << timer.format() << "\n";
  std::cout << "nonzero entries: " << pattern->nonzeroes() << "\n";
  std::cout << "storage size:    " << pattern->storage() << "\n";
  for (int i=0; i<pattern->nodes(); ++i)
  {
    auto const& cp = *pattern->pattern(i);
    std::cout << "chunk " << i << ": [" << cp.first() << "," << cp.last() << "[  nnz=" << cp.nonzeroes() << "  size=" << cp.storage() << "\n";
  }
  timer.start();
  NumaBCRSMatrix<Dune::FieldMatrix<double,1,2>,Index> matrix(pattern);
  std::cerr << "matrix creation:            " << timer.format() << "\n";

  // Fill the matrix with ones.
  for (auto r=matrix.begin(); r!=matrix.end(); ++r)
    for (auto c=r->begin(); c!=r->end(); ++c)
      *c = 1.0;

  Dune::BlockVector<Dune::FieldVector<double,2>> x(N);
  Dune::BlockVector<Dune::FieldVector<double,1>> y(N);
  for (int i=0; i<N; ++i)
  {
    x[i] = 1;
    y[i] = 0;
  }

  timer.start();
  // Compute 1000 matrix-vector products
  for (int i=0; i<1000; ++i)
    matrix.umv(x,y);
  std::cout << "1000 Ax: " << timer.format() << "\n";

  timer.start();
  for (int i=0; i<1000; ++i)
    matrix.umtv(x,y);
  std::cout << "1000 A^Tx: " << timer.format() << "\n";
  std::cout << "-----------------------------\n\n";
}

// Create a NxN Toeplitz matrix of bandwidth 5
template <class Index>
void toeplitz(Index const N, bool symmetric)
{
  std::cout << "running toeplitz with symmetric=" << symmetric << "\n";

  // First, sparsity pattern creation. The pattern is a superposition of
  // 5x5 blocks on the diagonal. To begin with, we add a couple of those blocks
  // individually.
  NumaCRSPatternCreator<Index> creator(N,N,symmetric,14);

  std::vector<int> idx(5);
  for (int k=0; k<100; ++k)
  {
    std::iota(begin(idx),end(idx),k);
    creator.addElements(begin(idx),end(idx),begin(idx),end(idx));
  }

  // The rest is filled en bloc.
  boost::timer::cpu_timer timer;
  std::vector<std::vector<Index>> cols(N-104), rows(N-104);
  for (Index k=0; k<cols.size(); ++k)
  {
    cols[k].resize(5);
    rows[k].resize(5);
    std::iota(begin(cols[k]),end(cols[k]),k+100);
    std::iota(begin(rows[k]),end(rows[k]),k+100);
  }
  std::cerr << "en bloc filling of indices: " << timer.format() << "\n";
  timer.start();
  creator.addElements(rows,cols);
  std::cerr << "en bloc adding elements:    " << timer.format() << "\n";

  // Next, create a sparsity pattern from the creator and a matrix.
  timer.start();
  creator.balance();
  std::cerr << "balance:                    " << timer.format() << "\n";
  timer.start();
  std::shared_ptr<NumaCRSPattern<Index>> pattern(new NumaCRSPattern<Index>(creator));
  std::cerr << "pattern creation:           " << timer.format() << "\n";
  std::cout << "nonzero entries: " << pattern->nonzeroes() << "\n";
  std::cout << "storage size:    " << pattern->storage() << "\n";
  for (int i=0; i<pattern->nodes(); ++i)
  {
    auto const& cp = *pattern->pattern(i);
    std::cout << "chunk " << i << ": [" << cp.first() << "," << cp.last() << "[  nnz=" << cp.nonzeroes() << "  size=" << cp.storage() << "\n";
  }
  timer.start();
  NumaBCRSMatrix<Dune::FieldMatrix<double,1,1>,Index> matrix(pattern);
  std::cerr << "matrix creation:            " << timer.format() << "\n";

  // Fill the matrix with ones.
  for (auto r=matrix.begin(); r!=matrix.end(); ++r)
    for (auto c=r->begin(); c!=r->end(); ++c)
      *c = 1.0;

  Dune::BlockVector<Dune::FieldVector<double,1>> x(N), y(N);
  for (int i=0; i<N; ++i)
    x[i] = 1;

  timer.start();
  // Compute maximum eigenvalue by power method
  double mx = 0;
  double dp;
  for (int i=0; i<2000; ++i)
  {
    dp = matrix.smv(.125,x,y);
    if (i%100==0 || i>1995)
    {
      mx = 0;
      for (int i=0; i<N; i+=16)
        mx += std::abs(y[i][0]);
      for (int i=0; i<N; ++i)
        x[i] = y[i]/mx;
    }
  }
  std::cout << "2000 iterations power method: " << timer.format() << "\n";
  std::cout.precision(8);
  std::cout << "max eigenvalue: " << 8*mx << "\n";
  std::cout << "dp: " << dp << "\n";

  matrix = 1.0;

  if (!symmetric)
  {
    for (int i=0; i<N; ++i)
      x[i] = 1;
    timer.start();
    for (int i=0; i<2000; ++i)
    {
      matrix.smtv(.125,x,y);
      if (i%100==0 || i>1995)
      {
        mx = 0;
        for (int i=0; i<N; i+=16)
          mx += std::abs(y[i][0]);
        for (int i=0; i<N; ++i)
          x[i] = y[i]/mx;
      }
    }
    std::cout << "2000 iterations power method: " << timer.format() << "\n";
    std::cout.precision(8);
    std::cout << "max eigenvalue: " << 8*mx << "\n";
  }
  std::cout << "-----------------------------\n\n";
}

int main(void)
{
  int N = 64000;
  toeplitz(N,false);
  toeplitz(static_cast<size_t>(N),true);
  divergence(2000);
  return 0;
}


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

#include "imgdraw2d/Drawer2D.h"

#include "ImgTestUtils.h"
#include <Eigen/Core>
#include <chrono>

using namespace imgdraw2d;

#define CHECK_IMAGE( image )                 IMAGE_CHECK_CASE( image, "drawer2dEigen" )

#define COMPARE_IMAGES( imageA, imageB )     IMAGES_COMPARE( imageA, imageB, "drawer2dEigen" )

typedef Eigen::Vector2d Vec2;
typedef Drawer2D< Vec2 > Drawer2DE;

BOOST_AUTO_TEST_SUITE( Drawer2DEigenSuite )

    BOOST_AUTO_TEST_CASE( fillRect ) {
        Drawer2DD drawer1( 10.0, 1.0 );
        drawer1.setDrawColor( Image::RED );
        drawer1.fillRect( PointD{10.0, 10.0}, 1.0, 1.0, 0.0 );
        Image& image1 = drawer1.image();

        Drawer2DE drawer2( 10.0, 1.0 );
        drawer2.setDrawColor( Image::RED );
        drawer2.fillRect( Vec2{10.0, 10.0}, 1.0, 1.0, 0.0 );
        Image& image2 = drawer2.image();

        COMPARE_IMAGES( image1, image2 );
    }

    BOOST_AUTO_TEST_CASE( drawArc_90 ) {
        Drawer2DD drawer1( 10.0, 1.0 );
        drawer1.setDrawColor( "blue" );
        drawer1.drawArc( PointD{10.0, 10.0}, 5.0, 1.0, 0.0, M_PI_2 );
        Image& image1 = drawer1.image();

        Drawer2DE drawer2( 10.0, 1.0 );
        drawer2.setDrawColor( "blue" );
        drawer2.drawArc( Vec2{10.0, 10.0}, 5.0, 1.0, 0.0, M_PI_2 );
        Image& image2 = drawer2.image();

        COMPARE_IMAGES( image1, image2 );
    }

    BOOST_AUTO_TEST_CASE( drawClothoid_distance ) {
        Drawer2DE drawer( 200.0 );
        drawer.setBackground( Image::WHITE );
        drawer.autoResize = false;
        drawer.resizeImage( 2.0 );

        drawer.setDrawColor( "black" );
        drawer.drawClothoid( Vec2(0.0, 0.0), 0.0, 0.01,  5.0, 1.0 );
        drawer.setDrawColor( "red" );
        drawer.drawClothoid( Vec2(0.0, 0.0), 0.0, 0.01, -5.0, 1.0 );

        drawer.setDrawColor( "green" );
        drawer.drawClothoid( Vec2(0.0, 0.0), 0.0, 0.01,  5.0, 2.0 );
        drawer.setDrawColor( "blue" );
        drawer.drawClothoid( Vec2(0.0, 0.0), 0.0, 0.01, -5.0, 2.0 );

        CHECK_IMAGE( drawer.image() );
    }

BOOST_AUTO_TEST_SUITE_END()


#include <frovedis.hpp>
#include <frovedis/matrix/blockcyclic_matrix.hpp>

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

using namespace frovedis;
using namespace std;

bool check(blockcyclic_matrix_local<float>& m) {
  return m.is_valid();
}

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    // constructing blockcylic matrix from colmajor matrix
    colmajor_matrix<float> cm (
             make_rowmajor_matrix_load<float>("./sample_4x4"));
    blockcyclic_matrix<float> bm1 (cm);

    // checking the validity of the created blockcyclic matrix
    BOOST_CHECK (bm1.data.map(check).get(0));

    // default constructor to create empty matrix
    blockcyclic_matrix<float> bm2, bm3;

    // overloaded copy =
    bm2 = bm1;

    // overloaded move =
    bm3 = std::move(bm1);

    // converting back to colmajor matrix and save in a text file
    bm2.to_colmajor().save("./cm");

    // saving these matrices in sample text files
    bm2.save("./bm2");
    bm3.save("./bm3");

    // checking whether saved files are having same data as with loaded file
    BOOST_CHECK (system("diff ./sample_4x4 ./cm") == 0);
    BOOST_CHECK (system("diff ./sample_4x4 ./bm2") == 0);
    BOOST_CHECK (system("diff ./sample_4x4 ./bm3") == 0);

    // deleting the saved files
    system("rm -f ./cm ./bm2 ./bm3");
}


/* -*- C++ -*- *****************************************************************
 * Copyright (c) 2013 United States Government as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All rights reserved.
 *
 * Licensed under the NASA Open Source Agreement, Version 1.3 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   https://opensource.org/licenses/NASA-1.3
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************
 *
 * Project: RoverSw
 * Module: FSD Locomotion
 * Author: Xavier Bouyssounouse
 *
 *****************************************************************************/

#include "FsdMatlabPlots.h"

#include <iomanip>
#include <iostream>
#include <fstream>
#include <list>
#include <vector>

#include <boost/progress.hpp>

using namespace std;
using namespace kn;

namespace
{
  bool const printFsdDebug = false;
  bool const printFsdWarnings = false;
}

const string FsdMatlabPlots::s_wheelNames[]  = {"Left Front", "Right Front", "Left Rear", "Right Rear"};
const string FsdMatlabPlots::s_wheelOper[]  = {"Drive Speed", "Drive Accel", "Steer Speed", "Steer Accel"};
const string FsdMatlabPlots::s_wheelUnit[]  = {"(m/s)", "(m/s^2)", "(rad/s)", "(rad/s^2)"};

void FsdMatlabPlots::init()
{
  m_lfcMat = string("magenta");
  m_rfcMat = string("green");
  m_lrcMat = string("blue");
  m_rrcMat = string("red");
  m_ocMat  = string("--black");

  if (!m_generatePlots) {
    string matName = "regression.txt";
    m_mat.open(matName.c_str());
    m_mat << setprecision(4);
  }
}

FsdMatlabPlots::FsdMatlabPlots(bool generatePlots)
  : m_OwnFsd(true), m_generatePlots(generatePlots), m_sampleFrequency(30.), m_time(0.),
    m_maxWheelSpeed(NAN), m_maxWheelAccel(NAN), m_maxSteerRate(NAN), m_maxSteerAccel(NAN)
{
  init();
  // Hard code values from K10 Red/Black Rovers
  Vector2Vector wheelLocations(4);

  double wheelBaseWidth  = .778;
  double wheelBaseLength = .812;
  unsigned int numWheels = 0;

  for (int y = 0; y < 2; ++y)
    for (int x = 0; x < 2; ++x) {
      Vector2 m((1 - 2 * y) * wheelBaseLength / 2.0, -(1 - 2 * x) * wheelBaseWidth / 2.0);
      wheelLocations[numWheels++] = m;
    }
  double wheelRadius = 0.127;

  m_fsd = new kn::FastestSteerAndDriveCWL(
    m_sampleFrequency, wheelRadius, wheelLocations);
}

FsdMatlabPlots::FsdMatlabPlots(kn::FastestSteerAndDriveCWL* fsd)
  : m_fsd(fsd), m_OwnFsd(false), m_sampleFrequency(30.),
    m_maxWheelSpeed(NAN), m_maxWheelAccel(NAN), m_maxSteerRate(NAN), m_maxSteerAccel(NAN)
{
  init();
}

FsdMatlabPlots::~FsdMatlabPlots()
{
  if (m_generatePlots) {
    ofstream plots("pathAll.m");
    for (std::list<std::string>::iterator iter = m_plotNames.begin(); iter != m_plotNames.end(); ++iter)
      plots << "disp('path_" << *iter << "'); path_" << *iter << endl;
  } else {
    m_mat.close();
  }

  if (m_OwnFsd)
    delete m_fsd;
}

void FsdMatlabPlots::initPath(
  const double wheelSpeed,
  const double wheelAccel,
  const double steerRate,
  const double steerAccel)
{
  m_commands.str("");

  m_commands << "% WheelAccel(m/s^2) \tSteerAccel(rad/s/s) \tSteerRate(rad/s)\n"
             << "% " << wheelAccel << "\t\t\t" << steerAccel << "\t\t\t" << steerRate << endl << endl
             << "% PathTime(s)      \tWheelSpeed(m/s) \tCurvature(1/m)         \tCrab(rad)\n";

  m_time = 0.;

  m_fsd->setTransition(wheelAccel, steerAccel, steerRate);

  m_maxWheelSpeed = wheelSpeed;
  m_maxWheelAccel = wheelAccel;
  m_maxSteerRate = steerRate;
  m_maxSteerAccel = steerAccel;

  if (m_OwnFsd) {
    kn::CoordinatedWheelLocomotion::CwlVector empty(m_fsd->numWheels() * 2, 0.);
    m_fsd->initPath(kn::WheelGroupState(ACE_Time_Value::zero, empty, empty, 0., 0., 0., 0., 0.));
  }
}

void FsdMatlabPlots::computePath(const double time, const double targetWheelSpeed, const double targetCurvature, const double targetCrab)
{
  m_commands << "% " << time << "\t\t\t" << targetWheelSpeed << "\t\t\t" << targetCurvature << "\t\t\t" << targetCrab << endl;

  boost::timer timer;
  timer.restart();
  m_fsd->clearPath();
  m_fsd->appendPath(time, targetWheelSpeed, targetCurvature, targetCrab);
  m_time += time;
  double elapsed = timer.elapsed();
  if (elapsed > 0)
    cout << "Real Time Ratio: " << time / elapsed << endl;
  if (printFsdDebug || printFsdWarnings)
    m_fsd->printWarnings(cerr, printFsdDebug);
}

double FsdMatlabPlots::computeSlowTime(const double accel, const double fastTime,
                                       const double fastSpeed, const double alpha)
{
  double slowSpeed = alpha * fastSpeed;
  double decelTime = (fastSpeed - slowSpeed) / accel;
  double decelPathLength = fastSpeed * decelTime + accel * decelTime * decelTime / 2.;
  double slowTime = 0.;

  if (fastTime > 0.) {
    // Time left over after reaching target curvature & speed, so add a slowdown path
    double pathLength = fastTime * fastSpeed;

    if (decelPathLength > pathLength) {
      // Not enough path length to reach target slowdown speed;
      slowTime = (sqrt(fastSpeed * fastSpeed + 2 * accel * pathLength) - fastSpeed) / accel;
    } else {
      // Target slowdown speed reached.
      slowTime = decelTime + (pathLength - decelPathLength) / slowSpeed;
    }
  }
  return slowTime;
}

void FsdMatlabPlots::computeAlphaPath(const double wheelAccel, const double steerAccel, const double steerRate,
                                      const double time, const double targetWheelSpeed,
                                      const double targetCurvature, const double alpha)
{
  initPath(wheelAccel, steerAccel, steerRate, targetWheelSpeed);
  computePath(NAN, targetWheelSpeed, targetCurvature);

  double slowTime = computeSlowTime(fabs(wheelAccel), time - m_fsd->getElapsedTime(), fabs(targetWheelSpeed), alpha);
  if (slowTime > 0)
    appendPath(slowTime, alpha * targetWheelSpeed, targetCurvature);
}

void FsdMatlabPlots::appendPath(const double time, const double targetWheelSpeed, const double targetCurvature, const double targetCrab)
{
  m_commands << "% " << time << "\t\t\t" << targetWheelSpeed << "\t\t\t" << targetCurvature << "\t\t\t" << targetCrab << endl;

  boost::timer timer;
  timer.restart();
  m_time += time;
  m_fsd->appendPath(m_time, targetWheelSpeed, targetCurvature, targetCrab);
  double elapsed = timer.elapsed();
  if (elapsed > 0)
    cout << "Real Time Ratio: " << time / elapsed << endl;
  if (printFsdDebug || printFsdWarnings)
    m_fsd->printWarnings(cerr, printFsdDebug);
}

void FsdMatlabPlots::computeAlignmentPath(const double pathTime)
{
  m_commands << "% " << pathTime << endl;

  m_fsd->clearPath();
  unsigned int numPoints = m_fsd->appendAlignmentPath(pathTime);
  m_time += static_cast<double>(numPoints) / m_sampleFrequency;

  if (printFsdDebug || printFsdWarnings)
    m_fsd->printWarnings(cerr, printFsdDebug);
}

void FsdMatlabPlots::appendAlphaPath(const double wheelAccel, const double steerAccel, const double steerRate,
                                     const double time, const double targetWheelSpeed, const double targetCurvature,
                                     const double alpha)
{
  m_fsd->setTransition(wheelAccel, steerAccel, steerRate);
  appendPath(NAN, targetWheelSpeed, targetCurvature);

  double slowTime = computeSlowTime(fabs(wheelAccel), time - m_fsd->getElapsedTime(), fabs(targetWheelSpeed), alpha);
  appendPath(slowTime, alpha * targetWheelSpeed, targetCurvature);
}

void FsdMatlabPlots::initPathFromCurrentState()
{
  m_commands << "% initPathFromCurrentState" << endl;

  // get the current state from FSD as it is extracted from

  double speed = m_fsd->getPpmSpeeds().back();
  double curvature = m_fsd->getPpmCurvatures().back();
  double curvatureRate = m_fsd->getPpmCurvatureRates().back();
  double crab = m_fsd->getPpmCrabs().back();
  double crabRate = m_fsd->getPpmCrabRates().back();

  DoubleVectorVector const& ppmPath = m_fsd->getPpmMotorPositions();
  DoubleVectorVector const& ppmMotorSpeeds =  m_fsd->getPpmMotorSpeeds();

  DoubleVector motorPositions;
  DoubleVector motorSpeeds;

  {
    motorPositions.reserve(ppmPath.size());
    DoubleVectorVector::const_iterator first, last = ppmPath.end();
    for (first = ppmPath.begin(); first != last; ++first) {
      motorPositions.push_back(first->back());
    }
  }

  {
    motorSpeeds.reserve(ppmMotorSpeeds.size());
    DoubleVectorVector::const_iterator first, last = ppmMotorSpeeds.end();
    for (first = ppmMotorSpeeds.begin(); first != last; ++first) {
      motorSpeeds.push_back(first->back());
    }
  }

  m_fsd->initPath(kn::WheelGroupState(ACE_Time_Value::zero,
                                      motorPositions, motorSpeeds,
                                      curvature, curvatureRate, speed,
                                      crab, crabRate));
}

void FsdMatlabPlots::initUnalignedSteerPositions(const double steerRate, const double steerAccel,
                                                 const DoubleVector& steerPositions)
{
  m_commands << "% initUnalignedSteerPositions" << endl;

  m_maxSteerAccel = steerAccel;
  m_maxSteerRate  = steerRate;
  m_maxWheelSpeed = 0.;
  m_maxWheelAccel = 0.;

  m_fsd->setTransition(m_maxWheelAccel, m_maxSteerAccel, m_maxSteerRate);

  double speed = 0.;
  double curvature = NAN;
  double curvatureRate = 0.;
  double crab = 0.;
  double crabRate = 0.;

  DoubleVector motorPositions;
  DoubleVector motorSpeeds;
  for (unsigned int i = 0; i < m_fsd->numWheels(); ++i) {
    motorPositions.push_back(0.);
    motorSpeeds.push_back(0.);
  }

  for (unsigned int i = 0; i < m_fsd->numWheels(); ++i) {
    motorPositions.push_back(steerPositions[i]);
    motorSpeeds.push_back(0.);
  }

  m_time = 0.;

  m_fsd->initPath(kn::WheelGroupState(ACE_Time_Value::zero,
                                      motorPositions, motorSpeeds,
                                      curvature, curvatureRate, speed,
                                      crab, crabRate));
}

void FsdMatlabPlots::initPathFromPreviousState(double timeOffset)
{
  DoubleVectorVector const& ppmPath = m_fsd->getPpmMotorPositions();
  int numSamples = ppmPath[0].size();

  int sampleInterrupt = 0;
  if (timeOffset <= 0) {
    // Assume time offset is from end of path
    sampleInterrupt = numSamples + static_cast<int>(timeOffset * m_sampleFrequency - .5);
  } else {
    // Assume time offset is from start of path
    sampleInterrupt = static_cast<int>(timeOffset * m_sampleFrequency + .5);
  }
  if (sampleInterrupt < 0) sampleInterrupt = 0;
  if (sampleInterrupt >= numSamples) sampleInterrupt = numSamples - 1;

  m_fsd->resizePath(sampleInterrupt);
}

void FsdMatlabPlots::setVariableNames(const string& suffix)
{
  m_stimeMat = string("time_") + suffix;
  m_xMat = string("x_") + suffix;
  m_yMat = string("y_") + suffix;

  m_lfdMat   = string("left_front_drive_") + suffix;
  m_rfdMat   = string("right_front_drive_") + suffix;
  m_lrdMat   = string("left_rear_drive_") + suffix;
  m_rrdMat   = string("right_rear_drive_") + suffix;
  m_driveMat = string("drive_") + suffix;
  m_driveSpeedMat = string("driveSpeed_") + suffix;
  m_maxDriveSpeedMat = string("maxDriveSpeed_") + suffix;

  m_lfsMat   = string("left_front_steer_") + suffix;
  m_rfsMat   = string("right_front_steer_") + suffix;
  m_lrsMat   = string("left_rear_steer_") + suffix;
  m_rrsMat   = string("right_rear_steer_") + suffix;
  m_steerMat = string("steer_") + suffix;
  m_steerRateMat = string("steerRate_") + suffix;
  m_maxSteerRateMat = string("maxSteerRate_") + suffix;

  m_fMat = string("sampleFrequency_") + suffix;
  m_curvatureMat = string("curvature_") + suffix;
  m_crabMat = string("crab_") + suffix;
  m_curvatureRateMat = string("curvatureRate_") + suffix;
  m_crabRateMat = string("crabRate_") + suffix;
  m_speedMat = string("speed_") + suffix;
}

void FsdMatlabPlots::outputMatlabPlots(const string& name, const string& suffix)
{
  const string varName = name + "Vars_" + suffix;

  outputMatlabVariables(varName, suffix);

  if (m_generatePlots) {
    const string matName = name + "_" + suffix + ".m";
    ofstream mat(matName.c_str());

    // Close al windows and clear all variables
    mat << "close all\n";
    mat << "clear all\n";

    // Load all the variables;
    mat << varName << endl;

    mat << m_driveMat << " = [" << m_lfdMat << "' " << m_rfdMat << "' " << m_lrdMat << "' " << m_rrdMat << "']';" << endl;
    mat << m_driveSpeedMat << " = diff(" << m_driveMat << "')' * " << m_fMat << ";" << endl;
    mat << m_maxDriveSpeedMat << " = max(abs(" << m_driveSpeedMat << "));" << endl;

    mat << m_steerMat << " = [" << m_lfsMat << "' " << m_rfsMat << "' " << m_lrsMat << "' " << m_rrsMat << "']';" << endl;
    mat << m_steerRateMat << " = diff(" << m_steerMat << "')' * " << m_fMat << ";" << endl;
    mat << m_maxSteerRateMat << " = max(abs(" << m_steerRateMat << "));" << endl << endl;

    // Generate sheet of matlab plots
    stdSheet(mat);
    sheetPostamble(mat, name, suffix);

    // Generate sheet of matlab error plots for wheel variables
    errSheet_1(mat);
    sheetPostamble(mat, name + "Wheel", suffix);

    // Generate detailed sheet
    errSheet_1a(mat, suffix);

    // Generate sheet of matlab error plots for vehicle variables
    errSheet_2(mat);
    sheetPostamble(mat, name + "Rover", suffix);
  }
}

void FsdMatlabPlots::sheetPostamble(ofstream& mat, const string& name, const string& suffix)
{
  mat << "orient landscape\n";

  mat << "axes('Position',[0 0 1 1],'Xlim',[0 1],'Ylim',[0 1],'Box','off','Visible','off',"
      << "'Units','normalized', 'clipping' , 'off');\n";
  mat << "text(0.5, 1,'\\bf " << name << "\\_" << suffix << "',"
      << "'HorizontalAlignment','center','VerticalAlignment', 'top', 'fontsize', 24);\n";

  mat << "print -dpdf " << name << "_" << suffix << endl;
}

void FsdMatlabPlots::stdSheet(ofstream& mat)
{
  mat << "figure('visible','off');\n";
  mat << "subplot(5,3,1)\n";
  mat << "plot(" << m_xMat << "," << m_yMat << ")\n";
  mat << "grid on\n";
  mat << "axis equal\n";
  mat << "xlabel({'X (m)'})\n";
  mat << "ylabel({'Y';'(m)'})\n\n";

  mat << "subplot(5,3,2)\n";
  mat << "plot(" << m_stimeMat << "," << m_speedMat << ")\n";
  mat << "grid on\n";
  mat << "xlabel({'time (s)'})\n";
  mat << "ylabel({'speed';'(m/s)'})\n\n";

  mat << "subplot(5,3,3)\n";
  mat << "plot(" << m_stimeMat << "(1:end-1), diff(" << m_speedMat << ") * " << m_fMat << ")\n";
  mat << "grid on\n";
  mat << "xlabel({'time (s)'})\n";
  mat << "ylabel({'acceleration';'(m/s^2)'})\n\n";

  mat << "subplot(5,3,4)\n";
  mat << "hold off\n";
  mat << "plot(" << m_stimeMat << "," << m_lfdMat << ", '" << m_lfcMat << "')\n";
  mat << "grid on\n";
  mat << "hold on\n";
  mat << "plot(" << m_stimeMat << "," << m_rfdMat << ", '" << m_rfcMat << "')\n";
  mat << "plot(" << m_stimeMat << "," << m_lrdMat << ", '" << m_lrcMat << "')\n";
  mat << "plot(" << m_stimeMat << "," << m_rrdMat << ", '" << m_rrcMat << "')\n";
  mat << "xlabel({'time (s)'})\n";
  mat << "ylabel({'Wheel Dist';'(m)'})\n\n";

  mat << "subplot(5,3,5)\n";
  mat << "hold off\n";
  mat << "plot(" << m_stimeMat << "(1:end-1), diff(" << m_lfdMat << ") * " << m_fMat << ", '" << m_lfcMat << "')\n";
  mat << "grid on\n";
  mat << "hold on\n";
  mat << "plot(" << m_stimeMat << "(1:end-1), diff(" << m_rfdMat << ") * " << m_fMat << ", '" << m_rfcMat << "')\n";
  mat << "plot(" << m_stimeMat << "(1:end-1), diff(" << m_lrdMat << ") * " << m_fMat << ", '" << m_lrcMat << "')\n";
  mat << "plot(" << m_stimeMat << "(1:end-1), diff(" << m_rrdMat << ") * " << m_fMat << ", '" << m_rrcMat << "')\n";
  mat << "plot(" << m_stimeMat << "," << m_speedMat  << ", '" << m_ocMat << "')\n";
  mat << "xlabel({'time (s)'})\n";
  mat << "ylabel({'wheel speed';'(m/s)'})\n\n";

  mat << "subplot(5,3,6)\n";
  mat << "hold off\n";
  mat << "plot(" << m_stimeMat << "(1:end-2), diff(diff(" << m_lfdMat  << ")) * " << m_fMat << " ^2, '" << m_lfcMat << "')\n";
  mat << "grid on\n";
  mat << "hold on\n";
  mat << "plot(" << m_stimeMat << "(1:end-2), diff(diff(" << m_rfdMat << ")) * " << m_fMat << " ^2, '" << m_rfcMat << "')\n";
  mat << "plot(" << m_stimeMat << "(1:end-2), diff(diff(" << m_lrdMat << ")) * " << m_fMat << " ^2, '" << m_lrcMat << "')\n";
  mat << "plot(" << m_stimeMat << "(1:end-2), diff(diff(" << m_rrdMat << ")) * " << m_fMat << " ^2, '" << m_rrcMat << "')\n";
  mat << "plot(" << m_stimeMat << "(1:end-1), diff("    << m_speedMat  << ") * " << m_fMat << ", '"    << m_ocMat << "')\n";
  mat << "xlabel({'time';'(s)'})\n";
  mat << "ylabel({'wheel accel';'(m/s^2)'})\n\n";

  mat << "subplot(5,3,7)\n";
  mat << "plot(" << m_stimeMat << "," << m_curvatureMat << ")\n";
  mat << "grid on\n";
  mat << "xlabel({'time (s)'})\n";
  mat << "ylabel({'curvature';'(1/m)'})\n\n";

  mat << "subplot(5,3,8)\n";
  mat << "hold off\n";
  mat << "plot(" << m_stimeMat << "," << m_curvatureRateMat << ")\n";
  mat << "grid on\n";
  mat << "hold on\n";
  mat << "plot(" << m_stimeMat << "(1:end-1), diff(" << m_curvatureMat << ") * " << m_fMat << ", '" << m_ocMat << "')\n";
  mat << "xlabel({'time (s)'})\n";
  mat << "ylabel({'dk/dt';'(1/m/s)'})\n\n";

  mat << "subplot(5,3,9)\n";
  mat << "hold off\n";
  mat << "plot(" << m_stimeMat << "(1:end-1), diff(" << m_curvatureRateMat << ") * " << m_fMat << ")\n";
  mat << "grid on\n";
  mat << "hold on\n";
  mat << "plot(" << m_stimeMat << "(1:end-2), diff(diff(" << m_curvatureMat << ")) * " << m_fMat << "^2, '" << m_ocMat << "')\n";
  mat << "xlabel({'time (s)'})\n";
  mat << "ylabel({'d^2k/dt^2';'(1/m/s^2)'})\n\n";

  mat << "subplot(5,3,10)\n";
  mat << "hold off\n";
  mat << "plot(" << m_stimeMat << "," << m_lfsMat << ", '" << m_lfcMat << "')\n";
  mat << "grid on\n";
  mat << "hold on\n";
  mat << "plot(" << m_stimeMat << "," << m_rfsMat << ", '" << m_rfcMat << "')\n";
  mat << "plot(" << m_stimeMat << "," << m_lrsMat << ", '" << m_lrcMat << "')\n";
  mat << "plot(" << m_stimeMat << "," << m_rrsMat << ", '" << m_rrcMat << "')\n";
  mat << "xlabel({'time (s)'})\n";
  mat << "ylabel({'wheel rotation';'(rad)'})\n\n";

  mat << "subplot(5,3,11)\n";
  mat << "hold off\n";
  mat << "plot(" << m_stimeMat << "(1:end-1), diff(" << m_lfsMat  << ") * " << m_fMat << ", '" << m_lfcMat << "')\n";
  mat << "grid on\n";
  mat << "hold on\n";
  mat << "plot(" << m_stimeMat << "(1:end-1), diff(" << m_rfsMat << ") * " << m_fMat << ", '" << m_rfcMat << "')\n";
  mat << "plot(" << m_stimeMat << "(1:end-1), diff(" << m_lrsMat << ") * " << m_fMat << ", '" << m_lrcMat << "')\n";
  mat << "plot(" << m_stimeMat << "(1:end-1), diff(" << m_rrsMat << ") * " << m_fMat << ", '" << m_rrcMat << "')\n";
  mat << "xlabel({'time (s)'})\n";
  mat << "ylabel({'steer speed';'(rad/s)'})\n\n";

  mat << "subplot(5,3,12)\n";
  mat << "hold off\n";
  mat << "plot(" << m_stimeMat << "(1:end-2), diff(diff(" << m_lfsMat  << ")) * " << m_fMat << "^2, '" << m_lfcMat << "')\n";
  mat << "grid on\n";
  mat << "hold on\n";
  mat << "plot(" << m_stimeMat << "(1:end-2), diff(diff(" << m_rfsMat << ")) * " << m_fMat << "^2, '" << m_rfcMat << "')\n";
  mat << "plot(" << m_stimeMat << "(1:end-2), diff(diff(" << m_lrsMat << ")) * " << m_fMat << "^2, '" << m_lrcMat << "')\n";
  mat << "plot(" << m_stimeMat << "(1:end-2), diff(diff(" << m_rrsMat << ")) * " << m_fMat << "^2, '" << m_rrcMat << "')\n";
  mat << "xlabel({'time (s)'})\n";
  mat << "ylabel({'steer accel';'(rad/s^2)'})\n\n";

  mat << "subplot(5,3,13)\n";
  mat << "hold off\n";
  mat << "plot(" << m_stimeMat << "," << m_crabMat << ")\n";
  mat << "grid on\n";
  mat << "xlabel({'time (s)'})\n";
  mat << "ylabel({'crab';'(rad)'})\n\n";

  mat << "subplot(5,3,14)\n";
  mat << "hold off\n";
  mat << "plot(" << m_stimeMat << "," << m_crabRateMat << ")\n";
  mat << "grid on\n";
  mat << "hold on\n";
  mat << "plot(" << m_stimeMat << "(1:end-1), diff(" << m_crabMat << ") * " << m_fMat << ")\n";
  mat << "xlabel({'time (s)'})\n";
  mat << "ylabel({'d/dt crab';'(rad/s)'})\n\n";

  mat << "subplot(5,3,15)\n";
  mat << "hold off\n";
  mat << "plot(" << m_lfsMat << "(1:end-1), diff(" << m_lfsMat  << ") * " << m_fMat << ", '" << m_lfcMat << "')\n";
  mat << "grid on\n";
  mat << "hold on\n";
  mat << "plot(" << m_rfsMat << "(1:end-1), diff(" << m_rfsMat << ") * " << m_fMat << ", '" << m_rfcMat << "')\n";
  mat << "plot(" << m_lrsMat << "(1:end-1), diff(" << m_lrsMat   << ") * " << m_fMat << ", 'blue')\n";
  mat << "plot(" << m_rrsMat << "(1:end-1), diff(" << m_rrsMat  << ") * " << m_fMat << ", 'red')\n";
  mat << "xlabel({'steer position (rad)'})\n";
  mat << "ylabel({'steer speed';'(rad/s)'})\n\n";
}

void FsdMatlabPlots::setDerivative(ofstream& mat, const string& var, unsigned int level)
{
  mat << "der = ";
  for (unsigned int i = 0; i < level; ++i)
    mat << "diff(";
  mat << var;
  for (unsigned int i = 0; i < level; ++i)
    mat << ")";
  if (level > 0)
    mat << " * " << m_fMat << "^" << level;
  mat << ";\n";
}

// Wheel Variables
void FsdMatlabPlots::errSheet_1(ofstream& mat)
{
  const unsigned int numCols = 4;
  const unsigned int numRows = 4;

  string times[3] = {m_stimeMat, m_stimeMat + "(1:end-1)", m_stimeMat + "(1:end-2)"};

  string colorWheelNames[numCols] = {m_lfcMat, m_rfcMat, m_lrcMat, m_rrcMat};
  string driveWheelNames[numCols] = {m_lfdMat, m_rfdMat, m_lrdMat, m_rrdMat};
  string steerWheelNames[numCols] = {m_lfsMat, m_rfsMat, m_lrsMat, m_rrsMat};

  mat << "figure('visible','off');\n";
  for (unsigned int row = 0; row < numRows; ++row)
    for (unsigned int col = 0; col < numCols; ++col)
    {
        mat << "\nsubplot(" << numRows << "," << numCols << "," << row * numRows + col + 1 << ")\n";
        mat << "grid on\n";
        unsigned int derLev = row % 2 + 1;
        bool isDriveWheel = (row <= 1);
        string wheelId = isDriveWheel ? driveWheelNames[col] : steerWheelNames[col];

        if (derLev == 1)
          mat << "der = diff(" << wheelId << ") * " << m_fMat << ";\n";
        else
          mat << "der = diff(diff(" << wheelId << ")) * " << m_fMat << "^2;\n";

        mat << "plot(" << times[derLev] << ", der, '" << colorWheelNames[col] << "')\n";
        if (col == 0)
          mat << "ylabel({'" << s_wheelOper[row] << "';'" << s_wheelUnit[row] << "'})\n";
        if (row == numRows - 1)
          mat << "xlabel('time(s)')\n";

        mat << "hold on\n";

        double maxWheel = NAN;
        if (derLev == 1)
          maxWheel = isDriveWheel ? m_maxWheelSpeed : m_maxSteerRate;
        else
          maxWheel = isDriveWheel ? m_maxWheelAccel : m_maxSteerAccel;

        //        mat << "if (max(der) > " << maxWheel << ")\n";
        mat << "  plot(" << times[derLev] << "," << maxWheel << ")\n";
        //        mat << "end\n";

        //        mat << "if (min(der) < " << -maxWheel << ")\n";
        mat << "  plot(" << times[derLev] << "," << -maxWheel << ")\n";
        //        mat << "end\n";

        if (row == 0)
          mat << "title({'" << s_wheelNames[col] << "'})\n";

        mat << "hold off\n";
    }
}

// Wheel Variables
void FsdMatlabPlots::errSheet_1a(ofstream& mat, const string& suffix)
{
  const unsigned int numCols = 4;
  const unsigned int numRows = 4;

  string times[3] = {m_stimeMat, m_stimeMat + "(1:end-1)", m_stimeMat + "(1:end-2)"};
  string driveWheelNames[numCols] = {m_lfdMat, m_rfdMat, m_lrdMat, m_rrdMat};
  string steerWheelNames[numCols] = {m_lfsMat, m_rfsMat, m_lrsMat, m_rrsMat};

  size_t maxNameLen = 0;
  for (unsigned int nameIdx = 0; nameIdx < numCols; ++nameIdx)
    maxNameLen = max(maxNameLen, s_wheelNames[nameIdx].length());

  size_t maxOperLen = 0;
  for (unsigned int operIdx = 0; operIdx < numCols; ++operIdx)
    maxOperLen = max(maxOperLen, s_wheelOper[operIdx].length());

  const unsigned int filename_sublen = maxNameLen + maxOperLen;

  for (unsigned int row = 0; row < numRows; ++row)
    for (unsigned int col = 0; col < numCols; ++col)
    {
        mat << "grid on\n";
        unsigned int derLev = row % 2 + 1;
        bool isDriveWheel = (row <= 1);
        string wheelId = isDriveWheel ? driveWheelNames[col] : steerWheelNames[col];

        double maxWheel = NAN;
        if (derLev == 1) {
          maxWheel = isDriveWheel ? m_maxWheelSpeed : m_maxSteerRate;
          mat << "der = diff(" << wheelId << ") * " << m_fMat << ";\n";
        } else {
          maxWheel = isDriveWheel ? m_maxWheelAccel : m_maxSteerAccel;
          mat << "der = diff(diff(" << wheelId << ")) * " << m_fMat << "^2;\n";
        }

        // Always have at least two underscores separting filename and suffix.  Keep total filename length same for
        // nice formatting.
        unsigned int numUnderScores = filename_sublen + 2 - s_wheelNames[col].length() - s_wheelOper[row].length();
        string filename = s_wheelNames[col];

        for (unsigned int idx = 0; idx < numUnderScores; ++idx)
          filename = filename + "_";

        filename = filename + s_wheelOper[row];

        // Swallow white space.
        size_t s = filename.find(' ');
        while (s != string::npos) {
          filename.erase(s,1);
          s = filename.find(' ');
        }

        mat << "limitPlot('" << filename << "', '" << suffix << "', '" << s_wheelNames[col] << "', 'time (s)', "
            << times[derLev] << ", '" << s_wheelOper[row] << " " << s_wheelUnit[row] << "', der, "
            << -maxWheel << ", " << maxWheel << ")\n";
    }
}

// Vehicle Variables
void FsdMatlabPlots::errSheet_2(ofstream& mat)
{
  string times[3] = {m_stimeMat, m_stimeMat + "(1:end-1)", m_stimeMat + "(1:end-2)"};

  ////////////////////////////////////////////////////////////////////////
  //                            //                                      //
  // rover speed                //      angular speed                   //
  //                            //                                      //
  // rover acceleration         //      centripetal acceleration        //
  //                            //                                      //
  // rover jerk                 //      centripetal jerk                //
  //                            //                                      //
  ////////////////////////////////////////////////////////////////////////

  const unsigned int numCols = 2;
  const unsigned int numRows = 3;

  unsigned int col = 0;
  unsigned int row = 0;
  const string roverOper[]  = {
    "'Rover Speed';'(m/s)'", "'Rover Acceleration';'(m/s^2)'", "'Rover Jerk';'(m/s^3)'"};

  mat << "figure('visible','off');\n";
  for (row = 0; row < numRows; ++row) {

    mat << "\nsubplot(" << numRows << "," << numCols << "," << row * numCols + col + 1 << ")\n";
    mat << "grid on\n";

    setDerivative(mat, m_speedMat, row);

    mat << "plot(" << times[row] << ", der)\n";

    // Display limits on plot (only wheel limits for now).
    double maxWheel = NAN;
    if (row == 0)
      maxWheel = m_maxWheelSpeed;
    else if (row == 1)
      maxWheel = m_maxWheelAccel;
    mat << "hold on\n";
    mat << "  plot(" << times[row] << "," << maxWheel << ")\n";
    mat << "hold off\n";
    if (row == numRows - 1)
      mat << "xlabel('time(s)')\n";
    mat << "ylabel({" << roverOper[row] << "})\n";
  }

  col = 1;
  row = 0;
  mat << "\nsubplot(" << numRows << "," << numCols << "," << row * numCols + col + 1 << ")\n";
  mat << "plot(" << times[0] << ", " << m_speedMat << " .* " << m_curvatureMat << ")\n";
  mat << "ylabel({'Angular Speed';'(rad/s)'})\n";

  col = 1;
  row = 1;
  string centAccel = "centripetalAccel";
  mat << "\nsubplot(" << numRows << "," << numCols << "," << row * numCols + col + 1 << ")\n";
  mat << centAccel << " = " << m_speedMat << " .^2 .* " << m_curvatureMat << ";\n";
  mat << "plot(" << times[0] << ", " << centAccel << ")\n";
  mat << "ylabel({'Centripetal Acceleration';'(m/s^2)'})\n";

  col = 1;
  row = 2;
  mat << "\nsubplot(" << numRows << "," << numCols << "," << row * numCols + col + 1 << ")\n";
  int derLevel = 1;
  setDerivative(mat, centAccel, derLevel);
  mat << "plot(" << times[derLevel] << ", der)\n";
  mat << "ylabel({'Centripetal Jerk';'(m/s^3)'})\n";
  mat << "xlabel('time (s)')\n";
}

void FsdMatlabPlots::outputMatlabVariables(const string& varName, const string& suffix)
{
  // Generate sheet of matlab plots

  if (m_generatePlots) {
       string matName = varName + ".m";
       m_mat.open(matName.c_str());
       m_mat << setprecision(10);
  }

  double f = m_fsd->getSampleFrequency();

  std::vector<CwlVector > const& ppmPath = m_fsd->getPpmMotorPositions();
  ATrans2 pose = ATrans2::Identity();
  ATrans2Vector path;
  m_fsd->renderTrajectory(pose, path);
  m_mat << m_commands.str() << endl;

  m_mat << "sampleFrequency_" << suffix << " = " << f << ";\n\n";

  m_mat << "time_" << suffix << " = ...\n[";
  for (unsigned int i = 0; i < path.size(); ++i)
    m_mat << i / f << " ";
  m_mat << "];\n\n";

  CwlVector ppmSpeeds = m_fsd->getPpmSpeeds();
  m_mat << "speed_" << suffix << " = ...\n[";
  for (unsigned int i = 0; i < path.size(); ++i)
    m_mat << ppmSpeeds[i] << " ";
  m_mat << "];\n\n";

  CwlVector ppmCurvatures = m_fsd->getPpmCurvatures();
  m_mat << "curvature_" << suffix << " = ...\n[";
  for (unsigned int i = 0; i < path.size(); ++i)
    m_mat << ppmCurvatures[i] << " ";
  m_mat << "];\n\n";

  CwlVector ppmCurvatureRates = m_fsd->getPpmCurvatureRates();
  m_mat << "curvatureRate_" << suffix << " = ...\n[";
  for (unsigned int i = 0; i < path.size(); ++i)
    m_mat << ppmCurvatureRates[i] << " ";
  m_mat << "];\n\n";

  CwlVector ppmCrabs = m_fsd->getPpmCrabs();
  m_mat << "crab_" << suffix << " = ...\n[";
  for (unsigned int i = 0; i < path.size(); ++i)
    m_mat << ppmCrabs[i] << " ";
  m_mat << "];\n\n";

  CwlVector ppmCrabRates = m_fsd->getPpmCrabRates();
  m_mat << "crabRate_" << suffix << " = ...\n[";
  for (unsigned int i = 0; i < path.size(); ++i)
    m_mat << ppmCrabRates[i] << " ";
  m_mat << "];\n\n";

  // Wheels
  const string wheels[] = {
    "left_front_drive", "right_front_drive", "left_rear_drive", "right_rear_drive",
    "left_front_steer", "right_front_steer", "left_rear_steer", "right_rear_steer"
  };
  for (unsigned int j = 0; j < 8; ++j) {
    m_mat << wheels[j] << "_" << suffix << " = ...\n[";
    for (unsigned int i = 0; i < ppmPath[j].size(); ++i) {

      double value = ppmPath[j][i];
      if (j < 4)
        value *= m_fsd->wheelRadius();
      m_mat << value << " ";
    }
    m_mat << "];\n\n";
  }

  const string var[] = {"x", "y", "heading"};
  for (unsigned int j = 0; j < 3; ++j) {
    m_mat << var[j] << "_" << suffix << " = ...\n[";
    for (unsigned int i = 0; i < path.size(); ++i) {
      if (j < 2) {
	m_mat << path[i].translation()[j] << " ";
      }
      else {
	m_mat << atan2(path[i].rotation()(1,0), path[i].rotation()(0,0)) << " ";
      }
    }
    m_mat << "];\n\n";
  }

  if (m_generatePlots)
    m_mat.close();
}

void FsdMatlabPlots::outputMatlab(const string& name, const string& suffix)
{
  cout << name << "_" << suffix << endl;
  // Output matlab code to set all variables from data.

  setVariableNames(suffix);

  // Output matlab code to plot variables.
  outputMatlabPlots(name, suffix);

  // Keep track of plots generated
  m_plotNames.push_back(suffix);
}

// Compares an entire directory of matlab plots with this directory, assuming the same testFsd.cpp file.
// Any differences will be automatically plotted and saved as pdf files.
void FsdMatlabPlots::regressionTest(const string& dir)
{
  // Generate sheet of matlab plots
  ofstream loadExt("loadExt.m");

  loadExt << "function [";
  for (std::list<std::string>::iterator iter = m_plotNames.begin(); iter != m_plotNames.end(); ++iter)
    loadExt << "x" + *iter << " "
            << "y" + *iter << " ";
  loadExt << "] = loadExt(d)\n"
          << "  curDir = pwd;\n"
          << "  cd(d);\n";
  for (std::list<std::string>::iterator iter = m_plotNames.begin(); iter != m_plotNames.end(); ++iter)
    loadExt  << "  pathVars" + *iter << "\n";
  loadExt << "  cd(curDir);\n"
          << "end\n";

  loadExt.close();

  ofstream pathComp("pathComp.m");

  pathComp << "[";
  for (std::list<std::string>::iterator iter = m_plotNames.begin(); iter != m_plotNames.end(); ++iter)
    pathComp << "x_ext" + *iter << " "
             << "y_ext" + *iter << " ";
  pathComp << "] = loadExt('" << dir << "');\n";
  for (std::list<std::string>::iterator iter = m_plotNames.begin(); iter != m_plotNames.end(); ++iter)
    pathComp  << "pathVars" + *iter << ";\n";

  for (std::list<std::string>::iterator iter = m_plotNames.begin(); iter != m_plotNames.end(); ++iter) {
    pathComp << "maxDiff = max(sqrt((x"
             << *iter << "-x_ext" << *iter << ").^2+(y" << *iter << "-y_ext" << *iter << ")));\n";
    pathComp << "if (maxDiff > 0)\n";
    pathComp << "  figure('visible','off');\n";
    pathComp << "  plot("
             << "x_ext" << *iter << ",y_ext" << *iter << ", 'r',"
             << "x"     << *iter << ",y"     << *iter << ", 'g');\n" ;
    pathComp << "  grid on;\n";
    pathComp << "  str = sprintf('path\\\\" << *iter << " Max Offset: %g', maxDiff);\n";
    pathComp << "  disp(str);\n";
    pathComp << "  title(str);\n";
    pathComp << "  h = legend('" << dir << "', 'current',2);\n";
    pathComp << "  orient landscape\n";
    pathComp << "  print -dpdf " << "CompDiff" << *iter << ".pdf\n";
    pathComp << "end\n\n";
  }
  pathComp.close();
  cout << "pathComp\n";
}

void FsdMatlabPlots::outputMatlabComp(const string& base, const string& sufA, const string& sufB) const
{
  const string matName = base + "Comp" + sufA + sufB + ".m";
  ofstream mat(matName.c_str());

  mat << setprecision(10);

  // Load the variables from both sets
  mat << base + "Vars" + sufA << endl;
  mat << base + "Vars" + sufB << endl;

  const string tA = string("time") + sufA;
  const string tB = string("time") + sufB;

  const string xA = string("x") + sufA;
  const string xB = string("x") + sufB;
  const string yA = string("y") + sufA;
  const string yB = string("y") + sufB;
  const string kA = string("curvature") + sufA;
  const string kB = string("curvature") + sufB;
  const string vA = string("speed") + sufA;
  const string vB = string("speed") + sufB;

  mat << "subplot(2,2,1)\n";
  mat << "plot(" << xA << "," << yA << ", 'red')\n";
  mat << "grid on\n";
  mat << "hold on\n";
  mat << "plot(" << xB << "," << yB << ", 'blue')\n";
  mat << "axis equal\n";
  mat << "xlabel('X (meters)')\n";
  mat << "ylabel('Y (meters)')\n";
  mat << "str = sprintf('Max distance offset: %f', max(sqrt((x_1-x_2).^2+(y_1-y_2).^2)));\n";
  mat << "title(str)\n";

  mat << "subplot(2,2,2)\n";
  mat << "plot(" << tA << "," << vA << ", 'red')\n";
  mat << "grid on\n";
  mat << "hold on\n";
  mat << "plot(" << tB << "," << vB << ", 'blue')\n";
  mat << "xlabel('time (s)')\n";
  mat << "ylabel('speed (m/s)')\n";

  mat << "subplot(2,2,3)\n";
  mat << "plot(" << tA << "," << kA << ", 'red')\n";
  mat << "grid on\n";
  mat << "hold on\n";
  mat << "plot(" << tB << "," << kB << ", 'blue')\n";
  mat << "xlabel('time (s)')\n";
  mat << "ylabel('curvature (m/s)')\n";

  mat << "orient landscape\n";
  mat << "print -dpdf " << base << "Comp" << sufA << sufB << endl;
}


#pragma once

// deal.II includes -----------------------------------------------------
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/grid/tria.h>

namespace boltzmann {
template <int dim>
class GridHandlerBase
{
 public:
  GridHandlerBase()
      : fe(1)
      , dofhandler_(triangulation_)
  { /* empty */
  }

  const dealii::Triangulation<dim>& triangulation() const { return triangulation_; }

  const dealii::DoFHandler<dim>& dofhandler() const { return dofhandler_; }

 protected:
  dealii::FE_Q<dim> fe;
  dealii::Triangulation<dim> triangulation_;
  dealii::DoFHandler<dim> dofhandler_;
};

}  // end namespace boltzmann


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/ext/std/pair.hpp>

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

#include <test/auto/product.hpp>
#include <test/cnumeric.hpp>
#include <test/injection.hpp>

#include <utility>
using namespace boost::hana;

namespace boost { namespace hana { namespace test {
    template <>
    auto instances<ext::std::Pair> = make<Tuple>(
        type<Product>
    );

    template <>
    auto objects<ext::std::Pair> = make<Tuple>(
          ::std::make_pair(cnumeric<int, 0>, cnumeric<int, 0>)
        , ::std::make_pair(cnumeric<int, 0>, cnumeric<int, 1>)
        , ::std::make_pair(cnumeric<int, 1>, cnumeric<int, 0>)
        , ::std::make_pair(cnumeric<int, 1>, cnumeric<int, 1>)
    );
}}}

int main() {
    test::check_datatype<ext::std::Pair>();

    using test::x;

    // Product
    {
        // first
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                first(std::make_pair(x<0>, x<1>)),
                x<0>
            ));
        }

        // second
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                second(std::make_pair(x<0>, x<1>)),
                x<1>
            ));
        }

        // make
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                make<ext::std::Pair>(x<0>, x<1>),
                std::make_pair(x<0>, x<1>)
            ));
        }
    }
}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// by S. Efthymiou, October 2018

#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include "Utils/all_utils.hpp"
#include "Utils/lookup.hpp"

#ifndef NETKET_MPS_PERIODIC_HPP
#define NETKET_MPS_PERIODIC_HPP

namespace netket {

template <typename T, bool diag>
class MPSPeriodic : public AbstractMachine<T> {
  using VectorType = typename AbstractMachine<T>::VectorType;
  using MatrixType = typename AbstractMachine<T>::MatrixType;
  using VectorRefType = typename AbstractMachine<T>::VectorRefType;
  using VectorConstRefType = typename AbstractMachine<T>::VectorConstRefType;
  using VisibleConstType = typename AbstractMachine<T>::VisibleConstType;

  const AbstractHilbert &hilbert_;

  // Number of sites
  int N_;
  // Physical dimension
  int d_;
  // Bond dimension
  int D_;
  // Second matrix dimension (D for normal MPS, 1 for diagonal)
  int Dsec_;
  // D squared
  int Dsq_;
  // Number of variational parameters
  int npar_;
  // Period of translational symmetry (has to be a divisor of N)
  int symperiod_;

  // Used for tree look up
  int Nleaves_;
  // Map from site that changes to the corresponding "leaves"
  // Shape (N_, nr of leaves for the corresponding site)
  std::vector<std::vector<int>> leaves_of_site_;
  // Contractions needed to produce each leaf
  // Shape (total leaves, 2)
  std::vector<std::vector<int>> leaf_contractions_;

  // MPS Matrices (stored as [symperiod, d, D, D] or [symperiod, d, D, 1])
  std::vector<std::vector<MatrixType>> W_;

  // Map from Hilbert states to MPS indices
  std::map<double, int> confindex_;
  // Identity Matrix
  MatrixType identity_mat_;

 public:
  using StateType = T;
  using LookupType = Lookup<T>;

  explicit MPSPeriodic(const AbstractHilbert &hilbert, double bond_dim,
                       int symperiod = -1)
      : hilbert_(hilbert),
        N_(hilbert.Size()),
        d_(hilbert.LocalSize()),
        D_(bond_dim),
        symperiod_(symperiod) {
    if (symperiod_ == -1) {
      symperiod_ = N_;
    }

    Init();
  }

  inline MatrixType prod(const MatrixType &m1, const MatrixType &m2) const {
    if (diag) {
      return m1.cwiseProduct(m2);
    }

    return m1 * m2;
  }

  inline T trace(const MatrixType &m) const {
    if (diag) {
      return m.sum();
    }
    return m.trace();
  }

  inline void setparamsident(MatrixType &m, VectorConstRefType pars) const {
    if (diag) {
      for (int i = 0; i < D_; i++) {
        m(i, 0) = T(1, 0) + pars(i);
      }
    } else {
      for (int i = 0; i < D_; i++) {
        for (int j = 0; j < D_; j++) {
          m(i, j) = pars(i * D_ + j);
          if (i == j) {
            m(i, j) += T(1, 0);
          }
        }
      }
    }
  }

  // Auxiliary function that defines the matrices
  void Init() {
    // Initialize parameters
    std::vector<MatrixType> pushback_vec;
    if (diag) {
      Dsec_ = 1;
      identity_mat_ = MatrixType::Ones(D_, 1);
    } else {
      Dsec_ = D_;
      identity_mat_ = MatrixType::Identity(D_, D_);
    }
    MatrixType init_mat = MatrixType::Zero(D_, Dsec_);
    Dsq_ = D_ * Dsec_;
    npar_ = symperiod_ * d_ * Dsq_;

    for (int site = 0; site < symperiod_; site++) {
      W_.push_back(pushback_vec);
      for (int spin = 0; spin < d_; spin++) {
        W_[site].push_back(init_mat);
      }
    }

    // Initialize tree parameters
    InitTree();

    // Machine creation messages
    if (diag) {
      InfoMessage() << "Periodic diagonal MPS machine with " << N_
                    << " sites created" << std::endl;
    } else {
      InfoMessage() << "Periodic MPS machine with " << N_ << " sites created"
                    << std::endl;
    }
    InfoMessage() << "Physical dimension d = " << d_
                  << " and bond dimension D = " << D_ << std::endl;
    if (symperiod_ < N_) {
      InfoMessage() << "Translation invariance is used. Number of "
                       "variational parameters is "
                    << npar_ << " instead of " << npar_ * N_ / symperiod_
                    << std::endl;
    } else {
      InfoMessage() << "Number of variational parameters is " << npar_
                    << std::endl;
    }
    // Initialize map from Hilbert space states to MPS indices
    auto localstates = hilbert_.LocalStates();
    for (int i = 0; i < d_; i++) {
      confindex_[localstates[i]] = i;
    }
  }

  void InitTree() {
    // Initializes vectors used for tree look up tables
    // leaves_of_site_ and leaf_contractions_

    std::vector<int> two_vector(2), empty_vector, level_start;
    std::vector<std::vector<int>> above;
    int level = 1, available_ind = 0, available_level = 0;
    bool available = false;

    level_start.push_back(0);
    level_start.push_back(N_);

    int length = level_start[level] - level_start[level - 1];
    while (length > 1 or available) {
      above.push_back(empty_vector);
      // Iterate level-1
      for (int i = 0; i < length - 1; i += 2) {
        // Construct above for level-1
        above.back().push_back(level_start[level] + i / 2);
        above.back().push_back(level_start[level] + i / 2);
        // Construct leaf_contractions_ for level
        two_vector[0] = level_start[level - 1] + i;
        two_vector[1] = level_start[level - 1] + i + 1;
        leaf_contractions_.push_back(two_vector);
      }
      if (length % 2 == 1) {
        if (available) {
          // Connect the two odd leaves
          above.back().push_back(level_start[level] + (length - 1) / 2);
          above[available_level].push_back(level_start[level] +
                                           (length - 1) / 2);

          two_vector[0] = level_start[level] - 1;
          two_vector[1] = available_ind;
          leaf_contractions_.push_back(two_vector);

          level_start.push_back(level_start.back() + length / 2 + 1);
          available = false;
        } else {
          available = true;
          available_ind = level_start[level] - 1;
          available_level = level - 1;
          level_start.push_back(level_start.back() + length / 2);
        }
      } else {
        level_start.push_back(level_start.back() + length / 2);
      }
      level++;
      length = level_start[level] - level_start[level - 1];
    }
    Nleaves_ = level_start.back();

    // Flatten above vector
    std::vector<int> flat_above;
    for (std::size_t l = 0; l < above.size(); l++) {
      for (std::size_t k = 0; k < above[l].size(); k++) {
        flat_above.push_back(above[l][k]);
      }
    }

    // Create leaves_of_site_ from above vector
    for (int i = 0; i < N_; i++) {
      std::vector<int> leaves;
      leaves.push_back(flat_above[i]);
      while (flat_above[leaves.back()] < Nleaves_ - 1) {
        leaves.push_back(flat_above[leaves.back()]);
      }
      leaves.push_back(Nleaves_ - 1);
      leaves_of_site_.push_back(leaves);
      leaves.clear();
    }
    Nleaves_ += -N_;
  }

  int Npar() const override { return npar_; }

  VectorType GetParameters() override {
    int k = 0;
    VectorType pars(npar_);

    for (int site = 0; site < symperiod_; site++) {
      for (int spin = 0; spin < d_; spin++) {
        for (int i = 0; i < D_; i++) {
          for (int j = 0; j < Dsec_; j++) {
            pars(k) = W_[site][spin](i, j);
            k++;
          }
        }
      }
    }
    return pars;
  }

  void SetParameters(VectorConstRefType pars) override {
    int k = 0;

    for (int site = 0; site < symperiod_; site++) {
      for (int spin = 0; spin < d_; spin++) {
        for (int i = 0; i < D_; i++) {
          for (int j = 0; j < Dsec_; j++) {
            W_[site][spin](i, j) = pars(k);
            k++;
          }
        }
      }
    }
  }

  // Auxiliary function used for setting initial random parameters and adding
  // identities in every matrix
  void SetParametersIdentity(VectorConstRefType pars) {
    int k = 0;
    for (int site = 0; site < symperiod_; site++) {
      for (int spin = 0; spin < d_; spin++) {
        setparamsident(W_[site][spin], pars.segment(k, Dsq_));
        k += Dsq_;
      }
    }
  }

  void InitRandomPars(int seed, double sigma) override {
    VectorType pars(npar_);

    netket::RandomGaussian(pars, seed, sigma);
    SetParametersIdentity(pars);
  }

  int Nvisible() const override { return N_; }

  void InitLookup(VisibleConstType v, LookupType &lt) override {
    for (int k = 0; k < Nleaves_; k++) {
      _InitLookup_check(lt, k);
      if (leaf_contractions_[k][0] < N_) {
        if (leaf_contractions_[k][1] < N_) {
          lt.M(k) = prod(W_[leaf_contractions_[k][0] % symperiod_]
                           [confindex_[v(leaf_contractions_[k][0])]],
                         W_[leaf_contractions_[k][1] % symperiod_]
                           [confindex_[v(leaf_contractions_[k][1])]]);
        } else {
          lt.M(k) = prod(W_[leaf_contractions_[k][0] % symperiod_]
                           [confindex_[v(leaf_contractions_[k][0])]],
                         lt.M(leaf_contractions_[k][1] - N_));
        }

      } else {
        if (leaf_contractions_[k][1] < N_) {
          lt.M(k) = prod(lt.M(leaf_contractions_[k][0] - N_),
                         W_[leaf_contractions_[k][1] % symperiod_]
                           [confindex_[v(leaf_contractions_[k][1])]]);
        } else {
          lt.M(k) = prod(lt.M(leaf_contractions_[k][0] - N_),
                         lt.M(leaf_contractions_[k][1] - N_));
        }
      }
    }
  }

  // Auxiliary function
  inline void _InitLookup_check(LookupType &lt, int i) {
    if (lt.MatrixSize() == i) {
      lt.AddMatrix(D_, Dsec_);
    } else {
      lt.M(i).resize(D_, Dsec_);
    }
  }

  // Auxiliary function for sorting indeces
  // (copied from stackexchange - original answer by Lukasz Wiklendt)
  inline std::vector<std::size_t> sort_indeces(const std::vector<int> &v) {
    // initialize original index locations
    std::vector<std::size_t> idx(v.size());
    std::iota(idx.begin(), idx.end(), 0);
    // sort indexes based on comparing values in v
    std::sort(idx.begin(), idx.end(),
              [&v](std::size_t i1, std::size_t i2) { return v[i1] < v[i2]; });
    return idx;
  }

  void UpdateLookup(VisibleConstType v, const std::vector<int> &tochange,
                    const std::vector<double> &newconf,
                    LookupType &lt) override {
    std::size_t nchange = tochange.size();
    if (nchange <= 0) {
      return;
    }

    MatrixType empty_matrix = MatrixType::Zero(D_, Dsec_);
    std::vector<std::size_t> sorted_ind = sort_indeces(tochange);

    std::set<int> leaves2update;
    std::size_t set_len = 0;
    std::map<int, MatrixType *> base_mat;

    for (std::size_t k = 0; k < nchange; k++) {
      int site = tochange[sorted_ind[k]];
      // Add changed visible matrices to map
      base_mat[site] =
          &(W_[site % symperiod_][confindex_[newconf[sorted_ind[k]]]]);
      // Add the rest of matrices that affects
      for (std::size_t l = 0; l < leaves_of_site_[site].size(); l++) {
        int leaf = leaves_of_site_[site][l];
        leaves2update.insert(leaf);
        if (leaves2update.size() > set_len) {
          set_len++;
          for (int i = 0; i < 2; i++) {
            int lc = leaf_contractions_[leaf - N_][i];
            if (lc < N_) {
              if (base_mat.count(lc) == 0) {
                base_mat[lc] = &(W_[lc % symperiod_][confindex_[v(lc)]]);
              }
            } else {
              base_mat[lc] = &(lt.M(lc - N_));
            }
          }
        }
      }
    }
    for (auto leaf : leaves2update) {
      lt.M(leaf - N_) = prod(*(base_mat[leaf_contractions_[leaf - N_][0]]),
                             *(base_mat[leaf_contractions_[leaf - N_][1]]));
    }
  }

  // Auxiliary function that calculates contractions from site1 to site2
  inline MatrixType mps_contraction(VisibleConstType v, const int &site1,
                                    const int &site2) {
    MatrixType c = identity_mat_;
    for (int site = site1; site < site2; site++) {
      c = prod(c, W_[site % symperiod_][confindex_[v(site)]]);
    }
    return c;
  }

  T LogVal(VisibleConstType v) override {
    return std::log(trace(mps_contraction(v, 0, N_)));
  }

  T LogVal(VisibleConstType /* v */, const LookupType &lt) override {
    return std::log(trace(lt.M(Nleaves_ - 1)));
  }

  VectorType LogValDiff(
      VisibleConstType v, const std::vector<std::vector<int>> &tochange,
      const std::vector<std::vector<double>> &newconf) override {
    const std::size_t nconn = tochange.size();

    std::vector<std::size_t> sorted_ind;
    VectorType logvaldiffs = VectorType::Zero(nconn);
    StateType current_psi = trace(mps_contraction(v, 0, N_));
    MatrixType new_prods(D_, Dsec_);

    for (std::size_t k = 0; k < nconn; k++) {
      std::size_t nchange = tochange[k].size();
      if (nchange > 0) {
        sorted_ind = sort_indeces(tochange[k]);
        int site = tochange[k][sorted_ind[0]];

        if (site == 0) {
          new_prods = W_[0][confindex_[newconf[k][sorted_ind[0]]]];
        } else {
          new_prods = prod(
              mps_contraction(v, 0, site),
              W_[site % symperiod_][confindex_[newconf[k][sorted_ind[0]]]]);
        }

        for (std::size_t i = 1; i < nchange; i++) {
          site = tochange[k][sorted_ind[i]];
          new_prods = prod(
              new_prods,
              prod(mps_contraction(v, tochange[k][sorted_ind[i - 1]] + 1, site),
                   W_[site % symperiod_]
                     [confindex_[newconf[k][sorted_ind[i]]]]));
        }
        site = tochange[k][sorted_ind[nchange - 1]];
        if (site < N_ - 1) {
          new_prods = prod(new_prods, mps_contraction(v, site + 1, N_));
        }
        logvaldiffs(k) = std::log(trace(new_prods) / current_psi);
      }
    }
    return logvaldiffs;
  }

  T LogValDiff(VisibleConstType v, const std::vector<int> &toflip,
               const std::vector<double> &newconf,
               const LookupType &lt) override {
    // Assumes number of levels > 1 (?)
    std::size_t nflip = toflip.size();
    if (nflip <= 0) {
      return T(0, 0);
    }

    MatrixType empty_matrix = MatrixType::Zero(D_, Dsec_);
    std::vector<std::size_t> sorted_ind = sort_indeces(toflip);

    std::set<int> leaves2update;
    std::map<int, MatrixType> ltpM;
    std::size_t set_len = 0;

    std::vector<bool> two_vector(2, false);
    std::map<int, std::vector<bool>> contractions_change;

    for (std::size_t k = 0; k < nflip; k++) {
      int site = toflip[sorted_ind[k]];
      // Add changed visible matrices to map
      ltpM[site] = W_[site % symperiod_][confindex_[newconf[sorted_ind[k]]]];

      // Add the rest of matrices that affects
      for (std::size_t l = 0; l < leaves_of_site_[site].size(); l++) {
        int leaf = leaves_of_site_[site][l];
        leaves2update.insert(leaf);
        if (leaves2update.size() > set_len) {
          set_len++;
          ltpM[leaf] = empty_matrix;
        }
        // Check the contractions of the new matrix (if they change)
        for (int i = 0; i < 2; i++) {
          if (std::find(toflip.begin(), toflip.end(),
                        leaf_contractions_[leaf - N_][i]) != toflip.end()) {
            two_vector[i] = true;
          } else if (leaves2update.find(leaf_contractions_[leaf - N_][i]) !=
                     leaves2update.end()) {
            two_vector[i] = true;
          }
        }
        contractions_change[leaf] = two_vector;
        two_vector[0] = false;
        two_vector[1] = false;
      }
    }

    // Calculate products
    for (auto leaf : leaves2update) {
      std::vector<MatrixType> m(2);
      for (int i = 0; i < 2; i++) {
        int lc = leaf_contractions_[leaf - N_][i];
        if (contractions_change[leaf][i]) {
          m[i] = ltpM[lc];
        } else {
          if (lc < N_) {
            m[i] = W_[lc % symperiod_][confindex_[v(lc)]];
          } else {
            m[i] = lt.M(lc - N_);
          }
        }
      }
      ltpM[leaf] = prod(m[0], m[1]);
    }
    return std::log(trace(ltpM[Nleaves_ + N_ - 1]) / trace(lt.M(Nleaves_ - 1)));
  }

  // Derivative with full calculation
  VectorType DerLog(VisibleConstType v) override {
    MatrixType temp_product(D_, Dsec_);
    std::vector<MatrixType> left_prods, right_prods;
    VectorType der = VectorType::Zero(npar_);

    // Calculate products
    left_prods.push_back(W_[0][confindex_[v(0)]]);
    right_prods.push_back(W_[(N_ - 1) % symperiod_][confindex_[v(N_ - 1)]]);
    for (int site = 1; site < N_ - 1; site++) {
      left_prods.push_back(prod(left_prods[site - 1],
                                W_[site % symperiod_][confindex_[v(site)]]));
      right_prods.push_back(
          prod(W_[(N_ - 1 - site) % symperiod_][confindex_[v(N_ - 1 - site)]],
               right_prods[site - 1]));
    }
    left_prods.push_back(prod(
        left_prods[N_ - 2], W_[(N_ - 1) % symperiod_][confindex_[v(N_ - 1)]]));
    right_prods.push_back(prod(W_[0][confindex_[v(0)]], right_prods[N_ - 2]));

    der.segment(confindex_[v(0)] * Dsq_, Dsq_) +=
        Eigen::Map<VectorType>(right_prods[N_ - 2].transpose().data(), Dsq_);
    for (int site = 1; site < N_ - 1; site++) {
      temp_product = prod(right_prods[N_ - site - 2], left_prods[site - 1]);
      der.segment((d_ * (site % symperiod_) + confindex_[v(site)]) * Dsq_,
                  Dsq_) +=
          Eigen::Map<VectorType>(temp_product.transpose().data(), Dsq_);
    }
    der.segment((d_ * ((N_ - 1) % symperiod_) + confindex_[v(N_ - 1)]) * Dsq_,
                Dsq_) +=
        Eigen::Map<VectorType>(left_prods[N_ - 2].transpose().data(), Dsq_);

    return der / trace(left_prods[N_ - 1]);
  }

  const AbstractHilbert &GetHilbert() const noexcept override {
    return hilbert_;
  }

  // Json functions
  void to_json(json &j) const override {
    j["Name"] = "MPSperiodic";
    j["Length"] = N_;
    j["BondDim"] = D_;
    j["PhysDim"] = d_;
    j["Diagonal"] = diag;
    j["SymmetryPeriod"] = symperiod_;
    for (int i = 0; i < symperiod_; i++) {
      for (int k = 0; k < d_; k++) {
        j["W" + std::to_string(d_ * i + k)] = W_[i][k];
      }
    }
  }

  void from_json(const json &pars) override {
    if (pars.at("Name") != "MPSperiodic") {
      throw InvalidInputError("Error while constructing MPS from Json input");
    }

    if (FieldExists(pars, "Length")) {
      N_ = pars["Length"];
    }
    if (N_ != hilbert_.Size()) {
      throw InvalidInputError(
          "Number of spins is incompatible with given Hilbert space");
    }

    if (FieldExists(pars, "PhysDim")) {
      d_ = pars["PhysDim"];
    }
    if (d_ != hilbert_.LocalSize()) {
      throw InvalidInputError(
          "Number of spins is incompatible with given Hilbert space");
    }

    if (FieldExists(pars, "BondDim")) {
      D_ = pars["BondDim"];
    } else {
      throw InvalidInputError("Unspecified bond dimension");
    }

    if (FieldExists(pars, "SymmetryPeriod")) {
      symperiod_ = pars["SymmetryPeriod"];
    } else {
      // Default is symperiod = N, resp. no translational symmetry
      symperiod_ = N_;
    }

    Init();

    // Loading parameters, if defined in the input
    from_jsonWeights(pars);
  }

  inline void from_jsonWeights(const json &pars) {
    for (int i = 0; i < symperiod_; i++) {
      for (int k = 0; k < d_; k++) {
        if (FieldExists(pars, "W" + std::to_string(d_ * i + k))) {
          W_[i][k] = pars["W" + std::to_string(d_ * i + k)];
        }
      }
    }
  }
};

}  // namespace netket

#endif