/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * 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.
*/

#include "NewtonEulerFrom1DLocalFrameR.hpp"
#include <boost/math/quaternion.hpp>
#include "NewtonEulerDS.hpp"
#include "Interaction.hpp"
#include "BlockVector.hpp"

//#define NERI_DEBUG

//#define NEFC3D_DEBUG
// #define DEBUG_NOCOLOR
// #define DEBUG_STDOUT
// #define DEBUG_MESSAGES
#include "debug.h"
/*
See devNotes.pdf for details. A detailed documentation is available in DevNotes.pdf: chapter 'NewtonEulerR: computation of \nabla q H'. Subsection 'Case FC3D: using the local frame local velocities'
*/
void NewtonEulerFrom1DLocalFrameR::NIcomputeJachqTFromContacts(SP::SiconosVector q1)
{
  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Nz = _Nc->getValue(2);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double Pz = _Pc1->getValue(2);
  double G1x = q1->getValue(0);
  double G1y = q1->getValue(1);
  double G1z = q1->getValue(2);
#ifdef NEFC3D_DEBUG
  printf("contact normal:\n");
  _Nc->display();
  printf("point de contact :\n");
  _Pc1->display();
  printf("center of masse :\n");
  q1->display();
#endif
  _Mabs_C->setValue(0, 0, Nx);
  _Mabs_C->setValue(0, 1, Ny);
  _Mabs_C->setValue(0, 2, Nz);

  _NPG1->zero();

  (*_NPG1)(0, 0) = 0;
  (*_NPG1)(0, 1) = -(G1z - Pz);
  (*_NPG1)(0, 2) = (G1y - Py);
  (*_NPG1)(1, 0) = (G1z - Pz);
  (*_NPG1)(1, 1) = 0;
  (*_NPG1)(1, 2) = -(G1x - Px);
  (*_NPG1)(2, 0) = -(G1y - Py);
  (*_NPG1)(2, 1) = (G1x - Px);
  (*_NPG1)(2, 2) = 0;

  //d1->computeMObjToAbs();
  //SimpleMatrix& Mobj1_abs = *d1->MObjToAbs();
  computeMObjToAbs(q1,_MObjToAbs);

  prod(*_NPG1, *_MObjToAbs, *_AUX1, true);
  prod(*_Mabs_C, *_AUX1, *_AUX2, true);

  for (unsigned int jj = 0; jj < 3; jj++)
    _jachqT->setValue(0, jj, _Mabs_C->getValue(0, jj));

  for (unsigned int jj = 3; jj < 6; jj++)
    _jachqT->setValue(0, jj, _AUX2->getValue(0, jj - 3));

#ifdef NEFC3D_DEBUG
  printf("NewtonEulerFrom1DLocalFrameR jhqt\n");
  _jachqT->display();
#endif
}

void NewtonEulerFrom1DLocalFrameR::NIcomputeJachqTFromContacts(SP::SiconosVector q1, SP::SiconosVector q2)
{
  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Nz = _Nc->getValue(2);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double Pz = _Pc1->getValue(2);
  double G1x = q1->getValue(0);
  double G1y = q1->getValue(1);
  double G1z = q1->getValue(2);

  _Mabs_C->setValue(0, 0, Nx);
  _Mabs_C->setValue(0, 1, Ny);
  _Mabs_C->setValue(0, 2, Nz);

  _NPG1->zero();

  (*_NPG1)(0, 0) = 0;
  (*_NPG1)(0, 1) = -(G1z - Pz);
  (*_NPG1)(0, 2) = (G1y - Py);
  (*_NPG1)(1, 0) = (G1z - Pz);
  (*_NPG1)(1, 1) = 0;
  (*_NPG1)(1, 2) = -(G1x - Px);
  (*_NPG1)(2, 0) = -(G1y - Py);
  (*_NPG1)(2, 1) = (G1x - Px);
  (*_NPG1)(2, 2) = 0;

//  d1->computeMObjToAbs();
//  SimpleMatrix& Mobj1_abs = *d1->MObjToAbs();

  computeMObjToAbs(q1,_MObjToAbs);

  prod(*_NPG1, *_MObjToAbs, *_AUX1, true);
  prod(*_Mabs_C, *_AUX1, *_AUX2, true);

  for (unsigned int jj = 0; jj < 3; jj++)
    _jachqT->setValue(0, jj, _Mabs_C->getValue(0, jj));

  for (unsigned int jj = 3; jj < 6; jj++)
    _jachqT->setValue(0, jj, _AUX2->getValue(0, jj - 3));

  double G2x = q2->getValue(0);
  double G2y = q2->getValue(1);
  double G2z = q2->getValue(2);

  _NPG2->zero();
  (*_NPG2)(0, 0) = 0;
  (*_NPG2)(0, 1) = -(G2z - Pz);
  (*_NPG2)(0, 2) = (G2y - Py);
  (*_NPG2)(1, 0) = (G2z - Pz);
  (*_NPG2)(1, 1) = 0;
  (*_NPG2)(1, 2) = -(G2x - Px);
  (*_NPG2)(2, 0) = -(G2y - Py);
  (*_NPG2)(2, 1) = (G2x - Px);
  (*_NPG2)(2, 2) = 0;

//  d2->computeMObjToAbs();
//  SimpleMatrix& Mobj2_abs = *d2->MObjToAbs();

  computeMObjToAbs(q2,_MObjToAbs);
  prod(*_NPG2, *_MObjToAbs, *_AUX1, true);
  prod(*_Mabs_C, *_AUX1, *_AUX2, true);

  for (unsigned int jj = 0; jj < 3; jj++)
    _jachqT->setValue(0, jj + 6, -_Mabs_C->getValue(0, jj));

  for (unsigned int jj = 3; jj < 6; jj++)
    _jachqT->setValue(0, jj + 6, -_AUX2->getValue(0, jj - 3));
}

void NewtonEulerFrom1DLocalFrameR::initComponents(Interaction& inter, VectorOfBlockVectors& DSlink, VectorOfVectors& workV, VectorOfSMatrices& workM)
{
  NewtonEulerR::initComponents(inter, DSlink, workV, workM);
  //proj_with_q  _jachqProj.reset(new SimpleMatrix(_jachq->size(0),_jachq->size(1)));
  unsigned int qSize = 7 * (inter.getSizeOfDS() / 6);
  _jachq.reset(new SimpleMatrix(1, qSize));

  /* VA 12/04/2016 All of what follows should be put in WorkM*/
  _Mabs_C.reset(new SimpleMatrix(1, 3));
  _MObjToAbs.reset(new SimpleMatrix(3, 3));
  _AUX1.reset(new SimpleMatrix(3, 3));
  _AUX2.reset(new SimpleMatrix(1, 3));
  _NPG1.reset(new SimpleMatrix(3, 3));
  _NPG2.reset(new SimpleMatrix(3, 3));
  //  _isContact=1;
}

void NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0)
{

  DEBUG_BEGIN("NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0 ) \n");
  DEBUG_PRINTF("with time =  %f\n",time);
  DEBUG_PRINTF("with inter =  %p\n",&inter);

  _jachq->setValue(0, 0, _Nc->getValue(0));
  _jachq->setValue(0, 1, _Nc->getValue(1));
  _jachq->setValue(0, 2, _Nc->getValue(2));
  if (inter.has2Bodies())
  {
    _jachq->setValue(0, 7, -_Nc->getValue(0));
    _jachq->setValue(0, 8, -_Nc->getValue(1));
    _jachq->setValue(0, 9, -_Nc->getValue(2));
  }

  for (unsigned int iDS =0 ; iDS < q0->getNumberOfBlocks()  ; iDS++)
  {
    SP::SiconosVector q = (q0->getAllVect())[iDS];
    double sign = 1.0;
    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq : ds%d->q :", iDS);
    DEBUG_EXPR_WE(q->display(););

    ::boost::math::quaternion<double>    quatGP;
    if (iDS == 0)
    {
      ::boost::math::quaternion<double>    quatAux(0, _Pc1->getValue(0) - q->getValue(0), _Pc1->getValue(1) - q->getValue(1),
                                                   _Pc1->getValue(2) - q->getValue(2));
      quatGP = quatAux;
    }
    else
    {
      sign = -1.0;
      //cout<<"NewtonEulerFrom1DLocalFrameR::computeJachq sign is -1 \n";
      ::boost::math::quaternion<double>    quatAux(0, _Pc2->getValue(0) - q->getValue(0), _Pc2->getValue(1) - q->getValue(1),
                                                   _Pc2->getValue(2) - q->getValue(2));
      quatGP = quatAux;
    }
    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :GP :%lf, %lf, %lf\n", quatGP.R_component_2(), quatGP.R_component_3(), quatGP.R_component_4());
    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :Q :%e,%e, %e, %e\n", q->getValue(3), q->getValue(4), q->getValue(5), q->getValue(6));
    ::boost::math::quaternion<double>    quatQ(q->getValue(3), q->getValue(4), q->getValue(5), q->getValue(6));
    ::boost::math::quaternion<double>    quatcQ(q->getValue(3), -q->getValue(4), -q->getValue(5), -q->getValue(6));
    ::boost::math::quaternion<double>    quat0(1, 0, 0, 0);
    ::boost::math::quaternion<double>    quatBuff;
    ::boost::math::quaternion<double>    _2qiquatGP;
    _2qiquatGP = quatGP;
    _2qiquatGP *= 2 * (q->getValue(3));
    quatBuff = (quatGP * quatQ) + (quatcQ * quatGP) - _2qiquatGP;

    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :quattBuuf : %e,%e,%e \n", quatBuff.R_component_2(), quatBuff.R_component_3(), quatBuff.R_component_4());

    _jachq->setValue(0, 7 * iDS + 3, sign * (quatBuff.R_component_2()*_Nc->getValue(0) +
                                             quatBuff.R_component_3()*_Nc->getValue(1) + quatBuff.R_component_4()*_Nc->getValue(2)));
    //cout<<"WARNING NewtonEulerFrom1DLocalFrameR set jachq \n";
    //_jachq->setValue(0,7*iDS+3,0);
    for (unsigned int i = 1; i < 4; i++)
    {
      ::boost::math::quaternion<double>    quatei(0, (i == 1) ? 1 : 0, (i == 2) ? 1 : 0, (i == 3) ? 1 : 0);
      _2qiquatGP = quatGP;
      _2qiquatGP *= 2 * (q->getValue(3 + i));
      quatBuff = quatei * quatcQ * quatGP - quatGP * quatQ * quatei - _2qiquatGP;
      _jachq->setValue(0, 7 * iDS + 3 + i, sign * (quatBuff.R_component_2()*_Nc->getValue(0) +
                                                   quatBuff.R_component_3()*_Nc->getValue(1) + quatBuff.R_component_4()*_Nc->getValue(2)));
    }
  }

  DEBUG_EXPR(_jachq->display(););
  DEBUG_END("NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0 \n");

}

void NewtonEulerFrom1DLocalFrameR::computeJachqT(Interaction& inter, SP::BlockVector q0 )
{
  DEBUG_BEGIN("NewtonEulerFrom1DLocalFrameR::computeJachqT(Interaction& inter, SP::BlockVector q0 \n")

  if (q0->getNumberOfBlocks()>1)
  {
    NIcomputeJachqTFromContacts((q0->getAllVect())[0], (q0->getAllVect())[1]);
  }
  else
  {
    NIcomputeJachqTFromContacts((q0->getAllVect())[0]);
  }

  DEBUG_END("NewtonEulerFrom1DLocalFrameR::computeJachqT(Interaction& inter, SP::BlockVector q0) \n");

}


#pragma once

