/**
 * \file
 * \author Thomas Fischer
 * \date   2010-06-21
 * \brief  Implementation of the Polygon class.
 *
 * \copyright
 * Copyright (c) 2012-2019, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "Polygon.h"

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

#include "BaseLib/quicksort.h"

#include "AnalyticalGeometry.h"

namespace GeoLib
{
Polygon::Polygon(const Polyline &ply, bool init) :
    Polyline(ply), _aabb(ply.getPointsVec(), ply._ply_pnt_ids)
{
    if (init)
    {
        initialise();
    }
    _simple_polygon_list.push_back(this);
}

Polygon::Polygon(Polygon const& other)
    : Polyline(other), _aabb(other._aabb)
{
    _simple_polygon_list.push_back(this);
    auto sub_polygon_it(other._simple_polygon_list.begin());
    for (sub_polygon_it++;  // the first entry is the polygon itself, skip the
                            // entry
         sub_polygon_it != other._simple_polygon_list.end();
         ++sub_polygon_it)
    {
        _simple_polygon_list.emplace_back(new Polygon(*(*sub_polygon_it)));
    }
}

Polygon::~Polygon()
{
    // remove polygons from list
    for (auto& polygon : _simple_polygon_list)
    {
        // the first entry of the list can be a pointer the object itself
        if (polygon != this)
        {
            delete polygon;
        }
    }
}

bool Polygon::initialise ()
{
    if (this->isClosed()) {
        ensureCCWOrientation();
        return true;
    }
    WARN("Polygon::initialise(): base polyline is not closed.");
    return false;
}

bool Polygon::isPntInPolygon(GeoLib::Point const& pnt) const
{
    MathLib::Point3d const& min_aabb_pnt(_aabb.getMinPoint());
    MathLib::Point3d const& max_aabb_pnt(_aabb.getMaxPoint());

    if (pnt[0] < min_aabb_pnt[0] || max_aabb_pnt[0] < pnt[0] ||
        pnt[1] < min_aabb_pnt[1] || max_aabb_pnt[1] < pnt[1])
    {
        return false;
    }

    if (_simple_polygon_list.size() == 1)
    {
        std::size_t n_intersections(0);
        const std::size_t n_nodes(getNumberOfPoints() - 1);
        for (std::size_t k(0); k < n_nodes; k++)
        {
            if (((*(getPoint(k)))[1] <= pnt[1] &&
                 pnt[1] <= (*(getPoint(k + 1)))[1]) ||
                ((*(getPoint(k + 1)))[1] <= pnt[1] &&
                 pnt[1] <= (*(getPoint(k)))[1]))
            {
                switch (getEdgeType(k, pnt))
                {
                    case EdgeType::TOUCHING:
                        return true;
                    case EdgeType::CROSSING:
                        n_intersections++;
                        break;
                    case EdgeType::INESSENTIAL:
                        break;
                    default:
                        // do nothing
                        ;
                }
            }
        }
        if (n_intersections % 2 == 1)
        {
            return true;
        }
    }
    else
    {
        for (auto it(_simple_polygon_list.begin()++);
             it != _simple_polygon_list.end();
             ++it)
        {
            if ((*it)->isPntInPolygon(pnt))
            {
                return true;
            }
        }
    }
    return false;
}

bool Polygon::isPntInPolygon(double x, double y, double z) const
{
    const GeoLib::Point pnt(x,y,z);
    return isPntInPolygon(pnt);
}

std::vector<GeoLib::Point> Polygon::getAllIntersectionPoints(
        GeoLib::LineSegment const& segment) const
{
    std::vector<GeoLib::Point> intersections;
    GeoLib::Point s;
    for (auto&& seg_it : *this)
    {
        if (GeoLib::lineSegmentIntersect(seg_it, segment, s)) {
            intersections.push_back(s);
        }
    }

    return intersections;
}

bool Polygon::containsSegment(GeoLib::LineSegment const& segment) const
{
    std::vector<GeoLib::Point> s(getAllIntersectionPoints(segment));

    GeoLib::Point const& a{segment.getBeginPoint()};
    GeoLib::Point const& b{segment.getEndPoint()};
    // no intersections -> check if at least one point of segment is in polygon
    if (s.empty()) {
        return (isPntInPolygon(a));
    }

    const double tol(std::numeric_limits<float>::epsilon());

    // one intersection, intersection in line segment end point
    if (s.size() == 1) {
        const double sqr_dist_as(MathLib::sqrDist(a,s[0]));
        if (sqr_dist_as < tol) {
            return (isPntInPolygon(b));
        }

        const double sqr_dist_bs(MathLib::sqrDist(b,s[0]));
        if (sqr_dist_bs < tol) {
            return (isPntInPolygon(a));
        }
    }

    // Sorting the intersection with respect to the distance to the point a.
    // This induces a partition of the line segment into sub segments.
    std::sort(s.begin(), s.end(),
        [&a] (GeoLib::Point const& p0, GeoLib::Point const& p1) {
            return MathLib::sqrDist(a, p0) < MathLib::sqrDist(a, p1);
        }
    );

    // remove sub segments with almost zero length
    for (std::size_t k(0); k<s.size()-1; ) {
        if (MathLib::sqrDist(s[k], s[k+1]) < tol) {
            s.erase(s.begin()+k+1);
        } else {
            k++;
        }
    }

    // Check if all sub segments are within the polygon.
    if (!isPntInPolygon(GeoLib::Point(0.5 * (a[0] + s[0][0]),
                                      0.5 * (a[1] + s[0][1]),
                                      0.5 * (a[2] + s[0][2]))))
    {
        return false;
    }
    const std::size_t n_sub_segs(s.size()-1);
    for (std::size_t k(0); k<n_sub_segs; k++) {
        if (!isPntInPolygon(GeoLib::Point(0.5 * (s[k][0] + s[k + 1][0]),
                                          0.5 * (s[k][1] + s[k + 1][1]),
                                          0.5 * (s[k][2] + s[k + 1][2]))))
        {
            return false;
        }
    }
    return isPntInPolygon(GeoLib::Point(0.5 * (s[0][0] + b[0]),
                                        0.5 * (s[0][1] + b[1]),
                                        0.5 * (s[0][2] + b[2])));
}

bool Polygon::isPolylineInPolygon(const Polyline& ply) const
{
    for (auto segment : ply) {
        if (!containsSegment(segment)) {
            return false;
        }
    }
    return true;
}

bool Polygon::isPartOfPolylineInPolygon(const Polyline& ply) const
{
    const std::size_t ply_size (ply.getNumberOfPoints());
    // check points
    for (std::size_t k(0); k < ply_size; k++) {
        if (isPntInPolygon (*(ply.getPoint(k)))) {
            return true;
        }
    }

    GeoLib::Point s;
    for (auto polygon_seg : *this) {
        for (auto polyline_seg : ply) {
            if (GeoLib::lineSegmentIntersect(polyline_seg, polygon_seg, s)) {
                return true;
            }
        }
    }

    return false;
}

bool Polygon::getNextIntersectionPointPolygonLine(
    GeoLib::LineSegment const& seg, GeoLib::Point & intersection,
    std::size_t& seg_num) const
{
    if (_simple_polygon_list.size() == 1) {
        for (auto seg_it(begin()+seg_num); seg_it != end(); ++seg_it) {
            if (GeoLib::lineSegmentIntersect(*seg_it, seg, intersection)) {
                seg_num = seg_it.getSegmentNumber();
                return true;
            }
        }
    } else {
        for (auto polygon : _simple_polygon_list)
        {
            for (auto seg_it(polygon->begin()); seg_it != polygon->end();
                 ++seg_it)
            {
                if (GeoLib::lineSegmentIntersect(*seg_it, seg, intersection)) {
                    seg_num = seg_it.getSegmentNumber();
                    return true;
                }
            }
        }
    }
    return false;
}

EdgeType Polygon::getEdgeType(std::size_t k, GeoLib::Point const& pnt) const
{
    switch (getLocationOfPoint(k, pnt))
    {
    case Location::LEFT: {
        const GeoLib::Point & v (*(getPoint(k)));
        const GeoLib::Point & w (*(getPoint(k + 1)));
        if (v[1] < pnt[1] && pnt[1] <= w[1])
        {
            return EdgeType::CROSSING;
        }

        return EdgeType::INESSENTIAL;
    }
    case Location::RIGHT: {
        const GeoLib::Point & v (*(getPoint(k)));
        const GeoLib::Point & w (*(getPoint(k + 1)));
        if (w[1] < pnt[1] && pnt[1] <= v[1])
        {
            return EdgeType::CROSSING;
        }

        return EdgeType::INESSENTIAL;
    }
    case Location::BETWEEN:
    case Location::SOURCE:
    case Location::DESTINATION:
        return EdgeType::TOUCHING;
    default:
        return EdgeType::INESSENTIAL;
    }
}

void Polygon::ensureCCWOrientation ()
{
    // *** pre processing: rotate points to xy-plan
    // *** copy points to vector - last point is identical to the first
    std::size_t n_pnts (this->getNumberOfPoints() - 1);
    std::vector<GeoLib::Point*> tmp_polygon_pnts;
    for (std::size_t k(0); k < n_pnts; k++)
    {
        tmp_polygon_pnts.push_back(new GeoLib::Point(*(this->getPoint(k))));
    }

    // rotate copied points into x-y-plane
    GeoLib::rotatePointsToXY(tmp_polygon_pnts);

    for (auto& tmp_polygon_pnt : tmp_polygon_pnts)
    {
        (*tmp_polygon_pnt)[2] =
            0.0;  // should be -= d but there are numerical errors
    }

    // *** get the left most upper point
    std::size_t min_x_max_y_idx (0); // for orientation check
    for (std::size_t k(0); k < n_pnts; k++)
    {
        if ((*(tmp_polygon_pnts[k]))[0] <= (*(tmp_polygon_pnts[min_x_max_y_idx]))[0])
        {
            if ((*(tmp_polygon_pnts[k]))[0] <
                (*(tmp_polygon_pnts[min_x_max_y_idx]))[0])
            {
                min_x_max_y_idx = k;
            }
            else if ((*(tmp_polygon_pnts[k]))[1] >
                     (*(tmp_polygon_pnts[min_x_max_y_idx]))[1])
            {
                min_x_max_y_idx = k;
            }
        }
    }
    // *** determine orientation
    GeoLib::Orientation orient;
    if (0 < min_x_max_y_idx && min_x_max_y_idx < n_pnts - 2)
    {
        orient = GeoLib::getOrientation(*tmp_polygon_pnts[min_x_max_y_idx - 1],
                                        *tmp_polygon_pnts[min_x_max_y_idx],
                                        *tmp_polygon_pnts[min_x_max_y_idx + 1]);
    }
    else
    {
        if (0 == min_x_max_y_idx)
        {
            orient = GeoLib::getOrientation(*tmp_polygon_pnts[n_pnts - 1],
                                            *tmp_polygon_pnts[0],
                                            *tmp_polygon_pnts[1]);
        }
        else
        {
            orient = GeoLib::getOrientation(*tmp_polygon_pnts[n_pnts - 2],
                                            *tmp_polygon_pnts[n_pnts - 1],
                                            *tmp_polygon_pnts[0]);
        }
    }

    if (orient != GeoLib::CCW)
    {
        // switch orientation
        std::size_t tmp_n_pnts (n_pnts);
        tmp_n_pnts++; // include last point of polygon (which is identical to the first)
        for (std::size_t k(0); k < tmp_n_pnts / 2; k++)
        {
            std::swap(_ply_pnt_ids[k], _ply_pnt_ids[tmp_n_pnts - 1 - k]);
        }
    }

    for (std::size_t k(0); k < n_pnts; k++)
    {
        delete tmp_polygon_pnts[k];
    }
}

#if __GNUC__ <= 4 && (__GNUC_MINOR__ < 9)
void Polygon::splitPolygonAtIntersection(
    const std::list<Polygon*>::iterator& polygon_it)
#else
void Polygon::splitPolygonAtIntersection(
    const std::list<Polygon*>::const_iterator& polygon_it)
#endif
{
    GeoLib::Polyline::SegmentIterator seg_it0((*polygon_it)->begin());
    GeoLib::Polyline::SegmentIterator seg_it1((*polygon_it)->begin());
    GeoLib::Point intersection_pnt;
    if (!GeoLib::lineSegmentsIntersect(*polygon_it, seg_it0, seg_it1,
                                       intersection_pnt))
    {
        return;
    }

    std::size_t idx0(seg_it0.getSegmentNumber());
    std::size_t idx1(seg_it1.getSegmentNumber());
    // adding intersection point to pnt_vec
    std::size_t const intersection_pnt_id (_ply_pnts.size());
    const_cast<std::vector<Point*>&>(_ply_pnts)
        .push_back(new GeoLib::Point(intersection_pnt));

    // split Polygon
    if (idx0 > idx1)
    {
        std::swap(idx0, idx1);
    }

    GeoLib::Polyline polyline0{(*polygon_it)->getPointsVec()};
    for (std::size_t k(0); k <= idx0; k++)
    {
        polyline0.addPoint((*polygon_it)->getPointID(k));
    }
    polyline0.addPoint(intersection_pnt_id);
    for (std::size_t k(idx1 + 1); k < (*polygon_it)->getNumberOfPoints(); k++)
    {
        polyline0.addPoint((*polygon_it)->getPointID(k));
    }

    GeoLib::Polyline polyline1{(*polygon_it)->getPointsVec()};
    polyline1.addPoint(intersection_pnt_id);
    for (std::size_t k(idx0 + 1); k <= idx1; k++)
    {
        polyline1.addPoint((*polygon_it)->getPointID(k));
    }
    polyline1.addPoint(intersection_pnt_id);

    // remove the polygon except the first
    if (*polygon_it != this)
    {
        delete *polygon_it;
    }
    // erase polygon_it and add two new polylines
    auto polygon1_it = _simple_polygon_list.insert(
        _simple_polygon_list.erase(polygon_it), new GeoLib::Polygon(polyline1));
    auto polygon0_it = _simple_polygon_list.insert(
        polygon1_it, new GeoLib::Polygon(polyline0));

    splitPolygonAtIntersection(polygon0_it);
    splitPolygonAtIntersection(polygon1_it);
}

void Polygon::splitPolygonAtPoint (const std::list<GeoLib::Polygon*>::iterator& polygon_it)
{
    std::size_t const n((*polygon_it)->getNumberOfPoints() - 1);
    std::vector<std::size_t> id_vec(n);
    std::vector<std::size_t> perm(n);
    for (std::size_t k(0); k < n; k++)
    {
        id_vec[k] = (*polygon_it)->getPointID (k);
        perm[k] = k;
    }

    BaseLib::quicksort (id_vec, 0, n, perm);

    for (std::size_t k(0); k < n - 1; k++)
    {
        if (id_vec[k] == id_vec[k + 1])
        {
            std::size_t idx0 = perm[k];
            std::size_t idx1 = perm[k + 1];

            if (idx0 > idx1)
            {
                std::swap(idx0, idx1);
            }

            // create two closed polylines
            GeoLib::Polyline polyline0{*(*polygon_it)};
            for (std::size_t j(0); j <= idx0; j++)
            {
                polyline0.addPoint((*polygon_it)->getPointID(j));
            }
            for (std::size_t j(idx1 + 1);
                 j < (*polygon_it)->getNumberOfPoints();
                 j++)
            {
                polyline0.addPoint((*polygon_it)->getPointID(j));
            }

            GeoLib::Polyline polyline1{*(*polygon_it)};
            for (std::size_t j(idx0); j <= idx1; j++)
            {
                polyline1.addPoint((*polygon_it)->getPointID(j));
            }

            // remove the polygon except the first
            if (*polygon_it != this)
            {
                delete *polygon_it;
            }
            // erase polygon_it and add two new polygons
            auto polygon1_it = _simple_polygon_list.insert(
                _simple_polygon_list.erase(polygon_it), new Polygon(polyline1));
            auto polygon0_it = _simple_polygon_list.insert(
                polygon1_it, new Polygon(polyline0));

            splitPolygonAtPoint(polygon0_it);
            splitPolygonAtPoint(polygon1_it);

            return;
        }
    }
}

bool operator==(Polygon const& lhs, Polygon const& rhs)
{
    if (lhs.getNumberOfPoints() != rhs.getNumberOfPoints())
    {
        return false;
    }

    const std::size_t n(lhs.getNumberOfPoints());
    const std::size_t start_pnt(lhs.getPointID(0));

    // search start point of first polygon in second polygon
    bool nfound(true);
    std::size_t k(0);
    for (; k < n-1 && nfound; k++) {
        if (start_pnt == rhs.getPointID(k)) {
            nfound = false;
            break;
        }
    }

    // case: start point not found in second polygon
    if (nfound)
    {
        return false;
    }

    // *** determine direction
    // opposite direction
    if (k == n-2) {
        for (k=1; k<n-1; k++) {
            if (lhs.getPointID(k) != rhs.getPointID(n-1-k)) {
                return false;
            }
        }
        return true;
    }

    // same direction - start point of first polygon at arbitrary position in second polygon
    if (lhs.getPointID(1) == rhs.getPointID(k+1)) {
        std::size_t j(k+2);
        for (; j<n-1; j++) {
            if (lhs.getPointID(j-k) != rhs.getPointID(j)) {
                return false;
            }
        }
        j=0; // new start point at second polygon
        for (; j<k+1; j++) {
            if (lhs.getPointID(n-(k+2)+j+1) != rhs.getPointID(j)) {
                return false;
            }
        }
        return true;
    }
    // opposite direction with start point of first polygon at arbitrary
    // position
    // *** ATTENTION
    WARN(
        "operator==(Polygon const& lhs, Polygon const& rhs) - not tested case "
        "(implementation is probably buggy) - please contact "
        "thomas.fischer@ufz.de mentioning the problem.");
    // in second polygon
    if (lhs.getPointID(1) == rhs.getPointID(k - 1))
    {
        std::size_t j(k - 2);
        for (; j > 0; j--)
        {
            if (lhs.getPointID(k - 2 - j) != rhs.getPointID(j))
            {
                return false;
            }
        }
        // new start point at second polygon - the point n-1 of a polygon is
        // equal to the first point of the polygon (for this reason: n-2)
        j = n - 2;
        for (; j > k - 1; j--)
        {
            if (lhs.getPointID(n - 2 + j + k - 2) != rhs.getPointID(j))
            {
                return false;
            }
        }
        return true;
    }
    return false;
}

} // end namespace GeoLib


#include "drake/systems/rendering/pose_aggregator.h"

#include <memory>
#include <sstream>
#include <string>
#include <vector>

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <gtest/gtest.h>

#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/math/autodiff.h"
#include "drake/systems/rendering/pose_bundle.h"
#include "drake/systems/rendering/pose_vector.h"

using drake::math::RigidTransform;
using drake::math::RigidTransformd;
using Eigen::AngleAxisd;
using Eigen::Vector3d;

namespace drake {
namespace systems {
namespace rendering {
namespace {

const int kNumBundlePoses = 2;
const int kNumSinglePoses = 2;

class PoseAggregatorTest : public ::testing::Test {
 protected:
  void SetUp() override {
    // Glue together a robot with one link and one degree of freedom.
    SquareTwistMatrix<double> I = SquareTwistMatrix<double>::Zero();
    I.block<3, 3>(3, 3, 3, 3) << Eigen::Matrix3d::Identity();

    // Allocate an input for a PoseBundle.
    aggregator_.AddBundleInput("bundle", kNumBundlePoses);
    // Allocate a second and third input for a PoseVector and FrameVelocity.
    const int kRandomModelInstanceId0 = 42;
    aggregator_.AddSinglePoseAndVelocityInput("single_xv",
                                              kRandomModelInstanceId0);
    // Allocate a fourth input for a PoseVector, without velocity.
    const int kRandomModelInstanceId1 = 43;
    aggregator_.AddSingleInput("single_x", kRandomModelInstanceId1);

    context_ = aggregator_.CreateDefaultContext();
    output_ = aggregator_.AllocateOutput();
  }

  PoseAggregator<double> aggregator_;
  std::unique_ptr<Context<double>> context_;
  std::unique_ptr<SystemOutput<double>> output_;
};

namespace {
PoseBundle<AutoDiffXd> ToAutoDiffXd(const PoseBundle<double>& original) {
  // TODO(sherm1): Consider moving value transmogrification into AbstractValue,
  // as discussed in https://github.com/RobotLocomotion/drake/issues/5454
  PoseBundle<AutoDiffXd> result(original.get_num_poses());
  for (int i = 0; i < original.get_num_poses(); i++) {
    RigidTransform<AutoDiffXd> pose(
        original.get_transform(i).cast<AutoDiffXd>());
    result.set_transform(i, pose);
    FrameVelocity<AutoDiffXd> velocity;
    velocity.set_velocity(multibody::SpatialVelocity<AutoDiffXd>(
        original.get_velocity(i).get_velocity().get_coeffs()));
    result.set_velocity(i, velocity);
    result.set_name(i, original.get_name(i));
    result.set_model_instance_id(i, original.get_model_instance_id(i));
  }
  return result;
}
}  // namespace

// Tests that PoseAggregator aggregates poses from two PoseVector inputs (one
// with velocity and one without), and a PoseBundle input.
TEST_F(PoseAggregatorTest, CompositeAggregation) {
  // Set some arbitrary translations in the PoseBundle input, and a velocity
  // for one of the poses.
  PoseBundle<double> generic_input(2);
  Vector3d translation_0{0, 1, 0};
  Vector3d translation_1{0, 1, 1};
  const int kSherlockModelInstanceId = 17;
  generic_input.set_name(0, "Sherlock");
  generic_input.set_transform(0, RigidTransformd(translation_0));
  generic_input.set_model_instance_id(0, kSherlockModelInstanceId);
  FrameVelocity<double> sherlock_velocity;
  sherlock_velocity.get_mutable_value() << 2.5, 5.0, 7.5, 10.0, 12.5, 15.0;
  generic_input.set_velocity(0, sherlock_velocity);

  const int kMycroftModelInstanceId = 13;
  generic_input.set_name(1, "Mycroft");
  generic_input.set_transform(1, RigidTransformd(translation_1));
  generic_input.set_model_instance_id(1, kMycroftModelInstanceId);
  aggregator_.get_input_port(0).FixValue(context_.get(), generic_input);

  // Set an arbitrary translation in the first PoseVector input.
  PoseVector<double> pose_vec1 = PoseVector<double>();
  pose_vec1.set_translation(Eigen::Translation<double, 3>(0.5, 1.5, 2.5));
  // TODO(russt): Consider implementing ToAutoDiffXd for BasicVector<double>?
  PoseVector<AutoDiffXd> autodiff_pose_vec1 = PoseVector<AutoDiffXd>();
  autodiff_pose_vec1.set_value(
      pose_vec1.get_value().template cast<AutoDiffXd>());
  aggregator_.get_input_port(1).FixValue(context_.get(), pose_vec1);

  // Set some numbers in the corresponding FrameVelocity input.
  FrameVelocity<double> frame_vel1 = FrameVelocity<double>();
  frame_vel1.get_mutable_value() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0;
  FrameVelocity<AutoDiffXd> autodiff_frame_vel1 = FrameVelocity<AutoDiffXd>();
  autodiff_frame_vel1.set_value(
      frame_vel1.get_value().template cast<AutoDiffXd>());
  aggregator_.get_input_port(2).FixValue(context_.get(), frame_vel1);

  // Set an arbitrary rotation in the second PoseVector input.
  PoseVector<double> pose_vec2 = PoseVector<double>();
  pose_vec2.set_rotation(Eigen::Quaternion<double>(0.5, 0.5, 0.5, 0.5));
  PoseVector<AutoDiffXd> autodiff_pose_vec2 = PoseVector<AutoDiffXd>();
  autodiff_pose_vec2.set_value(
      pose_vec2.get_value().template cast<AutoDiffXd>());
  aggregator_.get_input_port(3).FixValue(context_.get(), pose_vec2);

  aggregator_.CalcOutput(*context_, output_.get());

  // Extract the output PoseBundle.
  const PoseBundle<double>& bundle =
      output_->get_data(0)->get_value<PoseBundle<double>>();
  ASSERT_EQ(kNumBundlePoses + kNumSinglePoses, bundle.get_num_poses());

  // Check that the PoseBundle poses and velocities are passed through to the
  // output.
  EXPECT_EQ("bundle::Sherlock", bundle.get_name(0));
  EXPECT_EQ(kSherlockModelInstanceId, bundle.get_model_instance_id(0));
  const RigidTransformd& generic_pose_0 = bundle.get_transform(0);
  EXPECT_TRUE(CompareMatrices(RigidTransformd(translation_0).GetAsMatrix4(),
                              generic_pose_0.GetAsMatrix4()));
  EXPECT_TRUE(CompareMatrices(sherlock_velocity.get_value(),
                              bundle.get_velocity(0).get_value()));

  EXPECT_EQ("bundle::Mycroft", bundle.get_name(1));
  EXPECT_EQ(kMycroftModelInstanceId, bundle.get_model_instance_id(1));
  const RigidTransformd& generic_pose_1 = bundle.get_transform(1);
  EXPECT_TRUE(CompareMatrices(RigidTransformd(translation_1).GetAsMatrix4(),
                              generic_pose_1.GetAsMatrix4()));

  // Check that the PoseVector pose with velocity is passed through to the
  // output.
  EXPECT_EQ("single_xv", bundle.get_name(2));
  EXPECT_EQ(42, bundle.get_model_instance_id(2));
  const RigidTransformd& xv_pose = bundle.get_transform(2);
  EXPECT_TRUE(CompareMatrices(
      RigidTransformd(Vector3d(0.5, 1.5, 2.5)).GetAsMatrix4(),
      xv_pose.GetAsMatrix4()));
  for (int i = 0; i < 6; ++i) {
    EXPECT_EQ(1.0 + i, bundle.get_velocity(2)[i]);
  }

  // Check that the PoseVector pose without velocity is passed through to
  // the output.
  EXPECT_EQ("single_x", bundle.get_name(3));
  EXPECT_EQ(43, bundle.get_model_instance_id(3));
  const RigidTransformd& x_pose = bundle.get_transform(3);
  EXPECT_TRUE(CompareMatrices(
      RigidTransformd(Eigen::Quaternion<double>(0.5, 0.5, 0.5, 0.5),
                      Vector3d{0, 0, 0})
          .GetAsMatrix4(),
      x_pose.GetAsMatrix4()));

  // The sequel checks the AutoDiffXd conversion.
  auto autodiff_aggregator = aggregator_.ToAutoDiffXd();
  auto autodiff_context = autodiff_aggregator->CreateDefaultContext();
  autodiff_context->SetTimeStateAndParametersFrom(*context_);

  autodiff_aggregator->get_input_port(0).FixValue(autodiff_context.get(),
                                                  ToAutoDiffXd(generic_input));
  autodiff_aggregator->get_input_port(1).FixValue(autodiff_context.get(),
                                                  autodiff_pose_vec1);
  autodiff_aggregator->get_input_port(2).FixValue(autodiff_context.get(),
                                                  autodiff_frame_vel1);
  autodiff_aggregator->get_input_port(3).FixValue(autodiff_context.get(),
                                                  autodiff_pose_vec2);

  auto autodiff_output = autodiff_aggregator->AllocateOutput();
  autodiff_aggregator->CalcOutput(*autodiff_context, autodiff_output.get());
  const PoseBundle<AutoDiffXd>& autodiff_bundle =
      autodiff_output->get_data(0)->get_value<PoseBundle<AutoDiffXd>>();
  ASSERT_EQ(bundle.get_num_poses(), autodiff_bundle.get_num_poses());
  for (int i = 0; i < bundle.get_num_poses(); i++) {
    EXPECT_TRUE(
        CompareMatrices(bundle.get_transform(i).GetAsMatrix4(),
                        math::autoDiffToValueMatrix(
                            autodiff_bundle.get_transform(i).GetAsMatrix4())));
    EXPECT_TRUE(
        CompareMatrices(bundle.get_velocity(i).get_value(),
                        math::autoDiffToValueMatrix(
                            autodiff_bundle.get_velocity(i).get_value())));
    EXPECT_EQ(bundle.get_name(i), autodiff_bundle.get_name(i));
    EXPECT_EQ(bundle.get_model_instance_id(i),
              autodiff_bundle.get_model_instance_id(i));
  }
}

// Tests that PoseAggregator allocates no state variables in the context_.
TEST_F(PoseAggregatorTest, Stateless) { EXPECT_TRUE(context_->is_stateless()); }

// Tests that AddSinglePoseAndVelocityInput returns input ports for both
// the new ports.
TEST_F(PoseAggregatorTest, AddSinglePoseAndVelocityPorts) {
  auto ports = aggregator_.AddSinglePoseAndVelocityInput("test", 100);
  const InputPort<double>& pose_port = ports.pose_input_port;
  const InputPort<double>& velocity_port = ports.velocity_input_port;
  EXPECT_EQ(PoseVector<double>::kSize, pose_port.size());
  EXPECT_EQ(FrameVelocity<double>::kSize, velocity_port.size());
}

// Tests that PoseBundle supports Symbolic form.
GTEST_TEST(PoseBundleTest, Symbolic) {
  PoseBundle<symbolic::Expression> dut{1};
  multibody::SpatialVelocity<symbolic::Expression> msv =
    dut.get_velocity(0).get_velocity();
  // Just sanity check that some element got zero-initialized.
  const symbolic::Expression& x = msv[0];
  EXPECT_TRUE(x.EqualTo(0.0));
}

}  // namespace
}  // namespace rendering
}  // namespace systems
}  // namespace drake


// Copyright (c) 2009-2010 Satoshi Nakamoto
// Copyright (c) 2009-2014 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "infinitum.h"
#include "chain.h"

#include <boost/foreach.hpp>

using namespace std;

/**
 * CChain implementation
 */
