// GlobalRegistration.cpp : Defines the entry point for the console application.
//

#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/common/transforms.h>
#include <pcl/common/distances.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>

#include "GlobalRegistration/Helper.h"
#include "GlobalRegistration/RansacCurvature.h"

// Types
typedef pcl::PointXYZ PointT;
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointCloudT;
typedef pcl::FPFHSignature33 FeatureT;
typedef pcl::FPFHEstimationOMP<PointNT, PointNT, FeatureT> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;

std::string dir_name = "";

void do_all(int num) {
    Configuration config;

    RGBDTrajectory traj;
    RGBDInformation info;
#pragma omp parallel for schedule(dynamic)
    for (int i = 0; i < num; i++) {
        for (int j = i + 1; j < num; j++) {
            // Load object and scene
            pcl::console::print_highlight("Between fragments %d and %d.\n", i,
                                          j);
            bool smart_swapped = false;

            PointCloudT::Ptr object(new PointCloudT);
            PointCloudT::Ptr object_aligned(new PointCloudT);
            PointCloudT::Ptr scene(new PointCloudT);
            FeatureCloudT::Ptr object_features(new FeatureCloudT);
            FeatureCloudT::Ptr scene_features(new FeatureCloudT);
            pcl::console::print_highlight("Loading point clouds...\n");

            char filename[1024];
            sprintf(filename, "%scloud_bin_%d.pcd", dir_name.c_str(), i);
            pcl::io::loadPCDFile<PointNT>(filename, *scene);

            sprintf(filename, "%scloud_bin_%d.pcd", dir_name.c_str(), j);
            pcl::io::loadPCDFile<PointNT>(filename, *object);

            const float leaf = config.resample_leaf_;

            // Downsample
            pcl::console::print_highlight("Downsampling...\n");
            pcl::VoxelGrid<PointNT> grid;
            grid.setLeafSize(leaf, leaf, leaf);
            grid.setInputCloud(object);
            grid.filter(*object);
            grid.setInputCloud(scene);
            grid.filter(*scene);

            if (config.smart_swap_) {
                if (object->size() > scene->size()) {
                    PointCloudT::Ptr temp = object;
                    object = scene;
                    scene = temp;
                    smart_swapped = true;
                } else {
                    smart_swapped = false;
                }
            }

            if (config.estimate_normal_) {
                PointCloudT::Ptr scene_bak(new PointCloudT);
                pcl::copyPointCloud(*scene, *scene_bak);
                PointCloudT::Ptr object_bak(new PointCloudT);
                pcl::copyPointCloud(*object, *object_bak);

                // Estimate normals for scene
                pcl::console::print_highlight("Estimating scene normals...\n");
                pcl::NormalEstimationOMP<PointNT, PointNT> nest;
                nest.setRadiusSearch(config.normal_radius_);
                nest.setInputCloud(scene);
                nest.compute(*scene);
                for (int i = 0; i < scene->size(); i++) {
                    if (scene->points[i].normal_x *
                                scene_bak->points[i].normal_x +
                            scene->points[i].normal_y *
                                scene_bak->points[i].normal_y +
                            scene->points[i].normal_z *
                                scene_bak->points[i].normal_z <
                        0.0) {
                        scene->points[i].normal_x *= -1;
                        scene->points[i].normal_y *= -1;
                        scene->points[i].normal_z *= -1;
                    }
                }

                // Estimate normals for object
                pcl::console::print_highlight("Estimating object normals...\n");
                nest.setRadiusSearch(config.normal_radius_);
                nest.setInputCloud(object);
                nest.compute(*object);
                for (int i = 0; i < object->size(); i++) {
                    if (object->points[i].normal_x *
                                object_bak->points[i].normal_x +
                            object->points[i].normal_y *
                                object_bak->points[i].normal_y +
                            object->points[i].normal_z *
                                object_bak->points[i].normal_z <
                        0.0) {
                        object->points[i].normal_x *= -1;
                        object->points[i].normal_y *= -1;
                        object->points[i].normal_z *= -1;
                    }
                }
            }

            // Estimate features
            pcl::console::print_highlight("Estimating features...\n");
            FeatureEstimationT fest;
            fest.setRadiusSearch(config.feature_radius_);
            fest.setInputCloud(object);
            fest.setInputNormals(object);
            fest.compute(*object_features);
            fest.setInputCloud(scene);
            fest.setInputNormals(scene);
            fest.compute(*scene_features);

            pcl::console::print_highlight("Starting alignment...\n");
            RansacCurvature<PointNT, PointNT, FeatureT> align;
            align.setInputCloud(object);
            align.setSourceFeatures(object_features);
            align.setInputTarget(scene);
            align.setTargetFeatures(scene_features);
            align.setMaximumIterations(
                config.max_iteration_); // Number of RANSAC iterations
            align.setNumberOfSamples(
                config.num_of_samples_); // Number of points to sample for
                                         // generating/prerejecting a pose
            align.setCorrespondenceRandomness(
                config.correspondence_randomness_); // Number of nearest
                                                    // features to use
            align.setSimilarityThreshold(
                config.edge_similarity_); // Polygonal edge length similarity
                                          // threshold
            align.setMaxCorrespondenceDistance(
                config.max_correspondence_distance_); // Inlier threshold
            align.setInlierFraction(
                config.inlier_fraction_); // Required inlier fraction for
                                          // accepting a pose hypothesis
            align.setInlierNumber(config.inlier_number_);
            align.setAngleDiff(config.angle_difference_);
            {
                pcl::ScopeTime t("Alignment");
                align.align(*object_aligned);
            }

            if (align.hasConverged())
            // if (true)
            {
                align.getInformation();
                if (config.aux_data_) {
                    char test[1024];
                    sprintf(test, "test_%d_%d.txt", i, j);
                    align.writeAuxData(std::string(test));
                }
                Eigen::Matrix4f transformation;
                Eigen::Matrix<double, 6, 6> information;
                if (smart_swapped) {
                    transformation = align.getFinalTransformation().inverse();
                    information = align.information_target_;
                } else {
                    transformation = align.getFinalTransformation();
                    information = align.information_source_;
                }
#pragma omp critical
                {
                    // Print results
                    pcl::console::print_info("\n");
                    pcl::console::print_info(
                        "    | %6.3f %6.3f %6.3f | \n", transformation(0, 0),
                        transformation(0, 1), transformation(0, 2));
                    pcl::console::print_info(
                        "R = | %6.3f %6.3f %6.3f | \n", transformation(1, 0),
                        transformation(1, 1), transformation(1, 2));
                    pcl::console::print_info(
                        "    | %6.3f %6.3f %6.3f | \n", transformation(2, 0),
                        transformation(2, 1), transformation(2, 2));
                    pcl::console::print_info("\n");
                    pcl::console::print_info(
                        "t = < %0.3f, %0.3f, %0.3f >\n", transformation(0, 3),
                        transformation(1, 3), transformation(2, 3));
                    pcl::console::print_info("\n");
                    pcl::console::print_info("Inliers: %i/%i\n",
                                             align.getInliers().size(),
                                             object->size());
                    traj.data_.push_back(FramedTransformation(
                        i, j, num, transformation.cast<double>()));
                    info.data_.push_back(
                        FramedInformation(i, j, num, information));
                }
            } else {
                pcl::console::print_error("Alignment failed!\n");
            }
        }
    }
    traj.SaveToFile("result.txt");
    info.SaveToFile("result.info");
}

int fragment;
RGBDTrajectory segment_traj;
RGBDTrajectory init_traj;
RGBDTrajectory pose_traj;
RGBDTrajectory odometry_traj;
RGBDInformation odometry_info;

void create_init_traj() {
    init_traj.data_.clear();
    Eigen::Matrix4d base_mat = Eigen::Matrix4d::Identity();
    for (size_t i = 0; i < segment_traj.data_.size(); i++) {
        if (i % fragment == 0 && i > 0) {
            base_mat = init_traj.data_[i - 1].transformation_ *
                       segment_traj.data_[i].transformation_.inverse();
        }
        init_traj.data_.push_back(FramedTransformation(
            i, i, i + 1, base_mat * segment_traj.data_[i].transformation_));
    }
    init_traj.SaveToFile("init.log");
}

void create_pose_traj() {
    pose_traj.data_.clear();
    for (size_t i = 0; i < init_traj.data_.size(); i += fragment) {
        pose_traj.data_.push_back(FramedTransformation(
            i / fragment, i / fragment, i / fragment + 1,
            init_traj.data_[i].transformation_ *
                segment_traj.data_[0].transformation_.inverse()));
    }
    pose_traj.SaveToFile("pose.log");
}

void create_odometry(int num) {
    Configuration config;
    odometry_traj.data_.clear();
    odometry_info.data_.clear();
    for (int i = 1; i < num; i++) {
        odometry_traj.data_.push_back(FramedTransformation(
            i - 1, i, num,
            pose_traj.data_[i - 1].transformation_.inverse() *
                pose_traj.data_[i].transformation_));
        pcl::console::print_highlight("Betweeen fragments %d and %d.\n", i - 1,
                                      i);
        PointCloudT::Ptr object(new PointCloudT);
        PointCloudT::Ptr object_aligned(new PointCloudT);
        PointCloudT::Ptr scene(new PointCloudT);
        FeatureCloudT::Ptr object_features(new FeatureCloudT);
        FeatureCloudT::Ptr scene_features(new FeatureCloudT);
        pcl::console::print_highlight("Loading point clouds...\n");

        char filename[1024];
        sprintf(filename, "%scloud_bin_%d.pcd", dir_name.c_str(), i - 1);
        pcl::io::loadPCDFile<PointNT>(filename, *scene);

        sprintf(filename, "%scloud_bin_%d.pcd", dir_name.c_str(), i);
        pcl::io::loadPCDFile<PointNT>(filename, *object);

        const float leaf = config.resample_leaf_;

        // Downsample
        pcl::console::print_highlight("Downsampling...\n");
        pcl::VoxelGrid<PointNT> grid;
        grid.setLeafSize(leaf, leaf, leaf);
        grid.setInputCloud(object);
        grid.filter(*object);
        grid.setInputCloud(scene);
        grid.filter(*scene);

        if (config.estimate_normal_) {
            PointCloudT::Ptr scene_bak(new PointCloudT);
            pcl::copyPointCloud(*scene, *scene_bak);
            PointCloudT::Ptr object_bak(new PointCloudT);
            pcl::copyPointCloud(*object, *object_bak);

            // Estimate normals for scene
            pcl::console::print_highlight("Estimating scene normals...\n");
            pcl::NormalEstimationOMP<PointNT, PointNT> nest;
            nest.setRadiusSearch(config.normal_radius_);
            nest.setInputCloud(scene);
            nest.compute(*scene);
            for (int i = 0; i < scene->size(); i++) {
                if (scene->points[i].normal_x * scene_bak->points[i].normal_x +
                        scene->points[i].normal_y *
                            scene_bak->points[i].normal_y +
                        scene->points[i].normal_z *
                            scene_bak->points[i].normal_z <
                    0.0) {
                    scene->points[i].normal_x *= -1;
                    scene->points[i].normal_y *= -1;
                    scene->points[i].normal_z *= -1;
                }
            }

            // Estimate normals for object
            pcl::console::print_highlight("Estimating object normals...\n");
            nest.setRadiusSearch(config.normal_radius_);
            nest.setInputCloud(object);
            nest.compute(*object);
            for (int i = 0; i < object->size(); i++) {
                if (object->points[i].normal_x *
                            object_bak->points[i].normal_x +
                        object->points[i].normal_y *
                            object_bak->points[i].normal_y +
                        object->points[i].normal_z *
                            object_bak->points[i].normal_z <
                    0.0) {
                    object->points[i].normal_x *= -1;
                    object->points[i].normal_y *= -1;
                    object->points[i].normal_z *= -1;
                }
            }
        }

        // Estimate features
        pcl::console::print_highlight("Estimating features...\n");
        FeatureEstimationT fest;
        fest.setRadiusSearch(config.feature_radius_);
        fest.setInputCloud(object);
        fest.setInputNormals(object);
        fest.compute(*object_features);
        fest.setInputCloud(scene);
        fest.setInputNormals(scene);
        fest.compute(*scene_features);

        pcl::console::print_highlight("Starting alignment...\n");
        RansacCurvature<PointNT, PointNT, FeatureT> align;
        align.setInputCloud(object);
        align.setSourceFeatures(object_features);
        align.setInputTarget(scene);
        align.setTargetFeatures(scene_features);
        align.setMaximumIterations(
            config.max_iteration_); // Number of RANSAC iterations
        align.setNumberOfSamples(
            config.num_of_samples_); // Number of points to sample for
                                     // generating/prerejecting a pose
        align.setCorrespondenceRandomness(
            config.correspondence_randomness_); // Number of nearest features to
                                                // use
        align.setSimilarityThreshold(
            config.edge_similarity_); // Polygonal edge length similarity
                                      // threshold
        align.setMaxCorrespondenceDistance(
            config.max_correspondence_distance_); // Inlier threshold
        align.setInlierFraction(
            config.inlier_fraction_); // Required inlier fraction for accepting
                                      // a pose hypothesis
        align.setInlierNumber(config.inlier_number_);
        align.setAngleDiff(config.angle_difference_);
        {
            pcl::ScopeTime t("Alignment");
            // align.align (*object_aligned);
            align.align_redux(
                *object_aligned,
                odometry_traj.data_[i - 1].transformation_.cast<float>());
        }

        align.getInformation();
        odometry_info.data_.push_back(
            FramedInformation(i - 1, i, num, align.information_source_));
    }
    odometry_traj.SaveToFile("odometry.log");
    odometry_info.SaveToFile("odometry.info");
}

