/**
 * @file
 * $Revision: 6513 $
 * $Date: 2016-01-14 16:04:44 -0700 (Thu, 14 Jan 2016) $
 * $Id: CalculatorStrategy.cpp 6513 2016-01-14 23:04:44Z kbecker@GS.DOI.NET $
 *
 *   Unless noted otherwise, the portions of Isis written by the USGS are
 *   public domain. See individual third-party library and package descriptions
 *   for intellectual property information, user agreements, and related
 *   information.
 *
 *   Although Isis has been used by the USGS, no warranty, expressed or
 *   implied, is made by the USGS as to the accuracy and functioning of such
 *   software and related material nor shall the fact of distribution
 *   constitute any such warranty, and no responsibility is assumed by the
 *   USGS in connection therewith.
 *
 *   For additional information, launch
 *   $ISISROOT/doc//documents/Disclaimers/Disclaimers.html
 *   in a browser or see the Privacy & Disclaimers page on the Isis website,
 *   http://isis.astrogeology.usgs.gov, and the USGS privacy and disclaimers on
 *   http://www.usgs.gov/privacy.html.
 */
#include "CalculatorStrategy.h"

// boost library
#include <boost/foreach.hpp>

// other ISIS
#include "IException.h"
#include "InlineCalculator.h"
#include "PvlFlatMap.h"
#include "PvlObject.h"
#include "Resource.h"
#include "SpecialPixel.h"

using namespace std;

namespace Isis {

  /**
   * Default constructor.
   */
  CalculatorStrategy::CalculatorStrategy() :
                                         Strategy("Calculator", "Calculator"),
                                         m_equations(), m_initializers(0),
                                         m_initArgs(), m_calculator(0),
                                         m_result(0.0) { }

  /**
   * @brief Constructor loads from a Strategy object CalculatorStrategy
   *        definition
   *
   * This constructor loads and retains processing parameters form the
   * Calculator Strategy object definition as (typically) read form the
   * configuration file.
   *
   * @param definitnion Calculator Strategy PVL definition
   * @param globals Global Resource of keywords
   */
  CalculatorStrategy::CalculatorStrategy(const PvlObject &definition,
                                         const ResourceList &globals) :
                                         Strategy(definition, globals),
                                         m_equations(), m_initializers(0),
                                         m_initArgs(), m_calculator(0),
                                         m_result(0.0) {

    if ( getDefinition().hasGroup("Initializers") ) {
      const PvlObject &keydefs = getDefinition();
      PvlFlatMap *vars = new PvlFlatMap(keydefs.findGroup("Initializers"));
      m_initializers = QSharedPointer<PvlFlatMap> (vars);

      // Get initializer argument keyword replacement values
      if ( keydefs.hasKeyword("InitializersArgs") ) {
        PvlFlatMap args;
        args.add(keydefs["InitializersArgs"]);
        m_initArgs = args.allValues("InitializersArgs");
      }
    }

    PvlFlatMap parms( getDefinitionMap() );

    if ( parms.exists("Equation") ) {
      Equation eqn;
      eqn.equation = parms.get("Equation");
      InlineCalculator *calc = new InlineCalculator(eqn.equation);
      eqn.calculator = QSharedPointer<InlineCalculator> (calc);
      if ( parms.exists("Result") ) {
        eqn.store = parms.get("Result");
      }
      m_equations.append(eqn);
    }

    if ( definition.hasGroup("Equations") ) {
      PvlGroup eqns = getDefinition().findGroup("Equations");
      PvlGroup::PvlKeywordIterator i;
      for (i = eqns.begin(); i != eqns.end(); i++) {
        Equation eqn;
        eqn.equation = QString(*i);
        InlineCalculator *calc = new InlineCalculator(eqn.equation);
        eqn.calculator = QSharedPointer<InlineCalculator> (calc);
        eqn.store = i->name();
        m_equations.append(eqn);
      }

    }

    return;
  }

  /**
   * Destructor
   */
  CalculatorStrategy::~CalculatorStrategy() {
  }

  /**
   * @brief Calculates the result for a resoruce and stores it in the resource
   *
   * Calculates the result of the calculation for a resource and stores it in
   * the specified key in the resource. If there is an error in the calculation,
   * such as if the value of a variable is invalid, then the resource is
   * discarded.
   *
   * @param resource SharedResource to be written to
   * @param globals Global Resource of keywords
   *
   * @return int The number of keys written to the resource
   */
  int CalculatorStrategy::apply(SharedResource &resource, const ResourceList &globals) {

    int nInits = initialize(resource, globals);
    ResourceList variables = getGlobals(resource, globals);

    int ntotal = 0;
    BOOST_FOREACH ( Equation eqn, m_equations) {
      try {
        m_calculator = eqn.calculator;
        m_result = calculate(variables);
        if ( !eqn.store.isEmpty() ) {
          resource->add(eqn.store, QString::number(m_result));
        }
        ntotal++;
      }
      // Discard on error
      catch (IException &ie) {
        if ( isDebug() ) {
         cout << "Calculator error on " << resource->name() << " with equation "
              << eqn.equation << ": " << ie.what() << "\n";
        }
        m_result = Null;
        resource->discard();
        ntotal = 0;
        break;
      }
    }

    // Check for initialization only and return success
    if ( (0 == m_equations.size()) && (nInits > 0) ) { return (1); }
    return ((ntotal == 0) ? 0 : 1);
  }

  /**
   * Returns the result of the calculation.
   *
   * @return double The result of the calculation
   */
  double CalculatorStrategy::result() const {
    return (m_result);
  }

  /**
   * @brief Initializes a Resource.
   *
   * Initializes a resource based on the keys in the list of initializers.
   * Each key in the list of initializers is added to the resource. Returns the
   * number of keys added.
   *
   * @param resource SharedResource to be initialized
   *
   * @return int The number of keys added to the resource
   */
  int CalculatorStrategy::initialize(SharedResource &resource,
                                     const ResourceList &globals) {
    int ntotal(0);
    if ( !m_initializers.isNull() ) {
      ResourceList myGlobals = getGlobals(resource, globals);
      BOOST_FOREACH ( PvlKeyword key, *m_initializers) {
        PvlKeyword newkey(key.name());
        for (int i = 0 ; i < key.size() ; i++) {
          QString value = processArgs(key[i], m_initArgs, myGlobals);
          newkey.addValue(value);
          if ( isDebug() ) {
            cout << "Initializing " << key.name() << "[" << i << "] = "
                  << value << "\n";
          }
        }
        resource->add(newkey);
        ntotal++;
      }
    }
    return (ntotal);
  }

  /**
   * Calculates the result of the equation for a resource and returns the
   * result.
   *
   * @param resource SharedResource to be used for the calculation.
   *
   * @return double The result of the calculation.
   */
  double CalculatorStrategy::calculate(SharedResource &resource) {
    ResourceCalculatorVariablePool vars(resource);
    QVector<double> values = m_calculator->evaluate(&vars);
    return (values[0]);
  }

  /**
   * Calculates the result of the equation for a list of resource and returns
   * the result.
   *
   * @param resources List of Resources to be used for the calculation.
   *
   * @return double The result of the calculation.
   */
  double CalculatorStrategy::calculate(ResourceList &resources) {
    ResourceCalculatorVariablePool vars(resources);
    QVector<double> values = m_calculator->evaluate(&vars);
    return (values[0]);
  }

  /**
   * @brief Initializes a PvlFlatMap.
   *
   * Initializes a PvlFlatMap based on the keys in the list of initializers.
   * Each key in the list of initializers is added to the PvlFlatMap. Returns
   * the number of keys added.
   *
   * @param pvl PvlFlatMap to be initialized
   *
   * @return int The number of keys added to the PvlFlatMap
   */
  int CalculatorStrategy::initialize(PvlFlatMap &pvl) {
    int ntotal(0);
    if ( !m_initializers.isNull() ) {
      BOOST_FOREACH ( PvlKeyword key, *m_initializers) {
        pvl.add(key);
        ntotal++;
      }
    }
    return (ntotal);
  }

  /**
   * Calculates the result of the equation for a PvlFlatMap and returns the
   * result.
   *
   * @param pvl PvlFlatMap to be used for the calculation.
   *
   * @return double The result of the calculation.
   */
  double CalculatorStrategy::calculate(PvlFlatMap &pvl) {
    PvlFlatMapCalculatorVariablePool vars(pvl);
    QVector<double> values = m_calculator->evaluate(&vars);
    return (values[0]);
  }

  /**
   * Construct ResourceCalculatorVariablePool with a SharedResource.
   *
   * @param resource SharedResource of the resource
   */
  ResourceCalculatorVariablePool::ResourceCalculatorVariablePool(
      SharedResource &resource) {
    m_resources.push_back(resource);
  }

  /**
   * Construct ResourceCalculatorVariablePool with a SharedResource.
   *
   * @param resource SharedResource of the resource
   */
  ResourceCalculatorVariablePool::ResourceCalculatorVariablePool(
      ResourceList &resources) {
    m_resources.append(resources);
  }

  /**
   * Destruct ResourceCalculatorVariablePool.
   */
  ResourceCalculatorVariablePool::~ResourceCalculatorVariablePool() {
  }

  /**
   * Check existance of variable.
   *
   * @param variable QString of the variable.
   */
  bool ResourceCalculatorVariablePool::exists(const QString &variable) const {
    BOOST_FOREACH ( SharedResource resource, m_resources ) {
      if ( resource->exists(variable) ) {
            return (true);
      }
    }
    return (false);
  }

  /**
   * Returns a vector associated to a variable request from the
   * calculator.
   *
   * @param variable QString of the variable
   *
   * @param index int of the index
   */
  QVector<double> ResourceCalculatorVariablePool::value(const QString &variable,
                                                        const int &index) const {
    QVector<double> vars;
    BOOST_FOREACH ( SharedResource resource, m_resources ) {
      if ( resource->exists(variable) ) {
         vars.push_back(toDouble(resource->value(variable, index)));
         break;
      }
    }

    return (vars);
  }

  /**
   * Construct PvlFlatMapCalculatorVariablePool with a PvlFlatMap.
   *
   * @param pvl PvlFlatMap
   */
  PvlFlatMapCalculatorVariablePool::PvlFlatMapCalculatorVariablePool(
      PvlFlatMap &pvl) : m_pvl(pvl) {
  }

  /**
   * Destruct PvlFlatMapCalculatorVariablePool.
   */
  PvlFlatMapCalculatorVariablePool::~PvlFlatMapCalculatorVariablePool() {
  }

  /**
   * Check existance of variable.
   *
   * @param variable QString of the variable.
   */
  bool PvlFlatMapCalculatorVariablePool::exists(const QString &variable) const {
    return (m_pvl.exists(variable));
  }

  /**
   * Returns a vector associated to a variable request from the
   * calculator.
   *
   * @param variable QString of the variable
   * @param index int of the index
   */
  QVector<double> PvlFlatMapCalculatorVariablePool::value(const QString &variable,
                                                          const int &index) const {
    QVector<double> vars;
    vars.push_back(toDouble(m_pvl.get(variable, index)));
    return (vars);
  }

} // namespace Isis


#include "slice.h"

#include <cstdio>
#include <iostream>
#include <map>
#include <stdint.h>

#include <Eigen/Sparse>

using namespace Eigen;

using std::cout;
using std::endl;
using std::map;
using std::ostream;
using std::pair;
using std::vector;

void debug_print_sparse(const SparseMatrix<uint8_t> &mat) {
    for (int i = 0; i < mat.cols(); i++) {
        cout << "\t" << i;
    }
    cout << endl;
    for (int i = 0; i < mat.rows(); i++) {
        cout << i << ":\t";
        for (int j = 0; j < mat.cols(); j++) {
            cout << (int) mat.coeff(i, j) << "\t";
        }
        cout << endl;
    }
}

#define OUT_OF_PLANE (0)
#define POINT_IN_PLANE (1)
#define FACE_IN_PLANE (2)

// returns FACE_IN_PLANE if the face is fully in-plane (all verteces lie in the
// plane). returns POINT_IN_PLANE if the plane and face intersect at a single
// point. returns OUT_OF_PLANE if neither property holds.
//
// this function should only be called on faces that either intersect the plane
// or lie in it.
int inplane_status(const float z, const face* f) {
    edge *e0 = f->e;
    // check for in-plane conditions
    int verts_inplane = 0;
    int verts_above = 0;
    int verts_below = 0;

    do {
        vertex *v = e0->vert;
        if (v->loc[2] == z) {
            verts_inplane++;
        } else if (v->loc[2] > z) {
            verts_above++;
        } else {
            verts_below++;
        }
        e0 = e0->next;
    } while (e0 != f->e);

    // check for whole-tri-in-plane
    if (verts_above == 0 && verts_below == 0) {
        return FACE_IN_PLANE;

    }
    // check for single-point-in-plane
    if (verts_inplane == 1 && (verts_above == 0 || verts_below == 0)) {
        return POINT_IN_PLANE;
    }
    return OUT_OF_PLANE;
}