/*
Game Tree Search Algorithms
Copyright (C) Adam Stelmaszczyk <stelmaszczyk.adam@gmail.com>

This program is free software: 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 3 of the License, or
(at your option) any later version.

This program 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 General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <boost/math/distributions/binomial.hpp>
#include <unordered_map>
#include <unordered_set>
#include <sys/time.h>
#include <algorithm>
#include <iostream>
#include <assert.h>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <memory>
#include <random>
#include <vector>

using namespace std;

static const int MAX_SIMULATIONS = 10000000;
static const double UCT_C = sqrt(2);
static const double WIN_SCORE = 1;
static const double DRAW_SCORE = 0.5;
static const double LOSE_SCORE = 0;

static const int MAX_DEPTH = 20;
static const int INF = 2147483647;

struct Random {

    virtual ~Random() {}

    int uniform(int min, int max) const {
        mt19937 engine;
        uniform_int_distribution<int> distribution(min, max);
        return distribution(engine);
    }
};

struct Timer {
    double start_time;

    virtual ~Timer() {}

    void start() {
        start_time = get_time();
    }

    double get_time() const {
        timeval tv;
        gettimeofday(&tv, 0);
        return tv.tv_sec + tv.tv_usec * 1e-6;
    }

    double seconds_elapsed() const {
        return get_time() - start_time;
    }

    bool exceeded(double seconds) const {
        return seconds_elapsed() > seconds;
    }

    friend ostream &operator<<(ostream &os, const Timer &timer) {
        return os << setprecision(2) << fixed << timer.seconds_elapsed() << "s";
    }
};

template<class M>
struct Move {
    virtual ~Move() {}

    virtual void read(istream &stream) = 0;

    virtual ostream &to_stream(ostream &os) const = 0;

    friend ostream &operator<<(ostream &os, const Move &move) {
        return move.to_stream(os);
    }

    virtual bool operator==(const M &rhs) const = 0;

    virtual size_t hash() const = 0;
};

enum TTEntryType { EXACT_VALUE, LOWER_BOUND, UPPER_BOUND };

template<class M>
struct TTEntry {
    M move;
    int depth;
    int value;
    TTEntryType value_type;

    TTEntry() {}

    virtual ~TTEntry() {}

    TTEntry(const M &move, int depth, int value, TTEntryType value_type) :
            move(move), depth(depth), value(value), value_type(value_type) {}

    ostream &to_stream(ostream &os) {
        return os << "move: " << move << " depth: " << depth << " value: " << value << " value_type: " << value_type;
    }

    friend ostream &operator<<(ostream &os, const TTEntry &entry) {
        return entry.to_stream(os);
    }
};

template<class S, class M>
struct State {
    unsigned visits = 0;
    double score = 0;
    char player_to_move = 0;
    S *parent = nullptr;
    unordered_map<size_t, shared_ptr<S>> children = unordered_map<size_t, shared_ptr<S>>();

    State(char player_to_move) : player_to_move(player_to_move) {}

    virtual ~State() {}

    void update_stats(double result) {
        score += result;
        ++visits;
    }

    double get_uct(double c) const {
        assert(visits > 0);
        double parent_visits = 0.0;
        if (parent != nullptr) {
            parent_visits = parent->visits;
        }
        return score / visits + c * sqrt(log(parent_visits) / visits);
    }

    shared_ptr<S> create_child(const M &move) {
        S child = clone();
        child.make_move(move);
        child.parent = (S*) this;
        return make_shared<S>(child);
    }

    S* add_child(const M &move) {
        const auto child = create_child(move);
        const auto key = move.hash();
        const auto pair = children.insert({key, child});
        const auto it = pair.first;
        return it->second.get();
    }

    S* get_child(const M &move) const {
        const auto key = move.hash();
        const auto it = children.find(key);
        if (it == children.end()) {
            return nullptr;
        }
        return it->second.get();
    }

    virtual string to_executable_format() const {
        stringstream ss;
        ss << *this;
        return ss.str();
    }

    virtual void swap_players() {}

    virtual S clone() const = 0;

    virtual int get_goodness() const = 0;

    virtual vector<M> get_legal_moves(int max_moves) const = 0;

    virtual char get_enemy(char player) const = 0;

    virtual bool is_terminal() const = 0;

    virtual bool is_winner(char player) const = 0;

    virtual void make_move(const M &move) = 0;

    virtual void undo_move(const M &move) = 0;

    virtual ostream &to_stream(ostream &os) const = 0;

    friend ostream &operator<<(ostream &os, const State &state) {
        return state.to_stream(os);
    }

    virtual bool operator==(const S &other) const = 0;

    virtual size_t hash() const = 0;
};

template<class S, class M>
struct Algorithm {

    stringstream log;

    Algorithm() { }

    Algorithm(const Algorithm& algorithm) {}

    virtual ~Algorithm() {}

    string read_log() {
        string result = log.str();
        log.str("");
        return result;
    }

    virtual void reset() {}

    virtual M get_move(const S *state) = 0;

    virtual string get_name() const = 0;

    friend ostream &operator<<(ostream &os, const Algorithm &algorithm) {
        os << algorithm.get_name();
        return os;
    }
};

template<class S, class M>
struct Human : public Algorithm<S, M> {

    Human() : Algorithm<S, M>() {}

    M get_move(const S *state) override {
        const vector<M> &legal_moves = state->get_legal_moves();
        if (legal_moves.empty()) {
            stringstream stream;
            state->to_stream(stream);
            throw invalid_argument("Given state is terminal:\n" + stream.str());
        }
        while (true) {
            M move = M();
            move.read();
            if (find(legal_moves.begin(), legal_moves.end(), move) != legal_moves.end()) {
                return move;
            } else {
                cout << "Move " << move << " is not legal" << endl;
            }
        }
    }

    string get_name() const {
        return "Human";
    }
};

template<class S, class M>
struct Executable : public Algorithm<S, M> {

    const string executable;

    Executable(string executable) : Algorithm<S, M>(), executable(executable) {}

    M get_move(const S *state) override {
        const vector<M> &legal_moves = state->get_legal_moves();
        if (legal_moves.empty()) {
            stringstream stream;
            state->to_stream(stream);
            throw invalid_argument("Given state is terminal:\n" + stream.str());
        }

        stringstream cmd;
        cmd << "echo \"" << state->to_executable_format() << "\" | " << executable;

        const string output = run_cmd(cmd.str());
        stringstream stream_to_read(output);

        M move = M();
        move.read(stream_to_read);
        if (find(legal_moves.begin(), legal_moves.end(), move) != legal_moves.end()) {
            return move;
        } else {
            stringstream message;
            message << "Legal moves: ";
            for (auto move : legal_moves) {
                message << move << ", ";
            }
            message << endl;
            message << "Move " << move << " is not legal, state:" << endl;
            message << *state;
            throw runtime_error(message.str());
        }
    }

    string run_cmd(string cmd) {
        stringstream result;
        const int BUFFER_SIZE = 128;
        char buffer[BUFFER_SIZE];
        shared_ptr<FILE> pipe(popen(cmd.c_str(), "r"), pclose);
        if (!pipe) {
            throw runtime_error("popen() failed");
        }
        while (!feof(pipe.get())) {
            if (fgets(buffer, BUFFER_SIZE, pipe.get()) != NULL) {
                result << buffer;
            }
        }
        return result.str();
    }

    string get_name() const {
        return "Executable " + executable;
    }
};

template<class M>
struct MinimaxResult {
    int goodness;
    M best_move;
    bool completed;
};

template<class S, class M>
struct Minimax : public Algorithm<S, M> {
    unordered_map<size_t, TTEntry<M>> transposition_table;
    const double MAX_SECONDS;
    const int MAX_MOVES;
    function<vector<M>(const S*, int)> get_legal_moves;
    function<int(const S*)> get_goodness;
    Timer timer;
    int scout_cuts;
    int beta_cuts, cut_bf_sum;
    int tt_hits, tt_exacts, tt_cuts;
    int nodes, leafs;

    Minimax(double max_seconds = 1, int max_moves = INF, function<vector<M>(const S*, int)> get_legal_moves = nullptr, function<int(const S*)> get_goodness = nullptr) :
            Algorithm<S, M>(),
            transposition_table(unordered_map<size_t, TTEntry<M>>(1000000)),
            MAX_SECONDS(max_seconds),
            MAX_MOVES(max_moves),
            get_legal_moves(get_legal_moves),
            get_goodness(get_goodness),
            timer(Timer()) {}

    void reset() {
        transposition_table.clear();
    }

    M get_move(const S *state) override {
        if (state->is_terminal()) {
            stringstream stream;
            state->to_stream(stream);
            throw invalid_argument("Given state is terminal:\n" + stream.str());
        }
        if (get_legal_moves == nullptr) {
            get_legal_moves = &State<S,M>::get_legal_moves;
        }
        if (get_goodness == nullptr) {
            get_goodness = &State<S,M>::get_goodness;
        }
        timer.start();

        const auto moves = get_legal_moves(state, MAX_MOVES);
        this->log << "moves: " << moves.size() << endl;
        for (const auto move : moves) {
            this->log << move << ", ";
        }
        this->log << endl;

        M best_move;
        for (int max_depth = 1; max_depth <= MAX_DEPTH; ++max_depth) {
            scout_cuts = 0;
            beta_cuts = 0;
            cut_bf_sum = 0;
            tt_hits = 0;
            tt_exacts = 0;
            tt_cuts = 0;
            nodes = 0;
            leafs = 0;
            S clone = state->clone();
            auto result = minimax(&clone, max_depth, -INF, INF);
            if (result.completed) {
                best_move = result.best_move;
                this->log << "goodness: " << result.goodness
                << " time: " << timer
                << " move: " << best_move
                << " nodes: " << nodes
                << " leafs: " << leafs
                << " scout_cuts: " << scout_cuts
                << " beta_cuts: " << beta_cuts
                << " cutBF: " << (double) cut_bf_sum / beta_cuts
                << " tt_hits: " << tt_hits
                << " tt_exacts: " << tt_exacts
                << " tt_cuts: " << tt_cuts
                << " tt_size: " << transposition_table.size()
                << " max_depth: " << max_depth << endl;
            }
            if (timer.exceeded(MAX_SECONDS)) {
                break;
            }
        }
        return best_move;
    }

    // Find Minimax value of the given tree,
    // Minimax value lies within a range of [alpha; beta] window.
    // Whenever alpha >= beta, further checks of children in a node can be pruned.
    MinimaxResult<M> minimax(S *state, int depth, int alpha, int beta) {
        ++nodes;
        const int alpha_original = alpha;

        M best_move;
        if (depth == 0 || state->is_terminal()) {
            ++leafs;
            return {get_goodness(state), best_move, false};
        }

        TTEntry<M> entry;
        const bool entry_found = get_tt_entry(state, entry);
        if (entry_found && entry.depth >= depth) {
            ++tt_hits;
            if (entry.value_type == TTEntryType::EXACT_VALUE) {
                ++tt_exacts;
                return {entry.value, entry.move, true};
            }
            if (entry.value_type == TTEntryType::LOWER_BOUND && alpha < entry.value) {
                alpha = entry.value;
            }
            if (entry.value_type == TTEntryType::UPPER_BOUND && beta > entry.value) {
                beta = entry.value;
            }
            if (alpha >= beta) {
                ++tt_cuts;
                return {entry.value, entry.move, true};
            }
        }

        int max_goodness = -INF;

        bool completed = true;
        const auto legal_moves = get_legal_moves(state, MAX_MOVES);
        assert(legal_moves.size() > 0);
        for (int i = 0; i < legal_moves.size(); i++) {
            const auto move = legal_moves[i];
            state->make_move(move);
            int goodness;
            if (i > 0) {
                // null window search
                goodness = -minimax(
                    state,
                    depth - 1,
                    -alpha - 1,
                    -alpha
                ).goodness;
                if (alpha < goodness && goodness < beta) {
                    // failed high, do a full re-search
                    goodness = -minimax(
                        state,
                        depth - 1,
                        -beta,
                        -goodness
                    ).goodness;
                } else {
                    scout_cuts++;
                }
            }
            else {
                goodness = -minimax(
                    state,
                    depth - 1,
                    -beta,
                    -alpha
                ).goodness;
            }
            state->undo_move(move);
            if (timer.exceeded(MAX_SECONDS)) {
                completed = false;
                break;
            }
            if (max_goodness < goodness) {
                max_goodness = goodness;
                best_move = move;
                if (max_goodness >= beta) {
                    ++beta_cuts;
                    cut_bf_sum += i + 1;
                    break;
                }
            }
            if (alpha < max_goodness) {
                alpha = max_goodness;
            }
        }

        if (completed) {
            update_tt(state, alpha_original, beta, max_goodness, best_move, depth);
        }

        return {max_goodness, best_move, completed};
    }

    bool get_tt_entry(const S *state, TTEntry<M> &entry) const {
        const auto key = state->hash();
        const auto it = transposition_table.find(key);
        if (it == transposition_table.end()) {
            return false;
        }
        entry = it->second;
        return true;
    }

    void add_tt_entry(const S *state, const TTEntry<M> &entry) {
        const auto key = state->hash();
        transposition_table.insert({key, entry});
    }

    void update_tt(const S *state, int alpha, int beta, int max_goodness, const M &best_move, int depth) {
        TTEntryType value_type;
        if (max_goodness <= alpha) {
            value_type = TTEntryType::UPPER_BOUND;
        }
        else if (max_goodness >= beta) {
            value_type = TTEntryType::LOWER_BOUND;
        }
        else {
            value_type = TTEntryType::EXACT_VALUE;
        }
        const TTEntry<M> entry = {best_move, depth, max_goodness, value_type};
        add_tt_entry(state, entry);
    }

    string get_name() const {
        return "Minimax";
    }
};

template<class S, class M>
struct MonteCarloTreeSearch : public Algorithm<S, M> {
    const double max_seconds;
    const int max_simulations;
    const bool block;
    const Random random;

    MonteCarloTreeSearch(double max_seconds = 1,
                         int max_simulations = MAX_SIMULATIONS,
                         bool block = false) :
        Algorithm<S, M>(),
        max_seconds(max_seconds),
        block(block),
        max_simulations(max_simulations) {}

    M get_move(const S *root) override {
        if (root->is_terminal()) {
            stringstream stream;
            root->to_stream(stream);
            throw invalid_argument("Given state is terminal:\n" + stream.str());
        }
        Timer timer;
        timer.start();
        int simulation = 0;
        S clone = root->clone();
        while (simulation < max_simulations && !timer.exceeded(max_seconds)) {
            monte_carlo_tree_search(&clone);
            ++simulation;
        }
        this->log << "ratio: " << root->score / root->visits << endl;
        this->log << "simulations: " << simulation << endl;
        const auto legal_moves = root->get_legal_moves();
        this->log << "moves: " << legal_moves.size() << endl;
        for (const auto move : legal_moves) {
            this->log << "move: " << move;
            const auto child = root->get_child(move);
            if (child != nullptr) {
                this->log << " score: " << child->score
                << " visits: " << child->visits
                << " UCT: " << child->get_uct(UCT_C);
            }
            this->log << endl;
        }
        return get_most_visited_move(&clone);
    }

    void monte_carlo_tree_search(S *root) const {
        S *current = tree_policy(root, root);
        const auto result = rollout(current, root);
        propagate_up(current, result);
    }

    void propagate_up(S *current, double result) const {
        current->update_stats(result);
        if (current->parent) {
            propagate_up(current->parent, result);
        }
    }

    S* tree_policy(S *state, const S *root) const {
        if (state->is_terminal()) {
            return state;
        }
        const M move = get_tree_policy_move(state, root);
        const auto child = state->get_child(move);
        if (child == nullptr) {
            return state->add_child(move);
        }
        return tree_policy(child, root);
    }

    M get_most_visited_move(const S *state) const {
        const auto legal_moves = state->get_legal_moves();
        assert(legal_moves.size() > 0);
        M best_move;
        double max_visits = -INF;
        for (const auto move : legal_moves) {
            const auto child = state->get_child(move);
            if (child != nullptr) {
                const auto visits = child->visits;
                if (max_visits < visits) {
                    max_visits = visits;
                    best_move = move;
                }
            }
        }
        assert(max_visits != -INF);
        return best_move;
    }

    M get_best_move(S *state, const S *root) const {
        const auto legal_moves = state->get_legal_moves();
        assert(legal_moves.size() > 0);
        M best_move;
        if (state->player_to_move == root->player_to_move) {
            // maximize
            double best_uct = -INF;
            for (const auto move : legal_moves) {
                const auto child = state->get_child(move);
                if (child != nullptr) {
                    const auto uct = child->get_uct(UCT_C);
                    if (best_uct < uct) {
                        best_uct = uct;
                        best_move = move;
                    }
                } else {
                    return move;
                }
            }
        }
        else {
            // minimize
            double best_uct = INF;
            for (const auto move : legal_moves) {
                const auto child = state->get_child(move);
                if (child != nullptr) {
                    const auto uct = child->get_uct(-UCT_C);
                    if (best_uct > uct) {
                        best_uct = uct;
                        best_move = move;
                    }
                } else {
                    return move;
                }
            }
        }
        return best_move;
    }

    M get_random_move(const S *state) const {
        const auto legal_moves = state->get_legal_moves();
        assert(legal_moves.size() > 0);
        const int index = random.uniform(0, legal_moves.size() - 1);
        return legal_moves[index];
    }

    shared_ptr<M> get_winning_move(const S *state) const {
        const auto current_player = state->player_to_move;
        const auto legal_moves = state->get_legal_moves();
        S clone = state->clone();
        assert(legal_moves.size() > 0);
        for (const M &move : legal_moves) {
            clone.make_move(move);
            if (clone.is_winner(current_player)) {
                return make_shared<M>(move);
            }
            clone.undo_move(move);
        }
        return nullptr;
    }

    shared_ptr<M> get_blocking_move(const S *state) const {
        const auto current_player = state->player_to_move;
        const auto enemy = state->get_enemy(current_player);
        S enemy_state = state->clone();
        enemy_state.player_to_move = enemy;
        return get_winning_move(&enemy_state);
    }

    M get_tree_policy_move(S *state, const S *root) const {
        // If player has a winning move he makes it.
        auto move_ptr = get_winning_move(state);
        if (move_ptr != nullptr) {
            return *move_ptr;
        }
        if (block) {
            // If player has a blocking move he makes it.
            move_ptr = get_blocking_move(state);
            if (move_ptr != nullptr) {
                return *move_ptr;
            }
        }
        return get_best_move(state, root);
    }

    M get_default_policy_move(const S *state) const {
        // If player has a winning move he makes it.
        auto move_ptr = get_winning_move(state);
        if (move_ptr != nullptr) {
            return *move_ptr;
        }
        // If player has a blocking move he makes it.
        move_ptr = get_blocking_move(state);
        if (move_ptr != nullptr) {
            return *move_ptr;
        }
        return get_random_move(state);
    }

    double rollout(S *current, const S *root) const {
        if (current->is_terminal()) {
            if (current->is_winner(root->player_to_move)) {
                return WIN_SCORE;
            }
            if (current->is_winner(root->get_enemy(root->player_to_move))) {
                return LOSE_SCORE;
            }
            return DRAW_SCORE;
        }
        M move = get_default_policy_move(current);
        current->make_move(move);
        auto result = rollout(current, root);
        current->undo_move(move);
        return result;
    }

    string get_name() const {
        return "MonteCarloTreeSearch";
    }

};

struct OutcomeCounts {
    int wins = 0;
    int draws = 0;
    int loses = 0;
};

template<class S, class M>
struct Tester {
    S *root = nullptr;
    Algorithm<S, M> &algorithm_1;
    Algorithm<S, M> &algorithm_2;
    const int MATCHES;
    const bool VERBOSE;
    const bool SAVE;
    const double SIGNIFICANCE_LEVEL = 0.005; // two sided 99% confidence interval

    Tester(S *state,
           Algorithm<S, M> &algorithm_1,
           Algorithm<S, M> &algorithm_2,
           int matches = INF,
           bool verbose = false,
           bool save = false
    ) : root(state), algorithm_1(algorithm_1), algorithm_2(algorithm_2), MATCHES(matches), VERBOSE(verbose), SAVE(save) {}

    virtual ~Tester() {}

    OutcomeCounts start() {
        Timer all_timer;
        all_timer.start();
        OutcomeCounts outcome_counts;
        unordered_set<int> unique_game_hashes;
        const char enemy = root->get_enemy(root->player_to_move);
        for (int i = 1; i <= MATCHES; ++i) {
            int move_number = 1;
            auto current = root->clone();
            if (i % 4 == 0 || i % 4 == 2) {
                current.player_to_move = current.get_enemy(current.player_to_move);
            }
            if (i % 4 == 0 || i % 4 == 3) {
                current.swap_players();
            }
            if (VERBOSE) {
                cout << current << endl;
            }
            if (SAVE) {
                save_file(move_number, current);
            }
            int game_hash = current.hash();
            while (!current.is_terminal()) {
                auto &algorithm = (current.player_to_move == root->player_to_move) ? algorithm_1 : algorithm_2;
                if (VERBOSE) {
                    cout << current.player_to_move << " " << algorithm << endl;
                }
                algorithm.reset();
                Timer timer;
                timer.start();
                auto move = algorithm.get_move(&current);
                if (VERBOSE) {
                    cout << algorithm.read_log();
                    cout << timer << endl;
                }
                current.make_move(move);
                ++move_number;
                if (VERBOSE) {
                    cout << current << endl;
                }
                if (SAVE) {
                    save_file(move_number, current);
                }
                game_hash ^= current.hash();
            }
            cout << "Game " << i << ": ";
            auto insert = unique_game_hashes.insert(game_hash);
            if (!insert.second) {
                cout << "Not unique, not counting" << endl << endl;
                continue;
            }
            if (current.is_winner(root->player_to_move)) {
                ++outcome_counts.wins;
                cout << root->player_to_move << " " << algorithm_1 << " won" << endl;
            } else if (current.is_winner(enemy)) {
                ++outcome_counts.loses;
                cout << enemy << " " << algorithm_2 << " won" << endl;
            } else {
                ++outcome_counts.draws;
                cout << "draw" << endl;
            }
            const int unique_games_count = unique_game_hashes.size();
            cout << "Unique games: " << unique_games_count << endl;
            cout << root->player_to_move << " " << algorithm_1 << " wins: " << outcome_counts.wins << endl;
            cout << enemy << " " << algorithm_2 << " wins: " << outcome_counts.loses << endl;
            cout << "Draws: " << outcome_counts.draws << endl;
            const double successes = outcome_counts.wins + 0.5 * outcome_counts.draws;
            const double ratio = successes / unique_games_count;
            cout << "Ratio: " << ratio << endl;
            const double lower = boost::math::binomial_distribution<>::find_lower_bound_on_p(unique_games_count, successes, SIGNIFICANCE_LEVEL);
            const double upper = boost::math::binomial_distribution<>::find_upper_bound_on_p(unique_games_count, successes, SIGNIFICANCE_LEVEL);
            cout << "Lower confidence bound: " << lower << endl;
            cout << "Upper confidence bound: " << upper << endl;
            cout << endl;
            if (upper < 0.5 || lower > 0.5) {
                cout << "Total time: " << all_timer << endl;
                break;
            }
        }
        if (SAVE) {
            shell("convert -delay 100 -loop 0 $(ls -v *.gif) game.gif");
            shell("rm [0-9]*.gif");
        }
        return outcome_counts;
    }

    void save_file(int move_number, const S &state) const {
        const int FONT_SIZE = 16;
        stringstream ss;
        ss << state;
        vector<string> lines;
        string line;
        while (getline(ss, line)) {
            lines.push_back(line);
        }
        assert(lines.size() > 0);
        const int width = lines[0].length() * FONT_SIZE;
        const int height = lines.size() * FONT_SIZE;
        stringstream command;
        command << "convert -size " << width << "x" << height;
        command << " xc:black -font square.ttf -pointsize " << FONT_SIZE << " -fill white -draw \"";
        for (int y = 0; y < lines.size(); y++) {
            command << "text 0," << (y + 1) * FONT_SIZE << " '" << lines[y] << "' ";
        }
        command << "\" " << move_number << ".gif";
        shell(command.str());
    }

    void shell(const string &command) const {
        const int return_code = system(command.c_str());
        if (return_code != 0) {
            cout << "Command " << command << " returned " << return_code;
        }
    }
};


#include "CoordinateConversions.h"
#include "CoordinateConverter.h"
#include "GeneralFunctions.h"
#include "GeneralDefinitions.h"
#include "AutoThreadLock.h"
#include "AutoWriteLock.h"
#include "AutoReadLock.h"

#include <macgyver/Exception.h>
#include <boost/algorithm/string/predicate.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <boost/make_shared.hpp>
#include <boost/functional/hash.hpp>
#include <ogr_geometry.h>
#include <ogr_spatialref.h>

#include <newbase/NFmiLocation.h>
#include <unordered_map>

std::unordered_map<std::size_t,SmartMet::CoordinateConverter> *coordinateConverterCache = nullptr;
SmartMet::ModificationLock coordinateConverterCache_modificationLock;

bool convert(const OGRSpatialReference *sr_from,const OGRSpatialReference *sr_to,int nCount,double *x,double *y)
{
  try
  {
    if (coordinateConverterCache == nullptr)
    {
      SmartMet::AutoWriteLock writeLock(&coordinateConverterCache_modificationLock);
      if (coordinateConverterCache == nullptr)
        coordinateConverterCache = new std::unordered_map<std::size_t,SmartMet::CoordinateConverter>;
    }

    std::size_t hash = (std::size_t)sr_from;
    boost::hash_combine(hash, (std::size_t)sr_to);

    {
      SmartMet::AutoReadLock readLock(&coordinateConverterCache_modificationLock);
      auto rec = coordinateConverterCache->find(hash);
      if (rec != coordinateConverterCache->end())
        return rec->second.convert(nCount,x,y);
    }

    SmartMet::AutoWriteLock writeLock(&coordinateConverterCache_modificationLock);
    if (coordinateConverterCache->size() > 1000000)
      coordinateConverterCache->clear();

    SmartMet::CoordinateConverter tr(sr_from,sr_to);
    coordinateConverterCache->insert(std::pair<std::size_t,SmartMet::CoordinateConverter>(hash,tr));
    return tr.convert(nCount,x,y);
  }
  catch (...)
  {
    throw Fmi::Exception(BCP,"Operation failed!",nullptr);
  }
}

double getValue(double theAngle)
{
  try
  {
    if (::fabs(theAngle - 180) < 0.0000001)
      return 180;
    else if (::fabs(theAngle + 180) < 0.0000001)
      return -180;
    else if (theAngle > 180)
      return getValue(theAngle - 360);
    else if (theAngle < -180)
      return getValue(theAngle + 360);

    return theAngle;
  }
  catch (...)
  {
    throw Fmi::Exception(BCP,"Operation failed!",nullptr);
  }
}

void latlon_to_rotatedLatlon(double lat,double lon,double southPoleLat,double southPoleLon,double& rotLat,double& rotLon)
{
  try
  {
    if (southPoleLat == -90)
    {
      rotLat = lat;
      rotLon = lon;
      return;
    }

    double SinYPole = sin((southPoleLat + 90) * kDegToRad);
    double CosYPole = cos((southPoleLat + 90) * kDegToRad);

    double SinXReg = sin((lon - southPoleLon) * kDegToRad);
    double CosXReg = cos((lon - southPoleLon) * kDegToRad);
    double SinYReg = sin(lat * kDegToRad);
    double CosYReg = cos(lat * kDegToRad);
    double SinYRot = CosYPole * SinYReg - SinYPole * CosYReg * CosXReg;
    SinYRot = std::min(std::max(SinYRot, -1.), 1.);
    double YRot = asin(SinYRot) * kOneRad;

    double CosYRot = cos(YRot * kDegToRad);
    double CosXRot = (CosYPole * CosYReg * CosXReg + SinYPole * SinYReg) / CosYRot;
    CosXRot = std::min(std::max(CosXRot, -1.), 1.);
    double SinXRot = CosYReg * SinXReg / CosYRot;
    double XRot = acos(CosXRot) * kOneRad;
    if (SinXRot < 0.)
      XRot = getValue(-XRot);
    else
      XRot = getValue(XRot);

    rotLon = XRot;
    rotLat = YRot;
  }
  catch (...)
  {
    throw Fmi::Exception(BCP,"Operation failed!",nullptr);
  }
}

void rotatedLatlon_to_latlon(double rotLat,double rotLon,double southPoleLat,double southPoleLon,double& lat,double& lon)
{
  try
  {
    if (southPoleLat == -90)
    {
      lat = rotLat;
      lon = rotLon;
      return;
    }

    double SinYPole = sin((southPoleLat + 90) * kDegToRad);
    double CosYPole = cos((southPoleLat + 90) * kDegToRad);

    double SinXRot = sin(rotLon * kDegToRad);
    double CosXRot = cos(rotLon * kDegToRad);
    double SinYRot = sin(rotLat * kDegToRad);
    double CosYRot = cos(rotLat * kDegToRad);
    double SinYReg = CosYPole * SinYRot + SinYPole * CosYRot * CosXRot;
    SinYReg = std::min(std::max(SinYReg, -1.0), 1.0);

    double YReg = asin(SinYReg) * kOneRad;

    double CosYReg = cos(YReg * kDegToRad);
    double CosXReg = (CosYPole * CosYRot * CosXRot - SinYPole * SinYRot) / CosYReg;
    CosXReg = std::min(std::max(CosXReg, -1.0), 1.0);
    double SinXReg = CosYRot * SinXRot / CosYReg;

    double XRegVal = acos(CosXReg) * kOneRad;
    if (SinXReg < 0.)
      XRegVal = -XRegVal;

    XRegVal = XRegVal + southPoleLon;
    double XReg = XRegVal;

    lat = YReg;
    lon = XReg;
  }
  catch (...)
  {
    throw Fmi::Exception(BCP,"Operation failed!",nullptr);
  }
}

double latlon_distance(double lat1, double lon1, double lat2, double lon2)
{
  double p = 0.017453292519943295;    // Math.PI / 180
  double a = 0.5 - cos((lat2 - lat1) * p)/2 +
          cos(lat1 * p) * cos(lat2 * p) *
          (1 - cos((lon2 - lon1) * p))/2;

  return 12742 * asin(sqrt(a)); // 2 * R; R = 6371 km
}

double latlon_width(double lat, double longitudes)
{
  try
  {
    // Metric width of 1 degree
    double w = latlon_distance(lat,0,lat,1);
    return longitudes * w;
  }
  catch (...)
  {
    throw Fmi::Exception(BCP,"Operation failed!",nullptr);
  }
}

double latlon_height(double lon, double latitudes)
{
  try
  {
    // Metric width of 1 degree
    double h = latlon_distance(0,lon,1,lon);
    return latitudes * h;
  }
  catch (...)
  {
    throw Fmi::Exception(BCP,"Operation failed!",nullptr);
  }
}

std::pair<std::vector<SmartMet::T::Coordinate>, std::vector<double>> getIsocirclePoints(double lon1, double lat1, double lon2, double lat2, std::size_t steps)
{
  try
  {
    // Sanity checks
    if (lon1 == lon2 && lat1 == lat2)
      throw Fmi::Exception(BCP, "Ill-defined isocircle: start and end points are equal");

    if (std::abs(lon1 - lon2) == 180 && std::abs(lat1 - (90 - lat2)) == 90)
      throw Fmi::Exception(BCP, "Ill-defined isocircle: points at opposing ends of the earth");

    if (steps < 1 || steps > 10000)
      throw Fmi::Exception(BCP,"Number of points on isocircle must be 1-10000, not " + boost::lexical_cast<std::string>(steps));

    // Calculate bearing and distance to be travelled

    NFmiLocation startpoint(lon1, lat1);
    NFmiLocation endpoint(lon2, lat2);

    double bearing = startpoint.Direction(endpoint);
    double distance = startpoint.Distance(endpoint);

    std::vector<SmartMet::T::Coordinate> coordinates;
    auto cc = startpoint.GetLocation();
    coordinates.emplace_back(SmartMet::T::Coordinate(cc.X(),cc.Y()));

    std::vector<double> distances;
    distances.emplace_back(0);

    for (std::size_t i = 1; i < steps; i++)
    {
      // Should this be fixed? Probably not - the coordinates should behave the same
      double dist = i * distance / steps;
#ifdef WGS84
      auto loc = startpoint.GetLocation(bearing, dist);
#else
      const bool pacific_view = false;
      auto loc = startpoint.GetLocation(bearing, dist, pacific_view);
#endif
      auto cc = loc.GetLocation();
      coordinates.emplace_back(SmartMet::T::Coordinate(cc.X(),cc.Y()));
      distances.emplace_back(dist / 1000.0);
    }
    cc = endpoint.GetLocation();
    coordinates.emplace_back(SmartMet::T::Coordinate(cc.X(),cc.Y()));
    distances.emplace_back(distance / 1000.0);

    return std::make_pair(coordinates, distances);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

void latLon_bboxByCenter(double centerX,double centerY,double metricWidth,double metricHeight,double& lon1,double& lat1,double& lon2,double& lat2)
{
  try
  {
    OGRSpatialReference sr_latlon;
    sr_latlon.importFromEPSG(4326);
    sr_latlon.SetAxisMappingStrategy(OAMS_TRADITIONAL_GIS_ORDER);

    OGRSpatialReference sr_wgs84_world_mercator;
    sr_wgs84_world_mercator.importFromEPSG(3395);
    sr_wgs84_world_mercator.SetAxisMappingStrategy(OAMS_TRADITIONAL_GIS_ORDER);

    OGRCoordinateTransformation *transformation = OGRCreateCoordinateTransformation(&sr_latlon,&sr_wgs84_world_mercator);
    OGRCoordinateTransformation *reverseTransformation = OGRCreateCoordinateTransformation(&sr_wgs84_world_mercator,&sr_latlon);

    transformation->Transform(1,&centerX,&centerY);

    double xx1 = centerX - metricWidth/2;
    double yy1 = centerY - metricHeight/2;
    double xx2 = centerX + metricWidth/2;
    double yy2 = centerY + metricHeight/2;

    // Converting metric coordinates to latlon coordinates.

    reverseTransformation->Transform(1,&xx1,&yy1);
    reverseTransformation->Transform(1,&xx2,&yy2);

    if (transformation != nullptr)
      OCTDestroyCoordinateTransformation(transformation);

    if (reverseTransformation != nullptr)
      OCTDestroyCoordinateTransformation(reverseTransformation);

    lon1 = xx1;
    lat1 = yy1;
    lon2 = xx2;
    lat2 = yy2;
  }
  catch (...)
  {
    throw Fmi::Exception(BCP,"Operation failed!",nullptr);
  }
}


//
// Copyright (c) 2009, Markus Rickert
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <algorithm>
#include <stack>
#include <boost/graph/graphviz.hpp>
#include <rl/math/Quaternion.h>
#include <rl/math/Rotation.h>
#include <rl/math/Spatial.h>
#include <rl/math/Unit.h>

#include "Exception.h"
#include "Dynamic.h"
#include "Prismatic.h"
#include "Revolute.h"
#include "World.h"

namespace rl
{
	namespace mdl
	{
		Dynamic::Dynamic() :
			Kinematic(),
			G(),
			invM(),
			invMx(),
			M(),
			V()
		{
		}

		Dynamic::~Dynamic()
		{
		}

		void
		Dynamic::calculateCentrifugalCoriolis()
		{
			this->calculateCentrifugalCoriolis(this->V);
		}

		void
		Dynamic::calculateCentrifugalCoriolis(::rl::math::Vector& V)
		{
			::rl::math::Vector g(3);
			this->getWorldGravity(g);

			::rl::math::Vector tmp(this->getDof());
			tmp.setZero(); //TODO

			this->setAcceleration(tmp);
			this->setWorldGravity(0, 0, 0);

			this->inverseDynamics();
			V = this->getTorque();

			this->setWorldGravity(g);
		}

		void
		Dynamic::calculateGravity()
		{
			this->calculateGravity(this->G);
		}

		void
		Dynamic::calculateGravity(::rl::math::Vector& G)
		{
			::rl::math::Vector tmp(this->getDof());
			tmp.setZero(); //TODO

			this->setVelocity(tmp);
			this->setAcceleration(tmp);

			this->inverseDynamics();
			G = this->getTorque();
		}

		void
		Dynamic::calculateMassMatrix()
		{
			this->calculateMassMatrix(this->M);
		}

		void
		Dynamic::calculateMassMatrix(::rl::math::Matrix& M)
		{
			::rl::math::Vector g(3);
			this->getWorldGravity(g);

			::rl::math::Vector tmp(this->getDof());
			tmp.setZero(); //TODO

			this->setVelocity(tmp);
			this->setWorldGravity(0, 0, 0);

			for (::std::size_t i = 0; i < this->getDof(); ++i)
			{
				for (::std::size_t j = 0; j < this->getDof(); ++j)
				{
					tmp(j) = i == j ? 1 : 0;
				}

				this->setAcceleration(tmp);
				this->inverseDynamics();

				M.col(i) = this->getTorque();
			}

			this->setWorldGravity(g);
		}

		void
		Dynamic::calculateMassMatrixInverse()
		{
			this->calculateMassMatrixInverse(this->invM);
		}

		void
		Dynamic::calculateMassMatrixInverse(::rl::math::Matrix& invM)
		{
			::rl::math::Vector g(3);
			this->getWorldGravity(g);

			::rl::math::Vector tmp(this->getDof());
			tmp.setZero(); //TODO

			this->setVelocity(tmp);
			this->setWorldGravity(0, 0, 0);

			for (::std::size_t i = 0; i < this->getDof(); ++i)
			{
				for (::std::size_t j = 0; j < this->getDof(); ++j)
				{
					tmp(j) = i == j ? 1 : 0;
				}

				this->setTorque(tmp);
				this->forwardDynamics();

				invM.col(i) = this->getAcceleration();
			}

			this->setWorldGravity(g);
		}

		void
		Dynamic::calculateOperationalMassMatrixInverse()
		{
			this->calculateOperationalMassMatrixInverse(this->J, this->invM, this->invMx);
		}

		void
		Dynamic::calculateOperationalMassMatrixInverse(const ::rl::math::Matrix& J, const ::rl::math::Matrix& invM, ::rl::math::Matrix& invMx) const
		{
			invMx = J * invM * J.transpose(); // TODO
		}

		Model*
		Dynamic::clone() const
		{
			return new Dynamic(*this);
		}

		void
		Dynamic::eulerCauchy(const ::rl::math::Real& dt)
		{
			::rl::math::Vector y = this->getPosition();
			::rl::math::Vector dy = this->getVelocity();

			this->forwardDynamics();

			// f
			::rl::math::Vector f = this->getAcceleration();

			// y_0 + dy_0 * dt
			y += dt * dy; // TODO
			// dy_0 + f * dt
			dy += dt * f; // TODO

			this->setPosition(y);
			this->setVelocity(dy);
			this->setAcceleration(f);
		}

		void
		Dynamic::forwardDynamics()
		{
			for (::std::vector<Element*>::iterator i = this->elements.begin(); i != this->elements.end(); ++i)
			{
				(*i)->forwardDynamics1();
			}

			for (::std::vector<Element*>::reverse_iterator i = this->elements.rbegin(); i != this->elements.rend(); ++i)
			{
				(*i)->forwardDynamics2();
			}

			for (::std::vector<Element*>::iterator i = this->elements.begin(); i != this->elements.end(); ++i)
			{
				(*i)->forwardDynamics3();
			}
		}

		const ::rl::math::Vector&
		Dynamic::getCentrifugalCoriolis() const
		{
			return this->V;
		}

		const ::rl::math::Vector&
		Dynamic::getGravity() const
		{
			return this->G;
		}

		const ::rl::math::Matrix&
		Dynamic::getMassMatrixInverse() const
		{
			return this->invM;
		}

		const ::rl::math::Matrix&
		Dynamic::getMassMatrix() const
		{
			return this->M;
		}

		const ::rl::math::Matrix&
		Dynamic::getOperationalMassMatrixInverse() const
		{
			return this->invMx;
		}

		void
		Dynamic::getWorldGravity(::rl::math::Real& x, ::rl::math::Real& y, ::rl::math::Real& z) const
		{
			dynamic_cast<World*>(this->tree[this->root].get())->getGravity(x, y, z);
		}

		void
		Dynamic::getWorldGravity(::rl::math::Vector& xyz) const
		{
			this->getWorldGravity(xyz(0), xyz(1), xyz(2));
		}

		void
		Dynamic::inverseDynamics()
		{
			for (::std::vector<Element*>::iterator i = this->elements.begin(); i != this->elements.end(); ++i)
			{
				(*i)->inverseDynamics1();
			}

			for (::std::vector<Element*>::reverse_iterator i = this->elements.rbegin(); i != this->elements.rend(); ++i)
			{
				(*i)->inverseDynamics2();
			}
		}

		void
		Dynamic::inverseForce()
		{
			for (::std::vector<Element*>::reverse_iterator i = this->elements.rbegin(); i != this->elements.rend(); ++i)
			{
				(*i)->inverseForce();
			}
		}

		void
		Dynamic::rungeKuttaNystrom(const ::rl::math::Real& dt)
		{
			::rl::math::Vector y0 = this->getPosition();
			::rl::math::Vector dy0 = this->getVelocity();

			this->forwardDynamics();

			::rl::math::Vector f = this->getAcceleration();

			// k1 = dt / 2 * f
			::rl::math::Vector k1 = dt / 2 * f;

			// y_0 + dt / 2 * dy_0 + dt / 4 * k_1
			::rl::math::Vector y = y0 + dt / 2 * dy0 + dt / 4 * k1; // TODO
			// dy_0 + k1
			::rl::math::Vector dy = dy0 + k1;

			this->setPosition(y);
			this->setVelocity(dy);
			this->forwardDynamics();

			// k2 = dt / 2 * f
			::rl::math::Vector k2 = dt / 2 * this->getAcceleration();

			// dy_0 + k_2
			dy = dy0 + k2;

			this->setVelocity(dy);
			this->forwardDynamics();

			// k3 = dt / 2 * f
			::rl::math::Vector k3 = dt / 2 * this->getAcceleration();

			// y_0 + dt * dy_0 + dt * k_3
			y = y0 + dt * dy0 + dt * k3; // TODO
			// dy_0 + 2 * k_3
			dy = dy0 + 2 * k3;

			this->setPosition(y);
			this->setVelocity(dy);
			this->forwardDynamics();

			// k4 = dt / 2 * f
			::rl::math::Vector k4 = dt / 2 * this->getAcceleration();

			// y_0 + dy_0 * dt + dt / 3 * (k_1 + k_2 + k_3)
			y = y0 + dy0 * dt + dt / 3 * (k1 + k2 + k3); // TODO
			// dy_0 + 1 / 3 * (k_1 + 2 * k_2 + 2 * k_3 + k_4)
			dy = dy0 + 1.0f / 3.0f * (k1 + 2 * k2 + 2 * k3 + k4); // TODO

			this->setPosition(y);
			this->setVelocity(dy);
			this->setAcceleration(f);
		}

		void
		Dynamic::setWorldGravity(const ::rl::math::Real& x, const ::rl::math::Real& y, const ::rl::math::Real& z)
		{
			dynamic_cast<World*>(this->tree[this->root].get())->setGravity(x, y, z);
		}

		void
		Dynamic::setWorldGravity(const ::rl::math::Vector& xyz)
		{
			this->setWorldGravity(xyz(0), xyz(1), xyz(2));
		}

		void
		Dynamic::update()
		{
			Kinematic::update();

			this->M.resize(this->getDof(), this->getDof());
			this->V.resize(this->getDof());
			this->G.resize(this->getDof());
			this->invM.resize(this->getDof(), this->getDof());
			this->invMx.resize(6 * this->getOperationalDof(), 6 * this->getOperationalDof());
		}
	}
}


#pragma once
#ifndef SOLVER_KERNELS_H
#define SOLVER_KERNELS_H

#include <vector>
#include <memory>

#include <Eigen/Core>
#include <complex>
#include <engine/Vectormath_Defines.hpp>
#include <data/Spin_System.hpp>
#include <engine/Vectormath.hpp>
#include <engine/Backend_par.hpp>
#include <engine/Backend_seq.hpp>

namespace Engine
{
namespace Solver_Kernels
{
    // SIB
    void sib_transform(const vectorfield & spins, const vectorfield & force, vectorfield & out);

    // OSO coordinates
    void oso_rotate( std::vector<std::shared_ptr<vectorfield>> & configurations, std::vector<vectorfield> & searchdir);
    void oso_calc_gradients( vectorfield & residuals, const vectorfield & spins, const vectorfield & forces);
    scalar maximum_rotation(const vectorfield & searchdir, scalar maxmove);

    // Atlas coordinates
    void atlas_calc_gradients(vector2field & residuals, const vectorfield & spins, const vectorfield & forces, const scalarfield & a3_coords);
    void atlas_rotate(std::vector<std::shared_ptr<vectorfield>> & configurations, const std::vector<scalarfield> & a3_coords, const std::vector<vector2field> & searchdir);
    bool ncg_atlas_check_coordinates(const std::vector<std::shared_ptr<vectorfield>> & spins, std::vector<scalarfield> & a3_coords, scalar tol = -0.6);
    void lbfgs_atlas_transform_direction(std::vector<std::shared_ptr<vectorfield>> & configurations, std::vector<scalarfield> & a3_coords, std::vector<field<vector2field>> & atlas_updates,
                                         std::vector<field<vector2field>> & grad_updates, std::vector<vector2field> & searchdir, std::vector<vector2field> & grad_pr, scalarfield & rho);

    // LBFGS
    template <typename Vec>
    void lbfgs_get_searchdir(int & local_iter, scalarfield & rho, scalarfield & alpha, std::vector<field<Vec>> & q_vec, std::vector<field<Vec>> & searchdir,
                             std::vector<field<field<Vec>>> & delta_a, std::vector<field<field<Vec>>> & delta_grad, const std::vector<field<Vec>> & grad, std::vector<field<Vec>> & grad_pr,
                             const int num_mem, const scalar maxmove) {
        // std::cerr << "lbfgs searchdir \n";
        static auto dot = [] SPIRIT_LAMBDA (const Vec & v1, const Vec &v2) {return v1.dot(v2);};
        static auto set = [] SPIRIT_LAMBDA (const Vec & x) {return x;};

        scalar epsilon = sizeof(scalar) == sizeof(float) ? 1e-30 : 1e-300;

        int noi = grad.size();
        int nos = grad[0].size();
        int m_index = local_iter % num_mem; // memory index
        int c_ind = 0;

        if (local_iter == 0) // gradient descent
        {
            for(int img=0; img<noi; img++) {
                Backend::par::set(grad_pr[img], grad[img], set);
                auto & dir = searchdir[img];
                auto & g_cur = grad[img];
                Backend::par::set(dir, g_cur, [] SPIRIT_LAMBDA (const Vec & x){return -x;});
                auto & da = delta_a[img];
                auto & dg = delta_grad[img];
                for (int i = 0; i < num_mem; i++)
                {
                    rho[i] = 0.0;
                    auto dai = da[i].data();
                    auto dgi = dg[i].data();
                    Backend::par::apply(nos, [dai, dgi] SPIRIT_LAMBDA (int idx){
                        dai[idx] = Vec::Zero();
                        dgi[idx] = Vec::Zero();
                    });
                }
            }
        } else {
            for (int img=0; img<noi; img++)
            {
                auto da = delta_a[img][m_index].data();
                auto dg = delta_grad[img][m_index].data();
                auto g  = grad[img].data();
                auto g_pr = grad_pr[img].data();
                auto sd = searchdir[img].data();
                Backend::par::apply(nos, [da, dg, g, g_pr, sd] SPIRIT_LAMBDA (int idx) {
                    da[idx] = sd[idx];
                    dg[idx] = g[idx] - g_pr[idx];
                });
            }

            scalar rinv_temp = 0;
            for (int img=0; img<noi; img++)
                rinv_temp += Backend::par::reduce(delta_grad[img][m_index], delta_a[img][m_index], dot);
            std::cout << rinv_temp << " rinv_temp\n";
            if (rinv_temp > epsilon)
                rho[m_index] = 1.0 / rinv_temp;
            else
            {
                local_iter = 0;
                return lbfgs_get_searchdir(local_iter, rho, alpha, q_vec, searchdir,
                        delta_a, delta_grad, grad, grad_pr, num_mem, maxmove);
            }

            for (int img=0; img<noi; img++)
                Backend::par::set(q_vec[img], grad[img], set);

            for (int k = num_mem - 1; k > -1; k--)
            {
                c_ind = (k + m_index + 1) % num_mem;
                scalar temp=0;
                for (int img=0; img<noi; img++)
                    temp += Backend::par::reduce(delta_a[img][c_ind], q_vec[img], dot);
                std::cout << temp << " temp\n";
                alpha[c_ind] = rho[c_ind] * temp;
                for (int img=0; img<noi; img++)
                {
                    auto q=q_vec[img].data();
                    auto a=alpha.data();
                    auto d=delta_grad[img].data();
                    Backend::par::apply(nos, [c_ind, q, a, d] SPIRIT_LAMBDA (int idx){
                        q[idx] += -a[c_ind] * d[c_ind][idx];
                    });
                }
            }

            scalar dy2=0;
            for (int img=0; img<noi; img++)
                dy2 += Backend::par::reduce(delta_grad[img][m_index], delta_grad[img][m_index], dot);
            std::cout << dy2 << " dy2\n";
            for (int img=0; img<noi; img++)
            {
                scalar rhody2 = dy2 * rho[m_index];
                scalar inv_rhody2 = 0.0;
                if (rhody2 > epsilon)
                    inv_rhody2 = 1.0 / rhody2;
                else
                    inv_rhody2 = 1.0/(epsilon);
                Backend::par::set(searchdir[img], q_vec[img], [inv_rhody2] SPIRIT_LAMBDA (const Vec & q){
                    return inv_rhody2 * q;
                } );
            }

            for (int k = 0; k < num_mem; k++)
            {
                if (local_iter < num_mem)
                    c_ind = k;
                else
                    c_ind = (k + m_index + 1) % num_mem;

                scalar rhopdg = 0;
                for(int img=0; img<noi; img++)
                    rhopdg += Backend::par::reduce(delta_grad[img][c_ind], searchdir[img], dot);

                rhopdg *= rho[c_ind];
                std::cout << rhopdg << " rhopdg\n";
                for (int img=0; img<noi; img++)
                {
                    auto sd   = searchdir[img].data();
                    auto alph = alpha[c_ind];
                    auto da   = delta_a[img][c_ind].data();
                    Backend::par::apply( nos, [sd, alph, da, rhopdg] SPIRIT_LAMBDA (int idx){
                        sd[idx] += (alph - rhopdg) * da[idx];
                    });
                }
            }

            for (int img=0; img<noi; img++)
            {
                auto g    = grad[img].data();
                auto g_pr = grad_pr[img].data();
                auto sd   = searchdir[img].data();
                Backend::par::apply(nos, [g, g_pr, sd] SPIRIT_LAMBDA (int idx){
                    g_pr[idx] = g[idx];
                    sd[idx]   = -sd[idx];
                });
            }
        }
        local_iter++;
    }
}
}

#endif


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2020 INRIA.
 *
 * 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.
*/