int main(int argc, char *argv[]) {
    if (argc < 2) {
        std::cout << "Usage : " << std::endl;
        std::cout << "    GlobalRegistration.exe <dir>" << std::endl;
        std::cout
            << "    GlobalRegistration.exe <dir> <100-0.log> <segment_length>"
            << std::endl;
        return 0;
    }
    dir_name = std::string(argv[1]);
    int num_of_pcds =
        std::count_if(boost::filesystem::directory_iterator(
                          boost::filesystem::path(dir_name)),
                      boost::filesystem::directory_iterator(),
                      [](const boost::filesystem::directory_entry &e) {
                          return e.path().extension() == ".pcd";
                      });
    std::cout << num_of_pcds << " detected." << endl << std::endl;

    if (argc == 2) {
        do_all(num_of_pcds);
    }

    if (argc == 4) {
        segment_traj.LoadFromFile(argv[2]);
        sscanf(argv[3], "%d", &fragment);

        std::cout << "To run: create_init_traj()" << std::endl;
        create_init_traj();
        std::cout << "Done: create_init_traj()" << std::endl;

        std::cout << "To run: create_pose_traj()" << std::endl;
        create_pose_traj();
        std::cout << "Done: create_pose_traj()" << std::endl;

        std::cout << "To run: create_odometry()" << std::endl;
        create_odometry(num_of_pcds);
        std::cout << "Done: create_odometry()" << std::endl;

        if (boost::filesystem::exists("result.txt")) {
            std::cout << "result.txt detected. skip global registration."
                      << std::endl;
        } else {
            do_all(num_of_pcds);
        }
    }

    return 0;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2014 Navdeep Jaitly <ndjaitly@google.com and
//                    Benoit Steiner <benoit.steiner.goog@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;
using Eigen::array;

template <int DataLayout>
static void test_simple_reverse()
{
  Tensor<float, 4, DataLayout> tensor(2,3,5,7);
  tensor.setRandom();

  array<bool, 4> dim_rev;
  dim_rev[0] = false;
  dim_rev[1] = true;
  dim_rev[2] = true;
  dim_rev[3] = false;

  Tensor<float, 4, DataLayout> reversed_tensor;
  reversed_tensor = tensor.reverse(dim_rev);

  VERIFY_IS_EQUAL(reversed_tensor.dimension(0), 2);
  VERIFY_IS_EQUAL(reversed_tensor.dimension(1), 3);
  VERIFY_IS_EQUAL(reversed_tensor.dimension(2), 5);
  VERIFY_IS_EQUAL(reversed_tensor.dimension(3), 7);

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 5; ++k) {
        for (int l = 0; l < 7; ++l) {
          VERIFY_IS_EQUAL(tensor(i,j,k,l), reversed_tensor(i,2-j,4-k,l));
        }
      }
    }
  }

  dim_rev[0] = true;
  dim_rev[1] = false;
  dim_rev[2] = false;
  dim_rev[3] = false;

  reversed_tensor = tensor.reverse(dim_rev);

  VERIFY_IS_EQUAL(reversed_tensor.dimension(0), 2);
  VERIFY_IS_EQUAL(reversed_tensor.dimension(1), 3);
  VERIFY_IS_EQUAL(reversed_tensor.dimension(2), 5);
  VERIFY_IS_EQUAL(reversed_tensor.dimension(3), 7);

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 5; ++k) {
        for (int l = 0; l < 7; ++l) {
          VERIFY_IS_EQUAL(tensor(i,j,k,l), reversed_tensor(1-i,j,k,l));
        }
      }
    }
  }

  dim_rev[0] = true;
  dim_rev[1] = false;
  dim_rev[2] = false;
  dim_rev[3] = true;

  reversed_tensor = tensor.reverse(dim_rev);

  VERIFY_IS_EQUAL(reversed_tensor.dimension(0), 2);
  VERIFY_IS_EQUAL(reversed_tensor.dimension(1), 3);
  VERIFY_IS_EQUAL(reversed_tensor.dimension(2), 5);
  VERIFY_IS_EQUAL(reversed_tensor.dimension(3), 7);

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 5; ++k) {
        for (int l = 0; l < 7; ++l) {
          VERIFY_IS_EQUAL(tensor(i,j,k,l), reversed_tensor(1-i,j,k,6-l));
        }
      }
    }
  }
}

template <int DataLayout>
static void test_expr_reverse(bool LValue)
{
  Tensor<float, 4, DataLayout> tensor(2,3,5,7);
  tensor.setRandom();

  array<bool, 4> dim_rev;
  dim_rev[0] = false;
  dim_rev[1] = true;
  dim_rev[2] = false;
  dim_rev[3] = true;

  Tensor<float, 4, DataLayout> expected(2, 3, 5, 7);
  if (LValue) {
    expected.reverse(dim_rev) = tensor;
  } else {
    expected = tensor.reverse(dim_rev);
  }

  Tensor<float, 4, DataLayout> result(2,3,5,7);

  array<ptrdiff_t, 4> src_slice_dim;
  src_slice_dim[0] = 2;
  src_slice_dim[1] = 3;
  src_slice_dim[2] = 1;
  src_slice_dim[3] = 7;
  array<ptrdiff_t, 4> src_slice_start;
  src_slice_start[0] = 0;
  src_slice_start[1] = 0;
  src_slice_start[2] = 0;
  src_slice_start[3] = 0;
  array<ptrdiff_t, 4> dst_slice_dim = src_slice_dim;
  array<ptrdiff_t, 4> dst_slice_start = src_slice_start;

  for (int i = 0; i < 5; ++i) {
    if (LValue) {
      result.slice(dst_slice_start, dst_slice_dim).reverse(dim_rev) =
          tensor.slice(src_slice_start, src_slice_dim);
    } else {
      result.slice(dst_slice_start, dst_slice_dim) =
          tensor.slice(src_slice_start, src_slice_dim).reverse(dim_rev);
    }
    src_slice_start[2] += 1;
    dst_slice_start[2] += 1;
  }

  VERIFY_IS_EQUAL(result.dimension(0), 2);
  VERIFY_IS_EQUAL(result.dimension(1), 3);
  VERIFY_IS_EQUAL(result.dimension(2), 5);
  VERIFY_IS_EQUAL(result.dimension(3), 7);

  for (int i = 0; i < expected.dimension(0); ++i) {
    for (int j = 0; j < expected.dimension(1); ++j) {
      for (int k = 0; k < expected.dimension(2); ++k) {
        for (int l = 0; l < expected.dimension(3); ++l) {
          VERIFY_IS_EQUAL(result(i,j,k,l), expected(i,j,k,l));
        }
      }
    }
  }

  dst_slice_start[2] = 0;
  result.setRandom();
  for (int i = 0; i < 5; ++i) {
     if (LValue) {
       result.slice(dst_slice_start, dst_slice_dim).reverse(dim_rev) =
           tensor.slice(dst_slice_start, dst_slice_dim);
     } else {
       result.slice(dst_slice_start, dst_slice_dim) =
           tensor.reverse(dim_rev).slice(dst_slice_start, dst_slice_dim);
     }
    dst_slice_start[2] += 1;
  }

  for (int i = 0; i < expected.dimension(0); ++i) {
    for (int j = 0; j < expected.dimension(1); ++j) {
      for (int k = 0; k < expected.dimension(2); ++k) {
        for (int l = 0; l < expected.dimension(3); ++l) {
          VERIFY_IS_EQUAL(result(i,j,k,l), expected(i,j,k,l));
        }
      }
    }
  }
}

EIGEN_DECLARE_TEST(cxx11_tensor_reverse)
{
  CALL_SUBTEST(test_simple_reverse<ColMajor>());
  CALL_SUBTEST(test_simple_reverse<RowMajor>());
  CALL_SUBTEST(test_expr_reverse<ColMajor>(true));
  CALL_SUBTEST(test_expr_reverse<RowMajor>(true));
  CALL_SUBTEST(test_expr_reverse<ColMajor>(false));
  CALL_SUBTEST(test_expr_reverse<RowMajor>(false));
}


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <stdexcept>
#include <functional>
#include <random>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include <uncertainty_planning_core/uncertainty_planning_core.hpp>
#include <uncertainty_planning_examples/config_common.hpp>
#include <uncertainty_planning_examples/se2_common_config.hpp>
#include <uncertainty_planning_examples/se3_common_config.hpp>
#include <uncertainty_planning_examples/baxter_common_config.hpp>
#include <uncertainty_planning_examples/ur5_common_config.hpp>
#include <uncertainty_planning_examples/iiwa7_common_config.hpp>
#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>

using namespace uncertainty_contact_planning;

void demonstrate_se3(ros::Publisher& display_debug_publisher)
{
    std::cout << "Demonstrating SE(3)..." << std::endl;
    const uncertainty_planning_core::PLANNING_AND_EXECUTION_OPTIONS options = se3_common_config::GetOptions();
    const config_common::TASK_CONFIG_PARAMS extra_options = se3_common_config::GetExtraOptions();
    std::cout << PrettyPrint::PrettyPrint(options) << "\n" << PrettyPrint::PrettyPrint(extra_options) << std::endl;
    const std::pair<uncertainty_planning_core::SE3Config, uncertainty_planning_core::SE3Config> start_and_goal = se3_common_config::GetStartAndGoal();
    const uncertainty_planning_core::SE3SamplerPtr sampler = se3_common_config::GetSampler();
    const simple_robot_models::SE3_ROBOT_CONFIG robot_config = se3_common_config::GetDefaultRobotConfig(extra_options);
    const uncertainty_planning_core::SE3Robot robot = se3_common_config::GetRobot(robot_config);
    const uncertainty_planning_core::SE3SimulatorPtr simulator = se3_common_config::GetSimulator(extra_options, options.debug_level);
    uncertainty_planning_core::DemonstrateSE3Simulator(options, robot, simulator, sampler, start_and_goal.first, start_and_goal.second, display_debug_publisher);
}