CAmount CChain::GetMinSpendableOutputValue(uint64_t nOutputBlockHeight, uint64_t nInputBlockHeight) const {
    // Example: Start 0, End 2. The transaction with the UTXO is somewhere inside Cycle 0,
    //  and the transaction that is trying to spend it is somewhere inside Cycle 2.
    // We have to check then what the dust vote result was at the end of Cycle 0 (vmin[0]),
    //  and then what the result was at the end of Cycle 1 (vmin[1]), which are the cycle
    //  borders that are crossed between the middle of Cycle 0 and the middle of Cycle 2.

    int nStartCycle = GetCycle(nOutputBlockHeight);
    int nEndCycle = GetCycle(nInputBlockHeight);

    CAmount nMaximumFound = 0; // The highest prune threshold crossed is the one that we apply.
    for (int nCycle = nStartCycle; nCycle < nEndCycle; ++nCycle)
	nMaximumFound = std::max(nMaximumFound, vMinSpendableOutputValues[nCycle]);
    return nMaximumFound;
}

// Infinitum:: new CChain::SetTip that updates vMinSpendableOutputValues as well as the vChain
void CChain::SetTip(CBlockIndex *pindex) {
    if (pindex == NULL) {
        vChain.clear();
        vMinSpendableOutputValues.clear(); // Infinitum:: update vMinSpendableOutputValues
        return;
    }

    // Height ints:
    // -1 = no genesis block
    //  0 = just the genesis block (block at height #0)
    int nNewChainHeight = pindex->nHeight;
    int nFirstCommonNodeHeight = nNewChainHeight;

    vChain.resize(nNewChainHeight + 1);
    while (pindex && vChain[pindex->nHeight] != pindex) {
        vChain[pindex->nHeight] = pindex;
        pindex = pindex->pprev;
	--nFirstCommonNodeHeight;
    }

    // From first common height to new height, the vminspendableoutputvalues need recomputing.
    // The interval touched by the first common height + 1 (changed block) is the first interval
    //   to recompute.
    int nStartCycle = GetCycle(nFirstCommonNodeHeight + 1);
    if (nStartCycle < 0)
        nStartCycle = 0; // The genesis block ("Cycle #-1") doesn't matter, we never "start" at it.

    // The end cycle is the first *whole* cycle given by the new chain height.
    // If the end cycle is less than the start cycle, then the spendable output values array
    //   is empty.
    // End cycle can be -1 if there is no end cycle, which resizes the vminspend vector to 0.
    int nEndCycle = GetCycle(nNewChainHeight);
    if (nNewChainHeight % INFINITUM_CHAIN_CYCLE_BLOCKS != 0)
        --nEndCycle; // New height block doesn't land squarely at the end of its cycle:
                     //   that cycle isn't whole so it doesn't apply any dust pruning yet.

    // Cycles past the end are deleted
    if (nEndCycle < -1)
        nEndCycle = -1; // So we don't resize(-1). It happens when pushing the genesis in.
    vMinSpendableOutputValues.resize(nEndCycle + 1); // Cycle #0 is the first one, so +1 to fit it in

    // Cycles from start to end are recomputed/re-tallied.
    // nEndCycle is less than nStartCycle for most of the calls to this method, in which
    //   case this loop doesn't run (i.e. nothing to update)
    for (int nCycle = nStartCycle; nCycle <= nEndCycle; ++nCycle) {

	// A simple safeguard:
	// For the first 8 cycles, the vote is 0 (no dust pruning).
	if (nCycle < 8) {
	  vMinSpendableOutputValues[nCycle] = 1;
	  continue;
	}

	// Element 0 = ndustvote 0's count; element 1 = ndustvote 1's count etc.
	std::vector<int> vVoteCounts;
	vVoteCounts.resize(256);

	int64_t nFirstHeight = 1 + (nCycle * INFINITUM_CHAIN_CYCLE_BLOCKS);
	int64_t nLastHeight = (nCycle + 1) * INFINITUM_CHAIN_CYCLE_BLOCKS;

	// Tally votes
	for (int i = nFirstHeight; i <= nLastHeight; ++i) {

	    // A simple safeguard:
	    // If the network difficulty is below 268,435,456 (roughly 1-2 petahashes
	    //   network speed) for ANY block in a cycle, then that cycle's vote for
	    //   the dust threshold is automatically 0 (no dust pruning).
	    if (vChain[i]->nBits > 0x190FFFFF) {
	      vVoteCounts[0] = INFINITUM_CHAIN_CYCLE_BLOCKS;
	      break;
	    }

	    int nVote = (vChain[i]->nVersion >> 8) & 0xFF;
	    ++vVoteCounts[nVote];
	}

	// Compute the winning vote
	CAmount nWinningVote = 0;

	// Will discard the lowest 5% votes and keep whatever the next vote is,
	//   so the dust value is the lowest common denominator among the 95% of miners
	//   that are on the side of wanting the highest dust value.
	int nDiscardBudget = INFINITUM_CHAIN_CYCLE_BLOCKS / 20;
	int nIndex = 0;
	BOOST_FOREACH(int nVoteCount, vVoteCounts) {
	  nDiscardBudget -= nVoteCount;
	  if (nDiscardBudget < 0) {
            // Votes for "0" say dust is 2^0, votes for "1" say dust is 2^1, etc.
	    nWinningVote = 1 << nIndex;
	    break;
	  }
	  ++nIndex;
	}

	// Update it
	vMinSpendableOutputValues[nCycle] = nWinningVote;
    }
}