#include "BlockMatrixIterators.hpp"
#include <iostream>                                 // for operator<<, ostream
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include "SiconosAlgebra.hpp"
#include "SimpleMatrixFriends.hpp"
#include "BlockMatrix.hpp"
#include "SimpleMatrix.hpp"
#include "SiconosVector.hpp"
#include "SiconosMatrixSetBlock.hpp" // for setBlock
#include "SiconosAlgebraTools.hpp" // for isComparableTo
#include "Tools.hpp"
#include "SiconosException.hpp"

using  std::cout;
using std::endl;
using Siconos::Algebra::isComparableTo;

// =================================================
//                CONSTRUCTORS
// =================================================

BlockMatrix::BlockMatrix(const SiconosMatrix &m): SiconosMatrix(Siconos::BLOCK), _dimRow(0), _dimCol(0)
{
  _tabRow.reset(new Index());
  _tabCol.reset(new Index());
  if(m.isBlock())
  {
    const BlockMatrix& mB = static_cast<const BlockMatrix&>(m);
    unsigned int nbRows = m.numberOfBlocks(0);
    unsigned int nbCols = m.numberOfBlocks(1);
    _tabRow->reserve(nbRows);
    _tabCol->reserve(nbCols);

    // mat construction
    _mat.reset(new BlocksMat(nbRows, nbCols, nbRows * nbCols));

    unsigned int i, j;
    ConstBlocksIterator1 it1;
    ConstBlocksIterator2 it2;
    bool firstLoop = true;
    // We scan all the blocks of m ...
    for(it1 = mB._mat->begin1(); it1 != mB._mat->end1(); ++it1)
    {
      _dimRow += (*(it1.begin()))->size(0);
      _tabRow->push_back(_dimRow);
      for(it2 = it1.begin(); it2 != it1.end(); ++it2)
      {
        i = it2.index1();
        j = it2.index2();
        if((*it2)->isBlock())   // if the current matrix is a blockMatrix
          _mat->insert_element(i, j, std::shared_ptr<SiconosMatrix>(new BlockMatrix(**it2)));
        else
          _mat->insert_element(i, j, std::shared_ptr<SiconosMatrix>(new SimpleMatrix(**it2)));
        // _dimCol must be incremented only at first "column-loop"
        if(firstLoop)
        {
          _dimCol += (*it2)->size(1);
          _tabCol->push_back(_dimCol);
        }
      }
      firstLoop = false;
    }
  }
  else // if m is a SimpleMatrix
  {
    _tabRow->reserve(1);
    _tabCol->reserve(1);
    // _mat construction
    _mat.reset(new BlocksMat(1, 1, 1));
    _mat->insert_element(0, 0, std::shared_ptr<SiconosMatrix>(new SimpleMatrix(m)));

    _dimRow = m.size(0);
    _dimCol = m.size(1);
    _tabRow->push_back(_dimRow);
    _tabCol->push_back(_dimCol);
  }
}