void demonstrate_se2(ros::Publisher& display_debug_publisher)
{
    std::cout << "Demonstrating SE(2)..." << std::endl;
    const uncertainty_planning_core::PLANNING_AND_EXECUTION_OPTIONS options = se2_common_config::GetOptions();
    const config_common::TASK_CONFIG_PARAMS extra_options = se2_common_config::GetExtraOptions();
    std::cout << PrettyPrint::PrettyPrint(options) << "\n" << PrettyPrint::PrettyPrint(extra_options) << std::endl;
    const std::pair<uncertainty_planning_core::SE2Config, uncertainty_planning_core::SE2Config> start_and_goal = se2_common_config::GetStartAndGoal();
    const uncertainty_planning_core::SE2SamplerPtr sampler = se2_common_config::GetSampler();
    const simple_robot_models::SE2_ROBOT_CONFIG robot_config = se2_common_config::GetDefaultRobotConfig(extra_options);
    const uncertainty_planning_core::SE2Robot robot = se2_common_config::GetRobot(robot_config);
    const uncertainty_planning_core::SE2SimulatorPtr simulator = se2_common_config::GetSimulator(extra_options, options.debug_level);
    uncertainty_planning_core::DemonstrateSE2Simulator(options, robot, simulator, sampler, start_and_goal.first, start_and_goal.second, display_debug_publisher);
}

void demonstrate_baxter(ros::Publisher& display_debug_publisher)
{
    std::cout << "Demonstrating Baxter..." << std::endl;
    const uncertainty_planning_core::PLANNING_AND_EXECUTION_OPTIONS options = baxter_linked_common_config::GetOptions();
    const config_common::TASK_CONFIG_PARAMS extra_options = baxter_linked_common_config::GetExtraOptions();
    std::cout << PrettyPrint::PrettyPrint(options) << "\n" << PrettyPrint::PrettyPrint(extra_options) << std::endl;
    const std::vector<double> joint_uncertainty_params = baxter_linked_common_config::GetJointUncertaintyParams(extra_options);
    assert(joint_uncertainty_params.size() == 7);
    const std::vector<double> joint_distance_weights = baxter_linked_common_config::GetJointDistanceWeights();
    assert(joint_distance_weights.size() == 7);
    const std::pair<uncertainty_planning_core::LinkedConfig, uncertainty_planning_core::LinkedConfig> start_and_goal = baxter_linked_common_config::GetStartAndGoal();
    const uncertainty_planning_core::LinkedSamplerPtr sampler = baxter_linked_common_config::GetSampler();
    const simple_robot_models::LINKED_ROBOT_CONFIG robot_config = baxter_linked_common_config::GetDefaultRobotConfig(extra_options);
    const Eigen::Isometry3d base_transform = baxter_linked_common_config::GetBaseTransform();
    const uncertainty_planning_core::LinkedRobot robot = baxter_linked_common_config::GetRobot(base_transform, robot_config, joint_uncertainty_params, joint_distance_weights, extra_options.environment_id);
    const uncertainty_planning_core::LinkedSimulatorPtr simulator = baxter_linked_common_config::GetSimulator(extra_options, options.debug_level);
    uncertainty_planning_core::DemonstrateLinkedSimulator(options, robot, simulator, sampler, start_and_goal.first, start_and_goal.second, display_debug_publisher);
}

void demonstrate_ur5(ros::Publisher& display_debug_publisher)
{
    std::cout << "Demonstrating UR5..." << std::endl;
    const uncertainty_planning_core::PLANNING_AND_EXECUTION_OPTIONS options = ur5_linked_common_config::GetOptions();
    const config_common::TASK_CONFIG_PARAMS extra_options = ur5_linked_common_config::GetExtraOptions();
    std::cout << PrettyPrint::PrettyPrint(options) << "\n" << PrettyPrint::PrettyPrint(extra_options) << std::endl;
    const std::vector<double> joint_uncertainty_params = ur5_linked_common_config::GetJointUncertaintyParams(extra_options);
    assert(joint_uncertainty_params.size() == 6);
    const std::vector<double> joint_distance_weights = ur5_linked_common_config::GetJointDistanceWeights();
    assert(joint_distance_weights.size() == 6);
    const std::pair<uncertainty_planning_core::LinkedConfig, uncertainty_planning_core::LinkedConfig> start_and_goal = ur5_linked_common_config::GetStartAndGoal();
    const uncertainty_planning_core::LinkedSamplerPtr sampler = ur5_linked_common_config::GetSampler();
    const simple_robot_models::LINKED_ROBOT_CONFIG robot_config = ur5_linked_common_config::GetDefaultRobotConfig(extra_options);
    const Eigen::Isometry3d base_transform = ur5_linked_common_config::GetBaseTransform();
    const uncertainty_planning_core::LinkedRobot robot = ur5_linked_common_config::GetRobot(base_transform, robot_config, joint_uncertainty_params, joint_distance_weights);
    const uncertainty_planning_core::LinkedSimulatorPtr simulator = ur5_linked_common_config::GetSimulator(extra_options, options.debug_level);
    uncertainty_planning_core::DemonstrateLinkedSimulator(options, robot, simulator, sampler, start_and_goal.first, start_and_goal.second, display_debug_publisher);
}

void demonstrate_iiwa7(ros::Publisher& display_debug_publisher)
{
    std::cout << "Demonstrating IIWA7..." << std::endl;
    const uncertainty_planning_core::PLANNING_AND_EXECUTION_OPTIONS options = iiwa7_linked_common_config::GetOptions();
    const config_common::TASK_CONFIG_PARAMS extra_options = iiwa7_linked_common_config::GetExtraOptions();
    std::cout << PrettyPrint::PrettyPrint(options) << "\n" << PrettyPrint::PrettyPrint(extra_options) << std::endl;
    const std::vector<double> joint_uncertainty_params = iiwa7_linked_common_config::GetJointUncertaintyParams(extra_options);
    assert(joint_uncertainty_params.size() == 7);
    const std::vector<double> joint_distance_weights = iiwa7_linked_common_config::GetJointDistanceWeights();
    assert(joint_distance_weights.size() == 7);
    const std::pair<uncertainty_planning_core::LinkedConfig, uncertainty_planning_core::LinkedConfig> start_and_goal = iiwa7_linked_common_config::GetStartAndGoal();
    const uncertainty_planning_core::LinkedSamplerPtr sampler = iiwa7_linked_common_config::GetSampler();
    const simple_robot_models::LINKED_ROBOT_CONFIG robot_config = iiwa7_linked_common_config::GetDefaultRobotConfig(extra_options);
    const Eigen::Isometry3d base_transform = iiwa7_linked_common_config::GetBaseTransform();
    const uncertainty_planning_core::LinkedRobot robot = iiwa7_linked_common_config::GetRobot(base_transform, robot_config, joint_uncertainty_params, joint_distance_weights, extra_options.environment_id);
    const uncertainty_planning_core::LinkedSimulatorPtr simulator = iiwa7_linked_common_config::GetSimulator(extra_options, options.debug_level);
    uncertainty_planning_core::DemonstrateLinkedSimulator(options, robot, simulator, sampler, start_and_goal.first, start_and_goal.second, display_debug_publisher);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "simulator_demonstration_node");
    ros::NodeHandle nh;
    ROS_INFO("Starting Simulator Demonstration Node...");
    ros::Publisher display_debug_publisher = nh.advertise<visualization_msgs::MarkerArray>("uncertainty_planning_debug_display_markers", 1, true);
    std::string robot_type;
    ros::NodeHandle nhp("~");
    nhp.param(std::string("robot_type"), robot_type, std::string("baxter"));
    if (robot_type == "se2")
    {
        demonstrate_se2(display_debug_publisher);
    }
    else if (robot_type == "se3")
    {
        demonstrate_se3(display_debug_publisher);
    }
    else if (robot_type == "baxter")
    {
        demonstrate_baxter(display_debug_publisher);
    }
    else if (robot_type == "ur5")
    {
        demonstrate_ur5(display_debug_publisher);
    }
    else if (robot_type == "iiwa7")
    {
        demonstrate_iiwa7(display_debug_publisher);
    }
    else
    {
        std::cout << "Robot type [" << robot_type << "] is not recognized" << std::endl;
    }
    return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <fstream>

#include "RandNum_Generator.h"
#include "Degree_Generator.h"
#include "rfc5053_config.h"
#include "Partition.h"
#include "Array_Data_Types.h"
#include "Inter_Symbol_Generator.h"
#include "R10_Decoder.h"
#include "LT_Encoding.h"

#include "Utility.h"

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "storage_adaptors.h"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/graph_utility.hpp>

using namespace std;
//using namespace boost::numeric::ublas::;
using namespace boost::numeric::ublas;

//   enum { A, B, C, D, E, F, N };
//   const char* name = "ABCDEF";

// typedef boost::adjacency_matrix<boost::undirectedS> UGraph;
//  UGraph ug(N);

int main(int argc, char const *argv[])
{
	std::cout << "Raptor Codes Testing !" << std::endl;

    const int SYMBOL_LEN = 1;
    const int SYMBOL_SIZE = 1024;
    const int overhead = 3;

	class Array_Data_Symbol testing_symbol(SYMBOL_SIZE, SYMBOL_LEN);

	for (int i = 0; i < testing_symbol.K; ++i)
	{
		// if (i%2)
		// {
		// 	a.symbol[i].s[0] = 1;
		// }
		if (i%3)
		{
			testing_symbol.symbol[i].s[0] = 1;
		}
	}

    class LT_Encoding encoder(&testing_symbol);

   	class Array_Data_Symbol D(testing_symbol.K, SYMBOL_LEN);

    std::vector<uint32_t> ESI;

    for (int i = 0; i < testing_symbol.K + overhead; ++i)
    {
    	ESI.push_back(i);
    	D.ESIs.push_back(i);
    }

    // for (int i = 0; i < ESI.size(); ++i)
    // {
    // 	printf("ESI: %d\n", ESI[i]);
    // }

	//printf("ESI: %d\n", ESI.size());

    D.symbol = encoder.LTEnc_Generate(ESI);

	class R10_Decoder decoder(testing_symbol.K, SYMBOL_LEN);

	// class Array_Data_Symbol C = decoder.Get_Inter_Symbols(D, testing_symbol.K);

 //    std::cout << "testing_symbol: " << std::endl;
 //    for (int i = 0; i < testing_symbol.K; ++i)
 //    {
 //        printf("%d ", testing_symbol.symbol[i].s[0]);
 //    }
 //    std::cout << std::endl;

 //     printf("D: \n");

 //    for (int i = 0; i < D.symbol.size(); ++i)
 //    {
 //        printf("%d ", D.symbol[i].s[0]);
 //    }
 //    printf("\n");

	// printf("C: \n");

	// for (int i = 0; i < C.symbol.size(); ++i)
	// {
	// 	printf("%d ", C.symbol[i].s[0]);
	// }
	// printf("\n");

    // decoder.Inter_Symbols_Decoding(C);

    // drop some symbols for testing
     int loss = 1;
    D.symbol.erase(D.symbol.begin() + 3);
    D.ESIs.erase(D.ESIs.begin() + 3);

   class Array_Data_Symbol source = decoder.Get_Source_Symbol(D, testing_symbol.K + overhead - loss);

    // printf("Source Symbol: \n");

    // for (int i = 0; i < source.symbol.size(); ++i)
    // {
    //     printf("%d ", source.symbol[i].s[0]);
    // }
    // printf("\n");

    // std::cout << "Testing Symbol: " << std::endl;
    // for (int i = 0; i < testing_symbol.K; ++i)
    // {
    //     printf("%d ", testing_symbol.symbol[i].s[0]);
    // }
    // std::cout << std::endl;

   for (int i = 0; i < source.symbol.size(); ++i)
   {
        if (source.symbol[i].s[0] ^ testing_symbol.symbol[i].s[0] != 0)
        {

             std::cout << "decode fail!" << std::endl;
             return -1;

        }

   }

    std::cout << "decode successfully!" << std::endl;
    std::cout <<  "total symbols: " << source.K <<"  redundancy rate: " <<  (float)overhead / (float)SYMBOL_SIZE <<std::endl;

	return 0;
}