// returns the line segment across the face representing the line of
// intersection between the xy-plane at height z and the provided face.
// returns a zero-length line segment if face does not intersect plane
lineseg isect_tri_xy_plane(const float z, const face* f) {
    lineseg l;

    if (f->sides() != 3) {
        // TODO: triangularize!
        cout << "got a nontriangular mesh and I'm a stupid slicer." << endl;
        return l;
    }

    int points_found = 0;
    edge *e0 = f->e;
    do {
        // do this at the start so we can use continue within the loop
        e0 = e0->next;

        vertex *v = e0->next->vert,
               *v0 = e0->vert;
        float v_z = v->loc[2],
              v0_z = v0->loc[2];

        if (v_z == v0_z) {
            // check for edge-in-plane condition. since we know the whole
            // tri /isn't/ in-plane from the check above, we know that this
            // edge must be our levelset poly line segment
            if (v_z == z) {
                l.p1 = v0->loc;
                l.p2 = v->loc;
                points_found = 2;
                break;
            }
            continue;
        }

        float n = (v_z - z) / (v_z - v0_z);
        if (0 <= n && n <= 1) {
            Vector3f p = v->loc - n * (v->loc - v0->loc);
            if (points_found == 0) {
                l.p1 = p;
                points_found++;
            } else if (points_found == 1 && p != l.p1) {
                l.p2 = p;
                points_found++;
            }
        }
    } while (e0 != f->e);
    return l;
}

// assign faces to layer buckets. this modifies the levelset vector in-place.
void bucket_faces(
        const mesh &m, const bounds &b,
        const float layer_height, vector<levelset> &layers) {
    for (auto iter = m.faces.begin(); iter != m.faces.end(); iter++) {
        face* f = *iter;

        // find face bounds
        float z_min = INFINITY, z_max = -INFINITY;
        edge *e = f->e;
        vertex *v;
        do {
            v = e->vert;
            if (v->loc[2] > z_max) {
                z_max = v->loc[2];
            }
            if (v->loc[2] < z_min) {
                z_min = v->loc[2];
            }
            e = e->next;
        } while (e != f->e);

        // add face to appropriate buckets
        for (auto ls = layers.begin(); ls != layers.end(); ls++) {
            if (z_min <= ls->z && ls->z <= z_max) {
                ls->faces.push_back(f);
            } else if (ls->z > z_max) {
                // we're out of our good range, break
                break;
            }
        }
    }
}

// generates a list of line segments based on the intersection of the bucketed
// faces and the xy-plane at height ls.z
void find_line_segments(levelset &ls) {
    for (auto iter = ls.faces.begin(); iter != ls.faces.end(); iter++) {
        face* f = *iter;
        int inplane = inplane_status(ls.z, f);

        if (inplane == FACE_IN_PLANE) {
            ls.inplane.push_back(f);
            continue;
        } else if (inplane == POINT_IN_PLANE) {
            continue;
        }

        lineseg line = isect_tri_xy_plane(ls.z, f);
        if ((line.p1 - line.p2).norm() == 0) {
            cout << "warning: face doesn't intersect z = " << ls.z << endl;
            edge *e = f->e;
            do {
                vertex *v = e->vert;
                printf("  v[%lx]\t%f\t%f\t%f\n", (unsigned long) v,
                        v->loc[0], v->loc[1], v->loc[2]);
                e = e->next;
            } while (e != f->e);
        } else {
            ls.lines.push_back(line);
        }
    }
}

// creates a vertex in the vert_ids map and in the verts list. returns the ID to
// use for the next vertex (either next_id or next_id + 1)
int create_vertex(
        map<Vector3f, uint32_t, vector_comparitor> &vert_ids,
        Vector3f &loc, vector<Vector3f> &verts, int next_id) {
    if (vert_ids.find(loc) != vert_ids.end()) {
        return next_id;
    }
    vert_ids.insert(pair<Vector3f, uint32_t>(loc, next_id));
    verts.push_back(loc);
    return next_id + 1;
}

// converts an unsorted list of line segments to a list of ordered lists of
// verteces representing paths around the levelset.
void linesegs_to_vert_list(levelset &ls) {
    map<Vector3f, uint32_t, vector_comparitor> vert_ids;
    int next_id = 0;

    // build vert list
    for (auto iter = ls.lines.begin(); iter != ls.lines.end(); iter++) {
        next_id = create_vertex(vert_ids, iter->p1, ls.verteces, next_id);
        next_id = create_vertex(vert_ids, iter->p2, ls.verteces, next_id);
    }
    int n = ls.verteces.size();

    // build adjacency
    // TODO: handle double-edges
    vector<Triplet<uint8_t>> adjacents;
    for (auto iter = ls.lines.begin(); iter != ls.lines.end(); iter++) {
        auto i1 = vert_ids.find(iter->p1);
        if (i1 == vert_ids.end()) {
            cout << "warning: after building vert list, vert " <<
                iter->p1.transpose() << " is missing" << endl;
        }
        auto i2 = vert_ids.find(iter->p2);
        if (i2 == vert_ids.end()) {
            cout << "warning: after building vert list, vert " <<
                iter->p2.transpose() << " is missing" << endl;
        }
        adjacents.push_back(Triplet<uint8_t>(i1->second, i2->second, 1));
        adjacents.push_back(Triplet<uint8_t>(i2->second, i1->second, 1));
    }

    SparseMatrix<uint8_t> adj(n, n);
    adj.setFromTriplets(adjacents.begin(), adjacents.end());

    int vert = -1;
    vector<uint32_t> perimeter;
    vector<vector<uint32_t>*> perimeters;

    while (true) {
        if (vert == -1) {
            for (vert = 0; vert < adj.cols(); vert++) {
                for (int i = 0; i < adj.rows(); i++) {
                    if (adj.coeff(i, vert) > 0) {
                        goto next_vert;
                    }
                }
            }
            goto exit;
        } else {
            int next = -1;
            for (int r = 0; r < adj.rows(); r++) {
                if (adj.coeff(vert, r) > 0) {
                    adj.coeffRef(vert, r)--;
                    adj.coeffRef(r, vert)--;
                    next = r;
                    break;
                }
            }
            vert = next;
        }
next_vert:
        if (vert != -1) {
            perimeter.push_back(vert);
        } else if (perimeter.size() > 0) {
            ls.perimeters.push_back(perimeter);
            perimeter.clear();
        }
    }
exit:
    return;
}

void slice(const tooldef td, const mesh &m, vector<levelset> &levelsets) {
    levelsets.clear();

    bounds b = m.get_bounds();
    int level_count = ceil((b.max_z - b.min_z) / td.z_accuracy);
    for (int i = 0; i < level_count; i++) {
        levelset l;
        l.z = b.min_z + i * td.z_accuracy;
        levelsets.push_back(l);
    }
    levelset l;
    l.z = b.max_z;
    levelsets.push_back(l);

    bucket_faces(m, b, td.z_accuracy, levelsets);

    for (auto iter = levelsets.begin(); iter != levelsets.end(); iter++) {
        find_line_segments(*iter);
        linesegs_to_vert_list(*iter);
    }
}

lineseg::lineseg() {}

lineseg::lineseg(const lineseg &other) : p1(other.p1), p2(other.p2) {}

levelset::levelset() {}

levelset::levelset(const levelset &other) :
        verteces(other.verteces), perimeters(other.perimeters),
        inplane(other.inplane), z(other.z), faces(other.faces),
        lines(other.lines) {}

ostream& operator<< (ostream &out, const lineseg &l) {
    out << "(" << l.p1.transpose() << ",\t" << l.p2.transpose() << ")";
    return out;
}

ostream& operator<< (ostream &out, const levelset &ls) {
    out << "[verteces = " << ls.verteces.size()
        << " lines size " << ls.lines.size()
        << "]";
    return out;
}


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2013.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metamonad/pair.hpp>
#include <mpllibs/metamonad/first.hpp>
#include <mpllibs/metamonad/second.hpp>
#include <mpllibs/metamonad/returns.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/pair.hpp>
#include <boost/mpl/assert.hpp>

#include "common.hpp"

BOOST_AUTO_TEST_CASE(test_pair)
{
  using boost::mpl::equal_to;

  using mpllibs::metamonad::pair;
  using mpllibs::metamonad::first;
  using mpllibs::metamonad::second;
  using mpllibs::metamonad::returns;

  typedef returns<int11> returns11;
  typedef returns<int13> returns13;

  // test_first
  BOOST_MPL_ASSERT((equal_to<int11, first<pair<returns11, returns13> >::type>));

  // test_second
  BOOST_MPL_ASSERT((equal_to<int13, second<pair<returns11,returns13> >::type>));

  // test_currying1
  BOOST_MPL_ASSERT((
    equal_to<int11, first<pair<returns11>::type::apply<returns13> >::type>
  ));

  // test_currying2
  BOOST_MPL_ASSERT((
    equal_to<
      int11,
      first<pair<>::type::apply<returns11>::type::apply<returns13> >::type
    >
  ));
}


#include <EigenFaceMaskedArray.h>
#include <EigenFace.h>
#include <EigenFaceData.h>

#include <QColor>
#include <QImage>
#include <QString>
#include <QVector>

#include <Return.h>

#include <math.h>

EigenFaceMaskedArray::EigenFaceMaskedArray(const EigenFaceData * data, int id)
    : efData(data), Id(id)
{
    clear();
} // c'tor

EigenFaceMaskedArray::EigenFaceMaskedArray(const EigenFaceData * data, const QByteArray & ba)
    : efData(data)
{
    clear();
    if (data && data->isValid())
    {
        EigenFaceMaskedArray efma(data);
        efma.read(ba);
        if (efma.isValid())
            *this = efma;
    }
} // c'tor

EigenFaceMaskedArray::~EigenFaceMaskedArray()
{
} // d'tor

void EigenFaceMaskedArray::clear(void)
{
    Id = 0;
    VectorKey = 0;
    minData = INT_MAX, maxData = INT_MIN;
    Residual = 0.0;
    data.clear();
}

void EigenFaceMaskedArray::fill(qreal v)
{
    clear();
    minData = maxData = v;
    Residual = 0.0;
    if (efData)
        data.fill(v, efData->activePixels());
} // fill()

bool EigenFaceMaskedArray::isValid(void) const
{
    if ( ! efData)
        return false;
    if (! efData->mask().isNull())
    {
        if ( ! efData->mask().isValid())
            return false;
        if (efData->mask().count() != data.count())
            return false;
    }
    return true;
}

// IEIGENFACE_PRELIMINARY
/*
EigenFaceMaskedArray::EigenFaceMaskedArray(EigenFaceData * EFdata, int id, int length, float * fp)
    : efData(EFdata), Id(id), minData(INT_MAX), maxData(INT_MIN)
{
    if ( ! efData || ! efData->mask().isValid() || length != efData->mask().count())
        return;

    data.resize(length);
    int x = 0;
    while (length)
    {
        data[x] = *fp;
        if (*fp < minData)	minData = *fp;
        if (*fp > maxData)	maxData = *fp;
        ++fp, --length, ++x;
    }
}
*/

qreal EigenFaceMaskedArray::subtract(const qreal subtrahend)
{
    int n = data.size();
    qreal sumSquares = 0.0;
    minData = INT_MAX, maxData = INT_MIN;
    for (int x = 0; x < n; ++x)
    {
        qreal v = data[x] - subtrahend;
        data[x] = v;
        if (v < minData)	minData = v;
        if (v > maxData)	maxData = v;
        sumSquares += v * v;
    }
    return sumSquares;
} // subtract(c)

void EigenFaceMaskedArray::divideBy(const qreal divisor)
{
    int n = data.size();
    minData = INT_MAX, maxData = INT_MIN;
    for (int x = 0; x < n; ++x)
    {
        qreal v = data[x] / divisor;
        data[x] = v;
        if (v < minData)	minData = v;
        if (v > maxData)	maxData = v;
    }
} // divideBy()

// subtract other from this, return sum of square of result
qreal EigenFaceMaskedArray::subtract(const EigenFaceMaskedArray & other)
{
    int n = data.size();
    qreal sumSquares = 0.0;
    minData = INT_MAX, maxData = INT_MIN;
    for (int x = 0; x < n; ++x)
    {
        qreal v = data[x] - other.data[x];
        data[x] = v;
        if (v < minData)	minData = v;
        if (v > maxData)	maxData = v;
        sumSquares += v * v;
    }
    return sumSquares;
} // subtract(other)