BlockMatrix::BlockMatrix(const BlockMatrix &m): SiconosMatrix(Siconos::BLOCK), _dimRow(0), _dimCol(0)
{
  unsigned int nbRows = m.numberOfBlocks(0);
  unsigned int nbCols = m.numberOfBlocks(1);
  _tabRow.reset(new Index());
  _tabCol.reset(new Index());
  _tabRow->reserve(nbRows);
  _tabCol->reserve(nbCols);

  // _mat construction
  _mat.reset(new BlocksMat(nbRows, nbCols, nbRows * nbCols));

  unsigned int i, j;
  // We scan all the blocks of m ...
  ConstBlocksIterator1 it1;
  ConstBlocksIterator2 it2;
  bool firstLoop = true;
  // We scan all the blocks of m ...
  for(it1 = m._mat->begin1(); it1 != m._mat->end1(); ++it1)
  {
    _dimRow += (*(it1.begin()))->size(0);
    _tabRow->push_back(_dimRow);
    for(it2 = it1.begin(); it2 != it1.end(); ++it2)
    {
      i = it2.index1();
      j = it2.index2();
      if((*it2)->isBlock())   // if the current _matrix is a blockMatrix
        _mat->insert_element(i, j, std::shared_ptr<SiconosMatrix>(new BlockMatrix(**it2)));
      else
        _mat->insert_element(i, j, std::shared_ptr<SiconosMatrix>(new SimpleMatrix(**it2)));

      // _dimCol must be incremented only at first "column-loop"
      if(firstLoop)
      {
        _dimCol += (*it2)->size(1);
        _tabCol->push_back(_dimCol);
      }
    }
    firstLoop = false;
  }
}