#pragma once

#include <cstddef>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/iterator_facade.hpp>

#include "nifty/graph/detail/andres/grid-graph.hxx"

#include "nifty/tools/runtime_check.hxx"
#include "nifty/tools/for_each_coordinate.hxx"
#include "nifty/graph/undirected_graph_base.hxx"
#include "nifty/graph/detail/adjacency.hxx"
#include "nifty/graph/graph_tags.hxx"
#include "nifty/parallel/threadpool.hxx"
#include "nifty/array/arithmetic_array.hxx"
#include "nifty/xtensor/xtensor.hxx"

namespace nifty{
namespace graph{

namespace detail_graph{

    template<std::size_t DIM, bool SIMPLE_NH>
    class UndirectedGridGraphIter{
    public:
        typedef andres::graph::GridGraph<DIM> AGridGraph;
        typedef typename AGridGraph::AdjacencyIterator AGridGraphAdjacencyIter;
        typedef UndirectedAdjacency<int64_t,int64_t,int64_t,int64_t> NodeAdjacency;

        struct UnaryFunction{
            typedef NodeAdjacency value_type;
            template<class ADJ>
            NodeAdjacency operator()(const ADJ & adjacency)const{
                return NodeAdjacency(adjacency.vertex(), adjacency.vertex());
            }
        };

        typedef boost::transform_iterator<
            UnaryFunction,
            typename AGridGraph::AdjacencyIterator,
            NodeAdjacency,
            NodeAdjacency
        > OldAdjacencyIter;

        class AdjacencyIter
        : public boost::iterator_facade<
            AdjacencyIter,
            NodeAdjacency,
            std::random_access_iterator_tag,
            const NodeAdjacency &
        >
        {
        public:
            AdjacencyIter(const AGridGraphAdjacencyIter & iter)
            :   iter_(iter),
                adjacency_(){
            }
            bool equal(const AdjacencyIter & other)const{
                return iter_ == other.iter_;
            }
            void increment(){
                ++iter_;
            }
            void dencrement(){
                --iter_;
            }
            void advance(const std::size_t n){
                iter_+=n;
            }
            std::ptrdiff_t distance_to(const AdjacencyIter & other)const{
                return std::distance(iter_, other.iter_);
            }
            const NodeAdjacency & dereference()const{
                adjacency_ = NodeAdjacency(iter_->vertex(), iter_->edge());
                return adjacency_;
            }
        private:
            mutable AGridGraphAdjacencyIter iter_;
            mutable NodeAdjacency adjacency_;
        };

        class NodeIter : public boost::counting_iterator<int64_t>{
            using boost::counting_iterator<int64_t>::counting_iterator;
            using boost::counting_iterator<int64_t>::operator=;
        };

        class EdgeIter : public boost::counting_iterator<int64_t>{
            using boost::counting_iterator<int64_t>::counting_iterator;
            using boost::counting_iterator<int64_t>::operator=;
        };
    };

};

template<std::size_t DIM, bool SIMPLE_NH>
class UndirectedGridGraph;

template<std::size_t DIM>
class UndirectedGridGraph<DIM,true> : public
    UndirectedGraphBase<
        UndirectedGridGraph<DIM, true>,
        typename detail_graph::UndirectedGridGraphIter<DIM,true>::NodeIter,
        typename detail_graph::UndirectedGridGraphIter<DIM,true>::EdgeIter,
        typename detail_graph::UndirectedGridGraphIter<DIM,true>::AdjacencyIter
    >
{
private:
    typedef andres::graph::GridGraph<DIM> AndresGridGraphType;
    typedef typename AndresGridGraphType::VertexCoordinate AndresVertexCoordinate;
public:
    typedef nifty::array::StaticArray<int64_t, DIM> ShapeType;
    typedef nifty::array::StaticArray<int64_t, DIM> CoordinateType;

    typedef typename detail_graph::UndirectedGridGraphIter<DIM,true>::NodeIter      NodeIter;
    typedef typename detail_graph::UndirectedGridGraphIter<DIM,true>::EdgeIter      EdgeIter;
    typedef typename detail_graph::UndirectedGridGraphIter<DIM,true>::AdjacencyIter AdjacencyIter;

    typedef ContiguousTag EdgeIdTag;
    typedef ContiguousTag NodeIdTag;

    typedef SortedTag EdgeIdOrderTag;
    typedef SortedTag NodeIdOrderTag;

    UndirectedGridGraph()
    : gridGraph_(){
    }

    template<class T>
    UndirectedGridGraph(const nifty::array::StaticArray<T, DIM> & shape)
    : gridGraph_(){

        AndresVertexCoordinate ashape;
        std::copy(shape.rbegin(), shape.rend(), ashape.begin());
        gridGraph_.assign(ashape);

    }

    template<class T>
    void assign(const nifty::array::StaticArray<T, DIM> & shape){

        AndresVertexCoordinate ashape;
        std::copy(shape.rbegin(), shape.rend(), ashape.begin());
        gridGraph_.assign(ashape);

    }

    //void assign(const uint64_t numberOfNodes = 0, const uint64_t reserveNumberOfEdges = 0);

    // MUST IMPL INTERFACE
    int64_t u(const int64_t e)const{
        return gridGraph_.vertexOfEdge(e,0);
    }
    int64_t v(const int64_t e)const{
        return gridGraph_.vertexOfEdge(e,1);
    }

    int64_t findEdge(const int64_t u, const int64_t v)const{
        const auto r = gridGraph_.findEdge(u,v);
        if(r.first)
            return r.second;
        else
            return -1;
    }
    int64_t nodeIdUpperBound() const{
         return numberOfNodes() == 0 ? 0 : numberOfNodes()-1;
    }
    int64_t edgeIdUpperBound() const{
        return numberOfEdges() == 0 ? 0 : numberOfEdges()-1;
    }

    uint64_t numberOfEdges() const{
        return gridGraph_.numberOfEdges();
    }
    uint64_t numberOfNodes() const{
        return gridGraph_.numberOfVertices();
    }

    NodeIter nodesBegin()const{
        return NodeIter(0);
    }
    NodeIter nodesEnd()const{
        return NodeIter(this->numberOfNodes());
    }
    EdgeIter edgesBegin()const{
        return EdgeIter(0);
    }
    EdgeIter edgesEnd()const{
        return EdgeIter(this->numberOfEdges());
    }

    AdjacencyIter adjacencyBegin(const int64_t node)const{
        return AdjacencyIter(gridGraph_.adjacenciesFromVertexBegin(node));
    }
    AdjacencyIter adjacencyEnd(const int64_t node)const{
        return AdjacencyIter(gridGraph_.adjacenciesFromVertexEnd(node));
    }
    AdjacencyIter adjacencyOutBegin(const int64_t node)const{
        return AdjacencyIter(gridGraph_.adjacenciesFromVertexBegin(node));
    }
     AdjacencyIter adjacencyOutEnd(const int64_t node)const{
        return AdjacencyIter(gridGraph_.adjacenciesFromVertexEnd(node));
    }

    // optional (with default impl in base)
    //std::pair<int64_t,int64_t> uv(const int64_t e)const;

    template<class F>
    void forEachEdge(F && f)const{
        for(uint64_t edge=0; edge< numberOfEdges(); ++edge){
            f(edge);
        }
    }

    template<class F>
    void forEachNode(F && f)const{
        for(uint64_t node=0; node< numberOfNodes(); ++node){
            f(node);
        }
    }

    // serialization de-serialization

    uint64_t serializationSize() const{
        return DIM + 1;
    }

    template<class ITER>
    void serialize(ITER iter) const{
        for(auto d=0; d<DIM; ++d){
            *iter = gridGraph_.shape(d);
            ++iter;
        }
        // simple nh?
        *iter = true;
        ++iter;
    }

    template<class ITER>
    void deserialize(ITER iter);

    /**
     * @brief convert an image with DIM dimension to an edge map
     * @details convert an image with DIM dimension to an edge map
     * by applying a binary functor to the values of a node map at
     * the endpoints of an edge.
     *
     * @param       image the  input image
     * @param       binaryFunctor a binary functor
     * @param[out]  the result edge map
     *
     * @return [description]
     */
    template<class IMAGE, class BINARY_FUNCTOR, class EDGE_MAP>
    void imageToEdgeMap(
        const IMAGE & image,
        BINARY_FUNCTOR binaryFunctor,
        EDGE_MAP & edgeMap
    )const{
        for(const auto edge : this->edges()){
            const auto uv = this->uv(edge);
            CoordinateType cU, cV;
            nodeToCoordinate(uv.first,  cU);
            nodeToCoordinate(uv.second, cV);
            const auto uVal = xtensor::read(image, cU.asStdArray());
            const auto vVal = xtensor::read(image, cU.asStdArray());

            edgeMap[edge] = binaryFunctor(uVal, vVal);
        }
    }

    // TODO parallelize
    /**
     * @brief convert an affinity map with DIM+1 dimension to an edge map
     * @details convert an affinity map with DIM+1 dimension to an edge map
     * by assigning the affinity values to corresponding affinity values
     *
     * @param       image the input affinities
     * @param[out]  the result edge map
     *
     * @return [description]
     */
    template<class AFFINITIES, class EDGE_MAP>
    void affinitiesToEdgeMap(const AFFINITIES & affinities,
                             EDGE_MAP & edgeMap) const {
        NIFTY_CHECK_OP(affinities.shape()[0], ==, DIM, "wrong number of affinity channels")
        for(auto d=1; d<DIM+1; ++d){
            NIFTY_CHECK_OP(shape(d-1), ==, affinities.shape()[d], "wrong shape")
        }

        typedef nifty::array::StaticArray<int64_t, DIM+1> AffinityCoordType;

        CoordinateType cU, cV;
        for(const auto edge : this->edges()){

            const auto uv = this->uv(edge);
            nodeToCoordinate(uv.first,  cU);
            nodeToCoordinate(uv.second, cV);

            // find the correct affinity edge
            AffinityCoordType affCoord;
            for(std::size_t d = 0; d < DIM; ++d) {
                auto diff = cU[d] - cV[d];
                if(diff == 0) {
                    affCoord[d + 1] = cU[d];
                }
                else {
                    // TODO max for different direction convention
                    affCoord[d + 1] = std::min(cU[d], cV[d]);
                    affCoord[0] = d;
                }
            }

            edgeMap[edge] = xtensor::read(affinities, affCoord.asStdArray());
        }
    }

    template<class AFFINITIES, class LOCAL_FEATURES,
             class LIFTED_UVS, class LIFTED_FEATURES>
    std::size_t longRangeAffinitiesToLiftedEdges(const AFFINITIES & affinities,
                                            xt::xexpression<LOCAL_FEATURES> & localFeaturesExp,
                                            xt::xexpression<LIFTED_UVS> & liftedUvsExp,
                                            xt::xexpression<LIFTED_FEATURES> & liftedFeaturesExp,
                                            const std::vector<std::vector<int>> & offsets) const {

        auto & localFeatures = localFeaturesExp.derived_cast();
        auto & liftedUvs = liftedUvsExp.derived_cast();
        auto & liftedFeatures = liftedFeaturesExp.derived_cast();
        //
        typedef nifty::array::StaticArray<int64_t, DIM+1> AffinityCoordType;
        for(auto d=1; d<DIM+1; ++d){
            NIFTY_CHECK_OP(shape(d-1), ==, affinities.shape()[d], "wrong shape")
        }
        std::size_t affLen = affinities.shape()[0];

        AffinityCoordType affShape;
        affShape[0] = affLen;
        for(unsigned d = 0; d < DIM; ++d) {
            affShape[d + 1] = shape(d);
        }

        std::size_t liftedEdgeId = 0;
        tools::forEachCoordinate(affShape, [&](const AffinityCoordType & affCoord) {
            const auto & offset = offsets[affCoord[0]];
            CoordinateType cU, cV;

            for(unsigned d = 0; d < DIM; ++d) {
                cU[d] = affCoord[d + 1];
                cV[d] = affCoord[d + 1] + offset[d];
                // range check
                if(cV[d] >= shape(d) || cV[d] < 0) {
                    return;
                }
            }

            const std::size_t u = coordianteToNode(cU);
            const std::size_t v = coordianteToNode(cV);

            const std::size_t e = findEdge(u, v);
            if(e == -1) {
                liftedFeatures(liftedEdgeId) = xtensor::read(affinities, affCoord.asStdArray());
                liftedUvs(liftedEdgeId, 0) = std::min(u, v);
                liftedUvs(liftedEdgeId, 1) = std::max(u, v);
                ++liftedEdgeId;
            } else {
                localFeatures(e) = xtensor::read(affinities, affCoord.asStdArray());
            }

        });
        return liftedEdgeId;
    }

    template<class AFFINITIES, class LOCAL_FEATURES,
             class LIFTED_UVS, class LIFTED_FEATURES>
    std::size_t longRangeAffinitiesToLiftedEdges(const AFFINITIES & affinities,
                                            LOCAL_FEATURES & localFeatures,
                                            LIFTED_UVS & liftedUvs,
                                            LIFTED_FEATURES & liftedFeatures,
                                            const std::vector<std::vector<int>> & offsets,
                                            const std::vector<int> & strides) const {
        //
        typedef nifty::array::StaticArray<int64_t, DIM+1> AffinityCoordType;
        for(auto d=1; d<DIM+1; ++d){
            NIFTY_CHECK_OP(shape(d-1), ==, affinities.shape()[d], "wrong shape")
        }
        std::size_t affLen = affinities.shape()[0];

        AffinityCoordType affShape;
        affShape[0] = affLen;
        for(unsigned d = 0; d < DIM; ++d) {
            affShape[d + 1] = shape(d);
        }

        std::size_t liftedEdgeId = 0;
        tools::forEachCoordinate(affShape, [&](const AffinityCoordType & affCoord) {
            const auto & offset = offsets[affCoord[0]];
            CoordinateType cU, cV;

            for(unsigned d = 0; d < DIM; ++d) {
                cU[d] = affCoord[d + 1];
                cV[d] = affCoord[d + 1] + offset[d];
                // range check
                if(cV[d] >= shape(d) || cV[d] < 0) {
                    return;
                }
            }

            // check if we are in the strides for channels > DIM
            if(affCoord[0] > DIM) {
                bool inStride = true;
                for(unsigned d = 0; d < DIM; ++d) {
                    if(cU[d] % strides[d] != 0) {
                        inStride = false;
                        break;
                    }
                }
                if(!inStride) {
                    return;
                }
            }

            const std::size_t u = coordianteToNode(cU);
            const std::size_t v = coordianteToNode(cV);

            const std::size_t e = findEdge(u, v);
            if(e == -1) {
                liftedFeatures(liftedEdgeId) = xtensor::read(affinities, affCoord.asStdArray());
                liftedUvs(liftedEdgeId, 0) = std::min(u, v);
                liftedUvs(liftedEdgeId, 1) = std::max(u, v);
                ++liftedEdgeId;
            } else {
                localFeatures(e) = xtensor::read(affinities, affCoord.asStdArray());
            }

        });
        return liftedEdgeId;
    }

    /**
     * @brief convert an image with DIM dimension to an edge map
     * @details convert an image with DIM dimension to an edge map
     * by taking the values of the image at the
     * interpixel coordinates.
     * The shape of the image must be 2*shape-1
     *
     *
     * @param       image the  input image
     * @param       binaryFunctor a binary functor
     * @param[out]  the result edge map
     *
     * @return [description]
     */
    template<class IMAGE, class EDGE_MAP>
    void imageToInterpixelEdgeMap(
        const IMAGE & image,
        EDGE_MAP & edgeMap
    )const{

        for(auto d=0; d<DIM; ++d){
            NIFTY_CHECK_OP(shape(d)*2 - 1, ==, image.shape()[d],
                "wrong shape foer image to interpixel edge map")
        }

        for(const auto edge : this->edges()){
            const auto uv = this->uv(edge);
            CoordinateType cU,cV;
            nodeToCoordinate(uv.first,  cU);
            nodeToCoordinate(uv.second, cV);
            // FIXME I don't understand what is going on here ?!
            // But doesn't look right
            const auto uVal = xtensor::read(image, cU.asStdArray());
            cU += cV;
            edgeMap(edge) = xtensor::read(image, cU.asStdArray());
        }
    }

    uint64_t shape(const std::size_t d)const{
        return gridGraph_.shape(DIM-1-d);
    }

    // COORDINATE RELATED
    CoordinateType nodeToCoordinate(const uint64_t node)const{
        CoordinateType ret;
        nodeToCoordinate(node, ret);
        return ret;
    }

    template<class NODE_COORDINATE>
    void nodeToCoordinate(
        const uint64_t node,
        NODE_COORDINATE & coordinate
    )const{
        AndresVertexCoordinate aCoordinate;
        gridGraph_.vertex(node, aCoordinate);
        for(auto d=0; d<DIM; ++d){
            coordinate[d] = aCoordinate[DIM-1-d];
        }
    }

    template<class NODE_COORDINATE>
    uint64_t coordianteToNode(const NODE_COORDINATE & coordinate)const{
        AndresVertexCoordinate aCoordinate;
        for(auto d=0; d<DIM; ++d){
            aCoordinate[DIM-1-d] = coordinate[d];
        }
        return gridGraph_.vertex(aCoordinate);
    }

private:
    andres::graph::GridGraph<DIM> gridGraph_;

};

} // namespace nifty::graph
} // namespace nifty