EigenFaceVector EigenFaceMaskedArray::generateVector(int maxLayers)
{
    EigenFaceVector outVector;
    outVector.setVectorKey(vectorKey());

    if ( ! maxLayers || maxLayers > efData->layerCount())
        maxLayers = efData->layerCount();
    outVector.reserve(maxLayers);
    qreal finalResidual = Residual;

    for (int i = 0; i < maxLayers; ++i)
    {
        EigenFaceMaskedArray layer = efData->layer(i);
        qreal dp = dotProduct(layer);
        outVector.setCoefficient(i, dp);
        finalResidual -= dp * dp;
    }
    outVector.setResidual(finalResidual);

    return outVector;
}

Return EigenFaceMaskedArray::reconstruct(const EigenFaceVector & vector, int numCoefficients)
{
    if ( ! efData || ! efData->mask().isValid())
        return Return(EigenFace::ReturnInvalidStructure, "BitMask");
    if ( ! vector.isValid())
        return Return(EigenFace::ReturnInvalidStructure, "Vector");

    if ( ! numCoefficients || numCoefficients > vector.count())
        numCoefficients = vector.count();
    if (numCoefficients > efData->layerCount())
        numCoefficients = efData->layerCount();

    int meanFaceId = vector.vectorKey() >> 8;
    EigenFaceMaskedArray meanFace = efData->meanFace(meanFaceId);
    if ( ! meanFace.isValid())
        return Return(EigenFace::ReturnInvalidStructure, "MeanFace");
    data = meanFace.data;

    for (int i = 0; i < numCoefficients; ++i)
    {
        qreal coef = vector.coefficient(i);
        EigenFaceMaskedArray layer = efData->layer(i);
        if ( ! layer.isValid())
            return Return(EigenFace::ReturnInvalidStructure, "Layer");
        add(layer, coef);
    }

    return Return();
} // reconstruct()

qreal EigenFaceMaskedArray::dotProduct(const EigenFaceMaskedArray & other) const
{
    int n = data.size();
    qreal dp = 0.0;
    for (int x = 0; x < n; ++x)
        dp += data[x] * other.data[x];
    return dp;
} // dotProduct()

void EigenFaceMaskedArray::add(const EigenFaceMaskedArray & other, qreal coefficient)
{
    int n = data.size();
    minData = INT_MAX, maxData = INT_MIN;
    for (int x = 0; x < n; ++x)
    {
        qreal v = data[x] + coefficient * other.data[x];
        data[x] = v;
        if (v < minData)	minData = v;
        if (v > maxData)	maxData = v;
    }
} // add()


#pragma once

#include <stack>
#include <queue>
#include <vector>
#include <Eigen/Dense>
#include <memory>
#include <cmath>
#include <limits>
#include <cstdlib>
#include <time.h>
#include <string>
#include <map>
#include <queue>
#include <algorithm>
#include <iostream>
#include <fstream>

//ompl
#include "ompl/datastructures/NearestNeighbors.h"
#include <ompl/datastructures/BinaryHeap.h>
#include "ompl/tools/config/SelfConfig.h"
#include "ompl/datastructures/NearestNeighborsGNAT.h"
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/config.h>

//ros
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Bool.h>

//hdi_plan
#include "dynamic_motion_planner/quadrotor.hpp"
#include "dynamic_motion_planner/RRT_node.hpp"
#include "dynamic_motion_planner/chomp.hpp"
#include "dynamic_motion_planner/chomp_trajectory.hpp"
#include "utils/types.hpp"
#include "utils/utility_functions.hpp"
#include "generate_dynamic_scene/obstacle.hpp"
#include "generate_dynamic_scene/human.hpp"
#include <hdi_plan/obstacle_info.h>
#include <hdi_plan/point_array.h>

namespace hdi_plan {

class MotionPlanner {
public:
    MotionPlanner(const ros::NodeHandle& nh, const ros::NodeHandle& pnh);
    ~MotionPlanner();

    // debug
    ompl::base::RealVectorBounds get_space_info();
    double get_nn_size();
    void check_params();

private:
    // debug
    int debug{0};
    int iteration_count{0};
    bool if_find_solution_{false};
    std::ofstream solve_time_path_file;
    std::ofstream optimize_time_path_file;
	std::ofstream drone_actual_path_file;
	void visualize_node_tree();
	bool with_chomp_{true};

	double get_distance_check_ob(const std::shared_ptr<RRTNode>& node1, const std::shared_ptr<RRTNode>& node2);

    // drone
    double quadrotor_radius_{0.5};

    // ros nodes
    ros::NodeHandle nh_;
    ros::NodeHandle pnh_;

    // initialize stack for storing points that desire to insert in the future when an obstacle is removed
    std::stack<std::vector<Eigen::Vector3d>> sample_stack;

    // useful rrt nodes
    std::shared_ptr<RRTNode> start_;
    std::shared_ptr<RRTNode> goal_;
    std::shared_ptr<RRTNode> quadrotor_;

    // space configuration
    ompl::base::SpaceInformationPtr si_;
    std::shared_ptr<ompl::base::SE3StateSpace> space_;
    void setup_space();
    int dimension_{3};

    // geometric Near-neighbor Access Tree
    std::shared_ptr<ompl::NearestNeighborsGNAT<std::shared_ptr<RRTNode>>> nearest_neighbors_tree_;

    // setup
    bool load_params();
    void setup();
    double distance_function(const std::shared_ptr<RRTNode>& a, const std::shared_ptr<RRTNode>& b) ;
    void set_to_start_position();
    bool position_ready{false};

    //
    std::shared_ptr<RRTNode> generate_random_node();

    // quadrotor related
    Eigen::Vector3d quadrotor_state_;

    // subscriber
    ros::Subscriber sub_quadrotor_state_;
    void quadrotor_state_callback(const nav_msgs::Odometry::ConstPtr &msg);

    ros::Subscriber sub_obstacle_info_;
    void obstacle_info_callback(const hdi_plan::obstacle_info::ConstPtr &msg);

	ros::Subscriber sub_goal_point_;
	void goal_point_callback(const geometry_msgs::Point::ConstPtr &msg);

	ros::Subscriber sub_human_movement_;
	void human_movement_callback(const hdi_plan::obstacle_info::ConstPtr &msg);
	//void human_movement_callback_region_version(const geometry_msgs::Point::ConstPtr &msg);

    // publisher
    ros::Publisher pub_optimized_path_;
	ros::Publisher pub_go_to_pose_;
	ros::Publisher pub_max_velocity_;
    ros::Publisher pub_get_new_path_;

    ros::Publisher pub_solution_path_;
    ros::Publisher pub_node_pos_;
    ros::Publisher pub_blocked_node_pos_;

    // main loop
    bool solve();

	// RRTX node
	int node_index_{0};
	std::vector<std::shared_ptr<RRTNode>> node_list_;
	void add_node_to_nearest_neighbors_tree(const std::shared_ptr<RRTNode>& node);

    // for calculate the shrinking ball radius
    void calculateRRG();
    double max_distance_{1}; // the maximum length of a motion to be added to a tree
    double r_rrt_{5}; // a constant for r-disc rewiring calculations
    double rrg_r_{3}; // current value of the radius used for the neighbors

    // functions in the main loop
    void saturate(std::shared_ptr<RRTNode> random_node, const std::shared_ptr<RRTNode>& nearest_node, double distance) const;
    bool node_in_free_space_check(const std::shared_ptr<RRTNode>& random_node);
    bool edge_in_free_space_check(const std::shared_ptr<RRTNode>& node1, const std::shared_ptr<RRTNode>& node2);
    bool extend(std::shared_ptr<RRTNode> random_node);

    // extend related
    void update_neighbors_list(std::shared_ptr<RRTNode> random_node);
    bool find_best_parent(std::shared_ptr<RRTNode> random_node);

    // cost related members
    //double compute_cost_human_cost_region_version(const Eigen::Vector3d& state1, const Eigen::Vector3d& state2);
	//double compute_cost_with_weight(const Eigen::Vector3d& state1, const Eigen::Vector3d& state2, double weight=1);
    double combine_cost(double cost1, double cost2);
    bool is_cost_better_than(double cost1, double cost2);

    // rewire
    double epsilon_{0.5};

    // cannot use priority queue since we need to update the key value
    //std::priority_queue<std::shared_ptr<RRTNode>, std::vector<std::shared_ptr<RRTNode>>, node_compare> node_queue1;
    ompl::BinaryHeap<std::shared_ptr<RRTNode>, RRTNode::node_compare> node_queue;
	void rewire_neighbors_v1(std::shared_ptr<RRTNode> random_node);
	void rewire_neighbors_v2(std::shared_ptr<RRTNode> random_node);
	void rewire_neighbors_for_env_update(std::shared_ptr<RRTNode> random_node);
    void cull_neighbors(std::shared_ptr<RRTNode> random_node);
    void verify_queue(std::shared_ptr<RRTNode> node);
	void reduce_inconsistency_v1();
	void reduce_inconsistency_v2();
    void reduce_inconsistency_for_env_update();
	void update_lmc_v1(std::shared_ptr<RRTNode> node);
    void update_lmc_v2(std::shared_ptr<RRTNode> node);
    void update_lmc(std::shared_ptr<RRTNode> node);
	void update_lmc_for_env_update(std::shared_ptr<RRTNode> node);
    // find the solution path
    std::vector<Eigen::Vector3d> solution_path;
	std::vector<Eigen::Vector3d> solution_path_tmp;

    bool update_solution_path();
	bool update_solution_path_tmp();
    void publish_without_chomp();
    void optimize_solution_path();

    // obstacle related
    void initiate_obstacles();
    void update_obstacle();
    std::map<std::string, std::shared_ptr<Obstacle>> obstacle_map;
	std::vector<std::shared_ptr<Obstacle>> obstacle_update_info_list;
    void remove_obstacle(const std::shared_ptr<Obstacle>& obstacle);
    void add_obstacle(const std::shared_ptr<Obstacle>& obstacle);
    void propogate_descendants();
    bool check_if_node_inside_obstacle(const std::shared_ptr<Obstacle>& obstacle, const std::shared_ptr<RRTNode>& node);
	bool check_if_node_inside_obstacle_for_adding_ob(const std::shared_ptr<Obstacle>& obstacle, const std::shared_ptr<RRTNode>& node);
	bool check_if_node_inside_all_obstacles(const std::shared_ptr<RRTNode>& node, bool consider_human);
	std::vector<std::shared_ptr<RRTNode>> orphan_node_list;
	void verify_orphan(std::shared_ptr<RRTNode> node);
	bool check_if_node_in_orphan_list(const std::shared_ptr<RRTNode>& node);
	bool if_environment_change{false};

	// utils
	void make_parent_of(std::shared_ptr<RRTNode> parent_node, std::shared_ptr<RRTNode> node);

	// human part
	std::map<int, std::shared_ptr<Human>> human_map_tmp_;
    std::map<int, std::shared_ptr<Human>> human_map_;
	std::vector<int> human_id_list_;
	int get_human_id(const std::string human_name);
	bool exist_human_{false};
	int human_callback_count{0};
	void add_human_as_obstacle(int human_id);
	bool check_if_edge_collide_human(const std::shared_ptr<RRTNode>& node1, const std::shared_ptr<RRTNode>& node2);
	//void find_nodes_in_human_position(const Eigen::Vector2d& human_position);
	//Eigen::Vector2d human_position_;
	bool if_add_human_{false};
	//bool check_if_node_inside_human(const std::shared_ptr<RRTNode>& node);
	//double check_if_state_near_human(const Eigen::Vector3d& state1);

	// time dimension
	double total_plan_time_{30};
	double generate_random_time(const Eigen::Vector3d& state);
	double quadrotor_speed_{1};
	ros::WallTime start_time_;

    // path write to file name
    int path_file_num_{0};
    int chomp_path_file_num_{0};

    // chomp trajectory
	double discretization_;

	// chomp
	double collision_threshold_;
	double planning_time_limit_;
	int max_iterations_;
	int max_iterations_after_collision_free_;
	double learning_rate_;
	double obstacle_cost_weight_;
	double dynamic_obstacle_cost_weight_;
	double dynamic_collision_factor_;
	double smoothness_cost_weight_;
	double smoothness_cost_velocity_;
	double smoothness_cost_acceleration_;
	double smoothness_cost_jerk_;
	double ridge_factor_;
	double min_clearence_;
	double joint_update_limit_;