CBlockLocator CChain::GetLocator(const CBlockIndex *pindex) const {
    int nStep = 1;
    std::vector<uint256> vHave;
    vHave.reserve(32);

    if (!pindex)
        pindex = Tip();
    while (pindex) {
        vHave.push_back(pindex->GetBlockHash());
        // Stop when we have added the genesis block.
        if (pindex->nHeight == 0)
            break;
        // Exponentially larger steps back, plus the genesis block.
        int nHeight = std::max(pindex->nHeight - nStep, 0);
        if (Contains(pindex)) {
            // Use O(1) CChain index if possible.
            pindex = (*this)[nHeight];
        } else {
            // Otherwise, use O(log n) skiplist.
            pindex = pindex->GetAncestor(nHeight);
        }
        if (vHave.size() > 10)
            nStep *= 2;
    }

    return CBlockLocator(vHave);
}

const CBlockIndex *CChain::FindFork(const CBlockIndex *pindex) const {
    if (pindex == NULL) {
        return NULL;
    }
    if (pindex->nHeight > Height())
        pindex = pindex->GetAncestor(Height());
    while (pindex && !Contains(pindex))
        pindex = pindex->pprev;
    return pindex;
}

/** Turn the lowest '1' bit in the binary representation of a number into a '0'. */
int static inline InvertLowestOne(int n) { return n & (n - 1); }

/** Compute what height to jump back to with the CBlockIndex::pskip pointer. */
int static inline GetSkipHeight(int height) {
    if (height < 2)
        return 0;

    // Determine which height to jump back to. Any number strictly lower than height is acceptable,
    // but the following expression seems to perform well in simulations (max 110 steps to go back
    // up to 2**18 blocks).
    return (height & 1) ? InvertLowestOne(InvertLowestOne(height - 1)) + 1 : InvertLowestOne(height);
}

CBlockIndex* CBlockIndex::GetAncestor(int height)
{
    if (height > nHeight || height < 0)
        return NULL;

    CBlockIndex* pindexWalk = this;
    int heightWalk = nHeight;
    while (heightWalk > height) {
        int heightSkip = GetSkipHeight(heightWalk);
        int heightSkipPrev = GetSkipHeight(heightWalk - 1);
        if (pindexWalk->pskip != NULL &&
            (heightSkip == height ||
             (heightSkip > height && !(heightSkipPrev < heightSkip - 2 &&
                                       heightSkipPrev >= height)))) {
            // Only follow pskip if pprev->pskip isn't better than pskip->pprev.
            pindexWalk = pindexWalk->pskip;
            heightWalk = heightSkip;
        } else {
            assert(pindexWalk->pprev);
            pindexWalk = pindexWalk->pprev;
            heightWalk--;
        }
    }
    return pindexWalk;
}

const CBlockIndex* CBlockIndex::GetAncestor(int height) const
{
    return const_cast<CBlockIndex*>(this)->GetAncestor(height);
}

void CBlockIndex::BuildSkip()
{
    if (pprev)
        pskip = pprev->GetAncestor(GetSkipHeight(nHeight));
}