#pragma once
#ifndef OPENGM_SUBGRADIENT_SSVM_LEARNER_HXX
#define OPENGM_SUBGRADIENT_SSVM_LEARNER_HXX

#include <iomanip>
#include <vector>
#include <opengm/inference/inference.hxx>
#include <opengm/graphicalmodel/weights.hxx>
#include <opengm/utilities/random.hxx>
#include <opengm/learning/gradient-accumulator.hxx>
#include <opengm/learning/weight_averaging.hxx>

#ifdef WITH_OPENMP
#include <omp.h>
#endif

#include <boost/circular_buffer.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>

namespace opengm {
    namespace learning {

    template<class T>
    double gen_normal_3(T &generator)
    {
      return generator();
    }

    // Version that fills a vector
    template<class T>
    void gen_normal_3(T &generator,
                  std::vector<double> &res)
    {
      for(size_t i=0; i<res.size(); ++i)
        res[i]=generator();
    }

    template<class DATASET>
    class Rws
    {
    public:
        typedef DATASET DatasetType;
        typedef typename DATASET::GMType   GMType;
        typedef typename DATASET::GMWITHLOSS GMWITHLOSS;
        typedef typename DATASET::LossType LossType;
        typedef typename GMType::ValueType ValueType;
        typedef typename GMType::IndexType IndexType;
        typedef typename GMType::LabelType LabelType;
        typedef opengm::learning::Weights<double> WeightsType;
        typedef typename std::vector<LabelType>::const_iterator LabelIterator;
        typedef FeatureAccumulator<GMType, LabelIterator> FeatureAcc;

        typedef std::vector<LabelType> ConfType;
        typedef boost::circular_buffer<ConfType> ConfBuffer;
        typedef std::vector<ConfBuffer> ConfBufferVec;

        class Parameter{
        public:

            Parameter(){
                eps_ = 0.00001;
                maxIterations_ = 10000;
                stopLoss_ = 0.0;
                learningRate_ = 1.0;
                C_ = 1.0;
                averaging_ = -1;
                p_ = 10;
                sigma_ = 1.0;
            }

            double eps_;
            size_t maxIterations_;
            double stopLoss_;
            double learningRate_;
            double C_;
            int averaging_;
            size_t p_;
            double sigma_;
        };

        Rws(DATASET&, const Parameter& );

        template<class INF>
        void learn(const typename INF::Parameter& para);
        //template<class INF, class VISITOR>
        //void learn(typename INF::Parameter para, VITITOR vis);

        const opengm::learning::Weights<double>& getWeights(){return weights_;}
        Parameter& getLerningParameters(){return para_;}

        double getLoss(const GMType & gm ,const GMWITHLOSS  & gmWithLoss, std::vector<LabelType> & labels){

            double loss = 0 ;
            std::vector<LabelType> subConf(20,0);

            for(size_t fi=gm.numberOfFactors(); fi<gmWithLoss.numberOfFactors(); ++fi){
                for(size_t v=0; v<gmWithLoss[fi].numberOfVariables(); ++v){
                    subConf[v] = labels[ gmWithLoss[fi].variableIndex(v)];
                }
                loss +=  gmWithLoss[fi](subConf.begin());
            }
            return loss;
        }

    private:

        double updateWeights();

        DATASET& dataset_;
        WeightsType  weights_;
        Parameter para_;
        size_t iteration_;
        FeatureAcc featureAcc_;
        WeightRegularizer<ValueType> wReg_;
        WeightAveraging<double> weightAveraging_;
    };

    template<class DATASET>
    Rws<DATASET>::Rws(DATASET& ds, const Parameter& p )
    :   dataset_(ds),
        para_(p),
        iteration_(0),
        featureAcc_(ds.getNumberOfWeights()),
        wReg_(2, 1.0/p.C_),
        weightAveraging_(ds.getWeights(),p.averaging_)
    {
        featureAcc_.resetWeights();
        weights_ = opengm::learning::Weights<double>(ds.getNumberOfWeights());
    }