BlockMatrix::BlockMatrix(const std::vector<SP::SiconosMatrix >& m, unsigned int row, unsigned int col):
  SiconosMatrix(Siconos::BLOCK), _dimRow(0), _dimCol(0)
{
  if(m.size() != (row * col))
    THROW_EXCEPTION("number of blocks inconsistent with provided dimensions.");

  _tabRow.reset(new Index());
  _tabCol.reset(new Index());
  _tabRow->reserve(row);
  _tabCol->reserve(col);

  // _mat construction
  _mat.reset(new BlocksMat(row, col, row * col));

  unsigned int k = 0;
  bool firstRowLoop = true;
  bool firstColLoop = true;

  for(unsigned int i = 0; i < row; ++i)
  {
    for(unsigned int j = 0; j < col; ++j)
    {
      (*_mat)(i, j) = m[k++];

      // _dimCol must be incremented only at first "column-loop"
      if(firstColLoop)
      {
        _dimCol += m[k - 1]->size(1);
        _tabCol->push_back(_dimCol);
      }
      if(firstRowLoop)
      {
        _dimRow += m[k - 1]->size(0);
        _tabRow->push_back(_dimRow);
        firstRowLoop = false;
      }
    }
    firstColLoop = false;
    firstRowLoop = true;
  }
}