arith_uint256 GetBlockProof(const CBlockIndex& block)
{
    arith_uint256 bnTarget;
    bool fNegative;
    bool fOverflow;
    bnTarget.SetCompact(block.nBits, &fNegative, &fOverflow);
    if (fNegative || fOverflow || bnTarget == 0)
        return 0;
    // We need to compute 2**256 / (bnTarget+1), but we can't represent 2**256
    // as it's too large for a arith_uint256. However, as 2**256 is at least as large
    // as bnTarget+1, it is equal to ((2**256 - bnTarget - 1) / (bnTarget+1)) + 1,
    // or ~bnTarget / (nTarget+1) + 1.
    return (~bnTarget / (bnTarget + 1)) + 1;
}

int64_t GetBlockProofEquivalentTime(const CBlockIndex& to, const CBlockIndex& from, const CBlockIndex& tip, const Consensus::Params& params)
{
    arith_uint256 r;
    int sign = 1;
    if (to.nChainWork > from.nChainWork) {
        r = to.nChainWork - from.nChainWork;
    } else {
        r = from.nChainWork - to.nChainWork;
        sign = -1;
    }
    r = r * arith_uint256(params.nPowTargetSpacing) / GetBlockProof(tip);
    if (r.bits() > 63) {
        return sign * std::numeric_limits<int64_t>::max();
    }
    return sign * r.GetLow64();
}


#include <boost/algorithm/searching/boyer_moore.hpp>


/**************************************************************************\
|
|    Copyright (C) 2009 Marc Stevens
|
|    This program is free software: you can redistribute it and/or modify
|    it under the terms of the GNU General Public License as published by
|    the Free Software Foundation, either version 3 of the License, or
|    (at your option) any later version.
|
|    This program is distributed in the hope that it will be useful,
|    but WITHOUT ANY WARRANTY; without even the implied warranty of
|    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
|    GNU General Public License for more details.
|
|    You should have received a copy of the GNU General Public License
|    along with this program.  If not, see <http://www.gnu.org/licenses/>.
|
\**************************************************************************/

#include <stdexcept>
#include <iostream>
#include <fstream>
#include <vector>
#include <string>

#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>

#include "main.hpp"

#include <hashclash/sdr.hpp>
#include <hashclash/timer.hpp>
#include <hashclash/booleanfunction.hpp>

using namespace hashclash;
using namespace std;
namespace po = boost::program_options;
namespace fs = boost::filesystem;

boost::mutex mut;
std::string workdir;

int main(int argc, char** argv)
{
	hashclash::timer runtime(true);

	cout <<
		"Connect MD5 differential paths\n"
		"Copyright (C) 2009 Marc Stevens\n"
		"http://homepages.cwi.nl/~stevens/\n"
		<< endl;

	try {
		path_container container;
		vector< vector<int> > msgdiff(16);

		// Define program options
		po::options_description
			desc("Allowed options"),
			msg("Define message differences (as +bitnr and -bitnr, bitnr=1-32)"),
			all("Allowed options");

		int bestmaxcomp;
		desc.add_options()
			("help,h", "Show options.")
			("mod,m"
				, po::value<unsigned>(&container.modn)->default_value(1)
				, "Do only 1/m of all work.")

			("index,i"
				, po::value<unsigned>(&container.modi)->default_value(0)
				, "Do i-th part of all work (i=0,...,m-1).")

			("workdir,w"
				, po::value<string>(&workdir)->default_value("./data")
				, "Set working directory.")

			("inputfilelow"
				, po::value<string>(&container.inputfilelow)
				, "Use specified inputfile for lower paths.")

			("inputfilehigh"
				, po::value<string>(&container.inputfilehigh)
				, "Use specified inputfile for upper paths.")

			("showinputpaths,s"
				, po::bool_switch(&container.showinputpaths)
				, "Show all input paths.\n")

			("tstep,t"
				, po::value<unsigned>(&container.t)
				, "Do step t (=0,...,15).")

			("noverify"
				, po::bool_switch(&container.noverify)
				, "Do not verify rotations etc.")

			("noenhancepath"
				, po::bool_switch(&container.noenhancepath)
				, "Do not enhance path (rotations) for t=0-15.")

			("showstatistics"
				, po::bool_switch(&container.showstats)
				, "Show intermediate statistics.")

			("Qcondstart"
				, po::value<int>(&container.Qcondstart)->default_value(18)
				, "Count conditions from Q_t up.")
			("maxcomplexity"
				, po::value<int>(&bestmaxcomp)->default_value(-1000)
				, "Maximum complexity = \n"
				  "  #tunnels - #Qconds\n")

			("threads"
				, po::value<int>(&container.threads)->default_value(-1)
				, "Number of worker threads")
			;

		msg.add_options()
			("diffm0", po::value< vector<int> >(&msgdiff[0]), "delta m0")
			("diffm1", po::value< vector<int> >(&msgdiff[1]), "delta m1")
			("diffm2", po::value< vector<int> >(&msgdiff[2]), "delta m2")
			("diffm3", po::value< vector<int> >(&msgdiff[3]), "delta m3")
			("diffm4", po::value< vector<int> >(&msgdiff[4]), "delta m4")
			("diffm5", po::value< vector<int> >(&msgdiff[5]), "delta m5")
			("diffm6", po::value< vector<int> >(&msgdiff[6]), "delta m6")
			("diffm7", po::value< vector<int> >(&msgdiff[7]), "delta m7")
			("diffm8", po::value< vector<int> >(&msgdiff[8]), "delta m8")
			("diffm9", po::value< vector<int> >(&msgdiff[9]), "delta m9")
			("diffm10", po::value< vector<int> >(&msgdiff[10]), "delta m10")
			("diffm11", po::value< vector<int> >(&msgdiff[11]), "delta m11")
			("diffm12", po::value< vector<int> >(&msgdiff[12]), "delta m12")
			("diffm13", po::value< vector<int> >(&msgdiff[13]), "delta m13")
			("diffm14", po::value< vector<int> >(&msgdiff[14]), "delta m14")
			("diffm15", po::value< vector<int> >(&msgdiff[15]), "delta m15")
			;
		all.add(desc).add(msg);

		// Parse program options
		po::variables_map vm;
		po::store(po::parse_command_line(argc, argv, all), vm);
		{
			std::ifstream ifs("md5diffpathconnect.cfg");
			if (ifs) po::store(po::parse_config_file(ifs, all), vm);
		}
		po::notify(vm);
		container.bestmaxcomp = bestmaxcomp;

		// Process program options
		if (vm.count("help") || vm.count("tstep")==0) {
			cout << desc << endl;
			return 0;
		}
		if (container.inputfilelow.size() == 0)
		{
			cerr << "Error: inputfilelow must be given!" << endl;
			return 2;
		}
		if (container.inputfilehigh.size() == 0)
		{
			cerr << "Error: inputfilehigh must be given!" << endl;
			return 2;
		}
		if (container.modi >= container.modn) {
			cerr << "Error: i must be strictly less than m!" << endl;
			return 1;
		}
		if (container.modn != 1)
			cout << "Using set " << container.modi << " of " << container.modn << endl;

		for (unsigned k = 0; k < 16; ++k)
		{
			container.m_diff[k] = 0;
			for (unsigned j = 0; j < msgdiff[k].size(); ++j)
				if (msgdiff[k][j] > 0)
					container.m_diff[k] += 1<<(msgdiff[k][j]-1);
				else
					container.m_diff[k] -= 1<<(-msgdiff[k][j]-1);
			if (container.m_diff[k] != 0)
				cout << "delta_m[" << k << "] = " << naf(container.m_diff[k]) << endl;
		}

		if (container.noverify && !container.noenhancepath) {
			container.noenhancepath = true;
			cout << "Option noverify implies noenhancepath." << endl;
		}

		if (container.threads <= 0 || container.threads > boost::thread::hardware_concurrency())
			container.threads = boost::thread::hardware_concurrency();

		// Start job with given parameters
		dostep(container);

	} catch (exception& e) {
		cout << "Runtime: " << runtime.time() << endl;
		cerr << "Caught exception!!:" << endl << e.what() << endl;
		return 1;
	} catch (...) {
		cout << "Runtime: " << runtime.time() << endl;
		cerr << "Unknown exception caught!!" << endl;
		return 1;
	}
	cout << "Runtime: " << runtime.time() << endl;
	return 0;
}


/*
 * generic_model.cpp
 *
 *  Created on: Oct 19, 2009
 *      Author: sturm
 */
#include <boost/format.hpp>
#include "articulation_models/models/generic_model.h"

#include "articulation_models/utils.h"
#include "articulation_models/models/factory.h"
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_blas.h>

using namespace std;
using namespace boost;

using namespace Eigen;

using namespace articulation_msgs;

#include <iomanip>
#define VEC(a) setprecision(5)<<fixed<<a.x()<<" "<<a.y()<<" "<<a.z()<<" "<<a.w()<<" l="<<a.length()
#define VEC2(a) "t=["<<VEC(a.getOrigin())<<"] r=[]"<<VEC(a.getRotation())<<"]"
#define PRINT(a) cout << #a <<"=" << VEC(a)<<endl;
#define PRINT2(a) cout << #a <<"=" << VEC2(a)<<endl;