    // dynamic obstacle
    double planning_horizon_time_;
};

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SCALAR_SINECOSINE_HPP_INCLUDED
#define NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SCALAR_SINECOSINE_HPP_INCLUDED
#include <nt2/toolbox/trigonometric/functions/sinecosine.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/toolbox/trigonometric/functions/scalar/impl/trigo.hpp>
/////////////////////////////////////////////////////////////////////////////
// reference based Implementation
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::sinecosine_<mode>, tag::cpu_,
			     (A0)(A1)(mode),
			     (scalar_ < arithmetic_<A0> > )
			     (scalar_ < floating_<A1> > )
			     (scalar_ < floating_<A1> > )
			     )
  {
    typedef nt2::int32_t result_type;
    inline result_type operator()(A0 const& a0,A1 & a1,A1 & a2) const
    {
      a1 = impl::trig_base <A1,radian_tag,tag::not_simd_type, mode>::sincosa(A1(a0),a2);
      return 0;
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::sinecosine_<mode>, tag::cpu_,
			     (A0)(A1)(mode),
			     (scalar_ < arithmetic_<A0> > )
			     (scalar_ < floating_<A1> > )
			     )
  {
    typedef A1 result_type;
    inline A1 operator()(A0 const& a0,A1 & a2) const
    {
      return impl::trig_base <A1,radian_tag,tag::not_simd_type, mode>::sincosa(A1(a0),a2);
    }
  };

  /////////////////////////////////////////////////////////////////////////////
  // Implementation when type  is floating_
  /////////////////////////////////////////////////////////////////////////////
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::sinecosine_<mode>, tag::cpu_,
			     (A0)(mode),
			     (scalar_ < arithmetic_<A0> > )
			     )
  {
    typedef typename boost::dispatch::meta::as_floating<A0>::type etype;
    typedef boost::fusion::tuple<etype, etype>           result_type;
    NT2_FUNCTOR_CALL(1)
      {
	return sinecosine(etype(a0));
      }
  };

  /////////////////////////////////////////////////////////////////////////////
  // Implementation when type  is floating_
  /////////////////////////////////////////////////////////////////////////////

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::sinecosine_<mode>, tag::cpu_,
			     (A0)(mode),
			     (scalar_ < floating_<A0> > )
			     )
  {
    typedef boost::fusion::tuple<A0, A0>       result_type;
    NT2_FUNCTOR_CALL(1)
      {
	result_type res;
	boost::fusion::at_c<0>(res) = impl::trig_base < A0,radian_tag
	  ,  tag::not_simd_type, mode
	  >::sincosa(a0, boost::fusion::at_c<1>(res));
        return res;
      }
    };
} }
#endif


/**
 * \ file BlockLMSFilter.cpp
 */

#include <array>
#include <fstream>
#include <iostream>

#include <ATK/Adaptive/BlockLMSFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <ATK/Mock/SimpleSinusGeneratorFilter.h>
#include <ATK/Mock/TriangleCheckerFilter.h>

#include <ATK/Tools/SumFilter.h>

#include <Eigen/Core>

#include <gtest/gtest.h>

#include <boost/math/constants/constants.hpp>

constexpr gsl::index PROCESSSIZE = 1200;

TEST(BlockLMSFilter, destructor_test)
{
  ASSERT_NO_THROW(std::make_unique<ATK::BlockLMSFilter<double>>(100));
}

TEST(BlockLMSFilter, size_negative_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  ASSERT_THROW(filter.set_size(0), ATK::RuntimeError);
}

TEST(BlockLMSFilter, size_set_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  filter.set_size(10);
  ASSERT_EQ(filter.get_size(), 10);
}

TEST(BlockLMSFilter, build_throw_test)
{
  ASSERT_THROW(ATK::BlockLMSFilter<double> filter(0), ATK::RuntimeError);
}

TEST(BlockLMSFilter, blocksize_negative_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  ASSERT_THROW(filter.set_block_size(0), ATK::RuntimeError);
}

TEST(BlockLMSFilter, blocksize_set_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  filter.set_block_size(10);
  ASSERT_EQ(filter.get_block_size(), 10);
}

TEST(BlockLMSFilter, memory_negative_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  ASSERT_THROW(filter.set_memory(0), ATK::RuntimeError);
}

TEST(BlockLMSFilter, memory_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  filter.set_memory(0.5);
  ASSERT_EQ(filter.get_memory(), 0.5);
}

TEST(BlockLMSFilter, memory_positive1_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  ASSERT_THROW(filter.set_memory(1), ATK::RuntimeError);
}

TEST(BlockLMSFilter, mu_negative_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  ASSERT_THROW(filter.set_mu(0), ATK::RuntimeError);
}

TEST(BlockLMSFilter, mu_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  filter.set_mu(0.5);
  ASSERT_EQ(filter.get_mu(), 0.5);
}

TEST(BlockLMSFilter, mu_positive1_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  ASSERT_THROW(filter.set_mu(1), ATK::RuntimeError);
}

TEST(BlockLMSFilter, learning_set_test)
{
  ATK::BlockLMSFilter<double> filter(100);
  ASSERT_EQ(filter.get_learning(), true);
  filter.set_learning(false);
  ASSERT_EQ(filter.get_learning(), false);
}

TEST(BlockLMSFilter, memory_999_test)
{
  std::array<double, PROCESSSIZE> data;
  {
    std::ifstream input(ATK_SOURCE_TREE "/tests/data/input_lms.dat", std::ios::binary);
    input.read(reinterpret_cast<char*>(data.data()), PROCESSSIZE * sizeof(double));
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  ATK::BlockLMSFilter<double> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_output_sampling_rate(48000);
  filter.set_memory(.999);
  filter.set_mu(.0001);

  std::array<double, PROCESSSIZE> outdata;
  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_output_sampling_rate(48000);

  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generator, 0);
  output.set_input_port(0, &filter, 0);

  filter.process(100);
  output.process(PROCESSSIZE);
  ASSERT_NE(filter.get_w(), nullptr);

  std::array<double, PROCESSSIZE> ref;
  {
    std::ifstream input(ATK_SOURCE_TREE "/tests/data/output_blocklms.dat", std::ios::binary);
    input.read(reinterpret_cast<char*>(ref.data()), PROCESSSIZE * sizeof(double));
  }

  for (unsigned int i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(outdata[i], ref[i], 0.0001);
  }
}


#include <random>

#include <Eigen/Core>
#include <gtest/gtest.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <sensors/sensor-factory.h>
#include <vi-map/test/vi-map-generator.h>

#include "vi-map-helpers/gps-vi-map-nearest-neighbor-lookup.h"
#include "vi-map-helpers/vi-map-nearest-neighbor-lookup.h"