    template<class DATASET>
    template<class INF>
    void Rws<DATASET>::learn(const typename INF::Parameter& para){

        const size_t nModels = dataset_.getNumberOfModels();
        const size_t nWegihts = dataset_.getNumberOfWeights();

        //for(size_t wi=0; wi<nWegihts; ++wi){
        //    dataset_.getWeights().setWeight(wi, 0.0);
        //}

        RandomUniform<size_t> randModel(0, nModels);
        boost::math::normal_distribution<ValueType> nDist(0.0, para_.sigma_);
        std::vector< std::vector<ValueType> > noiseVecs(para_.p_, std::vector<ValueType>(nWegihts));
        std::vector<ValueType> lossVec(para_.p_);

        std::vector<ValueType> gradient(nWegihts);

        boost::variate_generator<boost::mt19937, boost::normal_distribution<> >
        generator(boost::mt19937(time(0)),boost::normal_distribution<>(0.0, para_.sigma_));

        std::cout<<"online mode "<<nWegihts<<"\n";

        std::cout <<"start loss"<< std::setw(6) << std::setfill(' ') << iteration_ << ':'
                          << std::setw(8) << dataset_. template getTotalLossParallel<INF>(para) <<"  \n\n\n\n";

        for(iteration_=0 ; iteration_<para_.maxIterations_; ++iteration_){

            // get random model
            const size_t gmi = randModel();

            // save the current weights
            WeightsType currentWeights  = dataset_.getWeights();

            featureAcc_.resetWeights();

            // lock the model
            dataset_.lockModel(gmi);

            for(size_t p=0; p<para_.p_; ++p){

                // fill noise
                gen_normal_3(generator, noiseVecs[p]);

                // add noise to the weights
                for(size_t wi=0; wi<nWegihts; ++wi){
                    const ValueType cw = currentWeights[wi];
                    const ValueType nw = cw + noiseVecs[p][wi];
                    dataset_.getWeights().setWeight(wi, nw);
                }

                const GMType & gm = dataset_.getModel(gmi);
                // do inference
                std::vector<LabelType> arg;
                opengm::infer<INF>(gm, para, arg);
                lossVec[p] = dataset_.getLoss(arg, gmi);

                //featureAcc_.accumulateModelFeatures(gm, dataset_.getGT(gmi).begin(), arg.begin());
                // update weights
                //const double wChange =updateWeights();
            }

            //for(size_t wi=0; wi<nWegihts; ++wi){
            //    gradient[wi] = featureAcc_.getWeight(wi);
            //}
            std::fill(gradient.begin(), gradient.end(),0.0);
            for(size_t p=0; p<para_.p_; ++p){
                for(size_t wi=0; wi<nWegihts; ++wi){
                    gradient[wi] += (1.0/para_.p_)*(noiseVecs[p][wi])*lossVec[p];
                }
            }

            const ValueType actualLearningRate = para_.learningRate_/(1.0 + iteration_);
            //const ValueType actualLearningRate = para_.learningRate_;///(1.0 + iteration_);
            // do update
            for(size_t wi=0; wi<nWegihts; ++wi){
                const ValueType oldWeight = currentWeights[wi];
                const ValueType newWeights = (oldWeight - actualLearningRate*gradient[wi])*para_.C_;
                //std::cout<<"wi "<<newWeights<<"\n";
                dataset_.getWeights().setWeight(wi, newWeights);
            }
            std::cout<<"\n";
            dataset_.unlockModel(gmi);

            if(iteration_%10==0){
            //if(iteration_%nModels*2 == 0 ){
                std::cout << '\n'
                          << std::setw(6) << std::setfill(' ') << iteration_ << ':'
                          << std::setw(8) << dataset_. template getTotalLossParallel<INF>(para) <<"  "<< std::flush;

            }

        }

        weights_ = dataset_.getWeights();
    }

    template<class DATASET>
    double Rws<DATASET>::updateWeights(){

        const size_t nWegihts = dataset_.getNumberOfWeights();

        WeightsType p(nWegihts);
        WeightsType newWeights(nWegihts);

        for(size_t wi=0; wi<nWegihts; ++wi){
            p[wi] =  dataset_.getWeights().getWeight(wi);
            p[wi] += para_.C_ * featureAcc_.getWeight(wi);
        }

        double wChange = 0.0;

        for(size_t wi=0; wi<nWegihts; ++wi){
            const double wOld = dataset_.getWeights().getWeight(wi);
            const double wNew = wOld - (para_.learningRate_/double(iteration_+1))*p[wi];
            newWeights[wi] = wNew;
        }

        weightAveraging_(newWeights);

        weights_ = dataset_.getWeights();
        return wChange;
    }
}
}
#endif


/**************************************************************************
** This file is a part of our work (Siggraph'16 paper, binary, code and dataset):
**
** Roto++: Accelerating Professional Rotoscoping using Shape Manifolds
** Wenbin Li, Fabio Viola, Jonathan Starck, Gabriel J. Brostow and Neill D.F. Campbell
**
** w.li AT cs.ucl.ac.uk
** http://visual.cs.ucl.ac.uk/pubs/rotopp
**
** Copyright (c) 2016, Wenbin Li
** All rights reserved.
**
** Redistribution and use in source and binary forms, with or without
** modification, are permitted provided that the following conditions are met:
**
** -- Redistributions of source code and data must retain the above
**    copyright notice, this list of conditions and the following disclaimer.
** -- Redistributions in binary form must reproduce the above copyright
**    notice, this list of conditions and the following disclaimer in the
**    documentation and/or other materials provided with the distribution.
**
** THIS WORK AND THE RELATED SOFTWARE, SOURCE CODE AND DATA IS PROVIDED BY
** THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
** WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
** MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN
** NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
** INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
** BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
** USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
** NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
** EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***************************************************************************/

#ifndef EIGENUTILS_HPP
#define EIGENUTILS_HPP

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <Eigen/Eigenvalues>

typedef Eigen::MatrixXd Matrix;
typedef Eigen::VectorXd Vector;

#include "baseDefs.hpp"

template <typename T>
Eigen::Map< Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>,
            Eigen::Unaligned,
            Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> >
            MatrixRowMapper(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& mat, int row, int M, int N)
{
    typedef Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> DynamicStride;
    assert (row < mat.rows());
    return Eigen::Map< Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>,
            Eigen::Unaligned,
            DynamicStride >(mat.data() + row, M, N, DynamicStride(M * mat.rows(), mat.rows()));
}

template <typename T>
Eigen::Map< Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>,
            Eigen::Unaligned,
            Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> >
            MatrixRowMapperMutable(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& mat, int row, int M, int N)
{
    typedef Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> DynamicStride;
    assert (row < mat.rows());
    return Eigen::Map< Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>,
            Eigen::Unaligned,
            DynamicStride >(mat.data() + row, M, N, DynamicStride(M * mat.rows(), mat.rows()));
}

template <typename T>
Eigen::Map< const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>,
            Eigen::Unaligned,
            Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> >
            MatrixRowMapper(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& mat, int row, int M, int N)
{
    typedef Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> DynamicStride;
    assert (row < mat.rows());
    return Eigen::Map< const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>,
            Eigen::Unaligned,
            DynamicStride >(mat.data() + row, M, N, DynamicStride(M * mat.rows(), mat.rows()));
}

template<typename T>
class Interpolator
{
    typedef Eigen::MatrixXd Matrix;
    typedef Eigen::VectorXd Vector;

public:
    Interpolator(T start, T end, const Vector& a, const Vector& b) :
        _begin(start), _end(end), _a(a), _b(b)
    {
        nassert (_a.size() == _b.size());
    }

    Vector get(T interpPoint)
    {
        const double x(interpPoint);

        nassert (x >= _begin);
        nassert (x <= _end);

        const double xb = (x - _begin) / (_end - _begin);
        const double xa = 1.0 - xb;

        return Vector(_a * xa) + (_b * xb);
    }

private:
    double _begin, _end;
    Vector _a, _b;
};

#endif // EIGENUTILS_HPP


/* jacobian.cpp -- Computation of Jacobian matrix

    Copyright (C) 2012-2014 University of Reading
    Copyright (C) 2015-2020 European Centre for Medium-Range Weather Forecasts

    Author: Robin Hogan <r.j.hogan@ecmwf.int>

    This file is part of the Adept library.

*/

#ifdef _OPENMP
#include <omp.h>
#endif

#include <adept_arrays.h>

namespace adept {

  namespace internal {
    static const int MULTIPASS_SIZE = ADEPT_REAL_PACKET_SIZE == 1 ? ADEPT_MULTIPASS_SIZE : ADEPT_REAL_PACKET_SIZE;
  }

  using namespace internal;

  template <typename T>
  T _check_long_double() {
    // The user may have requested Real to be of type "long double" by
    // specifying ADEPT_REAL_TYPE_SIZE=16. If the present system can
    // only support double then sizeof(long double) will be 8, but
    // Adept will not be emitting the best code for this, so it is
    // probably better to fail forcing the user to specify
    // ADEPT_REAL_TYPE_SIZE=8.
    ADEPT_STATIC_ASSERT(ADEPT_REAL_TYPE_SIZE != 16 || ADEPT_REAL_TYPE_SIZE == sizeof(Real),
			COMPILER_DOES_NOT_SUPPORT_16_BYTE_LONG_DOUBLE);
    return 1;
  }

#if ADEPT_REAL_PACKET_SIZE > 1
  void
  Stack::jacobian_forward_kernel(Real* __restrict gradient_multipass_b) const
  {

    // Loop forward through the derivative statements
    for (uIndex ist = 1; ist < n_statements_; ist++) {
      const Statement& statement = statement_[ist];
      // We copy the LHS to "a" in case it appears on the RHS in any
      // of the following statements
      Packet<Real> a; // Zeroed automatically
      // Loop through operations
      for (uIndex iop = statement_[ist-1].end_plus_one;
	   iop < statement.end_plus_one; iop++) {
	Packet<Real> g(gradient_multipass_b+index_[iop]*MULTIPASS_SIZE);
	Packet<Real> m(multiplier_[iop]);
	a += m * g;
      }
      // Copy the results
      a.put(gradient_multipass_b+statement.index*MULTIPASS_SIZE);
    } // End of loop over statements
  }
#else
  void
  Stack::jacobian_forward_kernel(Real* __restrict gradient_multipass_b) const
  {

    // Loop forward through the derivative statements
    for (uIndex ist = 1; ist < n_statements_; ist++) {
      const Statement& statement = statement_[ist];
      // We copy the LHS to "a" in case it appears on the RHS in any
      // of the following statements
      Block<MULTIPASS_SIZE,Real> a; // Zeroed automatically
      // Loop through operations
      for (uIndex iop = statement_[ist-1].end_plus_one;
	   iop < statement.end_plus_one; iop++) {
	for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	  a[i] += multiplier_[iop]*gradient_multipass_b[index_[iop]*MULTIPASS_SIZE+i];
	}
      }
      // Copy the results
      for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	gradient_multipass_b[statement.index*MULTIPASS_SIZE+i] = a[i];
      }
    } // End of loop over statements
  }