namespace articulation_models {

GenericModel::GenericModel() {
	setId(-1);
	sigma_position = 0.005;
	sigma_orientation = 360 * M_PI/180.0;

	avg_error_position = 0;
	avg_error_orientation = 0;
	bic = 0;

	last_error_jacobian = 0;
	evaluated = false;
	supress_similar = true;
	outlier_ratio = 0.5;
	sac_iterations = 100;
	optimizer_iterations = 10;
//	optimizer_iterations = 0;

	prior_outlier_ratio = log(0.01) / (- 0.05); // prior over outlier ratio

	complexity = 0;
	jacobian = Eigen::MatrixXd();
	hessian = Eigen::MatrixXd();
}

void GenericModel::setModel(const ModelMsg& model) {
	this->model = model;
	readParamsFromModel();
	prepareChannels();
}

ModelMsg GenericModel::getModel() {
    writeParamsToModel();
	model.name = getModelName();
	model.header = model.track.header;
	model.track.pose_flags.resize(model.track.pose.size(),TrackMsg::POSE_VISIBLE );
	if(model.track.pose.size())
		model.track.pose_flags[model.track.pose.size()-1] |= TrackMsg::POSE_END_OF_SEGMENT;
	return model;
}

void GenericModel::setTrack(const TrackMsg& track) {
	this->model.track = track;
	prepareChannels();
}

TrackMsg GenericModel::getTrack() {
	return model.track;
}

void GenericModel::setId(int id) {
	model.id = id;
}

int GenericModel::getId() {
	return(model.id);
}

// -- model information
std::string GenericModel::getModelName() {
	return MultiModelFactory::instance.getLongName(this);
}

size_t GenericModel::getDOFs() {
	return 0;
}

size_t GenericModel::getSamples() {
	return model.track.pose.size();
}

// -- params
void GenericModel::readParamsFromModel() {
	getParam("sigma_position",sigma_position);
	getParam("sigma_orientation",sigma_orientation);
	getParam("supress_similar",supress_similar);
	getParam("avg_error_position",avg_error_position);
	getParam("avg_error_orientation",avg_error_orientation);
	getParam("bic",bic);
	getParam("last_error_jacobian",last_error_jacobian);
	getParam("evaluated",evaluated);
//	getParam("complexity",complexity);//read-only
	getParam("jacobian",jacobian);
	getParam("hessian",hessian);
	getParam("loglikelihood",loglikelihood);
	getParam("outlier_ratio",outlier_ratio);
	getParam("sac_iterations",sac_iterations);
	getParam("prior_outlier_ratio",prior_outlier_ratio);
}

void GenericModel::writeParamsToModel() {
	setParam("sigma_position",sigma_position,ParamMsg::PRIOR);
	setParam("sigma_orientation",sigma_orientation,ParamMsg::PRIOR);
	setParam("supress_similar",supress_similar,ParamMsg::PRIOR);
	setParam("avg_error_position",avg_error_position,ParamMsg::EVAL);
	setParam("avg_error_orientation",avg_error_orientation,ParamMsg::EVAL);
	setParam("loglikelihood",loglikelihood,ParamMsg::EVAL);
	setParam("bic",bic,ParamMsg::EVAL);
	setParam("last_error_jacobian",last_error_jacobian,ParamMsg::EVAL);
	setParam("evaluated",evaluated,ParamMsg::EVAL);
	setParam("complexity",complexity,ParamMsg::PRIOR);
	setParam("jacobian",jacobian,ParamMsg::EVAL);
	setParam("hessian",hessian,ParamMsg::EVAL);
	setParam("dofs",getDOFs(),ParamMsg::EVAL);
	setParam("samples",getSamples(),ParamMsg::EVAL);
	setParam("outlier_ratio",outlier_ratio,ParamMsg::EVAL);
	setParam("sac_iterations",sac_iterations,ParamMsg::PRIOR);
	setParam("prior_outlier_ratio",prior_outlier_ratio,ParamMsg::PRIOR);
}

void GenericModel::prepareChannels() {
	channelOutlier = openChannel("outlier");
	channelLogLikelihood = openChannel("loglikelihood");
	channelInlierLogLikelihood = openChannel("loglikelihood_if_inlier");
	channelConfiguration.resize(getDOFs(),0);
	for(size_t i=0;i<getDOFs();i++) {
		channelConfiguration[i] = openChannel(str(format("q%d")%i));
	}

}

double GenericModel::getParam(std::string name) {
	for(size_t i=0;i<model.params.size();i++) {
		if(model.params[i].name == name) {
			return(model.params[i].value);
		}
	}
//	std::cerr << "WARNING (in GenericModel::getParam): undefined parameter '"<<name<<"'" << std::endl;
	return 0.00;
}

void GenericModel::getParam(std::string name,double& value) {
	if(hasParam(name))
		value = getParam(name);
}

void GenericModel::getParam(std::string name,Eigen::VectorXd &vec) {
	for(int i=0;i<vec.rows();i++)
		vec(i) = getParam( str(boost::format( name+"[%1%]") %i ) );
}

void GenericModel::getParam(std::string name,tf::Vector3 &vec) {
	vec.setX( getParam( name+".x" ) );
	vec.setY( getParam( name+".y" ) );
	vec.setZ( getParam( name+".z" ) );
}

void GenericModel::getParam(std::string name,tf::Quaternion &quat) {
	quat.setX( getParam( name+".x" ) );
	quat.setY( getParam( name+".y" ) );
	quat.setZ( getParam( name+".z" ) );
	quat.setW( getParam( name+".w" ) );
}

void GenericModel::getParam(std::string name,tf::Transform &t) {
	tf::Quaternion q;
	tf::Vector3 v;
	getParam(name+".orientation",q);
	getParam(name+".position",v);
	t = tf::Transform(q,v);
}

void GenericModel::getParam(std::string name,Eigen::MatrixXd &mat) {
	for(int r=0;r<mat.rows();r++)
		for(int c=0;c<mat.cols();c++)
			mat(r,c) = getParam( str(boost::format( name+"[%1%][%2%]") %r % c) );
}

void GenericModel::setParam(std::string name,double value,int type) {
	for(size_t i=0;i<model.params.size();i++) {
		if(model.params[i].name == name) {
			model.params[i].value = value;
			return;
		}
	}
	ParamMsg data;
	data.name = name;
	data.value = value;
	data.type = type;
	model.params.push_back(data);
}

void GenericModel::setParam(std::string name,const Eigen::VectorXd &vec,int type) {
	for(int i=0;i<vec.rows();i++)
		setParam( boost::str(boost::format( name+"[%1%]") %i ),vec(i),type );
}

void GenericModel::setParam(std::string name,const tf::Vector3 &vec,int type) {
	setParam( name+".x",vec.x(),type );
	setParam( name+".y",vec.y(),type );
	setParam( name+".z",vec.z(),type );
}

void GenericModel::setParam(std::string name,const tf::Quaternion &quat,int type) {
	setParam( name+".x",quat.x(),type );
	setParam( name+".y",quat.y(),type );
	setParam( name+".z",quat.z(),type );
	setParam( name+".w",quat.w(),type );
}

void GenericModel::setParam(std::string name,const tf::Transform &t,int type) {
	setParam(name+".position",t.getOrigin(),type);
	setParam(name+".orientation",t.getRotation(),type);
}

void GenericModel::setParam(std::string name,const Eigen::MatrixXd &mat,int type) {
	for(int r=0;r<mat.rows();r++)
		for(int c=0;c<mat.cols();c++)
			setParam( str(boost::format( name+"[%1%][%2%]") %r%c ),mat(r,c),type );
}

bool GenericModel::hasParam(std::string name) {
	for(size_t i=0;i<model.params.size();i++) {
		if(model.params[i].name == name)
			return(true);
	}
	return false;
}

// -- track data
int GenericModel::openChannel(std::string name,bool autocreate) {
	return(articulation_models::openChannel(model.track,name,autocreate));
}

// -- train model
bool GenericModel::fitModel() {
	if(!sampleConsensus()) {
//		if(DEBUG) cout <<"sampleConsesus failed of "<<getModelName()<<endl;
		return false;
	}
	if(!optimizeParameters()) {
//		if(DEBUG) cout <<"optimizeParameters failed of "<<getModelName()<<endl;
		return false;
	}
	if(!normalizeParameters()) {
		return false;
	}
    return true;
}

bool GenericModel::fitMinMaxConfigurations() {
	setParam("q_min",getMinConfigurationObserved(),ParamMsg::PARAM);
	setParam("q_max",getMaxConfigurationObserved(),ParamMsg::PARAM);
	return true;
}

// -- evaluate model
bool GenericModel::evaluateModel() {
	// need at least one data point
	if(model.track.pose.size() == 0)
		return false;

	// let getLogLikelihood() do the projection
	loglikelihood = getLogLikelihood(false);
	// evaluate some extra statistics

	// get params
	size_t n = model.track.pose.size();

	// compute pose difference and likelihoods
	double sum_position2 = 0;
	double sum_orientation2 = 0;

	double sum_position = 0;
	double sum_orientation = 0;
//	double sum_loglikelihood = 0;

	for(size_t i=0;i<n;i++) {
		tf::Transform p1 = poseToTransform(model.track.pose[i]);
		tf::Transform p2 = poseToTransform(model.track.pose_projected[i]);
		tf::Transform diff = p1.inverseTimes(p2);
		double err_position2 = diff.getOrigin().length2();
		double err_orientation2 = SQR(diff.getRotation().getAngle());
//		sum_loglikelihood += model.track.channels[channelInlierLogLikelihood].values[i];

		sum_position2 += err_position2;
		sum_orientation2 += err_orientation2;
		sum_position += sqrt(err_position2);
		sum_orientation += sqrt(err_orientation2);
	}

	// store into cache
	avg_error_position = sum_position/n;
	avg_error_orientation = sum_orientation/n;
	loglikelihood += - ((double)n)*getDOFs()*log(n);

	bic =
					-2*(loglikelihood )
					+complexity
					* log( n );

	last_error_jacobian = evalLatestJacobian();

	evaluated = true;

	if(model.track.pose.size()>=2) {
		VectorXd q1 = predictConfiguration( model.track.pose.front() );
		VectorXd q2 = predictConfiguration( model.track.pose.back() );

		jacobian  = predictJacobian( q2 );
		hessian = predictHessian( q2 );
		if(getDOFs()>=1 ) {
			VectorXd p(3);
			for(size_t i=0;i<model.track.pose.size();i++) {
				p += pointToEigen(model.track.pose[i].position) - pointToEigen(model.track.pose.front().position);
			}
			if(p.dot(jacobian.col(0))<0)	// make Jacobian point into current direction (of dof 1)
				jacobian *= -1;
				hessian *= -1;
		}
	}
	writeParamsToModel();
	return true;
}

double GenericModel::evalLatestJacobian() {
	if(model.track.pose.size()<2)
		return 0.00;

	// observed direction of motion
	VectorXd p1 = pointToEigen( model.track.pose[model.track.pose.size()-1].position );
	VectorXd p2 = pointToEigen( model.track.pose[model.track.pose.size()-2].position );
	VectorXd pD_obs = p1-p2;
	if(pD_obs.norm() == 0)
		return 0.00;
	pD_obs.normalize();
//	cout << "p1="<<p1<<endl;
//	cout << "p2="<<p2<<endl;
//	cout << "pD_obs="<<pD_obs<<endl;

	if(getDOFs()==0)
		return 2*M_PI;

	// predicted direction of motion
	VectorXd q1 = predictConfiguration( model.track.pose[model.track.pose.size()-1] );
	VectorXd q2 = predictConfiguration( model.track.pose[model.track.pose.size()-2] );
//	cout << "q1="<<q1<<endl;
//	cout << "q2="<<q2<<endl;
	MatrixXd J2 = predictJacobian( q2 );
//	cout << "J2="<<J2<<endl;
	VectorXd qD = (q1 - q2);
//	cout << "qD="<<qD<<endl;
	VectorXd pD_pred = J2 * qD; // reduce Jacobian
//	cout << "pD_pred="<<pD_pred<<endl;
	if(pD_pred.norm() == 0)
		return 0.00;
	pD_pred.normalize();
//	cout << "qD_pred.normalized="<<pD_pred<<endl;
//
//	cout << "angle_diff="<<acos( pD_obs.dot( pD_pred ) )<<endl;
//
//	// return angle between predicted motion and observed motion
	return acos( pD_obs.dot( pD_pred ) );
}

double GenericModel::getPositionError() {
	return avg_error_position;
}

double GenericModel::getOrientationError() {
	return avg_error_orientation;
}

double GenericModel::getBIC() {
	return bic;
}

// -- cartesian space
geometry_msgs::Pose GenericModel::predictPose(V_Configuration q) {
	geometry_msgs::Pose p;
	p.orientation.w = 1;
	return p;
}

M_CartesianJacobian GenericModel::predictJacobian(V_Configuration vq,double delta) {
	M_CartesianJacobian J;
	J.resize(3,getDOFs());
	VectorXd p = pointToEigen(predictPose(vq).position);
	for(size_t i=0;i<getDOFs();i++) {
		V_Configuration q = vq;
		q(i) += delta;
		J.col(i) = (pointToEigen( predictPose(q).position ) - p)/delta;
	}
	return J;
}

M_CartesianJacobian GenericModel::predictHessian(V_Configuration q,double delta) {
	M_CartesianJacobian H;
	H.resize(3*getDOFs(),getDOFs());
//	cout <<"dofs="<<getDOFs()<<" q.size"<<vq.size()<<endl;
	for(size_t i=0;i<getDOFs();i++) {
		V_Configuration qd = q;
		q(i) += delta;
		M_CartesianJacobian H_part;

		M_CartesianJacobian J = predictJacobian(q);
		M_CartesianJacobian Jd = predictJacobian(qd);

//		cout << J(0,0) << " "<< J(1,0) << " "<< J(2,0) << endl;
//		cout << "H_part "<<Jd(0,0) << " "<< Jd(1,0) << " "<< Jd(2,0) << endl;

		H_part = (Jd - J)/delta;
//		cout << "H_part "<<H_part(0,0) << " "<< H_part(1,0) << " "<< H_part(2,0) << endl;
		for(size_t r=0;r<3;r++) {
			for(size_t c=0;c<getDOFs();c++) {
				H(r+3*i,c) = H_part(r,c);
			}
		}
	}
	return H;
}

// -- configuration space
void GenericModel::setConfiguration(size_t index,V_Configuration q) {
	std::map<int,int> channel;
	// get channel ids and prepare channels
	for(size_t ch=0;ch<getDOFs();ch++) {
		std::stringstream s;
		s << "q" << ch;
		channel[ch] = openChannel( s.str() );
	}

	for(int j=0;j<q.rows();j++) {
		model.track.channels[ channel[j] ].values[index] = q[j];
	}
}

// -- configuration space
void GenericModel::setJacobian(size_t index,M_CartesianJacobian J) {
	std::map<int,std::map<int,int> > channel;
	// get channel ids and prepare channels
	for(int r=0;r<J.rows();r++)
		for(int c=0;c<J.cols();c++)
		channel[r][c] = openChannel( boost::str(boost::format( "J[%1%][%2%]") %r%c ) );

	for(int r=0;r<J.rows();r++)
		for(int c=0;c<J.cols();c++)
			model.track.channels[ channel[r][c] ].values[index] = J(r,c);
}

V_Configuration GenericModel::getConfiguration(size_t index) {
	std::map<int,int> channel;
	// get channel ids and prepare channels
	for(size_t ch=0;ch<getDOFs();ch++) {
		std::stringstream s;
		s << "q" << ch;
		channel[ch] = openChannel( s.str() );
	}

	V_Configuration q;
	if (getDOFs()) q.resize(getDOFs());
	for(int j=0;j<q.rows();j++) {
		q[j] = model.track.channels[ channel[j] ].values[index];
	}
	return(q);
}

V_Configuration GenericModel::predictConfiguration(geometry_msgs::Pose pose) {
	V_Configuration q;
	if(getDOFs()>0) q.resize(getDOFs());
	return q;
}

V_Configuration GenericModel::getMinConfigurationObserved() {
	if(getDOFs()==0) return V_Configuration();
	V_Configuration q_min(getDOFs());
	for(size_t j=0;j<getDOFs();j++) {
		q_min[j] = FLT_MAX;
	}

	for(size_t i=0;i<model.track.pose.size();i++) {
		V_Configuration q = getConfiguration(i);
		for(size_t j=0;j<getDOFs();j++) {
			q_min[j] = MIN( q_min[j], q[j] );
		}
	}

	return q_min;
}

V_Configuration GenericModel::getMaxConfigurationObserved() {
	if(getDOFs()==0) return V_Configuration();
	V_Configuration q_max(getDOFs());
	for(size_t j=0;j<getDOFs();j++) {
		q_max[j] = -FLT_MAX;
	}

	for(size_t i=0;i<model.track.pose.size();i++) {
		V_Configuration q = getConfiguration(i);
		for(size_t j=0;j<getDOFs();j++) {
			q_max[j] = MAX( q_max[j], q[j] );
		}
	}

	return q_max;
}

// -- projections of track data
void GenericModel::projectPoseToConfiguration() {
	// now estimate configurations, and store in channels q0..qn
	for(size_t i=0;i<model.track.pose.size();i++) {
		V_Configuration q;
		q = predictConfiguration( model.track.pose[i] );
		setConfiguration(i,q);
	}
}

void GenericModel::projectConfigurationToPose() {
	// now estimate poses
	model.track.pose_projected.resize(model.track.pose.size());
	for(size_t i=0;i<model.track.pose.size();i++) {
		V_Configuration q = getConfiguration(i);
		model.track.pose_projected[i] = predictPose( q );
	}
}

void GenericModel::projectConfigurationToJacobian() {
	// now estimate poses
	model.track.pose_projected.resize(model.track.pose.size());
	for(size_t i=0;i<model.track.pose.size();i++) {
		V_Configuration q = getConfiguration(i);
		setJacobian(i, predictJacobian( q ));
	}
}

void GenericModel::sampleConfigurationSpace(double resolution) {
//	cout << "resolution="<<resolution<<endl;
	// clear all poses
	model.track.pose_resampled.clear();

	if(getDOFs() == 0) {
		model.track.pose_resampled.push_back( predictPose(V_Configuration() ) );	// single sample, empty configuration
		return;
	}

	V_Configuration q_min(getDOFs());
	V_Configuration q_max(getDOFs());
	getParam("q_min",q_min);
	getParam("q_max",q_max);

	geometry_msgs::Pose p_min = predictPose( q_min );
	geometry_msgs::Pose p_max = predictPose( q_max );
	double dist =
			(positionToVector(p_min.position) - positionToVector(p_max.position)).length();
	if(isnan(dist) || isinf(dist)) return;

	// show all axes..
	for(size_t j=0;j<getDOFs();j++) {
		size_t num = MAX(2,1 + (int)( dist /resolution ));
		num = MIN(1000,MAX(num,fabs((double)(q_min[0]-q_max[0])/resolution)));
		double new_res = (q_max[j] - q_min[j]) / (num-1);

		for(size_t i=0;i<num;i++) {
			V_Configuration q(getDOFs());
			q[j] = q_min[j] + i*new_res;

			geometry_msgs::Pose pose = predictPose( q );
			model.track.pose_resampled.push_back(pose);
		}
	}
}

void GenericModel::keepLatestPoseOnly() {
	if(model.track.pose.size()<=1) return;
	model.track.pose.erase( model.track.pose.begin(), model.track.pose.begin() + (model.track.pose.size() -1) );
	for(size_t i = 0; i < model.track.channels.size(); i++) {
		model.track.channels[i].values.erase(
				model.track.channels[i].values.begin(),
				model.track.channels[i].values.begin() + (model.track.channels[i].values.size() -1)
		);
	}
}

double GenericModel::getInlierLogLikelihood( size_t index ) {
	geometry_msgs::Pose &pose_obs = model.track.pose[index];
	V_Configuration q_estimated = predictConfiguration(pose_obs);
	for(size_t i=0;i<(size_t)q_estimated.rows();i++) {
		model.track.channels[channelConfiguration[i]].values[index] =  q_estimated[i];
	}

	geometry_msgs::Pose pose_estimated = predictPose(q_estimated);
	model.track.pose_projected[index] = pose_estimated;

	tf::Transform diff = poseToTransform(pose_obs).inverseTimes( poseToTransform(pose_estimated) );
	double err_position = diff.getOrigin().length();
	double err_orientation = fabs(diff.getRotation().getAngle());

	double loglikelihood =
			- log(2*M_PI * sigma_position*sigma_orientation)
			- 0.5*(
					( SQR(err_position) / (SQR(sigma_position)) ) +
					( SQR(err_orientation) / SQR(sigma_orientation)) );	// 2-dim multivariate normal distribution
	return loglikelihood;
}

double GenericModel::getOutlierLogLikelihood() {
	// outside the 95% ellipsoid
	double chi2inv = 1.96;
//	chi2inv = 6.63;	//99% ellipsoid

	double err_position = chi2inv * sigma_position;
	double err_orientation = chi2inv * sigma_orientation;

	double loglikelihood =
			- log(2*M_PI * sigma_position*sigma_orientation)
			- 0.5*(
					( SQR(err_position) / (SQR(sigma_position)) ) +
					( SQR(err_orientation) / SQR(sigma_orientation)) );	// 2-dim multivariate normal distribution

	return( loglikelihood );
}

double GenericModel::getLogLikelihoodForPoseIndex(size_t index) {
//	double gamma_mixing = 0.1;
//	return (1-gamma_mixing)*getInlierLogLikelihood(pose_obs) + gamma_mixing * getOutlierLogLikelihood();
	double inlierLikelihood = getInlierLogLikelihood(index);
	double outlierLikelihood = getOutlierLogLikelihood();

//  mle-sac
	model.track.channels[channelInlierLogLikelihood].values[index] = inlierLikelihood;
	double pi = (1-outlier_ratio) * exp( inlierLikelihood );
	double po = outlier_ratio * exp( outlierLikelihood );
	model.track.channels[channelOutlier].values[index] =  po / (pi+po);
	double p = (log(exp(inlierLikelihood) + exp(outlierLikelihood)));
	model.track.channels[channelLogLikelihood].values[index] =  p;
	return p;

//	// ransac
//	return ((1-gamma_mixing)*inlierLikelihood + (gamma_mixing)*outlierLikelihood);

//	// m-sac
//	return (max(inlierLikelihood,outlierLikelihood));

}

double GenericModel::getLogLikelihood(bool estimate_outlier_ratio) {
	model.track.pose_projected.resize(model.track.pose.size());

	model.track.channels[channelInlierLogLikelihood].values.resize(model.track.pose.size());
	model.track.channels[channelOutlier].values.resize(model.track.pose.size());
	model.track.channels[channelLogLikelihood].values.resize(model.track.pose.size());
	for(size_t i=0;i<(size_t)getDOFs();i++) {
		model.track.channels[channelConfiguration[i]].values.resize(model.track.pose.size());
	}

	double sum_likelihood = 0;
	size_t n = getSamples();
	if(estimate_outlier_ratio) {
		outlier_ratio =0.5;
		for(size_t i=0;i<n;i++) {
			model.track.channels[channelOutlier].values[i] = 0.5;	// initial estimate of gamma
		}

		int iter = 0;
		double diff = 0;
		do{
			sum_likelihood = 0;
			for(size_t i=0;i<n;i++) {
				sum_likelihood += getLogLikelihoodForPoseIndex(i);
			}

			double outlier_ratio_new = 0;
			for(size_t i=0;i<n;i++) {
				outlier_ratio_new += model.track.channels[channelOutlier].values[i]/n;
			}
			diff = fabs(outlier_ratio - outlier_ratio_new);
			iter++;
			outlier_ratio = outlier_ratio_new;
//			cout <<"EM iter="<<iter<<" outlier_ratio="<<outlier_ratio<<endl;
		} while( diff>0.01 && iter<10 );
	} else {
		for(size_t i=0;i<n;i++) {
			sum_likelihood += getLogLikelihoodForPoseIndex(i);
		}
	}
	sum_likelihood += - prior_outlier_ratio * outlier_ratio * n;
	return sum_likelihood;
}

bool GenericModel::guessParameters() {
	// sample a minimum number of samples to compute parameters
	return false;
}

bool GenericModel::sampleConsensus() {
	// sample consensus
	writeParamsToModel();
	vector<articulation_msgs::ParamMsg> bestParams = model.params;
	double bestLikelihood = -DBL_MAX;
	bool goodGuess = false;
	for(size_t it=0;it<sac_iterations;it++) {
		if(!guessParameters()) continue;
		goodGuess = true;
		double likelihood = getLogLikelihood(true);
//		cout <<"RANSAC iteration="<<it<<", likelihood="<<likelihood<<endl;
		if(bestLikelihood < likelihood) {
			writeParamsToModel();
			bestParams = model.params;
			bestLikelihood = likelihood;
		}
	}
//	cout <<"RANSAC best likelihood="<<bestLikelihood<<endl;
	model.params = bestParams;
	readParamsFromModel();

	return goodGuess;
}

double
my_f (const gsl_vector *v, void *params)
{
  GenericModel *p = (GenericModel *)params;

  vector<double> delta( v->size );
  for(size_t i=0;i<v->size;i++) {
	  delta[i] = gsl_vector_get (v, i);
//	  cout <<"delta["<<i<<"]="<<delta[i]<<" ";
  }

  p->model.params = p->params_initial;
  p->readParamsFromModel();
  p->updateParameters(delta);
  p->writeParamsToModel();
  double likelihood = p->getLogLikelihood(true);
//  cout <<"likelihood="<<likelihood<<endl;
  return -likelihood;
}

/* The gradient of f, df = (df/dx, df/dy). */
void
my_df (const gsl_vector *v, void *params,
       gsl_vector *df)
{
	double likelihood = my_f(v,params);

	gsl_vector * v_delta =gsl_vector_alloc (v->size);

    double DELTA = 1e-3;
    for(size_t i=0;i<v->size;i++) {
        v_delta = gsl_vector_alloc (v->size);
        gsl_vector_memcpy(v_delta,v);
        gsl_vector_set(v_delta, i,gsl_vector_get(v, i)+DELTA);

    	double likelihood_delta = my_f(v_delta,params);
        gsl_vector_set(df, i,(likelihood_delta-likelihood)/DELTA);
	}
    gsl_vector_free (v_delta);
}

/* Compute both f and df together. */
void
my_fdf (const gsl_vector *x, void *params,
        double *f, gsl_vector *df)
{
  *f = my_f(x, params);
  my_df(x, params, df);
}

bool GenericModel::optimizeParameters() {
	// using gsl minimizer
	// dofs to optimize: complexity
	// stores current parameter vector as params_initial
	writeParamsToModel();
	params_initial = model.params;
	// calls updateParameters(..) to set new params vector

    size_t iter = 0;
    int status;

    const gsl_multimin_fdfminimizer_type *T;
    gsl_multimin_fdfminimizer *s;

    gsl_vector *x;
    gsl_multimin_function_fdf my_func;

    my_func.n = (int)complexity;
    my_func.f = my_f;
    my_func.df = my_df;
    my_func.fdf = my_fdf;
    my_func.params = this;

    x = gsl_vector_alloc ((int)complexity);
    gsl_vector_set_zero (x);
//    double likelihood_initial= -my_f(x,this);

    T = gsl_multimin_fdfminimizer_vector_bfgs2;
    s = gsl_multimin_fdfminimizer_alloc (T, (int)complexity);

    gsl_multimin_fdfminimizer_set (s, &my_func, x, 0.01, 0.1);

//    cout <<"optimizing "<<complexity<<" DOFs"<<endl;

//    for(size_t i=0;i<model.params.size();i++) {
//    	if(model.params[i].type != ParamMsg::PARAM) continue;
//    	cout <<" param "<<model.params[i].name<<"="<<model.params[i].value<<endl;
//    }

    if(optimizer_iterations>0) do
      {
        iter++;
//        cout <<"iter "<<iter<<".."<<endl;
        status = gsl_multimin_fdfminimizer_iterate (s);

        if (status)
          break;

        status = gsl_multimin_test_gradient (s->gradient, 1e-1);

//        if (status == GSL_SUCCESS)
//          printf ("Minimum found at:\n");

//        printf ("i=%5d ", (int)iter);
//        for(size_t i=0;i<s->x->size;i++) {
//            printf ("%5f ", gsl_vector_get(s->x,i));
//        }
//        printf("\n");
      }
    while (status == GSL_CONTINUE && iter < optimizer_iterations);

//    double likelihood_final = -my_f(s->x,this);
//    cout <<"likelihood_initial="<<likelihood_initial<<" ";
//    cout <<"likelihood_final="<<likelihood_final<<" ";
//    cout <<"after "<<iter<<" iterations";
//    cout << endl;

    gsl_multimin_fdfminimizer_free (s);
    gsl_vector_free (x);

	if(!fitMinMaxConfigurations())
		return false;

//	cout <<"type="<<getModelName()<<endl;
//    for(size_t i=0;i<model.params.size();i++) {
//    	if(model.params[i].type != ParamMsg::PARAM) continue;
//    	cout <<" param "<<model.params[i].name<<"="<<model.params[i].value<<endl;
//    }

    return true;
}

void GenericModel::updateParameters(std::vector<double> delta) {
}

bool GenericModel::normalizeParameters() {
	return true;
}

}