namespace vi_map_helpers {

constexpr size_t kSeed = 42u;

typedef Aligned<std::vector, Eigen::VectorXd> MeasurementsList;
typedef AlignedUnorderedSet<Eigen::VectorXd> MeasurementsSet;

Eigen::VectorXd getGroundTruthClosestDataItem(
    const Aligned<std::vector, Eigen::VectorXd>& measurements,
    const Eigen::VectorXd& query) {
  Eigen::VectorXd closest_measurement;

  double min_distance = std::numeric_limits<double>::max();
  for (size_t idx = 0u; idx < measurements.size(); ++idx) {
    const double distance = (query - measurements[idx]).squaredNorm();
    if (distance < min_distance) {
      closest_measurement = measurements[idx];
      min_distance = distance;
    }
  }
  return closest_measurement;
}

void getGroundTruthAllDataItemsWithinRadius(
    const MeasurementsList& measurements, const Eigen::VectorXd& query,
    const double search_radius,
    AlignedUnorderedSet<Eigen::VectorXd>* measurements_within_radius) {
  CHECK_NOTNULL(measurements_within_radius)->clear();

  for (size_t idx = 0u; idx < measurements.size(); ++idx) {
    const Eigen::VectorXd measurement = measurements[idx];
    const double distance = (query - measurement).norm();
    if (distance <= search_radius) {
      measurements_within_radius->emplace(measurement);
    }
  }
}

TEST(VIMapNearestNeighborLookupTest, EmptyMap) {
  vi_map::VIMap map;
  vi_map::VIMapGenerator generator(map, kSeed);
  generator.generateMap();

  VIMapNearestNeighborLookupVertexId nn_query_database(map);

  const Eigen::Vector3d p_query = Eigen::Vector3d::Zero();

  pose_graph::VertexId nn_vertex_id;
  nn_query_database.getClosestDataItem(p_query, &nn_vertex_id);
  EXPECT_FALSE(nn_vertex_id.isValid());
}

TEST(VIMapNearestNeighborLookupTest, SingleVertexMap) {
  vi_map::VIMap map;
  vi_map::VIMapGenerator generator(map, kSeed);
  const vi_map::MissionId mission_id = generator.createMission();
  CHECK(mission_id.isValid());
  aslam::Transformation T_M_I0;
  T_M_I0.getPosition() << 1, 2, 3;
  const pose_graph::VertexId vertex_id_0 =
      generator.createVertex(mission_id, T_M_I0);
  CHECK(vertex_id_0.isValid());

  generator.generateMap();

  VIMapNearestNeighborLookupVertexId nn_query_database(map);

  srand(kSeed);

  const size_t kNumQueries = 10u;
  for (size_t query_idx = 0u; query_idx < kNumQueries; ++query_idx) {
    Eigen::Vector3d p_query = 1e3 * Eigen::Vector3d::Random();
    pose_graph::VertexId nn_vertex_id;
    nn_query_database.getClosestDataItem(p_query, &nn_vertex_id);
    EXPECT_TRUE(nn_vertex_id.isValid());
    EXPECT_EQ(nn_vertex_id, vertex_id_0);
  }
}

typedef AlignedUnorderedMap<pose_graph::VertexId, Eigen::VectorXd>
    VertexIdToMeasurementMap;

TEST(VIMapNearestNeighborLookupTest, MultipleVertexMap) {
  vi_map::VIMap map;
  vi_map::VIMapGenerator generator(map, kSeed);
  const vi_map::MissionId mission_id = generator.createMission();
  CHECK(mission_id.isValid());

  srand(kSeed);

  const size_t kNumVertices = 10u;

  MeasurementsList p_G_Is;
  aslam::TransformationVector T_G_Is(kNumVertices);
  VertexIdToMeasurementMap vertex_id_to_data_item_map;

  for (size_t idx = 0u; idx < kNumVertices; ++idx) {
    T_G_Is[idx].getPosition() = 1e3 * Eigen::Vector3d::Random();

    const pose_graph::VertexId vertex_id =
        generator.createVertex(mission_id, T_G_Is[idx]);
    CHECK(vertex_id.isValid());

    const Eigen::VectorXd measurement =
        queryTypeToVector(T_G_Is[idx].getPosition());
    p_G_Is.emplace_back(measurement);
    vertex_id_to_data_item_map.emplace(vertex_id, measurement);
  }

  generator.generateMap();

  VIMapNearestNeighborLookupVertexId nn_query_database(map);

  const size_t kNumQueries = 10u;
  for (size_t query_idx = 0u; query_idx < kNumQueries; ++query_idx) {
    Eigen::Vector3d p_G_I_query = 1e3 * Eigen::Vector3d::Random();
    pose_graph::VertexId closest_vertex_id;
    nn_query_database.getClosestDataItem(p_G_I_query, &closest_vertex_id);
    EXPECT_TRUE(closest_vertex_id.isValid());

    const Eigen::VectorXd ground_truth_closest_p_G_I =
        getGroundTruthClosestDataItem(p_G_Is, queryTypeToVector(p_G_I_query));

    VertexIdToMeasurementMap::const_iterator p_G_I_iterator =
        vertex_id_to_data_item_map.find(closest_vertex_id);
    CHECK(p_G_I_iterator != vertex_id_to_data_item_map.end());

    EXPECT_EQ(p_G_I_iterator->second, ground_truth_closest_p_G_I);
  }
}

TEST(VIMapNearestNeighborLookupTest, MultipleVertexRadiusLookup) {
  vi_map::VIMap map;
  vi_map::VIMapGenerator generator(map, kSeed);
  const vi_map::MissionId mission_id = generator.createMission();
  CHECK(mission_id.isValid());

  srand(kSeed);

  const size_t kNumVertices = 7u;

  MeasurementsList p_G_Is;
  aslam::TransformationVector T_G_Is(kNumVertices);
  pose_graph::VertexIdList T_G_I_vertex_ids(kNumVertices);

  VertexIdToMeasurementMap vertex_id_to_data_item_map;

  for (size_t idx = 0u; idx < kNumVertices; ++idx) {
    T_G_Is[idx].getPosition() = 1e3 * Eigen::Vector3d::Random();

    const pose_graph::VertexId vertex_id =
        generator.createVertex(mission_id, T_G_Is[idx]);
    CHECK(vertex_id.isValid());

    T_G_I_vertex_ids[idx] = vertex_id;

    const Eigen::VectorXd p_G_I = queryTypeToVector(T_G_Is[idx].getPosition());
    p_G_Is.emplace_back(p_G_I);
    vertex_id_to_data_item_map.emplace(vertex_id, p_G_I);
  }

  generator.generateMap();

  VIMapNearestNeighborLookupVertexId nn_query_database(map);

  std::mt19937 random_number_generator(kSeed);
  std::uniform_real_distribution<> uniform_distribution(0.0, 1e3);

  const size_t kNumQueries = 10u;
  for (size_t query_idx = 0u; query_idx < kNumQueries; ++query_idx) {
    const Eigen::Vector3d p_G_I_query = 1e3 * Eigen::Vector3d::Random();

    const double search_radius = uniform_distribution(random_number_generator);

    pose_graph::VertexIdSet vertex_ids_within_search_radius;
    nn_query_database.getAllDataItemsWithinRadius(
        p_G_I_query, search_radius, &vertex_ids_within_search_radius);

    MeasurementsSet p_G_Is_within_search_radius;
    for (const pose_graph::VertexId& vertex_id :
         vertex_ids_within_search_radius) {
      CHECK(vertex_id.isValid());
      VertexIdToMeasurementMap::const_iterator p_G_I_iterator =
          vertex_id_to_data_item_map.find(vertex_id);
      CHECK(p_G_I_iterator != vertex_id_to_data_item_map.end());
      p_G_Is_within_search_radius.emplace(p_G_I_iterator->second);
    }

    MeasurementsSet ground_truth_p_G_Is_within_search_radius;
    getGroundTruthAllDataItemsWithinRadius(
        p_G_Is, queryTypeToVector(p_G_I_query), search_radius,
        &ground_truth_p_G_Is_within_search_radius);

    EXPECT_EQ(
        p_G_Is_within_search_radius, ground_truth_p_G_Is_within_search_radius);
  }
}

TEST(VIMapNearestNeighborLookupTest, MultipleGPSWGSMeasurementsNNLookup) {
  vi_map::VIMap map;
  vi_map::VIMapGenerator generator(map, kSeed);
  const vi_map::MissionId mission_id = generator.createMission();
  CHECK(mission_id.isValid());

  generator.generateMap();

  srand(kSeed);

  const size_t kNumMeasurements = 10u;

  MeasurementsList wgs_measurements_lat_lon;

  std::mt19937 random_number_generator(kSeed);
  std::uniform_real_distribution<> uniform_distribution_latitude(
      vi_map::GpsWgsMeasurement::kMinLatitudeDeg,
      vi_map::GpsWgsMeasurement::kMaxLatitudeDeg);
  std::uniform_real_distribution<> uniform_distribution_longitude(
      vi_map::GpsWgsMeasurement::kMinLongitudeDeg,
      vi_map::GpsWgsMeasurement::kMaxLongitudeDeg);
  std::uniform_real_distribution<> uniform_distribution_altitude(
      vi_map::GpsWgsMeasurement::kMinAltitudeMeters,
      vi_map::GpsWgsMeasurement::kMaxAltitudeMeters);

  vi_map::GpsWgs::UniquePtr gps_wgs_sensor =
      vi_map::createTestSensor<vi_map::GpsWgs>();
  CHECK(gps_wgs_sensor);
  const vi_map::SensorId sensor_id = gps_wgs_sensor->getId();
  CHECK(sensor_id.isValid());
  map.getSensorManager().addSensor(std::move(gps_wgs_sensor), mission_id);

  for (size_t idx = 0u; idx < kNumMeasurements; ++idx) {
    const vi_map::GpsWgsMeasurement wgs_measurement(
        sensor_id, static_cast<int64_t>(idx) + 1,
        uniform_distribution_latitude(random_number_generator),
        uniform_distribution_longitude(random_number_generator),
        uniform_distribution_altitude(random_number_generator));

    map.addOptionalSensorMeasurement(wgs_measurement, mission_id);
    wgs_measurements_lat_lon.emplace_back(queryTypeToVector(wgs_measurement));
  }

  VIMapNearestNeighborLookupGpsWgs nn_query_database(map);

  const size_t kNumQueries = 10u;
  const int64_t kInvalidTimestampNanoseconds = 0;
  for (size_t query_idx = 0u; query_idx < kNumQueries; ++query_idx) {
    const vi_map::GpsWgsMeasurement query_wgs_measurement(
        sensor_id, kInvalidTimestampNanoseconds,
        uniform_distribution_latitude(random_number_generator),
        uniform_distribution_longitude(random_number_generator),
        uniform_distribution_altitude(random_number_generator));

    vi_map::GpsWgsMeasurement closest_wgs_measurement;
    nn_query_database.getClosestDataItem(
        query_wgs_measurement, &closest_wgs_measurement);

    const Eigen::VectorXd ground_truth_closest_wgs_measurement =
        getGroundTruthClosestDataItem(
            wgs_measurements_lat_lon, queryTypeToVector(query_wgs_measurement));
    EXPECT_EQ(
        queryTypeToVector(closest_wgs_measurement),
        ground_truth_closest_wgs_measurement);
  }
}

TEST(VIMapNearestNeighborLookupTest, MultipleWGSMeasurementRadiusLookup) {
  vi_map::VIMap map;
  vi_map::VIMapGenerator generator(map, kSeed);
  const vi_map::MissionId mission_id = generator.createMission();
  CHECK(mission_id.isValid());

  generator.generateMap();

  srand(kSeed);

  const size_t kNumMeasurements = 10u;

  MeasurementsList wgs_measurements_lat_lon;

  std::mt19937 random_number_generator(kSeed);
  std::uniform_real_distribution<> uniform_distribution_latitude(
      vi_map::GpsWgsMeasurement::kMinLatitudeDeg,
      vi_map::GpsWgsMeasurement::kMaxLatitudeDeg);
  std::uniform_real_distribution<> uniform_distribution_longitude(
      vi_map::GpsWgsMeasurement::kMinLongitudeDeg,
      vi_map::GpsWgsMeasurement::kMaxLongitudeDeg);
  std::uniform_real_distribution<> uniform_distribution_altitude(
      vi_map::GpsWgsMeasurement::kMinAltitudeMeters,
      vi_map::GpsWgsMeasurement::kMaxAltitudeMeters);

  vi_map::GpsWgs::UniquePtr gps_wgs_sensor =
      vi_map::createTestSensor<vi_map::GpsWgs>();
  CHECK(gps_wgs_sensor);
  const vi_map::SensorId sensor_id = gps_wgs_sensor->getId();
  CHECK(sensor_id.isValid());
  map.getSensorManager().addSensor(std::move(gps_wgs_sensor), mission_id);

  for (size_t idx = 0u; idx < kNumMeasurements; ++idx) {
    const vi_map::GpsWgsMeasurement wgs_measurement(
        sensor_id, static_cast<int64_t>(idx) + 1,
        uniform_distribution_latitude(random_number_generator),
        uniform_distribution_longitude(random_number_generator),
        uniform_distribution_altitude(random_number_generator));

    map.addOptionalSensorMeasurement(wgs_measurement, mission_id);
    wgs_measurements_lat_lon.emplace_back(queryTypeToVector(wgs_measurement));
  }

  VIMapNearestNeighborLookupGpsWgs nn_query_database(map);

  std::uniform_real_distribution<> uniform_distribution_search_radius(
      0.0, 180.0);

  const size_t kNumQueries = 10u;
  const int64_t kInvalidTimestampNanoseconds = 0;
  for (size_t query_idx = 0u; query_idx < kNumQueries; ++query_idx) {
    const vi_map::GpsWgsMeasurement query_wgs_measurement(
        sensor_id, kInvalidTimestampNanoseconds,
        uniform_distribution_latitude(random_number_generator),
        uniform_distribution_longitude(random_number_generator),
        uniform_distribution_altitude(random_number_generator));

    const double search_radius =
        uniform_distribution_search_radius(random_number_generator);

    vi_map::GpsWgsMeasurementSet wgs_measurments_lat_lon_within_search_radius;
    nn_query_database.getAllDataItemsWithinRadius(
        query_wgs_measurement, search_radius,
        &wgs_measurments_lat_lon_within_search_radius);

    MeasurementsSet wgs_measurements_lat_lon_within_search_radius_as_vectors;
    for (const vi_map::GpsWgsMeasurement& wgs_measurement_within_search_radius :
         wgs_measurments_lat_lon_within_search_radius) {
      wgs_measurements_lat_lon_within_search_radius_as_vectors.emplace(
          queryTypeToVector(wgs_measurement_within_search_radius));
    }

    MeasurementsSet
        ground_truth_wgs_measurments_lat_lon_within_search_radius_as_vectors;
    getGroundTruthAllDataItemsWithinRadius(
        wgs_measurements_lat_lon, queryTypeToVector(query_wgs_measurement),
        search_radius,
        &ground_truth_wgs_measurments_lat_lon_within_search_radius_as_vectors);

    EXPECT_EQ(
        wgs_measurements_lat_lon_within_search_radius_as_vectors,
        ground_truth_wgs_measurments_lat_lon_within_search_radius_as_vectors);
  }
}

TEST(VIMapNearestNeighborLookupTest, MultipleGPSUTMMeasuremnentsNNLookup) {
  vi_map::VIMap map;
  vi_map::VIMapGenerator generator(map, kSeed);
  const vi_map::MissionId mission_id = generator.createMission();
  CHECK(mission_id.isValid());

  generator.generateMap();

  srand(kSeed);

  const size_t kNumMeasurements = 10u;

  std::mt19937 random_number_generator(kSeed);

  MeasurementsList utm_measurments_p_UTM_Bs;

  vi_map::GpsUtm::UniquePtr gps_utm_sensor =
      vi_map::createTestSensor<vi_map::GpsUtm>();
  CHECK(gps_utm_sensor);
  const vi_map::SensorId sensor_id = gps_utm_sensor->getId();
  CHECK(sensor_id.isValid());
  map.getSensorManager().addSensor(std::move(gps_utm_sensor), mission_id);

  for (size_t idx = 0u; idx < kNumMeasurements; ++idx) {
    const Eigen::Vector3d axis_angle_UTM_B = Eigen::Vector3d::Random();
    const aslam::Position3D p_UTM_B = 1e6 * aslam::Position3D::Random();

    const aslam::Transformation T_UTM_B(
        aslam::Quaternion(aslam::AngleAxis(axis_angle_UTM_B)), p_UTM_B);

    const vi_map::GpsUtmMeasurement utm_measurement(
        sensor_id, static_cast<int64_t>(idx) + 1, T_UTM_B,
        vi_map::UtmZone(32u, 'T'));

    map.addOptionalSensorMeasurement(utm_measurement, mission_id);

    utm_measurments_p_UTM_Bs.emplace_back(queryTypeToVector(utm_measurement));
  }

  VIMapNearestNeighborLookupGpsUtm nn_query_database(map);

  const size_t kNumQueries = 10u;
  const int64_t kInvalidTimestampNanoseconds = 0;
  for (size_t query_idx = 0u; query_idx < kNumQueries; ++query_idx) {
    const Eigen::Vector3d axis_angle_UTM_B = Eigen::Vector3d::Random();
    const aslam::Position3D p_UTM_B = 1e6 * aslam::Position3D::Random();

    const aslam::Transformation T_UTM_B(
        aslam::Quaternion(aslam::AngleAxis(axis_angle_UTM_B)), p_UTM_B);

    const vi_map::GpsUtmMeasurement query_utm_measurement(
        sensor_id, kInvalidTimestampNanoseconds, T_UTM_B,
        vi_map::UtmZone(32u, 'T'));

    vi_map::GpsUtmMeasurement closest_utm_measurement;
    nn_query_database.getClosestDataItem(
        query_utm_measurement, &closest_utm_measurement);

    const Eigen::VectorXd ground_truth_closest_utm_measurement_p_UTM_B =
        getGroundTruthClosestDataItem(
            utm_measurments_p_UTM_Bs, queryTypeToVector(query_utm_measurement));
    EXPECT_EQ(
        queryTypeToVector(closest_utm_measurement),
        ground_truth_closest_utm_measurement_p_UTM_B);
  }
}

TEST(VIMapNearestNeighborLookupTest, MultipleUTMMeasurementsRadiusLookup) {
  vi_map::VIMap map;
  vi_map::VIMapGenerator generator(map, kSeed);
  const vi_map::MissionId mission_id = generator.createMission();
  CHECK(mission_id.isValid());

  generator.generateMap();

  srand(kSeed);

  const size_t kNumMeasurements = 10u;

  MeasurementsList utm_measurments_p_UTM_Bs;

  vi_map::GpsUtm::UniquePtr gps_utm_sensor =
      vi_map::createTestSensor<vi_map::GpsUtm>();
  CHECK(gps_utm_sensor);
  const vi_map::SensorId sensor_id = gps_utm_sensor->getId();
  CHECK(sensor_id.isValid());
  map.getSensorManager().addSensor(std::move(gps_utm_sensor), mission_id);
  for (size_t idx = 0u; idx < kNumMeasurements; ++idx) {
    const Eigen::Vector3d axis_angle_UTM_B = Eigen::Vector3d::Random();
    const aslam::Position3D p_UTM_B = 1e6 * aslam::Position3D::Random();

    const aslam::Transformation T_UTM_B(
        aslam::Quaternion(aslam::AngleAxis(axis_angle_UTM_B)), p_UTM_B);

    const vi_map::GpsUtmMeasurement utm_measurement(
        sensor_id, static_cast<int64_t>(idx) + 1, T_UTM_B,
        vi_map::UtmZone(32u, 'T'));

    map.addOptionalSensorMeasurement(utm_measurement, mission_id);

    utm_measurments_p_UTM_Bs.emplace_back(queryTypeToVector(utm_measurement));
  }

  std::mt19937 random_number_generator(kSeed);
  std::uniform_real_distribution<> uniform_distribution(0.0, 1e6);

  VIMapNearestNeighborLookupGpsUtm nn_query_database(map);

  const size_t kNumQueries = 10u;
  const int64_t kInvalidTimestampNanoseconds = 0;
  for (size_t query_idx = 0u; query_idx < kNumQueries; ++query_idx) {
    const double search_radius = uniform_distribution(random_number_generator);

    const Eigen::Vector3d axis_angle_UTM_B = Eigen::Vector3d::Random();
    const aslam::Position3D p_UTM_B = 1e6 * aslam::Position3D::Random();

    const aslam::Transformation T_UTM_B(
        aslam::Quaternion(aslam::AngleAxis(axis_angle_UTM_B)), p_UTM_B);

    const vi_map::GpsUtmMeasurement query_utm_measurement(
        sensor_id, kInvalidTimestampNanoseconds, T_UTM_B,
        vi_map::UtmZone(32u, 'T'));

    vi_map::GpsUtmMeasurementSet utm_measurments_p_UTM_Bs_within_search_radius;
    nn_query_database.getAllDataItemsWithinRadius(
        query_utm_measurement, search_radius,
        &utm_measurments_p_UTM_Bs_within_search_radius);

    MeasurementsSet utm_measurments_p_UTM_Bs_within_search_radius_as_vectors;
    for (const vi_map::GpsUtmMeasurement& utm_measurement_within_search_radius :
         utm_measurments_p_UTM_Bs_within_search_radius) {
      utm_measurments_p_UTM_Bs_within_search_radius_as_vectors.emplace(
          queryTypeToVector(utm_measurement_within_search_radius));
    }

    MeasurementsSet
        ground_truth_utm_measurments_p_UTM_Bs_within_search_radius_as_vectors;
    getGroundTruthAllDataItemsWithinRadius(
        utm_measurments_p_UTM_Bs, queryTypeToVector(query_utm_measurement),
        search_radius,
        &ground_truth_utm_measurments_p_UTM_Bs_within_search_radius_as_vectors);

    EXPECT_EQ(
        utm_measurments_p_UTM_Bs_within_search_radius_as_vectors,
        ground_truth_utm_measurments_p_UTM_Bs_within_search_radius_as_vectors);
  }
}

}  // namespace vi_map_helpers