BlockMatrix::BlockMatrix(SP::SiconosMatrix A, SP::SiconosMatrix B, SP::SiconosMatrix C, SP::SiconosMatrix D):
  SiconosMatrix(Siconos::BLOCK), _dimRow(0), _dimCol(0)
{
  if(A->size(0) != B->size(0) || C->size(0) != D->size(0) ||  A->size(1) != C->size(1) ||  B->size(1) != D->size(1))
    THROW_EXCEPTION("inconsistent sizes between A, B, C or D SiconosMatrices.");

  // _mat = [ A B ]
  //       [ C D ]

  // _mat construction
  _mat.reset(new BlocksMat(2, 2, 4));

  _tabRow.reset(new Index());
  _tabCol.reset(new Index());
  _tabRow->reserve(2);
  _tabCol->reserve(2);

  (*_mat)(0, 0) = A;
  (*_mat)(0, 1) = B;
  (*_mat)(1, 0) = C;
  (*_mat)(1, 1) = D;
  _dimRow = A->size(0);
  _tabRow->push_back(_dimRow);
  _dimRow += C->size(0);
  _tabRow->push_back(_dimRow);
  _dimCol = A->size(1);
  _tabCol->push_back(_dimCol);
  _dimCol += B->size(1);
  _tabCol->push_back(_dimCol);

}

BlockMatrix::~BlockMatrix()
{

  _mat->clear();

  _tabRow->clear();
  _tabCol->clear();
}

// =================================================
//    get number of blocks
// =================================================

unsigned int BlockMatrix::numberOfBlocks(unsigned int dim) const
{
  if(dim == 0)
    return _tabRow->size();
  else
    return _tabCol->size();
}

// =================================================
//        get Ublas component (dense ...)
// =================================================

// return the boost dense _matrix of the block (i, j)
const DenseMat  BlockMatrix::getDense(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp;
  tmp = (*_mat)(row, col);

  if(tmp->num() != Siconos::DENSE)
    THROW_EXCEPTION("the matrix at (row, col) is not a Dense matrix");

  return (tmp->getDense());
}

// return the boost triangular matrix of the block (i, j)
const TriangMat BlockMatrix::getTriang(unsigned int row, unsigned int col) const
{
  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::TRIANGULAR)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Triangular matrix");
  }
  return (tmp->getTriang());
}

// return the boost symmetric matrix of the block (i, j)
const SymMat BlockMatrix::getSym(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::SYMMETRIC)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Symmmetric matrix");
  }
  return (tmp->getSym());
}

// return the boost sparse matrix of the block (i, j)
const SparseMat  BlockMatrix::getSparse(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::SPARSE)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Sparse matrix");
  }
  return (tmp->getSparse());
}

// return the boost sparse matrix of the block (i, j)
const SparseCoordinateMat  BlockMatrix::getSparseCoordinate(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::SPARSE_COORDINATE)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Sparse matrix");
  }
  return (tmp->getSparseCoordinate());
}
// return the boost banded matrix of the block (i, j)
const BandedMat  BlockMatrix::getBanded(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::BANDED)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Banded matrix");
  }
  return (tmp->getBanded());
}

// return the boost zero matrix of the block (i, j)
const ZeroMat  BlockMatrix::getZero(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::ZERO)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Zero matrix");
  }
  return (tmp->getZero());
}

// return the boost identity matrix of the block (i, j)
const IdentityMat  BlockMatrix::getIdentity(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::IDENTITY)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Identity matrix");
  }
  return (tmp->getIdentity());
}

// The following functions return the corresponding pointers
DenseMat*  BlockMatrix::dense(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::DENSE)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Dense matrix");
  }

  return (tmp->dense());
}

TriangMat* BlockMatrix::triang(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::TRIANGULAR)
  {
    THROW_EXCEPTION("The matrix at (row, col) is not a Triangular matrix");
  }
  return (tmp->triang());
}
SymMat* BlockMatrix::sym(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::SYMMETRIC)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Symmmetric matrix");
  }
  return (tmp->sym());
}

SparseMat*  BlockMatrix::sparse(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::SPARSE)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Sparse matrix");
  }
  return (tmp->sparse());
}
SparseCoordinateMat*  BlockMatrix::sparseCoordinate(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::SPARSE_COORDINATE)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Sparse coordinate matrix");
  }
  return (tmp->sparseCoordinate());
}

BandedMat*  BlockMatrix::banded(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::BANDED)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Banded matrix");
  }
  return (tmp->banded());
}

ZeroMat*  BlockMatrix::zero_mat(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::ZERO)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Zero matrix");
  }
  return (tmp->zero_mat(row, col));
}