#endif

  void
  Stack::jacobian_forward_kernel_extra(Real* __restrict gradient_multipass_b,
				       uIndex n_extra) const
  {

    // Loop forward through the derivative statements
    for (uIndex ist = 1; ist < n_statements_; ist++) {
      const Statement& statement = statement_[ist];
      // We copy the LHS to "a" in case it appears on the RHS in any
      // of the following statements
      Block<MULTIPASS_SIZE,Real> a; // Zeroed automatically
      // Loop through operations
      for (uIndex iop = statement_[ist-1].end_plus_one;
	   iop < statement.end_plus_one; iop++) {
	for (uIndex i = 0; i < n_extra; i++) {
	  a[i] += multiplier_[iop]*gradient_multipass_b[index_[iop]*MULTIPASS_SIZE+i];
	}
      }
      // Copy the results
      for (uIndex i = 0; i < n_extra; i++) {
	gradient_multipass_b[statement.index*MULTIPASS_SIZE+i] = a[i];
      }
    } // End of loop over statements
  }

  // Compute the Jacobian matrix, parallelized using OpenMP. Normally
  // the user would call the jacobian or jacobian_forward functions,
  // and the OpenMP version would only be called if OpenMP is
  // available and the Jacobian matrix is large enough for
  // parallelization to be worthwhile.  Note that jacobian_out must be
  // allocated to be at least of size m*n, where m is the number of
  // dependent variables and n is the number of independents. The
  // independents and dependents must have already been identified
  // with the functions "independent" and "dependent", otherwise this
  // function will fail with FAILURE_XXDEPENDENT_NOT_IDENTIFIED. The
  // offsets in memory of the two dimensions are provided by
  // dep_offset and indep_offset. This is implemented using a forward
  // pass, appropriate for m>=n.
  void
  Stack::jacobian_forward_openmp(Real* jacobian_out,
				 Index dep_offset, Index indep_offset) const
  {

    // Number of blocks to cycle through, including a possible last
    // block containing fewer than MULTIPASS_SIZE variables
    int n_block = (n_independent() + MULTIPASS_SIZE - 1)
      / MULTIPASS_SIZE;
    uIndex n_extra = n_independent() % MULTIPASS_SIZE;

#pragma omp parallel
    {
      //      std::vector<Block<MULTIPASS_SIZE,Real> >
      //	gradient_multipass_b(max_gradient_);
      uIndex gradient_multipass_size = max_gradient_*MULTIPASS_SIZE;
      Real* __restrict gradient_multipass_b
	= alloc_aligned<Real>(gradient_multipass_size);

#pragma omp for schedule(static)
      for (int iblock = 0; iblock < n_block; iblock++) {
	// Set the index to the dependent variables for this block
	uIndex i_independent =  MULTIPASS_SIZE * iblock;

	uIndex block_size = MULTIPASS_SIZE;
	// If this is the last iteration and the number of extra
	// elements is non-zero, then set the block size to the number
	// of extra elements. If the number of extra elements is zero,
	// then the number of independent variables is exactly divisible
	// by MULTIPASS_SIZE, so the last iteration will be the
	// same as all the rest.
	if (iblock == n_block-1 && n_extra > 0) {
	  block_size = n_extra;
	}

	// Set the initial gradients all to zero
	for (uIndex i = 0; i < gradient_multipass_size; i++) {
	  gradient_multipass_b[i] = 0.0;
	}
	// Each seed vector has one non-zero entry of 1.0
	for (uIndex i = 0; i < block_size; i++) {
	  gradient_multipass_b[independent_index_[i_independent+i]*MULTIPASS_SIZE+i] = 1.0;
	}

	jacobian_forward_kernel(gradient_multipass_b);

	// Copy the gradients corresponding to the dependent variables
	// into the Jacobian matrix
	if (indep_offset == 1) {
	  for (uIndex idep = 0; idep < n_dependent(); idep++) {
	    for (uIndex i = 0; i < block_size; i++) {
	      jacobian_out[idep*dep_offset+i_independent+i]
		= gradient_multipass_b[dependent_index_[idep]*MULTIPASS_SIZE+i];
	    }
	  }
	}
	else {
	  for (uIndex idep = 0; idep < n_dependent(); idep++) {
	    for (uIndex i = 0; i < block_size; i++) {
	      jacobian_out[(i_independent+i)*indep_offset+idep*dep_offset]
		= gradient_multipass_b[dependent_index_[idep]*MULTIPASS_SIZE+i];
	    }
	  }
	}
      } // End of loop over blocks
      free_aligned(gradient_multipass_b);
    } // End of parallel section
  } // End of jacobian function

  // Compute the Jacobian matrix; note that jacobian_out must be
  // allocated to be of size m*n, where m is the number of dependent
  // variables and n is the number of independents. The independents
  // and dependents must have already been identified with the
  // functions "independent" and "dependent", otherwise this function
  // will fail with FAILURE_XXDEPENDENT_NOT_IDENTIFIED. This is
  // implemented using a forward pass, appropriate for m>=n.
  void
  Stack::jacobian_forward(Real* jacobian_out,
			  Index dep_offset, Index indep_offset) const
  {
    if (independent_index_.empty() || dependent_index_.empty()) {
      throw(dependents_or_independents_not_identified());
    }

    // If either of the offsets are zero, set them to the size of the
    // other dimension, which assumes that the full Jacobian matrix is
    // contiguous in memory.
    if (dep_offset <= 0) {
      dep_offset = n_independent();
    }
    if (indep_offset <= 0) {
      indep_offset = n_dependent();
    }

#ifdef _OPENMP
    if (have_openmp_
	&& !openmp_manually_disabled_
	&& n_independent() > MULTIPASS_SIZE
	&& omp_get_max_threads() > 1) {
      // Call the parallel version
      jacobian_forward_openmp(jacobian_out, dep_offset, indep_offset);
      return;
    }
#endif

    // For optimization reasons, we process a block of
    // MULTIPASS_SIZE columns of the Jacobian at once; calculate
    // how many blocks are needed and how many extras will remain
    uIndex n_block = n_independent() / MULTIPASS_SIZE;
    uIndex n_extra = n_independent() % MULTIPASS_SIZE;

    ///gradient_multipass_.resize(max_gradient_);
    uIndex gradient_multipass_size = max_gradient_*MULTIPASS_SIZE;
    Real* __restrict gradient_multipass_b
      = alloc_aligned<Real>(gradient_multipass_size);

    // Loop over blocks of MULTIPASS_SIZE columns
    for (uIndex iblock = 0; iblock < n_block; iblock++) {
      // Set the index to the dependent variables for this block
      uIndex i_independent =  MULTIPASS_SIZE * iblock;

      // Set the initial gradients all to zero
      ///zero_gradient_multipass();
      for (uIndex i = 0; i < gradient_multipass_size; i++) {
	gradient_multipass_b[i] = 0.0;
      }

      // Each seed vector has one non-zero entry of 1.0
      for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	gradient_multipass_b[independent_index_[i_independent+i]*MULTIPASS_SIZE+i] = 1.0;
      }

      jacobian_forward_kernel(gradient_multipass_b);

      // Copy the gradients corresponding to the dependent variables
      // into the Jacobian matrix
      if (indep_offset == 1) {
	for (uIndex idep = 0; idep < n_dependent(); idep++) {
	  for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	    jacobian_out[idep*dep_offset+i_independent+i]
	      = gradient_multipass_b[dependent_index_[idep]*MULTIPASS_SIZE+i];
	  }
	}
      }
      else {
	for (uIndex idep = 0; idep < n_dependent(); idep++) {
	  for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	    jacobian_out[(i_independent+i)*indep_offset+idep*dep_offset]
	      = gradient_multipass_b[dependent_index_[idep]*MULTIPASS_SIZE+i];
	  }
	}
      }
      i_independent += MULTIPASS_SIZE;
    } // End of loop over blocks

    // Now do the same but for the remaining few columns in the matrix
    if (n_extra > 0) {
      uIndex i_independent =  MULTIPASS_SIZE * n_block;
      ///zero_gradient_multipass();
      for (uIndex i = 0; i < gradient_multipass_size; i++) {
	gradient_multipass_b[i] = 0.0;
      }

      for (uIndex i = 0; i < n_extra; i++) {
	gradient_multipass_b[independent_index_[i_independent+i]*MULTIPASS_SIZE+i] = 1.0;
      }

      jacobian_forward_kernel_extra(gradient_multipass_b, n_extra);

      if (indep_offset == 1) {
	for (uIndex idep = 0; idep < n_dependent(); idep++) {
	  for (uIndex i = 0; i < n_extra; i++) {
	    jacobian_out[idep*dep_offset+i_independent+i]
	      = gradient_multipass_b[dependent_index_[idep]*MULTIPASS_SIZE+i];
	  }
	}
      }
      else {
	for (uIndex idep = 0; idep < n_dependent(); idep++) {
	  for (uIndex i = 0; i < n_extra; i++) {
	    jacobian_out[(i_independent+i)*indep_offset+idep*dep_offset]
	      = gradient_multipass_b[dependent_index_[idep]*MULTIPASS_SIZE+i];
	  }
	}
      }
    }

    free_aligned(gradient_multipass_b);
  }

  // Compute the Jacobian matrix, parallelized using OpenMP.  Normally
  // the user would call the jacobian or jacobian_reverse functions,
  // and the OpenMP version would only be called if OpenMP is
  // available and the Jacobian matrix is large enough for
  // parallelization to be worthwhile.  Note that jacobian_out must be
  // allocated to be at least of size m*n, where m is the number of
  // dependent variables and n is the number of independents. The
  // independents and dependents must have already been identified
  // with the functions "independent" and "dependent", otherwise this
  // function will fail with FAILURE_XXDEPENDENT_NOT_IDENTIFIED. The
  // offsets in memory of the two dimensions are provided by
  // dep_offset and indep_offset.  This is implemented using a reverse
  // pass, appropriate for m<n.
  void
  Stack::jacobian_reverse_openmp(Real* jacobian_out,
				 Index dep_offset, Index indep_offset) const
  {

    // Number of blocks to cycle through, including a possible last
    // block containing fewer than MULTIPASS_SIZE variables
    int n_block = (n_dependent() + MULTIPASS_SIZE - 1)
      / MULTIPASS_SIZE;
    uIndex n_extra = n_dependent() % MULTIPASS_SIZE;

    // Inside the OpenMP loop, the "this" pointer may be NULL if the
    // adept::Stack pointer is declared as thread-local and if the
    // OpenMP memory model uses thread-local storage for private
    // data. If this is the case then local pointers to or copies of
    // the following members of the adept::Stack object may need to be
    // made: dependent_index_ n_statements_ statement_ multiplier_
    // index_ independent_index_ n_dependent() n_independent().
    // Limited testing implies this is OK though.

#pragma omp parallel
    {
      std::vector<Block<MULTIPASS_SIZE,Real> >
	gradient_multipass_b(max_gradient_);

#pragma omp for schedule(static)
      for (int iblock = 0; iblock < n_block; iblock++) {
	// Set the index to the dependent variables for this block
	uIndex i_dependent =  MULTIPASS_SIZE * iblock;

	uIndex block_size = MULTIPASS_SIZE;
	// If this is the last iteration and the number of extra
	// elements is non-zero, then set the block size to the number
	// of extra elements. If the number of extra elements is zero,
	// then the number of independent variables is exactly divisible
	// by MULTIPASS_SIZE, so the last iteration will be the
	// same as all the rest.
	if (iblock == n_block-1 && n_extra > 0) {
	  block_size = n_extra;
	}

	// Set the initial gradients all to zero
	for (std::size_t i = 0; i < gradient_multipass_b.size(); i++) {
	  gradient_multipass_b[i].zero();
	}
	// Each seed vector has one non-zero entry of 1.0
	for (uIndex i = 0; i < block_size; i++) {
	  gradient_multipass_b[dependent_index_[i_dependent+i]][i] = 1.0;
	}

	// Loop backward through the derivative statements
	for (uIndex ist = n_statements_-1; ist > 0; ist--) {
	  const Statement& statement = statement_[ist];
	  // We copy the RHS to "a" in case it appears on the LHS in any
	  // of the following statements
	  Real a[MULTIPASS_SIZE];
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	  // For large blocks, we only process the ones where a[i] is
	  // non-zero
	  uIndex i_non_zero[MULTIPASS_SIZE];
#endif
	  uIndex n_non_zero = 0;
	  for (uIndex i = 0; i < block_size; i++) {
	    a[i] = gradient_multipass_b[statement.index][i];
	    gradient_multipass_b[statement.index][i] = 0.0;
	    if (a[i] != 0.0) {
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	      i_non_zero[n_non_zero++] = i;
#else
	      n_non_zero = 1;
#endif
	    }
	  }

	  // Only do anything for this statement if any of the a values
	  // are non-zero
	  if (n_non_zero) {
	    // Loop through the operations
	    for (uIndex iop = statement_[ist-1].end_plus_one;
		 iop < statement.end_plus_one; iop++) {
	      // Try to minimize pointer dereferencing by making local
	      // copies
	      Real multiplier = multiplier_[iop];
	      Real* __restrict gradient_multipass
		= &(gradient_multipass_b[index_[iop]][0]);
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	      // For large blocks, loop over only the indices
	      // corresponding to non-zero a
	      for (uIndex i = 0; i < n_non_zero; i++) {
		gradient_multipass[i_non_zero[i]] += multiplier*a[i_non_zero[i]];
	      }
#else
	      // For small blocks, do all indices
	      for (uIndex i = 0; i < block_size; i++) {
	      //	      for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
		gradient_multipass[i] += multiplier*a[i];
	      }
#endif
	    }
	  }
	} // End of loop over statement
	// Copy the gradients corresponding to the independent
	// variables into the Jacobian matrix
	if (dep_offset == 1) {
	  for (uIndex iindep = 0; iindep < n_independent(); iindep++) {
	    for (uIndex i = 0; i < block_size; i++) {
	      jacobian_out[iindep*indep_offset+i_dependent+i]
		= gradient_multipass_b[independent_index_[iindep]][i];
	    }
	  }
	}
	else {
	  for (uIndex iindep = 0; iindep < n_independent(); iindep++) {
	    for (uIndex i = 0; i < block_size; i++) {
	      jacobian_out[iindep*indep_offset+(i_dependent+i)*dep_offset]
		= gradient_multipass_b[independent_index_[iindep]][i];
	    }
	  }
	}
      } // End of loop over blocks
    } // end #pragma omp parallel
  } // end jacobian_reverse_openmp

  // Compute the Jacobian matrix; note that jacobian_out must be
  // allocated to be of size m*n, where m is the number of dependent
  // variables and n is the number of independents. The independents
  // and dependents must have already been identified with the
  // functions "independent" and "dependent", otherwise this function
  // will fail with FAILURE_XXDEPENDENT_NOT_IDENTIFIED.  This is
  // implemented using a reverse pass, appropriate for m<n.
  void
  Stack::jacobian_reverse(Real* jacobian_out,
			  Index dep_offset, Index indep_offset) const
  {
    if (independent_index_.empty() || dependent_index_.empty()) {
      throw(dependents_or_independents_not_identified());
    }

    // If either of the offsets are zero, set them to the size of the
    // other dimension, which assumes that the full Jacobian matrix is
    // contiguous in memory.
    if (dep_offset <= 0) {
      dep_offset = n_independent();
    }
    if (indep_offset <= 0) {
      indep_offset = n_dependent();
    }

#ifdef _OPENMP
    if (have_openmp_
	&& !openmp_manually_disabled_
	&& n_dependent() > MULTIPASS_SIZE
	&& omp_get_max_threads() > 1) {
      // Call the parallel version
      jacobian_reverse_openmp(jacobian_out,
			      dep_offset, indep_offset);
      return;
    }
#endif

    //    gradient_multipass_.resize(max_gradient_);
    std::vector<Block<MULTIPASS_SIZE,Real> >
      gradient_multipass_b(max_gradient_);

    // For optimization reasons, we process a block of
    // MULTIPASS_SIZE rows of the Jacobian at once; calculate
    // how many blocks are needed and how many extras will remain
    uIndex n_block = n_dependent() / MULTIPASS_SIZE;
    uIndex n_extra = n_dependent() % MULTIPASS_SIZE;
    uIndex i_dependent = 0; // uIndex of first row in the block we are
			    // currently computing
    // Loop over the of MULTIPASS_SIZE rows
    for (uIndex iblock = 0; iblock < n_block; iblock++) {
      // Set the initial gradients all to zero
      //      zero_gradient_multipass();
      for (std::size_t i = 0; i < gradient_multipass_b.size(); i++) {
	gradient_multipass_b[i].zero();
      }

      // Each seed vector has one non-zero entry of 1.0
      for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	gradient_multipass_b[dependent_index_[i_dependent+i]][i] = 1.0;
      }
      // Loop backward through the derivative statements
      for (uIndex ist = n_statements_-1; ist > 0; ist--) {
	const Statement& statement = statement_[ist];
	// We copy the RHS to "a" in case it appears on the LHS in any
	// of the following statements
	Real a[MULTIPASS_SIZE];
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	// For large blocks, we only process the ones where a[i] is
	// non-zero
	uIndex i_non_zero[MULTIPASS_SIZE];
#endif
	uIndex n_non_zero = 0;
	for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	  a[i] = gradient_multipass_b[statement.index][i];
	  gradient_multipass_b[statement.index][i] = 0.0;
	  if (a[i] != 0.0) {
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	    i_non_zero[n_non_zero++] = i;
#else
	    n_non_zero = 1;
#endif
	  }
	}
	// Only do anything for this statement if any of the a values
	// are non-zero
	if (n_non_zero) {
	  // Loop through the operations
	  for (uIndex iop = statement_[ist-1].end_plus_one;
	       iop < statement.end_plus_one; iop++) {
	    // Try to minimize pointer dereferencing by making local
	    // copies
	    Real multiplier = multiplier_[iop];
	    Real* __restrict gradient_multipass
	      = &(gradient_multipass_b[index_[iop]][0]);
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	    // For large blocks, loop over only the indices
	    // corresponding to non-zero a
	    for (uIndex i = 0; i < n_non_zero; i++) {
	      gradient_multipass[i_non_zero[i]] += multiplier*a[i_non_zero[i]];
	    }
#else
	    // For small blocks, do all indices
	    for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	      gradient_multipass[i] += multiplier*a[i];
	    }
#endif
	  }
	}
      } // End of loop over statement
      // Copy the gradients corresponding to the independent variables
      // into the Jacobian matrix
      if (dep_offset == 1) {
	for (uIndex iindep = 0; iindep < n_independent(); iindep++) {
	  for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	    jacobian_out[iindep*indep_offset+i_dependent+i]
	      = gradient_multipass_b[independent_index_[iindep]][i];
	  }
	}
      }
      else {
	for (uIndex iindep = 0; iindep < n_independent(); iindep++) {
	  for (uIndex i = 0; i < MULTIPASS_SIZE; i++) {
	    jacobian_out[iindep*indep_offset+(i_dependent+i)*dep_offset]
	      = gradient_multipass_b[independent_index_[iindep]][i];
	  }
	}
      }
      i_dependent += MULTIPASS_SIZE;
    } // End of loop over blocks

    // Now do the same but for the remaining few rows in the matrix
    if (n_extra > 0) {
      for (std::size_t i = 0; i < gradient_multipass_b.size(); i++) {
	gradient_multipass_b[i].zero();
      }
      //      zero_gradient_multipass();
      for (uIndex i = 0; i < n_extra; i++) {
	gradient_multipass_b[dependent_index_[i_dependent+i]][i] = 1.0;
      }
      for (uIndex ist = n_statements_-1; ist > 0; ist--) {
	const Statement& statement = statement_[ist];
	Real a[MULTIPASS_SIZE];
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	uIndex i_non_zero[MULTIPASS_SIZE];
#endif
	uIndex n_non_zero = 0;
	for (uIndex i = 0; i < n_extra; i++) {
	  a[i] = gradient_multipass_b[statement.index][i];
	  gradient_multipass_b[statement.index][i] = 0.0;
	  if (a[i] != 0.0) {
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	    i_non_zero[n_non_zero++] = i;
#else
	    n_non_zero = 1;
#endif
	  }
	}
	if (n_non_zero) {
	  for (uIndex iop = statement_[ist-1].end_plus_one;
	       iop < statement.end_plus_one; iop++) {
	    Real multiplier = multiplier_[iop];
	    Real* __restrict gradient_multipass
	      = &(gradient_multipass_b[index_[iop]][0]);
#if MULTIPASS_SIZE > MULTIPASS_SIZE_ZERO_CHECK
	    for (uIndex i = 0; i < n_non_zero; i++) {
	      gradient_multipass[i_non_zero[i]] += multiplier*a[i_non_zero[i]];
	    }
#else
	    for (uIndex i = 0; i < n_extra; i++) {
	      gradient_multipass[i] += multiplier*a[i];
	    }
#endif
	  }
	}
      }
      if (dep_offset == 1) {
	for (uIndex iindep = 0; iindep < n_independent(); iindep++) {
	  for (uIndex i = 0; i < n_extra; i++) {
	    jacobian_out[iindep*indep_offset+i_dependent+i]
	      = gradient_multipass_b[independent_index_[iindep]][i];
	  }
	}
      }
      else {
	for (uIndex iindep = 0; iindep < n_independent(); iindep++) {
	  for (uIndex i = 0; i < n_extra; i++) {
	    jacobian_out[iindep*indep_offset+(i_dependent+i)*dep_offset]
	      = gradient_multipass_b[independent_index_[iindep]][i];
	  }
	}
      }
    }
  }

  // Return the Jacobian matrix in the matrix "jac", using the forward
  // or reverse method depending which would be faster
  void Stack::jacobian(Array<2,Real,false> jac) const {
    if (jac.dimension(0) != n_dependent()
	|| jac.dimension(1) != n_independent()) {
      throw size_mismatch("Jacobian matrix has wrong size");
    }
    if (n_independent() <= n_dependent()) {
      jacobian_forward(jac.data(), jac.offset(0), jac.offset(1));
    }
    else {
      jacobian_reverse(jac.data(), jac.offset(0), jac.offset(1));
    }
  }

  // Return the Jacobian matrix in the matrix "jac", explicitly
  // specifying whether to use the forward or reverse method
  void Stack::jacobian_forward(Array<2,Real,false> jac) const {
    if (jac.dimension(0) != n_dependent()
	|| jac.dimension(1) != n_independent()) {
      throw size_mismatch("Jacobian matrix has wrong size");
    }
    jacobian_forward(jac.data(), jac.offset(0), jac.offset(1));
  }

  void Stack::jacobian_reverse(Array<2,Real,false> jac) const {
    if (jac.dimension(0) != n_dependent()
	|| jac.dimension(1) != n_independent()) {
      throw size_mismatch("Jacobian matrix has wrong size");
    }
    jacobian_reverse(jac.data(), jac.offset(0), jac.offset(1));
  }

  // Return the Jacobian matrix using the forward or reverse method
  // depending which would be faster
  Array<2,Real,false> Stack::jacobian() const {
    Array<2,Real,false> jac(n_dependent(), n_independent());
    if (n_independent() <= n_dependent()) {
      jacobian_forward(jac.data(), jac.offset(0), jac.offset(1));
    }
    else {
      jacobian_reverse(jac.data(), jac.offset(0), jac.offset(1));
    }
    return jac;
  }

  // Return the Jacobian matrix, explicitly specifying whether to use
  // the forward or reverse method
  Array<2,Real,false> Stack::jacobian_forward() const {
    Array<2,Real,false> jac(n_dependent(), n_independent());
    jacobian_forward(jac.data(), jac.offset(0), jac.offset(1));
    return jac;
  }

  Array<2,Real,false> Stack::jacobian_reverse() const {
    Array<2,Real,false> jac(n_dependent(), n_independent());
    jacobian_reverse(jac.data(), jac.offset(0), jac.offset(1));
    return jac;
  }

} // End namespace adept