MAPLAB_UNITTEST_ENTRYPOINT


///////////////////////////////////////////////////////////////////////////////
// cells.hpp                                                                 //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_INCLUDE_CELLS_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_INCLUDE_CELLS_HPP_ER_2010

#include <boost/statistics/detail/non_parametric/contingency_table/cells/cells.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/cells/non_empty_cells_count.hpp>

#endif


#include "algorithm/simulated_annealing_merger.h"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_01.hpp>

#include "debug/debug_time.h"
#include "debug/debug_file_io.h"

#include "algorithm/reliability_calculator_variance.h"
#include "algorithm/srncp_tile_merger.h"
#include "algorithm/simple1d_tile_merger.h"
#include "tiles/tile_junction.h"
#include "image/in_out.h"

simulated_annealing_merger::simulated_annealing_merger(int check_steps, float convergence_criterion, std::string merge_prior, float name_energy_after_merging, float name_energy_prior_merging, bool name_extended, int name_steps_to_convergence, long seed, int series_start, int series_steps, int series_stop, float temperature) :
    abstract_tile_merger(), check_steps(check_steps), convergence_criterion(convergence_criterion), temperature(temperature), merge_prior(merge_prior), name_steps_to_convergence(name_steps_to_convergence), name_extended(name_extended), name_energy_prior_merging(name_energy_prior_merging), name_energy_after_merging(name_energy_after_merging),  series_start(series_start), series_stop(series_stop), series_steps(series_steps), seed(seed)   {
    //empty
}

simulated_annealing_merger::simulated_annealing_merger(std::vector<std::string> msettings) {
    this->series_start = -1;
    this->series_stop = -1;
    this->series_steps = -1;
    this->temperature = -1;
    this->convergence_criterion = -1;
    this->check_steps = -1;
    this->name_extended = false;
    std::string option, value;

    std::vector<std::string>::iterator it;
    for (it = msettings.begin(); it != msettings.end(); ++it) {
        option = it->substr(0, it->find_first_of('-'));
        value = it->substr(it->find_first_of('-') + 1);
        if (option.compare("temp") == 0) {
            this->temperature = (!value.empty() ? boost::lexical_cast<float>(value) : 0.f);
            PRINTLN("temperature set to: " << this->temperature);
        } else if (option.compare("conv") == 0) {
            this->convergence_criterion = (!value.empty() ? boost::lexical_cast<float>(value) : 0.f);
            PRINTLN("convergence set to: " << this->convergence_criterion);
        } else if (option.compare("pre") == 0) {
            if (!value.empty()) this->merge_prior = value;
            else PRINTLN("Specify value simple/srncp for option pre- ...");
        } else if (option.compare("steps") == 0) {
            this->check_steps = (!value.empty() ? boost::lexical_cast<float>(value) : 0.f);
        } else if (option.compare("saveinfo") == 0) {
            this->name_extended = true;
        } else if (option.compare("series") == 0) {
            PRINTLN("Series-option was selected. DEFAULT printout is to C:\\ImageSeries\\SimAnn\\ . Please make sure this directory exists...")
            if (!value.empty()) {
                //value should look like: x or x-y-z
                //                PRINTLN("value: " << value);
                if (value.find_first_of('-') != value.find_last_of('-') && value.find_last_of('-') != std::string::npos) { //there are (technically more than) 2 '-' and the last one doesnt return npos
                    try {
                        this->series_start = boost::lexical_cast<int>(value.substr(0, value.find_first_of('-'))); // = x;
                        value = value.substr(value.find_first_of('-') + 1);
                        //                    PRINTLN("value: " << value);
                        this->series_stop = boost::lexical_cast<int>(value.substr(0, value.find_first_of('-'))); // = y;
                        value = value.substr(value.find_first_of('-') + 1);
                        //                    PRINTLN("value: " << value);
                        this->series_steps = boost::lexical_cast<int>(value); // = z;
                        PRINTLN(series_start << " " << series_stop << " " << series_steps << " ");
                    } catch (boost::bad_lexical_cast const& e) {
                        PRINTLN("Error: " << e.what());
                        PRINTLN("Please make sure your syntax is series-x OR series-x-y-z (x,y,z arbitrary positive integer values)");
                    }
                } else {
                    this->series_steps = boost::lexical_cast<int>(value); // = x;
                }
            } else {
                this->series_steps = -1;
                PRINTLN("Option series-x with no integer x commited. Setting steps to " << this->series_steps << "and preventing output...");
            }
        } else if (option.compare("seed") == 0) {
            if(!value.empty())  {
                this->seed  = boost::lexical_cast<long>(value.substr(0, value.find_first_of('-')));
            } else  {
                this->seed = -1;
                PRINTLN("Seed-x option was selected, but no x was given. Taking random seed instead.")
            }
        } else if (option.compare("help") == 0) {
            this->usage_help();
        } else {
            PRINTLN("option \"" << option << "\" not supported. Please use temp-0.01 ,conv-100000 and pre-t (values arbitrary).");
        }
    }
    if (this->temperature == -1) {
        this->temperature = 0.1;
        PRINTLN("No temperature specified: Setting it to: " << this->temperature);
    }
    if (this->convergence_criterion == -1) {
        this->convergence_criterion = 0.01;
        PRINTLN("No convergence_criterion specified: Setting it to: " << this->convergence_criterion);
    }
    if (this->check_steps == -1) {
        this->check_steps = 5000;
        PRINTLN("step-value not specified: Setting check_steps to: " << this->check_steps);
    } else if (this->check_steps == 0) {
        this->check_steps = INT_MAX;
    }
}

simulated_annealing_merger::simulated_annealing_merger() {
    //empty
}

simulated_annealing_merger::~simulated_annealing_merger() {
    //empty
}

void simulated_annealing_merger::merge_tiles(sharedptr<tiled_image> ti) {
    debug_file_io * file = new debug_file_io("energy.txt");

    //Mersenne twister pseudorandom number generator with prime of 2^19937-1
    boost::random::mt19937 gen((this->seed != -1) ? this->seed : std::time(NULL)); //Boost Random Generator // ternary operator
    boost::random::uniform_int_distribution<> rand_w(0, ti->get_tilecount_width() - 1);
    boost::random::uniform_int_distribution<> rand_h(0, ti->get_tilecount_height() - 1);
    boost::random::uniform_01<> dist;

    /* Prerun with Merger */
    if (!this->merge_prior.empty()) {
        if (this->merge_prior.compare("simple") == 0) {
            PRINTLN("Preprocessing image with simple merger");
            sharedptr<simple1d_tile_merger> my_merger(new simple1d_tile_merger());
            my_merger->merge_tiles(ti);
        } else if (this->merge_prior.compare("srncp") == 0) {
            sharedptr<abstract_reliability_calculator> rc(new reliability_calculator_variance());
            sharedptr<srncp_tile_merger> my_merger(new srncp_tile_merger(rc));
            my_merger->merge_tiles(ti);
        } else PRINTLN("Merger " << this->merge_prior << " supported. Please use simple or srncp");

    }

    float total_energy = 0.0f;
    float abort_parameter = 0.0f;
    float pre_energy = 0.0f;
    float post_energy = 0.0f;
    float prob_accept = 0.0f; // Acceptance probability of step. Calculated in loop
    float prob_add = 0.f; //  If prob_add > 0.5 => 2Pi is added to the tile, else -2Pi is added to the tile
    long w, h;
    int steps = 0;
    sharedptr<tile> cur_tile;

    /* Calculate the overall "energy" of every junction in the image as squared differences */
    long junc_arr_size = ti->get_size_of_junction_array();
    if (junc_arr_size == 0) {
        ti->create_all_junctions();
        junc_arr_size = ti->get_size_of_junction_array(); //Do not delete, otherwise junc_arr_size stays zero
    }

    for (int i = 0; i < junc_arr_size; i++) {
        total_energy += ti->get_junction_at(i)->calc_junction_squared_difference();
    }
    PRINTLN("Start total energy: " << total_energy);
    this->name_energy_prior_merging = total_energy;
    while (steps < (250 * junc_arr_size) && abort_parameter < FLT_MAX) { //TODO: Find a perfect convergence criterion...
        file->append_to_file(boost::lexical_cast<std::string>(steps) + " " + boost::lexical_cast<std::string>(total_energy) + "\n");
        steps++;

        w = rand_w(gen);
        h = rand_h(gen);
        prob_add = dist(gen);
        prob_accept = dist(gen);

        cur_tile = ti->get_tile_at(w, h);
        pre_energy = calc_energy(ti->get_neighbouring_junctions(cur_tile));

        if (prob_add > 0.5) cur_tile->add_value(2 * M_PI);
        else cur_tile->add_value(-2 * M_PI);

        post_energy = calc_energy(ti->get_neighbouring_junctions(cur_tile));

        if (prob_accept < std::exp(-(post_energy - pre_energy) / (this->temperature - (this->temperature / (250 * junc_arr_size)) * steps))) {
            total_energy += post_energy - pre_energy;

            /* Save intermediate image if chosen */
            if (this->series_steps != -1 && steps % this->series_steps == 0) { //Series saving is "on"
                if (this->series_start != -1) { //Range saving is "on"
                    if (steps >= this->series_start && steps <= this->series_stop) { //it is in range
                        this->write_intermediate(ti, boost::lexical_cast<std::string>(steps));
                    }
                } else { //Range saving is "off"
                    this->write_intermediate(ti, boost::lexical_cast<std::string>(steps));
                }
            }
            /* End saving intermediate image... */

        } else {
            //discard changes
            if (prob_add < 0.5) cur_tile->add_value(2 * M_PI);
            else cur_tile->add_value(-2 * M_PI);
        }
        if (steps % this->check_steps == 0) { //Check every 10.000 step if the total energy is the same as 10.000 steps before. True: End!
            PRINTLN("steps: " << steps << " at total energy: " << total_energy);
            PRINTLN("temperature according to accept-choice: " << (this->temperature - (this->temperature / (250 * junc_arr_size)) * steps));
            //Ternary operator:
            abort_parameter = (((abort_parameter - total_energy) < -this->convergence_criterion || (abort_parameter - total_energy) > 0) ? total_energy : FLT_MAX);
            //         if (abort_parameter != total_energy) {
            //            abort_parameter = total_energy;
            //         } else {
            //            abort_parameter = FLT_MAX;
            //         }
        }
    }
    PRINTLN("End total energy:  " << total_energy << " & number of steps to \"convergence\": " << steps);
    this->name_energy_after_merging = total_energy;
    this->name_steps_to_convergence = steps;

    delete file;
}