#include "SmallFieldVole.h"
#ifdef ENABLE_SOFTSPOKEN_OT

#include <cryptoTools/Common/Matrix.h>
#include <cryptoTools/Common/TestCollection.h>
#include <cryptoTools/Crypto/AES.h>
#include <cryptoTools/Crypto/Blake2.h>
#include "libOTe/Tools/SilentPprf.h"

#include <boost/log/core.hpp> // For BOOST_LOG_UNREACHABLE()

#ifdef BOOST_LOG_UNREACHABLE
#define UNREACHABLE() BOOST_LOG_UNREACHABLE()
#else
#define UNREACHABLE()
#endif

namespace osuCrypto
{
namespace SoftSpokenOT
{

// Output: v (or w) in inOut[1 ...], and u in inOut[0]. If blocks > 2**depth, this pattern repeats
// once every 2**depth blocks.
template<size_t depth, size_t blocks = superBlkSize>
static TRY_FORCEINLINE void xorReduce(block* BOOST_RESTRICT inOut, size_t maxDepth)
{
	assert((blocks & ((1 << depth) - 1)) == 0); // Can't reduce partial trees.

	// Force unroll outer loop using template recursion.
	xorReduce<depth - 1, (depth > 1 ? blocks : 0)>(inOut, maxDepth);

	if (depth <= maxDepth)
	{
		size_t stride = 1 << (depth - 1);
		for (size_t i = 0; i < blocks; i += 2 * stride)
		{
			for (size_t j = 0; j < depth; ++j)
				inOut[i + j] ^= inOut[i + stride + j];
			inOut[i + depth] = inOut[i + stride];
		}
	}
}

// Base case
template<> TRY_FORCEINLINE void xorReduce<0, 0>(block* BOOST_RESTRICT inOut, size_t maxDepth) {}

// outU is unused when this is called by the receiver, and a few operations might be saved by not
// computing it. However, it's just a few XORs, and the compiler will likely optimize it out anyway.
static TRY_FORCEINLINE void xorReducePath(
	size_t fieldBits, size_t fieldSize, size_t superBlk, block (*BOOST_RESTRICT path)[superBlkSize],
	block* BOOST_RESTRICT outU, block* BOOST_RESTRICT outVW, bool isReceiver,
	bool correctionPresent = false)
{
	// Reduce up the combining tree, continuing for as many nodes just got completed.
	// However, the root is skipped, in case it might have a different size.
	size_t treeDepth = 0;
	for (size_t blkInTree = superBlk; treeDepth < (fieldBits - 1) / superBlkShift
	     && blkInTree % superBlkSize == 0; ++treeDepth)
	{
		blkInTree /= superBlkSize;

		xorReduce<superBlkShift>(path[treeDepth], superBlkShift);

		for (size_t depth = 0; depth < superBlkShift; ++depth)
			outVW[treeDepth * superBlkShift + depth] ^= path[treeDepth][depth + 1];
		path[treeDepth + 1][(blkInTree - 1) % superBlkSize] = path[treeDepth][0];
	}

	// Reduce the root of the combining tree.
	if ((superBlk & (fieldSize - 1)) == 0)
	{
		size_t depthRemaining = 1 + (fieldBits - 1) % superBlkShift;
		xorReduce<superBlkShift>(path[treeDepth], depthRemaining);

		if (correctionPresent)
			for (size_t j = 0; j < depthRemaining; ++j)
				outVW[treeDepth * superBlkShift + j] ^= path[treeDepth][j + 1];
		else
			for (size_t j = 0; j < depthRemaining; ++j)
				outVW[treeDepth * superBlkShift + j] = path[treeDepth][j + 1];
		if (!isReceiver)
			*outU = path[treeDepth][0];
	}
}

template<size_t fieldBitsConst>
TRY_FORCEINLINE void SmallFieldVoleSender::generateImpl(
	size_t blockIdx, const AES& aes, block* BOOST_RESTRICT outU, block* BOOST_RESTRICT outV) const
{
	// Allow the compiler to hardcode fieldBits based on the template parameter.
	const size_t fieldBits = fieldBitsConst > 0 ? fieldBitsConst : this->fieldBits;
	constexpr size_t fieldBitsMax =
		fieldBitsConst > 0 ? fieldBitsConst : SmallFieldVoleSender::fieldBitsMax;
	const size_t fieldSize = 1 << fieldBits;

	block* BOOST_RESTRICT seeds = this->seeds.get();
	block blockIdxBlock = toBlock(blockIdx);

	if (fieldBits <= superBlkShift) // >= 1 VOLEs per superblock.
	{
		if (fieldBitsConst == 0)
			UNREACHABLE();

		const size_t volePerSuperBlk = superBlkSize / fieldSize;
		for (size_t nVole = 0; nVole < numVoles; nVole += volePerSuperBlk)
		{
			// TODO: Try combining the block index into the AES key. It gives the same PRG, but
			// might be faster.
			block input[superBlkSize], hashes[superBlkSize];
			for (size_t i = 0; i < superBlkSize; ++i, ++seeds)
				input[i] = blockIdxBlock ^ *seeds;
			aes.hashBlocks<superBlkSize>(input, hashes);

			xorReduce<superBlkShift>(hashes, fieldBits);
			for (size_t i = 0; i < volePerSuperBlk; ++i, ++outU)
			{
				for (size_t j = 0; j < fieldBits; ++j, ++outV)
					*outV = hashes[i * fieldSize + j + 1];
				*outU = hashes[i * fieldSize];
			}
		}
	}
	else // > 1 super block per VOLE.
	{
		for (size_t nVole = 0; nVole < numVoles; ++nVole, outV += fieldBits, ++outU)
		{
			block path[divCeil(fieldBitsMax, superBlkShift)][superBlkSize];
			for (size_t i = 0; i < fieldBits; ++i)
				// GCC seems to generate better code with an open coded memset.
				outV[i] = toBlock(0UL);

			#ifdef __GNUC__
			#pragma GCC unroll 4
			#endif
			for (size_t superBlk = 0; superBlk < fieldSize;)
			{
				block input[superBlkSize];
				for (size_t i = 0; i < superBlkSize; ++i, ++superBlk, ++seeds)
					input[i] = blockIdxBlock ^ *seeds;
				aes.hashBlocks<superBlkSize>(input, path[0]);
				xorReducePath(fieldBits, fieldSize, superBlk, path, outU, outV, false);
			}
		}
	}
}

template<size_t fieldBitsConst>
TRY_FORCEINLINE void SmallFieldVoleReceiver::generateImpl(
	size_t blockIdx, const AES& aes,
	block* BOOST_RESTRICT outW, const block* BOOST_RESTRICT correction) const
{
	// Allow the compiler to hardcode fieldBits based on the template parameter.
	const size_t fieldBits = fieldBitsConst > 0 ? fieldBitsConst : this->fieldBits;
	constexpr size_t fieldBitsMax =
		fieldBitsConst > 0 ? fieldBitsConst : SmallFieldVoleSender::fieldBitsMax;
	const size_t fieldSize = 1 << fieldBits;

	block* BOOST_RESTRICT seeds = this->seeds.get();
	block blockIdxBlock = toBlock(blockIdx);

	bool correctionPresent = (correction != nullptr);
	const u8* BOOST_RESTRICT deltaPtr = deltaUnpacked.get();

	if (fieldBits <= superBlkShift)
	{
		// >= 1 VOLEs per superblock. This case can only occur for small fieldSize, small enough
		// that this cannot be the variable size implementation (fieldBitsConst != 0).
		if (fieldBitsConst == 0)
			UNREACHABLE();

		// Avoid compilation trouble when this if branch is unreachable.
		constexpr size_t fieldBits_ = std::min(fieldBitsMax, (size_t) superBlkShift);

		// This is a trickier case, as we need to decide how many VOLES will fit
		// per super block, and how many AES calls to use. Try to get as close to 8 AES invocations
		// per superblock as possible.
		constexpr size_t aesPerVole = (1 << fieldBits_) - 1;
		constexpr size_t volePerSuperBlk = divNearest(superBlkSize, aesPerVole);
		constexpr size_t aesPerSuperBlk = aesPerVole * volePerSuperBlk;
		constexpr size_t fieldsPerSuperBlk = volePerSuperBlk << fieldBits_;

		for (size_t nVole = 0; nVole < numVoles; nVole += volePerSuperBlk,
		     correction += volePerSuperBlk, deltaPtr += fieldBits * volePerSuperBlk)
		{
			block input[aesPerSuperBlk], hashes[aesPerSuperBlk], xorHashes[fieldsPerSuperBlk];
			for (size_t i = 0; i < aesPerSuperBlk; ++i, ++seeds)
				input[i] = blockIdxBlock ^ *seeds;
			aes.hashBlocks<aesPerSuperBlk>(input, hashes);

			// Intersperse the hashes with zeros, because the zeroth seed for each VOLE is unknown.
			for (size_t i = 0; i < volePerSuperBlk; ++i)
			{
				xorHashes[i * fieldSize] = toBlock(0UL);
				for (size_t j = 0; j < aesPerVole; ++j)
					xorHashes[i * fieldSize + j + 1] = hashes[i * aesPerVole + j];
			}

			xorReduce<fieldBits_, fieldsPerSuperBlk>(xorHashes, fieldBits);
			if (correctionPresent)
				for (size_t i = 0; i < volePerSuperBlk; ++i)
					for (size_t j = 0; j < fieldBits; ++j, ++outW)
						*outW = xorHashes[i * fieldSize + j + 1] ^
							correction[i] & block::allSame(deltaPtr[i * fieldBits + j]);
			else
				for (size_t i = 0; i < volePerSuperBlk; ++i)
					for (size_t j = 0; j < fieldBits; ++j, ++outW)
						*outW = xorHashes[i * fieldSize + j + 1];
		}
	}
	else
	{
		// > 1 super block per VOLE. Do blocks of 8, or 7 at the start because the zeroth seed in a
		// VOLE is unknown.
		for (size_t nVole = 0; nVole < numVoles; ++nVole, outW += fieldBits, deltaPtr += fieldBits)
		{
			block path[divCeil(fieldBitsMax, superBlkShift)][superBlkSize];
			if (correctionPresent)
				for (size_t i = 0; i < fieldBits; ++i)
					outW[i] = correction[nVole] & block::allSame(deltaPtr[i]);
			else
				for (size_t i = 0; i < fieldBits; ++i)
					outW[i] = toBlock(0UL);

			block input0[superBlkSize -  1];
			for (size_t i = 0; i < superBlkSize - 1; ++i, ++seeds)
				input0[i] = blockIdxBlock ^ *seeds;
			aes.hashBlocks<superBlkSize - 1>(input0, &path[0][1]);

			// The zeroth seed is unknown, so set the corresponding path element to zero.
			path[0][0] = toBlock(0UL);

			size_t superBlk = superBlkSize;
			xorReducePath(fieldBits, fieldSize, superBlk, path, nullptr, outW, true);

			#ifdef __GNUC__
			#pragma GCC unroll 3
			#endif
			while (superBlk < fieldSize)
			{
				block input[superBlkSize];
				for (size_t i = 0; i < superBlkSize; ++i, ++superBlk, ++seeds)
					input[i] = blockIdxBlock ^ *seeds;
				aes.hashBlocks<superBlkSize>(input, path[0]);

				xorReducePath(fieldBits, fieldSize, superBlk, path, nullptr, outW,
				              true, correctionPresent);
			}
		}
	}
}

template<size_t fieldBitsConst, typename T, T Func>
struct SmallFieldVoleBase::call_member_func {};

template<size_t fieldBitsConst, typename Class, typename Return, typename... Params, Return (Class::*Func)(Params...) const>
struct SmallFieldVoleBase::call_member_func<fieldBitsConst, Return (Class::*)(Params...) const, Func>
{
	static Return call(const Class& this_, Params... params)
	{
		this_.template generateImpl<fieldBitsConst>(std::forward<Params>(params)...);
	}
};

SmallFieldVoleSender::SmallFieldVoleSender(size_t fieldBits_, size_t numVoles_) :
	SmallFieldVoleBase(fieldBits_, numVoles_),
	numVolesPadded(computeNumVolesPadded(fieldBits_, numVoles_)),
	generatePtr(selectGenerateImpl(fieldBits))
{
	seeds.reset(new block[numVolesPadded * fieldSize()]);
	std::fill_n(seeds.get(), numVolesPadded * fieldSize(), toBlock(0UL));
}

SmallFieldVoleReceiver::SmallFieldVoleReceiver(size_t fieldBits_, size_t numVoles_) :
	SmallFieldVoleBase(fieldBits_, numVoles_),
	numVolesPadded(computeNumVolesPadded(fieldBits_, numVoles_)),
	generatePtr(selectGenerateImpl(fieldBits))
{
	seeds.reset(new block[numVolesPadded * (fieldSize() - 1)]);
	std::fill_n(seeds.get(), numVolesPadded * (fieldSize() - 1), toBlock(0UL));
}

SmallFieldVoleSender::SmallFieldVoleSender(size_t fieldBits_, size_t numVoles_, span<const block> seeds_) :
	SmallFieldVoleSender(fieldBits_, numVoles_)
{
	size_t numSeeds = numVoles * fieldSize();
	if ((size_t) seeds_.size() != numSeeds)
		throw RTE_LOC;
	gsl::copy(seeds_, span<block>(seeds.get(), numSeeds));
}

SmallFieldVoleReceiver::SmallFieldVoleReceiver(size_t fieldBits_, size_t numVoles_, BitVector delta_) :
	SmallFieldVoleReceiver(fieldBits_, numVoles_)
{
	if ((size_t) delta_.size() != numBaseOTs())
		throw RTE_LOC;
	delta = std::move(delta_);
	deltaUnpacked.reset(new u8[wPadded()]);
	for (size_t i = 0; i < delta.size(); ++i)
		deltaUnpacked[i] = -(u8) delta[i];
}

SmallFieldVoleReceiver::SmallFieldVoleReceiver(size_t fieldBits_, size_t numVoles_,
	span<const block> seeds_, BitVector delta_) :
	SmallFieldVoleReceiver(fieldBits_, numVoles_, delta_)
{
	size_t numSeeds = numVoles * (fieldSize() - 1);
	if ((size_t) seeds_.size() != numSeeds)
		throw RTE_LOC;
	gsl::copy(seeds_, span<block>(seeds.get(), numSeeds));
}

SmallFieldVoleSender::SmallFieldVoleSender(size_t fieldBits_, size_t numVoles_, Channel& chl,
	PRNG& prng, span<const std::array<block, 2>> baseMessages, size_t numThreads, bool malicious) :
	SmallFieldVoleSender(fieldBits_, numVoles_)
{
	SilentMultiPprfSender pprf(fieldSize(), numVoles);
	pprf.setBase(baseMessages);

	MatrixView<block> seedView(seeds.get(), numVoles, fieldSize());
	pprf.expand(chl, span<const block>(), prng, seedView, PprfOutputFormat::BlockTransposed, numThreads);

	// Prove consistency
	if (malicious)
	{
		std::vector<std::array<block, 2>> corrections(numVoles, {block::allSame(0)});
		std::vector<std::array<block, 2>> hashes(numVoles, {block::allSame(0)});
		for (size_t row = 0; row < numVoles; ++row)
		{
			Blake2 hasher(2 * sizeof(block));

			for (size_t col = 0; col < fieldSize(); ++col)
			{
				Blake2 prg(3 * sizeof(block));
				std::array<block, 3> prgOut;
				prg.Update(seeds[row * fieldSize() + col]);
				prg.Final(prgOut);

				for (int i = 0; i < 2; ++i)
					corrections[row][i] ^= prgOut[i];
				hasher.Update(&prgOut[0], 2);
				seeds[row * fieldSize() + col] = prgOut[2];
			}

			// TODO: probably fine to hash together, not separately.
			hasher.Final(hashes[row]);
		}

		chl.asyncSend(std::move(corrections));
		chl.asyncSend(std::move(hashes));
	}
}

// The choice bits (as expected by SilentMultiPprfReceiver) store the locations of the complements
// of the active paths (because they tell which messages were transferred, not which ones weren't),
// in big endian. We want delta, which is the locations of the active paths, in little endian.
static BitVector choicesToDelta(const BitVector& choices, size_t fieldBits, size_t numVoles)
{
	if ((size_t) choices.size() != numVoles * fieldBits)
		throw RTE_LOC;

	BitVector delta(choices.size());
	for (size_t i = 0; i < numVoles; ++i)
		for (size_t j = 0; j < fieldBits; ++j)
			delta[i * fieldBits + j] = 1 ^ choices[(i + 1) * fieldBits - j - 1];
	return delta;
}

SmallFieldVoleReceiver::SmallFieldVoleReceiver(size_t fieldBits_, size_t numVoles_, Channel& chl,
	PRNG& prng, span<const block> baseMessages, BitVector choices, size_t numThreads,
	bool malicious) :
	SmallFieldVoleReceiver(fieldBits_, numVoles_, choicesToDelta(choices, fieldBits_, numVoles_))
{
	SilentMultiPprfReceiver pprf;
	pprf.configure(fieldSize(), numVoles);

	if (pprf.mDepth != fieldBits) // Sanity check: log2ceil.
		throw RTE_LOC;
	if (pprf.baseOtCount() != numBaseOTs()) // Sanity check
		throw RTE_LOC;

	pprf.setBase(baseMessages);
	pprf.setChoiceBits(PprfOutputFormat::BlockTransposed, choices);

	Matrix<block> seedsFull(numVoles, fieldSize());
	pprf.expand(chl, prng, seedsFull, PprfOutputFormat::BlockTransposed, false, numThreads);

	// Check consistency
	if (malicious)
	{
		std::vector<std::array<block, 2>> totals(numVoles, {block::allSame(0)});
		std::vector<std::array<block, 2>> entryHashes(numVoles * fieldSize(), {block::allSame(0)});
		block* seedMatrix = seedsFull.data();
		for (size_t row = 0; row < numVoles; ++row)
		{

			for (size_t col = 0; col < fieldSize(); ++col)
			{
				Blake2 prg(3 * sizeof(block));
				std::array<block, 3> prgOut;
				prg.Update(seedMatrix[row * fieldSize() + col]);
				prg.Final(prgOut);

				for (int i = 0; i < 2; ++i)
				{
					totals[row][i] ^= prgOut[i];
					entryHashes[row * fieldSize() + col][i] = prgOut[i];
				}
				seedMatrix[row * fieldSize() + col] = prgOut[2];
			}
		}

		std::vector<std::array<block, 2>> corrections(numVoles, {block::allSame(0)});
		std::vector<std::array<block, 2>> hashes(numVoles, {block::allSame(0)});
		chl.recv(&corrections[0], corrections.size());
		chl.recv(&hashes[0], hashes.size());

		int eq = 1;
		for (size_t row = 0; row < numVoles; ++row)
		{
			for (int i = 0; i < 2; ++i)
				corrections[row][i] ^= totals[row][i];

			size_t rowDelta = 0;
			for (size_t bit = 0; bit < fieldBits; ++bit)
				rowDelta |= (size_t) delta[row * fieldBits + bit] << bit;

			for (size_t col = 0; col < fieldSize(); ++col)
			{
				block isUnknownSeed = block::allSame(col == rowDelta);
				for (int i = 0; i < 2; ++i)
					entryHashes[row * fieldSize() + col][i] ^= isUnknownSeed & corrections[row][i];
			}

			Blake2 hasher(2 * sizeof(block));
			std::array<block, 2> hash;
			for (size_t col = 0; col < fieldSize(); ++col)
				hasher.Update(entryHashes[row * fieldSize() + col]);
			hasher.Final(hash);

			for (int i = 0; i < 2; ++i)
				eq &= (hash[i] == hashes[row][i]);
		}

		// TODO: Should delay abort until the VOLE consistency check, to stop the two events from
		// being distinguished.
		if (!eq)
			throw std::runtime_error("PPRF failed consistency check.");
	}

	// Reorder seeds to handle the (Delta ^) part, moving the unknown seeds to column 0. This
	// effectively makes it compute w = sum_x (Delta ^ x) * r_x instead of v = sum_x x * r_x.
	// TODO: Might be best to do this as part of going down the pprf tree. There, it would cost
	// O(numVoles * fieldSize) time, rather than O(numVoles * fieldSize * fieldBits).
	for (size_t i = 0; i < fieldBits; ++i)
	{
		size_t dMask = (size_t) 1 << i;
		block* matrix = seedsFull.data();
		// Bit hack to iterate over all values that are have a 0 in position i (from Hacker's
		// Delight).
		for (size_t col = 0; col < fieldSize(); col = (col + dMask + 1) & ~dMask)
			for (size_t row = 0; row < numVoles; ++row)
				cswapBytes(
					matrix[row * fieldSize() + col],
					matrix[row * fieldSize() + col + dMask],
					deltaUnpacked[row * fieldBits + i]);
	}

	// Remove the unknown seeds so that generate doesn't have to skip over them.
	MatrixView<block> seedView(seeds.get(), numVoles, fieldSize() - 1);
	for (size_t row = 0; row < numVoles; ++row)
		gsl::copy(seedsFull[row].subspan(1), seedView[row]);
}

// TODO: Malicious version. Should use an actual hash function for bottom layer of tree.

#define VOLE_GEN_FUNC(party, n) &SmallFieldVole##party::generateImpl<n>
#define VOLE_FIELD_BITS_TABLE(party, n) \
	&call_member_func<n, decltype(VOLE_GEN_FUNC(party, n)), VOLE_GEN_FUNC(party, n)>::call

decltype(SmallFieldVoleSender::generatePtr)
SmallFieldVoleSender::selectGenerateImpl(size_t fieldBits)
{
	static const decltype(generatePtr) table[] = {
		VOLE_FIELD_BITS_TABLE(Sender, 1),
		VOLE_FIELD_BITS_TABLE(Sender, 2),
		VOLE_FIELD_BITS_TABLE(Sender, 3),
		VOLE_FIELD_BITS_TABLE(Sender, 4),
		VOLE_FIELD_BITS_TABLE(Sender, 5),
		VOLE_FIELD_BITS_TABLE(Sender, 6),
		VOLE_FIELD_BITS_TABLE(Sender, 7),
		VOLE_FIELD_BITS_TABLE(Sender, 8),
		VOLE_FIELD_BITS_TABLE(Sender, 9),
		VOLE_FIELD_BITS_TABLE(Sender, 10)
	};
	if (fieldBits <= sizeof(table) / sizeof(table[0]))
		return table[fieldBits - 1];
	else
		return VOLE_FIELD_BITS_TABLE(Sender, 0);
}

decltype(SmallFieldVoleReceiver::generatePtr)
SmallFieldVoleReceiver::selectGenerateImpl(size_t fieldBits)
{
	static const decltype(generatePtr) table[] = {
		VOLE_FIELD_BITS_TABLE(Receiver, 1),
		VOLE_FIELD_BITS_TABLE(Receiver, 2),
		VOLE_FIELD_BITS_TABLE(Receiver, 3),
		VOLE_FIELD_BITS_TABLE(Receiver, 4),
		VOLE_FIELD_BITS_TABLE(Receiver, 5),
		VOLE_FIELD_BITS_TABLE(Receiver, 6),
		VOLE_FIELD_BITS_TABLE(Receiver, 7),
		VOLE_FIELD_BITS_TABLE(Receiver, 8),
		VOLE_FIELD_BITS_TABLE(Receiver, 9),
		VOLE_FIELD_BITS_TABLE(Receiver, 10)
	};
	if (fieldBits <= sizeof(table) / sizeof(table[0]))
		return table[fieldBits - 1];
	else
		return VOLE_FIELD_BITS_TABLE(Receiver, 0);
}

#undef VOLE_FIELD_BITS_CASE

void tests::xorReduction()
{
	const bool print = false;

	auto arrayAssertEq = [=](auto x, auto y) {
		if (print)
		{
			std::copy(x.begin(), x.end(), std::ostream_iterator<block>(std::cout, ", "));
			std::cout << '\n';
			std::copy(y.begin(), y.end(), std::ostream_iterator<block>(std::cout, ", "));
			std::cout << '\n';
		}

		if (x != y)
			throw UnitTestFail(LOCATION);
	};

	if (print)
		std::cout << '\n';
	const std::array<block, 8> testSuperBlock = {
		block::allSame(0x20), block::allSame(0x03), block::allSame(0xf9), block::allSame(0x2d),
		block::allSame(0x3a), block::allSame(0xb9), block::allSame(0xa3), block::allSame(0xbb)
	};

	std::array<block, 8> depth1 = testSuperBlock;
	const std::array<block, 8> desiredDepth1 = {
		block::allSame(0x23), block::allSame(0x03), block::allSame(0xd4), block::allSame(0x2d),
		block::allSame(0x83), block::allSame(0xb9), block::allSame(0x18), block::allSame(0xbb)
	};
	xorReduce<3>(depth1.data(), 1);
	arrayAssertEq(depth1, desiredDepth1);

	std::array<block, 8> depth2 = testSuperBlock;
	const std::array<block, 8> desiredDepth2 = {
		block::allSame(0xf7), block::allSame(0x2e), block::allSame(0xd4), block::allSame(0x2d),
		block::allSame(0x9b), block::allSame(0x02), block::allSame(0x18), block::allSame(0xbb)
	};
	xorReduce<3>(depth2.data(), 2);
	arrayAssertEq(depth2, desiredDepth2);

	std::array<block, 8> depth3 = testSuperBlock;
	const std::array<block, 8> desiredDepth3 = {
		block::allSame(0x6c), block::allSame(0x2c), block::allSame(0xcc), block::allSame(0x9b),
		block::allSame(0x9b), block::allSame(0x02), block::allSame(0x18), block::allSame(0xbb)
	};
	xorReduce<3>(depth3.data(), 3);
	arrayAssertEq(depth3, desiredDepth3);
}

}
}

#endif