IdentityMat*  BlockMatrix::identity(unsigned int row, unsigned int col) const
{

  SP::SiconosMatrix tmp = (*_mat)(row, col);
  if(tmp->num() != Siconos::IDENTITY)
  {
    THROW_EXCEPTION("the matrix at (row, col) is not a Identity matrix");
  }
  return (tmp->identity());
}

double* BlockMatrix::getArray(unsigned int i, unsigned int j) const
{
  SP::SiconosMatrix tmp = (*_mat)(i, j);
  return tmp->getArray();
}

// ===========================
//       fill matrix
// ===========================

void BlockMatrix::zero()
{
  BlocksMat::iterator1 it;
  BlocksMat::iterator2 it2;
  for(it = _mat->begin1(); it != _mat->end1(); ++it)
  {
    for(it2 = it.begin(); it2 != it.end(); ++it2)
    {
      (*it2)->zero();
    }
  }
}

void BlockMatrix::randomize()
{
  BlocksMat::iterator1 it;
  BlocksMat::iterator2 it2;
  for(it = _mat->begin1(); it != _mat->end1(); ++it)
  {
    for(it2 = it.begin(); it2 != it.end(); ++it2)
    {
      (*it2)->randomize();
    }
  }
}

void BlockMatrix::randomize_sym()
{
  THROW_EXCEPTION("not yet implemented for block matrices.");
}

void BlockMatrix::eye()
{
  BlocksMat::iterator1 it;
  BlocksMat::iterator2 it2;

  for(it = _mat->begin1(); it != _mat->end1(); ++it)
  {
    for(it2 = it.begin(); it2 != it.end(); ++it2)
    {
      if(it2.index1() == it2.index2())
        (*it2)->eye();
      else
        (*it2)->zero();
    }
  }
}

unsigned int BlockMatrix::size(unsigned int index) const
{
  if(index == 0) return _dimRow;
  else return _dimCol;
};

//=======================
// set matrix dimensions
//=======================

void BlockMatrix::resize(unsigned int, unsigned int, unsigned int, unsigned int, bool)
{
  THROW_EXCEPTION("forbidden for block matrices.");
}

//=======================
//       get norm
//=======================

double BlockMatrix::normInf()const
{
  double sum = 0, norm = 0;
  for(unsigned int i = 0; i < size(0); i++)
  {
    for(unsigned int j = 0; j < size(1); j++)
    {
      sum += (*this)(i, j);
    }
    if(fabs(sum) > norm) norm = fabs(sum);
    sum = 0;
  }
  return norm;
}

//=====================
// screen display
//=====================

void BlockMatrix::display(void)const
{
  std::cout << "==========> BlockMatrix (" << numberOfBlocks(0) << " X " << numberOfBlocks(1) << " blocks): " << std::endl;
  BlocksMat::iterator1 it;
  BlocksMat::iterator2 it2;
  for(it = _mat->begin1(); it != _mat->end1(); ++it)
  {
    for(it2 = it.begin(); it2 != it.end(); ++it2)
    {
      (*it2)->display();
    }
  }
  std::cout << "===========================================================================================" << std::endl;
}
void BlockMatrix::displayExpert(bool brief)const
{
  std::cout << "==========> BlockMatrix (" << numberOfBlocks(0) << " X " << numberOfBlocks(1) << " blocks): " << std::endl;
  BlocksMat::iterator1 it;
  BlocksMat::iterator2 it2;
  for(it = _mat->begin1(); it != _mat->end1(); ++it)
  {
    for(it2 = it.begin(); it2 != it.end(); ++it2)
    {
      (*it2)->displayExpert(brief);
    }
  }
  std::cout << "===========================================================================================" << std::endl;
}

//=====================
// convert to a string
//=====================

std::string BlockMatrix::toString() const
{
  return ::toString(*this);
}

//=====================
// convert to an ostream
//=====================

std::ostream& operator<<(std::ostream& os, const BlockMatrix& bm)
{
  BlocksMat::iterator1 it;
  BlocksMat::iterator2 it2;
  os << "[" << bm.numberOfBlocks(0) << "," << bm.numberOfBlocks(1) << "](";
  for(it = bm._mat->begin1(); it != bm._mat->end1(); ++it)
  {
    for(it2 = it.begin(); it2 != it.end(); ++it2)
    {
      if(it2 != it.begin()) os << ",";
      if(*it2) os << **it2;
      else os << "(nil)";
    }
  }
  os << ")";
  return os;
}

//=============================
// Elements access (get or set)
//=============================

double& BlockMatrix::operator()(unsigned int row, unsigned int col)
{
  unsigned int nbRow = 0;
  unsigned int nbCol = 0;

  while(row >= (*_tabRow)[nbRow] && nbRow < _tabRow->size())
    nbRow ++;

  while(col >= (*_tabCol)[nbCol] && nbCol < _tabCol->size())
    nbCol ++;

  unsigned int posRow = row;
  unsigned int posCol = col;

  if(nbRow != 0)
    posRow -= (*_tabRow)[nbRow - 1];
  if(nbCol != 0)
    posCol -= (*_tabCol)[nbCol - 1];

  SP::SiconosMatrix tmp = (*_mat)(nbRow, nbCol);
  return (*tmp)(posRow, posCol);
}

double BlockMatrix::operator()(unsigned int row, unsigned int col) const
{

  unsigned int nbRow = 0;
  unsigned int nbCol = 0;

  while(row >= (*_tabRow)[nbRow] && nbRow < _tabRow->size())
    nbRow ++;

  while(col >= (*_tabCol)[nbCol] && nbCol < _tabCol->size())
    nbCol ++;

  unsigned int posRow = row;
  unsigned int posCol = col;

  if(nbRow != 0)
    posRow -= (*_tabRow)[nbRow - 1];
  if(nbCol != 0)
    posCol -= (*_tabCol)[nbCol - 1];

  SP::SiconosMatrix tmp = (*_mat)(nbRow, nbCol);
  return (*tmp)(posRow, posCol);
}

double BlockMatrix::getValue(unsigned int row, unsigned int col) const
{
  unsigned int nbRow = 0;
  unsigned int nbCol = 0;

  while(row >= (*_tabRow)[nbRow] && nbRow < _tabRow->size())
    nbRow ++;

  while(col >= (*_tabCol)[nbCol] && nbCol < _tabCol->size())
    nbCol ++;

  unsigned int posRow = row;
  unsigned int posCol = col;

  if(nbRow != 0)
    posRow -= (*_tabRow)[nbRow - 1];
  if(nbCol != 0)
    posCol -= (*_tabCol)[nbCol - 1];

  SP::SiconosMatrix tmp = (*_mat)(nbRow, nbCol);
  return (*tmp)(posRow, posCol);
}

void BlockMatrix::setValue(unsigned int row, unsigned int col, double value)
{
  unsigned int nbRow = 0;
  unsigned int nbCol = 0;

  while(row >= (*_tabRow)[nbRow] && nbRow < _tabRow->size())
    nbRow ++;

  while(col >= (*_tabCol)[nbCol] && nbCol < _tabCol->size())
    nbCol ++;

  unsigned int posRow = row;
  unsigned int posCol = col;

  if(nbRow != 0)
    posRow -= (*_tabRow)[nbRow - 1];
  if(nbCol != 0)
    posCol -= (*_tabCol)[nbCol - 1];

  SP::SiconosMatrix tmp = (*_mat)(nbRow, nbCol);
  (*tmp)(posRow, posCol) = value;
}

//============================================
// Access (get or set) to blocks of elements
//============================================

void BlockMatrix::getRow(unsigned int r, SiconosVector &v) const
{
  unsigned int numRow = 0, posRow = r, start = 0, stop = 0;

  if(r > _dimRow)
    THROW_EXCEPTION("row number is out of range");

  // Verification of the size of the result vector
  if(v.size() != _dimCol)
    THROW_EXCEPTION("inconsistent sizes");

  // Find the row-block number where "r" is
  while(r >= (*_tabRow)[numRow] && numRow < _tabRow->size())
    numRow ++;

  // Computation of the value of the index row into this block
  if(numRow != 0)
    posRow -= (*_tabRow)[numRow - 1];

  for(unsigned int j = 0; j < _tabCol->size(); j++)
  {
    start = stop;
    SP::SiconosMatrix tmp = (*_mat)(numRow, j);
    stop += tmp->size(1);
    ublas::subrange(*(v.dense()), start, stop) = ublas::row(*(tmp->dense()), posRow);
  }
}

void BlockMatrix::getCol(unsigned int c, SiconosVector &v) const
{
  unsigned int numCol = 0, posCol = c, start = 0, stop = 0;

  if(c > _dimCol)
    THROW_EXCEPTION("column number is out of range");

  // Verification of the size of the result vector
  if(v.size() != _dimRow)
    THROW_EXCEPTION("inconsistent sizes");

  // Find the column-block number where "c" is
  while(c >= (*_tabCol)[numCol] && numCol < _tabCol->size())
    numCol ++;

  // Computation of the value of the index column into this block
  if(numCol != 0)
    posCol -= (*_tabCol)[numCol - 1];

  for(unsigned int i = 0; i < _tabRow->size(); i++)
  {
    start = stop;
    SP::SiconosMatrix tmp = (*_mat)(i, numCol);
    stop += tmp->size(0);
    ublas::subrange(*(v.dense()), start, stop) = ublas::column(tmp->getDense(), posCol);
  }
}

void BlockMatrix::setRow(unsigned int r, const SiconosVector &v)
{

  unsigned int numRow = 0, posRow = r, start = 0, stop = 0;

  if(v.size() != _dimCol)
    THROW_EXCEPTION("inconsistent sizes");

  while(r >= (*_tabRow)[numRow] && numRow < _tabRow->size())
    numRow ++;

  if(numRow != 0)
    posRow -= (*_tabRow)[numRow - 1];

  for(unsigned int j = 0; j < _tabCol->size(); j++)
  {
    start = stop;
    SP::SiconosMatrix tmp = (*_mat)(numRow, j);
    stop += tmp->size(1);
    ublas::row(*(tmp->dense()), posRow) = ublas::subrange(*(v.dense()), start, stop);
  }
}

void BlockMatrix::setCol(unsigned int col, const SiconosVector &v)
{

  unsigned int numCol = 0, posCol = col, start = 0, stop = 0;

  if(v.size() != _dimRow)
    THROW_EXCEPTION("inconsistent sizes");

  while(col >= (*_tabCol)[numCol] && numCol < _tabCol->size())
    numCol ++;

  if(numCol != 0)
    posCol -= (*_tabCol)[numCol - 1];

  for(unsigned int i = 0; i < _tabRow->size(); i++)
  {
    start = stop;
    SP::SiconosMatrix tmp = (*_mat)(i, numCol);
    stop += tmp->size(0);
    ublas::column(*(tmp->dense()), posCol) = ublas::subrange(*(v.dense()), start, stop);
  }
}