float simulated_annealing_merger::calc_energy(sharedptr<std::vector<sharedptr<tile_junction> > > junc_vc) {
    float energy = 0.0f;
    std::vector<sharedptr<tile_junction> >::iterator it;
    for (it = junc_vc->begin(); it != junc_vc->end(); ++it) {

        energy += (*it)->calc_junction_squared_difference();
    }
    return energy;
}

//float simulated_annealing_merger::keep_change(float total_energy, float pre_energy, float post_energy) {
//   total_energy -= (pre_energy - post_energy);  //Difference of this
//}

void simulated_annealing_merger::write_intermediate(sharedptr<tiled_image> ti, std::string name) {
    sharedptr<row_major_float_image> unwrapped_img = ti->convert_to_float_image();
    std::string output_filename = "C:\\ImageSeries\\SimAnn\\" + name + ".raw";
    if (!write_image(output_filename, unwrapped_img)) {
        PRINTLN("Error: Could not save file. \n Note: This programm has no cross-platform creation of folders (yet) supported.");
        PRINTLN("If you see this error message, please check if the specified output (or .\\Unwrapped\\ when no -o option called) is an EXISTING FOLDER.");
        return;
    }
}

std::string simulated_annealing_merger::get_name() {
    std::string name = "simann_";
    if (this->name_extended) {
        name += "te" + boost::lexical_cast<std::string>(this->temperature).substr(0, 5);
        name += "_c";
        name += boost::lexical_cast<std::string>(this->convergence_criterion).substr(0, 5);
        if (!this->merge_prior.empty()) name += "_" + this->merge_prior.substr(0, 3);
        name += "_#";
        name += boost::lexical_cast<std::string>(this->check_steps) + "_";
        name += boost::lexical_cast<std::string>(this->name_steps_to_convergence);
        name += "_pE";
        name += boost::lexical_cast<std::string>(this->name_energy_prior_merging).substr(0, 6);
        name += "_aE";
        name += boost::lexical_cast<std::string>(this->name_energy_after_merging).substr(0, 6);
    }
    return name;
}

void simulated_annealing_merger::usage_help() {
    PRINTLN("*---------------------------------------------------------------------------*");
    PRINTLN("Usage of the simulated annealing merger...");
    PRINTLN("Options");
    PRINTLN("temp-x :  Temperature x mostly between 1 and 0. Default at 0.1");
    PRINTLN("conv-x :  The method will stop (~convergence) when following is met:");
    PRINTLN("          0 > E1-E2 > -|x|");
    PRINTLN("          (E1 = Energy before flipping one tile. E2 = Energy after flipping)");
    PRINTLN("          DO NOT SUBMIT A SIGN! --- input x should be a small positive value!");
    PRINTLN("          Default is 0.1");
    PRINTLN("steps-x:  Convergence will be checked every x iterations. ");
    PRINTLN("          Values >> 1000 will be faster. ")
    PRINTLN("          Values around 1 to 10 may be error-prone due to local minima... ");
    PRINTLN("          A value of 0 will turn this criterion off!");
    PRINTLN("          Default is 5000");
    PRINTLN("pre-x  :  Prerun the x=simple or x=srncp merger (faster)");
    PRINTLN("saveinfo: Write temp-x, conv-x and steps for convergence into filename");
    PRINTLN("          Default turned off");
    PRINTLN("series-z:      Every z-th step the image will be saved to harddrive");
    PRINTLN("  OR  ");
    PRINTLN("series-x-y-z : Like above, but for a range:");
    PRINTLN("             : Start saving at image x. Stop saving before image y. Save every z-th step!");
    PRINTLN("seed-x  : Seed used for the random generator. Same seeds on the same picture will grant same output.");
    PRINTLN("          Default is random");
    PRINTLN("*---------------------------------------------------------------------------*");
}


///////////////////////////////////////////////////////////////////////////////
// distribution::survival::record::time_since_entry.hpp                		 //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RECORD_TIME_SINCE_ENTRY_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RECORD_TIME_SINCE_ENTRY_HPP_ER_2009
#include <boost/utility/enable_if.hpp>
#include <boost/mpl/apply.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/type_traits/remove_cv.hpp>
#include <boost/fusion/include/has_key.hpp>
#include <boost/fusion/include/value_at_key.hpp>
#include <boost/statistics/detail/distribution/survival/record/key/entry_time.hpp>
#include <boost/statistics/detail/traits/remove_cvref.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace survival{
namespace record{

template<typename M,typename T>
typename traits::remove_cvref<
    typename boost::fusion::result_of::value_at_key<M,tag::entry_time>::type
>::type
time_since_entry(
    const M& m,
    const T& t
)
{
    //Warning: does not check for < 0 (intentional)
    return (t - boost::fusion::at_key<tag::entry_time>(m));
}

}// record
}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif


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

#include <boost/hana/map.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/comparable.hpp>
#include <laws/foldable.hpp>
#include <laws/searchable.hpp>
#include <support/minimal_product.hpp>
namespace hana = boost::hana;

template <int i>
auto key() { return hana::test::ct_eq<i>{}; }

template <int i>
auto val() { return hana::test::ct_eq<-i>{}; }

template <int i, int j>
auto p() { return ::minimal_product(key<i>(), val<j>()); }

int main() {
    auto maps = hana::make_tuple(
        hana::make_map(),
        hana::make_map(p<1, 1>()),
        hana::make_map(p<1, 2>()),
        hana::make_map(p<1, 1>(), p<2, 2>()),
        hana::make_map(p<1, 1>(), p<2, 2>(), p<3, 3>())
    );

    hana::test::TestComparable<hana::map_tag>{maps};
    hana::test::TestSearchable<hana::map_tag>{maps, hana::make_tuple(key<1>(), key<4>())};
    hana::test::TestFoldable<hana::map_tag>{maps};
}


#pragma once
/*!
@file
Maths concepts.
 */

#include <concepts>
#include <Eigen/Core>

namespace qpp_e::maths
{
    template < class Derived >
    concept Matrix = requires(Derived m)
    {
        []<class D>(Eigen::MatrixBase<D> const&){}(m);
    };

    template < class T >
    concept RealNumber = std::is_arithmetic_v<T>;

    template < class T >
    concept ComplexNumber = requires (T c)
    {
        std::real(c);
        std::imag(c);
    };

    template < class T >
    concept arithmetic = std::is_arithmetic_v<T>;

} /* namespace qpp_e::maths */


/*

Copyright (c) 2017, Brian Bingham
All rights reserved

This file is part of the usv_gazebo_dynamics_plugin package, known as this Package.

This Package 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 Package s 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 package.  If not, see <http://www.gnu.org/licenses/>.

*/

#include <boost/thread.hpp>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/JointState.h>
#include <ros/time.h>
#include <tf2/LinearMath/Transform.h>

#include <usv_gazebo_plugins/usv_gazebo_dynamics_plugin.h>

#define GRAVITY 9.815

using namespace gazebo;

UsvPlugin::UsvPlugin()
{
}

UsvPlugin::~UsvPlugin()
{
  rosnode_->shutdown();
  spinner_thread_->join();
  delete rosnode_;
  delete spinner_thread_;
}

void UsvPlugin::FiniChild()
{
}

double UsvPlugin::getSdfParamDouble(sdf::ElementPtr sdfPtr, const std::string &param_name, double default_val)
{
  double val = default_val;
  if (sdfPtr->HasElement(param_name) && sdfPtr->GetElement(param_name)->GetValue())
  {
    val = sdfPtr->GetElement(param_name)->Get<double>();
    ROS_INFO_STREAM("Parameter found - setting <" << param_name << "> to <" << val << ">.");

  }
  else{
    ROS_INFO_STREAM("Parameter <" << param_name << "> not found: Using default value of <" << val << ">.");
  }
  return val;
}

void UsvPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
{
  ROS_INFO("Loading usv_gazebo_dynamics_plugin");
  model_ = _parent;
  world_ = model_->GetWorld();

  // Retrieve model paramters from SDF
  // Set default values
  node_namespace_ = "";

  water_level_ = 0.5;
  water_density_ = 997.7735;

  param_X_dot_u_ = 5;  // Added mass
  param_Y_dot_v_ = 5;
  param_N_dot_r_ = 1;
  param_X_u_ = 20;  // linear drag
  param_X_uu_ = 0.0;
  param_Y_v_ = 20;
  param_Y_vv_ = 0.0;
  param_Z_w_ = 20;
  param_K_p_ = 20.0;
  param_M_q_ = 20.0;
  param_N_r_ = 20;
  param_N_rr_ = 0.0;

  param_boat_width_ = 1.0;
  param_boat_length_ = 1.35;

  param_metacentric_length_ = 0.4 ; // From clearpath
  param_metacentric_width_ = 0.4;  // ditto
  param_boat_area_ = 0.48;  // Clearpath: 1.2m in length, 0.2m in width, 2 pontoons.

  //  Enumerating model
  ROS_INFO_STREAM("Enumerating Model...");
  ROS_INFO_STREAM("Model name = "<< model_->GetName());
  physics::Link_V links = model_->GetLinks();
  for (int ii=0; ii<links.size(); ii++){
    ROS_INFO_STREAM("Link: "<<links[ii]->GetName());
  }

  // Get parameters from SDF
  if (_sdf->HasElement("robotNamespace"))
  {
    node_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
  }

  if (!_sdf->HasElement("bodyName") || !_sdf->GetElement("bodyName")->GetValue())
  {
    link_ = model_->GetLink();
    link_name_ = link_->GetName();
    ROS_INFO_STREAM("Did not find SDF parameter bodyName");
  }
  else {
    link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
    //link_name_ = "thrust_link";
    link_ = model_->GetLink(link_name_);

    ROS_INFO_STREAM("Found SDF parameter bodyName as <"<<link_name_<<">");
  }
  if (!link_)
  {
    ROS_FATAL("usv_gazebo_dynamics_plugin error: bodyName: %s does not exist\n", link_name_.c_str());
    return;
  }
  else
  {
    ROS_INFO_STREAM("USV Model Link Name = " << link_name_);
  }

  water_level_ = getSdfParamDouble(_sdf,"waterLevel",water_level_);
  water_density_ = getSdfParamDouble(_sdf,"waterDensity",water_density_);

  param_X_dot_u_ = getSdfParamDouble(_sdf,"xDotU",param_X_dot_u_);
  param_Y_dot_v_ = getSdfParamDouble(_sdf,"yDotV",param_Y_dot_v_);
  param_N_dot_r_ = getSdfParamDouble(_sdf,"nDotR",param_N_dot_r_);

  param_X_u_ = getSdfParamDouble(_sdf,"xU",param_X_u_);
  param_X_uu_ = getSdfParamDouble(_sdf,"xUU",param_X_uu_);
  param_Y_v_ = getSdfParamDouble(_sdf,"yV",param_Y_v_);
  param_Y_vv_ = getSdfParamDouble(_sdf,"yVV",param_Y_vv_);
  param_Z_w_ = getSdfParamDouble(_sdf,"zW",param_Z_w_);
  param_K_p_ = getSdfParamDouble(_sdf,"kP",param_K_p_);
  param_M_q_ = getSdfParamDouble(_sdf,"mQ",param_M_q_);
  param_N_r_ = getSdfParamDouble(_sdf,"nR",param_N_r_);
  param_N_rr_ = getSdfParamDouble(_sdf,"nRR",param_N_rr_);

  param_boat_area_ = getSdfParamDouble(_sdf,"boatArea",param_boat_area_);
  param_boat_width_ = getSdfParamDouble(_sdf,"boatWidth",param_boat_width_);
  param_boat_length_ = getSdfParamDouble(_sdf,"boatLength",param_boat_length_);

  param_metacentric_length_ = getSdfParamDouble(_sdf,"metacentricLength",
						param_metacentric_length_);
  param_metacentric_width_ = getSdfParamDouble(_sdf,"metacentricWidth",
						param_metacentric_width_);

  // Wave parameters
  std::ostringstream buf;
  math::Vector2d tmpm;
  std::vector<float> tmpv(2,0);
  param_wave_n_ = _sdf->GetElement("wave_n")->Get<int>();
  for (int ii=0; ii < param_wave_n_; ii++){
    buf.str("");
    buf<<"wave_amp"<<ii;
    param_wave_amps_.push_back(_sdf->GetElement(buf.str())->Get<float>());
    ROS_INFO_STREAM("Wave Amplitude "<<ii<<": "<<param_wave_amps_[ii]);
    buf.str("");
    buf<<"wave_period"<<ii;
    param_wave_periods_.push_back(_sdf->GetElement(buf.str())->Get<float>());
    buf.str("");
    buf<<"wave_direction"<<ii;
    tmpm=_sdf->GetElement(buf.str())->Get<math::Vector2d>();
    tmpv[0] = tmpm.x;
    tmpv[1] = tmpm.y;
    param_wave_directions_.push_back(tmpv);
    ROS_INFO_STREAM("Wave Direction "<<ii<<": "<<param_wave_directions_[ii][0]
		    << ", " << param_wave_directions_[ii][1]);

  }

  // Get inertia and mass of vessel
  math::Vector3 inertia = link_->GetInertial()->GetPrincipalMoments();
  double mass = link_->GetInertial()->GetMass();

  // Report some of the pertinent parameters for verification
  ROS_INFO("USV Dynamics Parameters: From URDF XACRO model definition");
  ROS_INFO_STREAM("Vessel Mass (rigid-body): " << mass);
  ROS_INFO_STREAM("Vessel Inertia Vector (rigid-body): X:" << inertia[0] <<
		  " Y:"<<inertia[1] <<
		  " Z:"<<inertia[2]);
  ROS_INFO("USV Dynamics Parameters: Plugin Parameters");

  //initialize time and odometry position
  prev_update_time_ = this->world_->GetSimTime();

  // Initialize the ROS node
  int argc = 0;
  char** argv = NULL;
  ros::init(argc, argv, "usv_dynamics_gazebo",
	    ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
  rosnode_ = new ros::NodeHandle( node_namespace_ );

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->spinner_thread_ = new boost::thread( boost::bind( &UsvPlugin::spin, this) );
  this->updateConnection = event::Events::ConnectWorldUpdateBegin(
    boost::bind(&UsvPlugin::UpdateChild, this));

  // Initialize Added Mass Matrix
  Ma_ = Eigen::MatrixXd(6,6);
  Ma_ << param_X_dot_u_ ,   0,   0, 0, 0, 0,
        0,   param_Y_dot_v_,   0, 0, 0, 0,
        0,   0,   0.1, 0, 0, 0,
        0,   0,   0, 0.1, 0, 0,
        0,   0,   0, 0, 0.1, 0,
        0,   0,   0, 0, 0, param_N_dot_r_ ;

  Cmat_ =  Eigen::MatrixXd::Zero(6,6);
  Dmat_ =  Eigen::MatrixXd::Zero(6,6);
  state_dot_ = Eigen::VectorXd(6);
  state_ = Eigen::VectorXd(6);
  amassVec_ = Eigen::VectorXd(6);

  int NN = 2; // must be factor of 2! - only 2 for now!!
  // x,y grid step increments
  dx_ = param_boat_length_/NN;
  dy_ = param_boat_width_/NN;
  // Vector for interating throug grid points on boat
  for (int ii=-NN/2; ii<0; ii++){
    II_.push_back(ii);
  }
  for (int ii=1; ii<=NN/2; ii++){
    II_.push_back(ii);
  }
  // Precalculate this to save some time.
  buoy_frac_ = (param_boat_area_/(NN*NN))*GRAVITY*water_density_;

}

void UsvPlugin::UpdateChild()
{
  common::Time time_now = this->world_->GetSimTime();
  //common::Time step_time = time_now - prev_update_time_;
  double dt = (time_now - prev_update_time_).Double();
  prev_update_time_ = time_now;

  // Get Pose/Orientation from Gazebo (if no state subscriber is active)
  pose_ = link_->GetWorldPose();
  euler_ = pose_.rot.GetAsEuler();

  // Get body-centered linear and angular rates
  vel_linear_body_ = link_->GetRelativeLinearVel();
  ROS_DEBUG_STREAM_THROTTLE(0.5,"Vel linear: " << vel_linear_body_);
  vel_angular_body_ = link_->GetRelativeAngularVel();
  ROS_DEBUG_STREAM_THROTTLE(0.5,"Vel angular: " << vel_angular_body_);
  // Estimate the linear and angular accelerations.
  // Note the the GetRelativeLinearAccel() and AngularAccel() functions
  // appear to be unreliable
  math::Vector3 accel_linear_body = (vel_linear_body_ - prev_lin_vel_) /dt;
  prev_lin_vel_ = vel_linear_body_;
  ROS_DEBUG_STREAM_THROTTLE(0.5,"Accel linear: " << accel_linear_body);
  math::Vector3 accel_angular_body = (vel_angular_body_ - prev_ang_vel_) /dt;
  prev_ang_vel_ = vel_angular_body_;
  ROS_DEBUG_STREAM_THROTTLE(0.5,"Accel angular: " << accel_angular_body);

  // Create state and derivative of state (accelerations)
  state_dot_ << accel_linear_body.x, accel_linear_body.y, accel_linear_body.z,
    accel_angular_body.x, accel_angular_body.y, accel_angular_body.z;

  state_ << vel_linear_body_.x, vel_linear_body_.y, vel_linear_body_.z,
    vel_angular_body_.x, vel_angular_body_.y, vel_angular_body_.z;

  // Added Mass
  amassVec_ = -1.0*Ma_*state_dot_;
  ROS_DEBUG_STREAM_THROTTLE(1.0,"state_dot_: \n" << state_dot_);
  ROS_DEBUG_STREAM_THROTTLE(1.0,"amassVec :\n" << amassVec_);

  // Coriolis - added mass components
  Cmat_(0,5) = param_Y_dot_v_ * vel_linear_body_.y;
  Cmat_(1,5) = param_X_dot_u_ * vel_linear_body_.x;
  Cmat_(5,0) = param_Y_dot_v_ * vel_linear_body_.y;
  Cmat_(5,1) = param_X_dot_u_ * vel_linear_body_.x;
  Cvec_ = -1.0*Cmat_*state_;
  ROS_DEBUG_STREAM_THROTTLE(1.0,"Cvec :\n" << Cvec_);

  // Drag
  //Eigen::MatrixXd Dmat = Eigen::MatrixXd(6,6);
  Dmat_(0,0) = param_X_u_ + param_X_uu_*std::abs(vel_linear_body_.x);
  Dmat_(1,1) = param_Y_v_ + param_Y_vv_*std::abs(vel_linear_body_.y);
  Dmat_(2,2) = param_Z_w_;
  Dmat_(3,3) = param_K_p_;
  Dmat_(4,4) = param_M_q_;
  Dmat_(5,5) = param_N_r_ + param_N_rr_*std::abs(vel_angular_body_.z);
  ROS_DEBUG_STREAM_THROTTLE(1.0,"Dmat :\n" << Dmat_);
  Dvec_ = -1.0*Dmat_*state_;
  ROS_DEBUG_STREAM_THROTTLE(1.0,"Dvec :\n" << Dvec_);

  // Vehicle frame transform
  tf2::Quaternion vq = tf2::Quaternion();
  tf2::Matrix3x3 m;
  m.setEulerYPR(euler_.z,euler_.y,euler_.x);
  m.getRotation(vq);
  tf2::Transform xform_v = tf2::Transform(vq);

  // Sum all forces - in body frame
  Eigen::VectorXd forceSum = amassVec_ + Dvec_;// + buoyVec;
  // Forces in fixed frame
  ROS_DEBUG_STREAM_THROTTLE(1.0,"forceSum :\n" << forceSum);

  // Add dynamic forces/torques to link at CG
  link_->AddRelativeForce(math::Vector3(forceSum(0),forceSum(1),forceSum(2)));
  link_->AddRelativeTorque(math::Vector3(forceSum(3),forceSum(4),forceSum(5)));

  // Distribute upward buoyancy force

  float ddx, ddy, ddz, buoy_force;
  double w, k, dz, Ddotx;
  math::Vector3 X;  // location of vehicle base link
  tf2::Vector3 bpnt(0,0,0);     // grid points on boat
  tf2::Vector3 bpnt_w(0,0,0);   // in world coordinates

  // Loop over boat grid points
  //for (std::list<int>::iterator ii=Ilist.begin();ii != Ilist.end(); ii++){
  for (std::vector<int>::iterator it=II_.begin(); it != II_.end(); ++it){
    bpnt.setX((*it)*dx_);  // grid point in boat fram
    for (std::vector<int>::iterator jt=II_.begin(); jt != II_.end(); ++jt){
      //for (std::list<int>::iterator jj=Jlist.begin(); jj != Jlist.end(); jj++)
      bpnt.setY((*jt)*dy_);

      // Transform from vessel to water/world frame
      bpnt_w = xform_v*bpnt;

      // Debug
      ROS_DEBUG_STREAM_THROTTLE(1.0,"[" << (*it) <<","<<(*jt)<< "] grid points"<<bpnt.x() <<","<<bpnt.y() <<","<<bpnt.z());
      ROS_DEBUG_STREAM_THROTTLE(1.0,"v frame euler "<<euler_);
      ROS_DEBUG_STREAM_THROTTLE(1.0,"in water frame"<<bpnt_w.x() <<","<<bpnt_w.y() <<","<<bpnt_w.z());

      // Vertical location of boat grid point in world frame
      ddz = pose_.pos.z+bpnt_w.z();
      ROS_DEBUG_STREAM("Z, pose: " << pose_.pos.z <<", bpnt: "<<bpnt_w.z() << ", dd: " << ddz);

      // Find vertical displacement of wave field
      X.x = pose_.pos.x+bpnt_w.x();  // World location of grid point
      X.y = pose_.pos.y+bpnt_w.y();
      // sum vertical dsplacement over all waves
      dz = 0.0;
      for (int ii=0; ii < param_wave_n_; ii++){
	Ddotx=param_wave_directions_[ii][0]*X.x
	  +param_wave_directions_[ii][1]*X.y;
	w = 2.0*3.14159 / param_wave_periods_[ii];
	k = w*w/9.81;
	dz += param_wave_amps_[ii]*cos(k*Ddotx-w*time_now.Float());;
      }
      ROS_DEBUG_STREAM_THROTTLE(1.0,"wave disp: " << dz);

      // Buoyancy force at grid point
      buoy_force = (((water_level_+dz)-(ddz))*(buoy_frac_));
      ROS_DEBUG_STREAM("buoy_force: " << buoy_force);
      // Apply force at grid point
      // From web, Appears that position is in the link frame
      // and force is in world frame
      link_->AddForceAtRelativePosition(math::Vector3(0,0,buoy_force),
					math::Vector3(bpnt.x(),bpnt.y(),bpnt.z()));
    }
  }
}

void UsvPlugin::spin()
{
    while(ros::ok()) ros::spinOnce();
}

GZ_REGISTER_MODEL_PLUGIN(UsvPlugin);


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm accumulator median
#include <boost/test/unit_test.hpp>
#include "fern/algorithm/accumulator/median.h"

namespace faa = fern::algorithm::accumulator;

BOOST_AUTO_TEST_CASE(default_construct)
{
    faa::Median<int> median;
    // Don't calculate the median. Division by zero.
}

BOOST_AUTO_TEST_CASE(accumulate)
{
    {
        faa::Median<int> median(5);
        // 5
        BOOST_CHECK_EQUAL(median(), 5);

        median(2);
        // 2 5
        BOOST_CHECK_EQUAL(median(), 3);

        median(3);
        // 2 3 5
        BOOST_CHECK_EQUAL(median(), 3);

        median = 8;
        // 8
        BOOST_CHECK_EQUAL(median(), 8);
    }

    {
        faa::Median<int, double> median(5);
        // 5
        BOOST_CHECK_EQUAL(median(), 5.0);

        median(2);
        // 2 5
        BOOST_CHECK_EQUAL(median(), 3.5);

        median = 3;
        // 2 3 5
        BOOST_CHECK_EQUAL(median(), 3.0);
    }
}

BOOST_AUTO_TEST_CASE(merge)
{
    {
        auto median(faa::Median<int>(15) | faa::Median<int>(5));
        BOOST_CHECK_EQUAL(median(), 10);
    }

    {
        auto median(faa::Median<int, double>(5) | faa::Median<int, double>(20));
        BOOST_CHECK_EQUAL(median(), 12.5);
    }
}