void BlockMatrix::addSimple(unsigned int& indRow, unsigned int& indCol, const SiconosMatrix& m)
{
  // Add a part of m (starting from (indRow,indCol) to the current matrix.
  // m must be a SimpleMatrix.

  // At the end of the present function, indRow (resp. indCol) is equal to indRow + the corresponding dimension of the added sub-matrix.

  unsigned int row = m.size(0) - indRow; // number of rows of the block to be added.
  unsigned int col = m.size(1) - indCol; // number of columns of the block to be added.
  unsigned int initCol = indCol;

  if(row > _dimRow || col > _dimCol) THROW_EXCEPTION("invalid ranges");

  Siconos::UBLAS_TYPE numM = m.num();

  // iterators through this
  BlocksMat::iterator1 it1;
  BlocksMat::iterator2 it2;
  unsigned int currentRow = 0, currentCol = 0, currentNum;
  for(it1 = _mat->begin1(); it1 != _mat->end1(); ++it1)
  {
    for(it2 = it1.begin(); it2 != it1.end(); ++it2)
    {
      if((*it2)->isBlock())   // if the sub-block is also a BlockMatrix ...
        (std::static_pointer_cast<BlockMatrix>(*it2))->addSimple(indRow, indCol, m);

      else
      {
        currentCol = (*it2)->size(1);
        currentRow = (*it2)->size(0);
        currentNum = (*it2)->num();
        if(numM != currentNum) THROW_EXCEPTION("inconsistent types.");

        if(numM == Siconos::DENSE)
          noalias(*(*it2)->dense()) += ublas::subrange(*m.dense(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::TRIANGULAR)
          noalias(*(*it2)->triang()) += ublas::subrange(*m.triang(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::SYMMETRIC)
          noalias(*(*it2)->sym()) += ublas::subrange(*m.sym(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::SPARSE)
          noalias(*(*it2)->sparse()) += ublas::subrange(*m.sparse(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::BANDED)
          noalias(*(*it2)->banded()) += ublas::subrange(*m.banded(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::ZERO) {}
        else
          THROW_EXCEPTION("inconsistent types.");
      }
      indCol += currentCol;
    }
    indRow += currentRow;
    indCol = initCol;
  }
}

void BlockMatrix::subSimple(unsigned int& indRow, unsigned int& indCol, const SiconosMatrix& m)
{
  // subtract a part of m (starting from (indRow,indCol) to the current matrix.
  // m must be a SimpleMatrix.

  // At the end of the present function, indRow (resp. indCol) is equal to indRow + the corresponding dimension of the subtracted sub-matrix.

  unsigned int row = m.size(0) - indRow; // number of rows of the block to be added.
  unsigned int col = m.size(1) - indCol; // number of columns of the block to be added.
  unsigned int initCol = indCol;
  if(row > _dimRow || col > _dimCol) THROW_EXCEPTION("invalid ranges");

  Siconos::UBLAS_TYPE numM = m.num();

  // iterators through this
  BlocksMat::iterator1 it1;
  BlocksMat::iterator2 it2;
  unsigned int currentRow = 0, currentCol = 0;
  Siconos::UBLAS_TYPE currentNum;
  for(it1 = _mat->begin1(); it1 != _mat->end1(); ++it1)
  {
    for(it2 = it1.begin(); it2 != it1.end(); ++it2)
    {
      if((*it2)->isBlock())   // if the sub-block is also a BlockMatrix ...
        (std::static_pointer_cast<BlockMatrix>(*it2))->subSimple(indRow, indCol, m);

      else
      {
        currentCol = (*it2)->size(1);
        currentRow = (*it2)->size(0);
        currentNum = (*it2)->num();
        if(numM != currentNum) THROW_EXCEPTION("inconsistent types.");

        if(numM == Siconos::DENSE)
          noalias(*(*it2)->dense()) -= ublas::subrange(*m.dense(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::TRIANGULAR)
          noalias(*(*it2)->triang()) -= ublas::subrange(*m.triang(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::SYMMETRIC)
          noalias(*(*it2)->sym()) -= ublas::subrange(*m.sym(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::SPARSE)
          noalias(*(*it2)->sparse()) -= ublas::subrange(*m.sparse(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::BANDED)
          noalias(*(*it2)->banded()) -= ublas::subrange(*m.banded(), indRow, indRow + currentRow, indCol, indCol + currentCol);
        else if(numM == Siconos::ZERO) {}
        else
          THROW_EXCEPTION("inconsistent types.");
      }
      indCol += currentCol;
    }
    indRow += currentRow;
    indCol = initCol;
  }
}

//===============
//  Assignment
//===============

BlockMatrix& BlockMatrix::operator = (const SiconosMatrix &m)
{
  if(&m == this) return *this;  // auto-assignment.

  if(m.size(0) != _dimRow || m.size(1) != _dimCol)
    THROW_EXCEPTION("Left and Right values have inconsistent sizes.");

  // Warning: we do not reallocate the blocks, but only copy the values. This means that
  // all blocks are already allocated and that dim of m and mat are to be consistent.
  // Thus, _tabRow and _tabCol remains unchanged.
  // If m and mat are not "block-consistent", we use the () operator for a component-wise copy.

  if(m.isBlock())
  {
    if(isComparableTo(*this, m))
    {
      const BlockMatrix& mB = static_cast<const BlockMatrix&>(m);
      // iterators through this
      BlocksIterator1 it1;
      BlocksIterator2 it2;
      // iterators through m
      ConstBlocksIterator1 itM1 = mB._mat->begin1();
      ConstBlocksIterator2 itM2;

      for(it1 = _mat->begin1(); it1 != _mat->end1(); ++it1)
      {
        itM2 = itM1.begin();
        for(it2 = it1.begin(); it2 != it1.end(); ++it2)
        {
          (**it2) = (**itM2);
          itM2++; // increment column pos. in m.
        }
        itM1++; // increment row pos. in m.
      }
    }
    else
    {
      for(unsigned int i = 0; i < _dimRow; ++i)
        for(unsigned int j = 0; j < _dimCol; ++j)
          (*this)(i, j) = m(i, j);
    }
  }
  else // if m is a SimpleMatrix
  {
    BlocksIterator1 it;
    BlocksIterator2 it2;
    unsigned int posRow = 0;
    unsigned int posCol = 0;
    Index subDim(2);
    Index subPos(4);

    for(it = _mat->begin1(); it != _mat->end1(); ++it)
    {
      for(it2 = it.begin(); it2 != it.end(); ++it2)
      {
        // a sub-block of m is copied into this
        subDim[0] = (*it2)->size(0);
        subDim[1] = (*it2)->size(1);
        subPos[0] = posRow;
        subPos[1] = posCol;
        subPos[2] = 0;
        subPos[3] = 0;
        setBlock(createSPtrConstSiconosMatrix(m), *it2, subDim, subPos);
        posCol += subDim[1];
      }
      posRow += (*it)->size(0);
      posCol = 0;
    }
  }

  return *this;
}

BlockMatrix& BlockMatrix::operator = (const BlockMatrix &m)
{
  if(&m == this) return *this;  // auto-assignment.

  if(m.size(0) != _dimRow || m.size(1) != _dimCol)
    THROW_EXCEPTION("Left and Right values have inconsistent sizes.");

  // Warning: we do not reallocate the blocks, but only copy the values. This means that
  // all blocks are already allocated and that dim of m and mat are to be consistent.
  // Thus, _tabRow and _tabCol remains unchanged.
  // If m and mat are not "block-consistent", we use the () operator for a componet-wise copy.

  if(isComparableTo(*this, m))
  {
    const BlockMatrix& mB = static_cast<const BlockMatrix&>(m);
    // iterators through this
    BlocksIterator1 it1;
    BlocksIterator2 it2;
    // iterators through m
    ConstBlocksIterator1 itM1 = mB._mat->begin1();
    ConstBlocksIterator2 itM2;

    for(it1 = _mat->begin1(); it1 != _mat->end1(); ++it1)
    {
      itM2 = itM1.begin();
      for(it2 = it1.begin(); it2 != it1.end(); ++it2)
      {
        (**it2) = (**itM2);
        itM2++; // increment column pos. in m.
      }
      itM1++; // increment row pos. in m.
    }
  }
  else
  {
    for(unsigned int i = 0; i < _dimRow; ++i)
      for(unsigned int j = 0; j < _dimCol; ++j)
        (*this)(i, j) = m(i, j);
  }
  return *this;
}

BlockMatrix& BlockMatrix::operator = (const DenseMat &m)
{
  THROW_EXCEPTION("Not yet implemented.");
  return *this;
}

//=================================
// Op. and assignment (+=, -= ... )
//=================================

BlockMatrix& BlockMatrix::operator += (const SiconosMatrix &m)
{
  if(&m == this)
  {
    BlocksMat::iterator1 it1;
    BlocksMat::iterator2 it2;
    for(it1 = _mat->begin1(); it1 != _mat->end1(); ++it1)
    {
      for(it2 = it1.begin(); it2 != it1.end(); ++it2)
      {
        **it2 += **it2;
      }
    }
    return *this;
  }

  if(m.size(0) != _dimRow || m.size(1) != _dimCol)
    THROW_EXCEPTION("Left and Right values have inconsistent sizes.");

  if(m.isBlock())
  {
    if(isComparableTo(m, *this))
    {
      const BlockMatrix& mB = static_cast<const BlockMatrix&>(m);
      // iterators through this
      BlocksMat::iterator1 it1;
      BlocksMat::iterator2 it2;
      // iterators through m
      BlocksMat::const_iterator1 itM1 = mB._mat->begin1();
      BlocksMat::const_iterator2 itM2;

      for(it1 = _mat->begin1(); it1 != _mat->end1(); ++it1)
      {
        itM2 = itM1.begin();
        for(it2 = it1.begin(); it2 != it1.end(); ++it2)
        {
          (**it2) += (**itM2);
          itM2++; // increment column pos. in m.
        }
        itM1++; // increment row pos. in m.
      }
    }
    else
    {
      for(unsigned int i = 0; i < _dimRow; ++i)
        for(unsigned int j = 0; j < _dimCol; ++j)
          (*this)(i, j) += m(i, j);
    }
  }
  else // if m is a SimpleMatrix
  {
    unsigned int indRow = 0, indCol = 0;
    addSimple(indRow, indCol, m); // a sub-block of m is added to each block of this.
  }
  return *this;
}

BlockMatrix& BlockMatrix::operator -= (const SiconosMatrix &m)
{
  if(&m == this)
  {
    BlocksMat::iterator1 it1;
    BlocksMat::iterator2 it2;
    for(it1 = _mat->begin1(); it1 != _mat->end1(); ++it1)
    {
      for(it2 = it1.begin(); it2 != it1.end(); ++it2)
      {
        **it2 -= **it2;
      }
    }
    return *this;
  }

  if(m.size(0) != _dimRow || m.size(1) != _dimCol)
    THROW_EXCEPTION("Left and Right values have inconsistent sizes.");

  if(m.isBlock())
  {
    if(isComparableTo(m, *this))
    {
      const BlockMatrix& mB = static_cast<const BlockMatrix&>(m);
      // iterators through this
      BlocksMat::iterator1 it1;
      BlocksMat::iterator2 it2;
      // iterators through m
      BlocksMat::const_iterator1 itM1 = mB._mat->begin1();
      BlocksMat::const_iterator2 itM2;

      for(it1 = _mat->begin1(); it1 != _mat->end1(); ++it1)
      {
        itM2 = itM1.begin();
        for(it2 = it1.begin(); it2 != it1.end(); ++it2)
        {
          (**it2) -= (**itM2);
          itM2++; // increment column pos. in m.
        }
        itM1++; // increment row pos. in m.
      }
    }
    else
    {
      for(unsigned int i = 0; i < _dimRow; ++i)
        for(unsigned int j = 0; j < _dimCol; ++j)
          (*this)(i, j) -= m(i, j);
    }
  }
  else // if m is a SimpleMatrix
  {
    unsigned int indRow = 0, indCol = 0;
    subSimple(indRow, indCol, m); // a sub-block of m is subtracted to each block of this.
  }
  return *this;
}

void BlockMatrix::trans()
{
  THROW_EXCEPTION("not yet implemented.");
}

void BlockMatrix::trans(const SiconosMatrix &m)
{
  THROW_EXCEPTION("not yet implemented.");
}

void BlockMatrix::PLUFactorizationInPlace()
{
  THROW_EXCEPTION("not yet implemented for Block Matrices.");
}
void BlockMatrix::Factorize()
{
  THROW_EXCEPTION("not yet implemented for Block Matrices.");
}

void BlockMatrix::PLUInverseInPlace()
{
  THROW_EXCEPTION("not yet implemented for Block Matrices.");
}

void BlockMatrix::PLUForwardBackwardInPlace(SiconosMatrix &B)
{
  THROW_EXCEPTION("not yet implemented for Block Matrices.");
}
void BlockMatrix::Solve(SiconosMatrix &B)
{
  THROW_EXCEPTION("not yet implemented for Block Matrices.");
}

void BlockMatrix::PLUForwardBackwardInPlace(SiconosVector &B)
{
  THROW_EXCEPTION("not yet implemented for Block Matrices.");
}
void BlockMatrix::Solve(SiconosVector &B)
{
  THROW_EXCEPTION("not yet implemented for Block Matrices.");
}

SP::SiconosMatrix BlockMatrix::block(unsigned int row, unsigned int col)
{
  return (*_mat)(row, col);
}

SPC::SiconosMatrix BlockMatrix::block(unsigned int row, unsigned int col) const
{
  return std::shared_ptr<SiconosMatrix>((*_mat)(row, col));
}

size_t BlockMatrix::nnz(double tol)
{
  size_t nnz = 0;
  BlocksMat::iterator1 it;
  BlocksMat::iterator2 it2;
  for(it = _mat->begin1(); it != _mat->end1(); ++it)
  {
    for(it2 = it.begin(); it2 != it.end(); ++it2)
      nnz += (**it2).nnz();
  }
  return nnz;
}