#pragma once

#if defined(_OPENMP)
#include <omp.h>
#endif

#include <chrono>
#include <cmath>
#include <cstdint>
#include <functional>
#include <fstream>
#include <iostream>
#include <stdexcept>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/maybe.hpp>
#include <common_robotics_utilities/openmp_helpers.hpp>
#include <common_robotics_utilities/voxel_grid.hpp>
#include <voxelized_geometry_tools/signed_distance_field.hpp>

namespace voxelized_geometry_tools
{
namespace signed_distance_field_generation
{
using common_robotics_utilities::voxel_grid::GridIndex;
using common_robotics_utilities::voxel_grid::GridSizes;

struct BucketCell
{
  double distance_square = 0.0;
  int32_t update_direction = 0;
  uint32_t location[3] = {0u, 0u, 0u};
  uint32_t closest_point[3] = {0u, 0u, 0u};
};

typedef common_robotics_utilities::voxel_grid
    ::VoxelGrid<BucketCell, std::vector<BucketCell>> DistanceField;

inline int32_t GetDirectionNumber(
    const int32_t dx, const int32_t dy, const int32_t dz)
{
  return ((dx + 1) * 9) + ((dy + 1) * 3) + (dz + 1);
}

inline std::vector<std::vector<std::vector<std::vector<int32_t>>>>
MakeNeighborhoods()
{
  // First vector<>: 2 - the first bucket queue, the points we know are zero
  // distance, start with a complete set of neighbors to check. Every other
  // bucket queue checks fewer neighbors.
  // Second vector<>: 27 (# of source directions in fully-connected 3d grid).
  // Third vector<>:
  std::vector<std::vector<std::vector<std::vector<int32_t>>>> neighborhoods;
  // I don't know why there are 2 initial neighborhoods.
  neighborhoods.resize(2);
  for (size_t n = 0; n < neighborhoods.size(); n++)
  {
    neighborhoods[n].resize(27);
    // Loop through the source directions.
    for (int32_t dx = -1; dx <= 1; dx++)
    {
      for (int32_t dy = -1; dy <= 1; dy++)
      {
        for (int32_t dz = -1; dz <= 1; dz++)
        {
          const int32_t direction_number = GetDirectionNumber(dx, dy, dz);
          // Loop through the target directions.
          for (int32_t tdx = -1; tdx <= 1; tdx++)
          {
            for (int32_t tdy = -1; tdy <= 1; tdy++)
            {
              for (int32_t tdz = -1; tdz <= 1; tdz++)
              {
                // Ignore the case of ourself.
                if (tdx == 0 && tdy == 0 && tdz == 0)
                {
                  continue;
                }
                // Why is one set of neighborhoods larger than the other?
                if (n >= 1)
                {
                  if ((abs(tdx) + abs(tdy) + abs(tdz)) != 1)
                  {
                    continue;
                  }
                  if ((dx * tdx) < 0 || (dy * tdy) < 0 || (dz * tdz) < 0)
                  {
                    continue;
                  }
                }
                std::vector<int32_t> new_point;
                new_point.resize(3);
                new_point[0] = tdx;
                new_point[1] = tdy;
                new_point[2] = tdz;
                neighborhoods[n][direction_number].push_back(new_point);
              }
            }
          }
        }
      }
    }
  }
  return neighborhoods;
}

inline double ComputeDistanceSquared(
    const int32_t x1, const int32_t y1, const int32_t z1,
    const int32_t x2, const int32_t y2, const int32_t z2)
{
  const int32_t dx = x1 - x2;
  const int32_t dy = y1 - y2;
  const int32_t dz = z1 - z2;
  return double((dx * dx) + (dy * dy) + (dz * dz));
}

class MultipleThreadIndexQueueWrapper
{
public:

  explicit MultipleThreadIndexQueueWrapper(const size_t max_queues)
  {
    per_thread_queues_.resize(
        common_robotics_utilities::openmp_helpers::GetNumOmpThreads(),
        ThreadIndexQueues(max_queues));
  }

  const GridIndex& Query(const int32_t distance_squared, const size_t idx) const
  {
    size_t working_index = idx;
    for (size_t thread = 0; thread < per_thread_queues_.size(); thread++)
    {
      const auto& current_thread_queue =
          per_thread_queues_.at(thread).at(distance_squared);
      const size_t current_thread_queue_size = current_thread_queue.size();
      if (working_index < current_thread_queue_size)
      {
        return current_thread_queue.at(working_index);
      }
      else
      {
        working_index -= current_thread_queue_size;
      }
    }
    throw std::runtime_error("Failed to find item");
  }

  size_t NumQueues() const
  {
    return per_thread_queues_.at(0).size();
  }

  size_t Size(const int32_t distance_squared) const
  {
    size_t total_size = 0;
    for (size_t thread = 0; thread < per_thread_queues_.size(); thread++)
    {
      total_size += per_thread_queues_.at(thread).at(distance_squared).size();
    }
    return total_size;
  }

  void Enqueue(const int32_t distance_squared, const GridIndex& index)
  {
    const int32_t thread_num
        = common_robotics_utilities::openmp_helpers::GetContextOmpThreadNum();
    per_thread_queues_.at(thread_num).at(distance_squared).push_back(index);
  }

  void ClearCompletedQueues(const int32_t distance_squared)
  {
    for (size_t thread = 0; thread < per_thread_queues_.size(); thread++)
    {
      per_thread_queues_.at(thread).at(distance_squared).clear();
    }
  }

private:
  typedef std::vector<std::vector<GridIndex>> ThreadIndexQueues;
  std::vector<ThreadIndexQueues> per_thread_queues_;

};

inline DistanceField BuildDistanceFieldSerial(
    const Eigen::Isometry3d& grid_origin_transform,
    const GridSizes& grid_sizes, const std::vector<GridIndex>& points)
{
  if (!grid_sizes.UniformCellSize())
  {
    throw std::invalid_argument(
        "Cannot build distance field from grid with non-uniform cells");
  }
  const std::chrono::time_point<std::chrono::steady_clock> start_time
      = std::chrono::steady_clock::now();
  // Make the DistanceField container
  BucketCell default_cell;
  default_cell.distance_square = std::numeric_limits<double>::infinity();
  DistanceField distance_field(grid_origin_transform, grid_sizes, default_cell);
  // Compute maximum distance square
  const int64_t max_distance_square =
      (distance_field.GetNumXCells() * distance_field.GetNumXCells())
      + (distance_field.GetNumYCells() * distance_field.GetNumYCells())
      + (distance_field.GetNumZCells() * distance_field.GetNumZCells());
  // Make bucket queue
  std::vector<std::vector<BucketCell>> bucket_queue(max_distance_square + 1);
  bucket_queue[0].reserve(points.size());
  // Set initial update direction
  int32_t initial_update_direction = GetDirectionNumber(0, 0, 0);
  // Mark all provided points with distance zero and add to the bucket queue
  for (size_t index = 0; index < points.size(); index++)
  {
    const GridIndex& current_index = points[index];
    auto query = distance_field.GetMutable(current_index);
    if (query)
    {
      query.Value().location[0] = static_cast<uint32_t>(current_index.X());
      query.Value().location[1] = static_cast<uint32_t>(current_index.Y());
      query.Value().location[2] = static_cast<uint32_t>(current_index.Z());
      query.Value().closest_point[0] = static_cast<uint32_t>(current_index.X());
      query.Value().closest_point[1] = static_cast<uint32_t>(current_index.Y());
      query.Value().closest_point[2] = static_cast<uint32_t>(current_index.Z());
      query.Value().distance_square = 0.0;
      query.Value().update_direction = initial_update_direction;
      bucket_queue[0].push_back(query.Value());
    }
    // If the point is outside the bounds of the SDF, skip
    else
    {
      throw std::runtime_error("Point for BuildDistanceField out of bounds");
    }
  }
  // HERE BE DRAGONS
  // Process the bucket queue
  const std::vector<std::vector<std::vector<std::vector<int>>>> neighborhoods =
      MakeNeighborhoods();
  for (size_t bq_idx = 0; bq_idx < bucket_queue.size(); bq_idx++)
  {
    for (const auto& cur_cell : bucket_queue[bq_idx])
    {
      // Get the current location
      const double x = cur_cell.location[0];
      const double y = cur_cell.location[1];
      const double z = cur_cell.location[2];
      // Pick the update direction
      // Only the first bucket queue gets the larger set of neighborhoods?
      // Don't really userstand why.
      const size_t direction_switch = (bq_idx > 0) ? 1 : 0;
      // Make sure the update direction is valid
      if (cur_cell.update_direction < 0 || cur_cell.update_direction > 26)
      {
        continue;
      }
      // Get the current neighborhood list
      const std::vector<std::vector<int>>& neighborhood =
          neighborhoods[direction_switch][cur_cell.update_direction];
      // Update the distance from the neighboring cells
      for (size_t nh_idx = 0; nh_idx < neighborhood.size(); nh_idx++)
      {
        // Get the direction to check
        const int32_t dx = neighborhood[nh_idx][0];
        const int32_t dy = neighborhood[nh_idx][1];
        const int32_t dz = neighborhood[nh_idx][2];
        const int32_t nx = static_cast<int32_t>(x + dx);
        const int32_t ny = static_cast<int32_t>(y + dy);
        const int32_t nz = static_cast<int32_t>(z + dz);
        auto neighbor_query =
            distance_field.GetMutable(static_cast<int64_t>(nx),
                                      static_cast<int64_t>(ny),
                                      static_cast<int64_t>(nz));
        if (!neighbor_query)
        {
          // "Neighbor" is outside the bounds of the SDF
          continue;
        }
        // Update the neighbor's distance based on the current
        const int32_t new_distance_square =
            static_cast<int32_t>(ComputeDistanceSquared(
                                   nx, ny, nz,
                                   cur_cell.closest_point[0],
                                   cur_cell.closest_point[1],
                                   cur_cell.closest_point[2]));
        if (new_distance_square > max_distance_square)
        {
          // Skip these cases
          continue;
        }
        if (new_distance_square < neighbor_query.Value().distance_square)
        {
          // If the distance is better, time to update the neighbor
          neighbor_query.Value().distance_square = new_distance_square;
          neighbor_query.Value().closest_point[0] = cur_cell.closest_point[0];
          neighbor_query.Value().closest_point[1] = cur_cell.closest_point[1];
          neighbor_query.Value().closest_point[2] = cur_cell.closest_point[2];
          neighbor_query.Value().location[0] = nx;
          neighbor_query.Value().location[1] = ny;
          neighbor_query.Value().location[2] = nz;
          neighbor_query.Value().update_direction =
              GetDirectionNumber(dx, dy, dz);
          // Add the neighbor into the bucket queue
          bucket_queue[new_distance_square].push_back(neighbor_query.Value());
        }
      }
    }
    // Clear the current queue now that we're done with it
    bucket_queue[bq_idx].clear();
  }
  const std::chrono::time_point<std::chrono::steady_clock> end_time
      = std::chrono::steady_clock::now();
  const std::chrono::duration<double> elapsed = end_time - start_time;
  std::cout << "Computed DistanceField in " << elapsed.count() << " seconds"
            << std::endl;
  return distance_field;
}

inline DistanceField BuildDistanceFieldParallel(
    const Eigen::Isometry3d& grid_origin_transform,
    const GridSizes& grid_sizes,
    const std::vector<GridIndex>& points)
{
  if (!grid_sizes.UniformCellSize())
  {
    throw std::invalid_argument(
        "Cannot build distance field from grid with non-uniform cells");
  }
  const std::chrono::time_point<std::chrono::steady_clock> start_time
      = std::chrono::steady_clock::now();
  // Make the DistanceField container
  BucketCell default_cell;
  default_cell.distance_square = std::numeric_limits<double>::infinity();
  DistanceField distance_field(grid_origin_transform, grid_sizes, default_cell);
  // Compute maximum distance square
  const int64_t max_distance_square =
      (distance_field.GetNumXCells() * distance_field.GetNumXCells())
      + (distance_field.GetNumYCells() * distance_field.GetNumYCells())
      + (distance_field.GetNumZCells() * distance_field.GetNumZCells());
  // Make bucket queue
  std::vector<std::vector<BucketCell>> bucket_queue(max_distance_square + 1);
  bucket_queue[0].reserve(points.size());
  MultipleThreadIndexQueueWrapper bucket_queues(max_distance_square + 1);
  // Set initial update direction
  int32_t initial_update_direction = GetDirectionNumber(0, 0, 0);
  // Mark all provided points with distance zero and add to the bucket queues
  // points MUST NOT CONTAIN DUPLICATE ENTRIES!
#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (size_t index = 0; index < points.size(); index++)
  {
    const GridIndex& current_index = points[index];
    auto query = distance_field.GetMutable(current_index);
    if (query)
    {
      query.Value().location[0] = static_cast<uint32_t>(current_index.X());
      query.Value().location[1] = static_cast<uint32_t>(current_index.Y());
      query.Value().location[2] = static_cast<uint32_t>(current_index.Z());
      query.Value().closest_point[0] = static_cast<uint32_t>(current_index.X());
      query.Value().closest_point[1] = static_cast<uint32_t>(current_index.Y());
      query.Value().closest_point[2] = static_cast<uint32_t>(current_index.Z());
      query.Value().distance_square = 0.0;
      query.Value().update_direction = initial_update_direction;
      bucket_queues.Enqueue(0, current_index);
    }
    // If the point is outside the bounds of the SDF, skip
    else
    {
      throw std::runtime_error("Point for BuildDistanceField out of bounds");
    }
  }
  // HERE BE DRAGONS
  // Process the bucket queue
  const std::vector<std::vector<std::vector<std::vector<int>>>> neighborhoods =
      MakeNeighborhoods();
  for (int32_t current_distance_square = 0;
       current_distance_square
           < static_cast<int32_t>(bucket_queues.NumQueues());
       current_distance_square++)
  {
#if defined(_OPENMP)
#pragma omp parallel for
#endif
    for (size_t idx = 0; idx < bucket_queues.Size(current_distance_square);
         idx++)
    {
      const GridIndex& current_index =
          bucket_queues.Query(current_distance_square, idx);
      // Get the current location
      const BucketCell& cur_cell =
          distance_field.GetImmutable(current_index).Value();
      const double x = cur_cell.location[0];
      const double y = cur_cell.location[1];
      const double z = cur_cell.location[2];
      // Pick the update direction
      // Only the first bucket queue gets the larger set of neighborhoods?
      // Don't really userstand why.
      const size_t direction_switch = (current_distance_square > 0) ? 1 : 0;
      // Make sure the update direction is valid
      if (cur_cell.update_direction < 0 || cur_cell.update_direction > 26)
      {
        continue;
      }
      // Get the current neighborhood list
      const std::vector<std::vector<int32_t>>& neighborhood =
          neighborhoods[direction_switch][cur_cell.update_direction];
      // Update the distance from the neighboring cells
      for (size_t nh_idx = 0; nh_idx < neighborhood.size(); nh_idx++)
      {
        // Get the direction to check
        const int32_t dx = neighborhood[nh_idx][0];
        const int32_t dy = neighborhood[nh_idx][1];
        const int32_t dz = neighborhood[nh_idx][2];
        const int32_t nx = static_cast<int32_t>(x + dx);
        const int32_t ny = static_cast<int32_t>(y + dy);
        const int32_t nz = static_cast<int32_t>(z + dz);
        const GridIndex neighbor_index(static_cast<int64_t>(nx),
                                       static_cast<int64_t>(ny),
                                       static_cast<int64_t>(nz));
        auto neighbor_query = distance_field.GetMutable(neighbor_index);
        if (!neighbor_query)
        {
          // "Neighbor" is outside the bounds of the SDF
          continue;
        }
        // Update the neighbor's distance based on the current
        const int32_t new_distance_square =
            static_cast<int32_t>(ComputeDistanceSquared(
                                   nx, ny, nz,
                                   cur_cell.closest_point[0],
                                   cur_cell.closest_point[1],
                                   cur_cell.closest_point[2]));
        if (new_distance_square > max_distance_square)
        {
          // Skip these cases
          continue;
        }
        if (new_distance_square < neighbor_query.Value().distance_square)
        {
          // If the distance is better, time to update the neighbor
          neighbor_query.Value().distance_square = new_distance_square;
          neighbor_query.Value().closest_point[0] = cur_cell.closest_point[0];
          neighbor_query.Value().closest_point[1] = cur_cell.closest_point[1];
          neighbor_query.Value().closest_point[2] = cur_cell.closest_point[2];
          neighbor_query.Value().location[0] = nx;
          neighbor_query.Value().location[1] = ny;
          neighbor_query.Value().location[2] = nz;
          neighbor_query.Value().update_direction =
              GetDirectionNumber(dx, dy, dz);
          // Add the neighbor into the bucket queue
          bucket_queues.Enqueue(new_distance_square, neighbor_index);
        }
      }
    }
    // Clear the current queues now that we're done with it
    bucket_queues.ClearCompletedQueues(current_distance_square);
  }
  const std::chrono::time_point<std::chrono::steady_clock> end_time
      = std::chrono::steady_clock::now();
  const std::chrono::duration<double> elapsed = end_time - start_time;
  std::cout << "Computed DistanceField in " << elapsed.count() << " seconds"
            << std::endl;
  return distance_field;
}

inline DistanceField BuildDistanceField(
    const Eigen::Isometry3d& grid_origin_transform,
    const GridSizes& grid_sizes,
    const std::vector<GridIndex>& points,
    const bool use_parallel)
{
  if (use_parallel)
  {
    return BuildDistanceFieldParallel(
        grid_origin_transform, grid_sizes, points);
  }
  else
  {
    return BuildDistanceFieldSerial(grid_origin_transform, grid_sizes, points);
  }
}

template<typename SDFBackingStore>
class SignedDistanceFieldResult
{
private:
  SignedDistanceField<SDFBackingStore> distance_field_;
  double maximum_ = 0.0;
  double minimum_ = 0.0;

public:
  SignedDistanceFieldResult(
      const SignedDistanceField<SDFBackingStore>& distance_field,
      const double maximum, const double minimum)
      : distance_field_(distance_field),
        maximum_(maximum), minimum_(minimum)
  {
    if (minimum_ > maximum_)
    {
      throw std::invalid_argument("minimum_ > maximum_");
    }
  }

  const SignedDistanceField<SDFBackingStore>& DistanceField() const
  {
    return distance_field_;
  }

  const SignedDistanceField<SDFBackingStore>& MutableDistanceField()
  {
    return distance_field_;
  }

  double Maximum() const { return maximum_; }

  double Minimum() const { return minimum_; }
};

template<typename SDFBackingStore>
SignedDistanceFieldResult<SDFBackingStore> MakeSignedDistanceFieldResult(
    const SignedDistanceField<SDFBackingStore>& signed_distance_field,
    const double maximum, const double minimum)
{
  return SignedDistanceFieldResult<SDFBackingStore>(
      signed_distance_field, maximum, minimum);
}

template<typename T, typename SDFBackingStore=std::vector<float>>
inline SignedDistanceFieldResult<SDFBackingStore> ExtractSignedDistanceField(
    const Eigen::Isometry3d& grid_origin_tranform, const GridSizes& grid_sizes,
    const std::function<bool(const GridIndex&)>& is_filled_fn,
    const float oob_value, const std::string& frame, const bool use_parallel)
{
  const std::chrono::time_point<std::chrono::steady_clock> start_time
      = std::chrono::steady_clock::now();
  std::vector<GridIndex> filled;
  std::vector<GridIndex> free;
  for (int64_t x_index = 0; x_index < grid_sizes.NumXCells(); x_index++)
  {
    for (int64_t y_index = 0; y_index < grid_sizes.NumYCells(); y_index++)
    {
      for (int64_t z_index = 0; z_index < grid_sizes.NumZCells(); z_index++)
      {
        const GridIndex current_index(x_index, y_index, z_index);
        if (is_filled_fn(current_index))
        {
          // Mark as filled
          filled.push_back(current_index);
        }
        else
        {
          // Mark as free space
          free.push_back(current_index);
        }
      }
    }
  }
  // Make two distance fields, one for distance to filled voxels, one for
  // distance to free voxels.
  const DistanceField filled_distance_field =
      BuildDistanceField(
        grid_origin_tranform, grid_sizes, filled, use_parallel);
  const DistanceField free_distance_field =
      BuildDistanceField(grid_origin_tranform, grid_sizes, free, use_parallel);
  // Generate the SDF
  SignedDistanceField<SDFBackingStore> new_sdf(
      grid_origin_tranform, frame, grid_sizes, oob_value);
  double max_distance = -std::numeric_limits<double>::infinity();
  double min_distance = std::numeric_limits<double>::infinity();
  for (int64_t x_index = 0; x_index < new_sdf.GetNumXCells(); x_index++)
  {
    for (int64_t y_index = 0; y_index < new_sdf.GetNumYCells(); y_index++)
    {
      for (int64_t z_index = 0; z_index < new_sdf.GetNumZCells(); z_index++)
      {
        const double distance1 =
            std::sqrt(
                filled_distance_field.GetImmutable(x_index, y_index, z_index)
                    .Value().distance_square)
            * new_sdf.GetResolution();
        const double distance2 =
            std::sqrt(
                free_distance_field.GetImmutable(x_index, y_index, z_index)
                    .Value().distance_square)
            * new_sdf.GetResolution();
        const double distance = distance1 - distance2;
        if (distance > max_distance)
        {
          max_distance = distance;
        }
        if (distance < min_distance)
        {
          min_distance = distance;
        }
        new_sdf.SetValue(
            x_index, y_index, z_index, static_cast<float>(distance));
      }
    }
  }
  const std::chrono::time_point<std::chrono::steady_clock> end_time
      = std::chrono::steady_clock::now();
  const std::chrono::duration<double> elapsed = end_time - start_time;
  std::cout << "Computed SDF for grid size in " << elapsed.count() << " seconds"
            << std::endl;
  return MakeSignedDistanceFieldResult<SDFBackingStore>(
      new_sdf, max_distance, min_distance);
}

template<typename T, typename BackingStore=std::vector<T>,
         typename SDFBackingStore=std::vector<float>>
inline SignedDistanceFieldResult<SDFBackingStore> ExtractSignedDistanceField(
    const common_robotics_utilities::voxel_grid
        ::VoxelGridBase<T, BackingStore>& grid,
    const std::function<bool(const GridIndex&)>& is_filled_fn,
    const float oob_value, const std::string& frame,
    const bool use_parallel, const bool add_virtual_border)
{
  if (!grid.HasUniformCellSize())
  {
    throw std::invalid_argument("Grid must have uniform resolution");
  }
  if (add_virtual_border == false)
  {
    // This is the conventional single-pass result
    return ExtractSignedDistanceField<T, SDFBackingStore>(
        grid.GetOriginTransform(), grid.GetGridSizes(), is_filled_fn, oob_value,
        frame, use_parallel);
  }
  else
  {
    const int64_t x_axis_size_offset =
        (grid.GetNumXCells() > 1) ? INT64_C(2) : INT64_C(0);
    const int64_t x_axis_query_offset =
        (grid.GetNumXCells() > 1) ? INT64_C(1) : INT64_C(0);
    const int64_t y_axis_size_offset =
        (grid.GetNumYCells() > 1) ? INT64_C(2) : INT64_C(0);
    const int64_t y_axis_query_offset =
        (grid.GetNumYCells() > 1) ? INT64_C(1) : INT64_C(0);
    const int64_t z_axis_size_offset =
        (grid.GetNumZCells() > 1) ? INT64_C(2) : INT64_C(0);
    const int64_t z_axis_query_offset =
        (grid.GetNumZCells() > 1) ? INT64_C(1) : INT64_C(0);
    // We need to lie about the size of the grid to add a virtual border
    const int64_t num_x_cells = grid.GetNumXCells() + x_axis_size_offset;
    const int64_t num_y_cells = grid.GetNumYCells() + y_axis_size_offset;
    const int64_t num_z_cells = grid.GetNumZCells() + z_axis_size_offset;
    // Make some deceitful helper functions that hide our lies about size
    // For the free space SDF, we lie and say the virtual border is filled
    const std::function<bool(const GridIndex&)> free_is_filled_fn
        = [&] (const GridIndex& virtual_border_grid_index)
    {
      // Is there a virtual border on our axis?
      if (x_axis_size_offset > 0)
      {
        // Are we a virtual border cell?
        if ((virtual_border_grid_index.X() == 0)
            || (virtual_border_grid_index.X() == (num_x_cells - 1)))
        {
          return true;
        }
      }
      // Is there a virtual border on our axis?
      if (y_axis_size_offset > 0)
      {
        // Are we a virtual border cell?
        if ((virtual_border_grid_index.Y() == 0)
            || (virtual_border_grid_index.Y() == (num_y_cells - 1)))
        {
          return true;
        }
      }
      // Is there a virtual border on our axis?
      if (z_axis_size_offset > 0)
      {
        // Are we a virtual border cell?
        if ((virtual_border_grid_index.Z() == 0)
            || (virtual_border_grid_index.Z() == (num_z_cells - 1)))
        {
          return true;
        }
      }
      const GridIndex real_grid_index(
            virtual_border_grid_index.X() - x_axis_query_offset,
            virtual_border_grid_index.Y() - y_axis_query_offset,
            virtual_border_grid_index.Z() - z_axis_query_offset);
      return is_filled_fn(real_grid_index);
    };
    // For the filled space SDF, we lie and say the virtual border is empty
    const std::function<bool(const GridIndex&)> filled_is_filled_fn
        = [&] (const GridIndex& virtual_border_grid_index)
    {
      // Is there a virtual border on our axis?
      if (x_axis_size_offset > 0)
      {
        // Are we a virtual border cell?
        if ((virtual_border_grid_index.X() == 0)
            || (virtual_border_grid_index.X() == (num_x_cells - 1)))
        {
          return false;
        }
      }
      // Is there a virtual border on our axis?
      if (y_axis_size_offset > 0)
      {
        // Are we a virtual border cell?
        if ((virtual_border_grid_index.Y() == 0)
            || (virtual_border_grid_index.Y() == (num_y_cells - 1)))
        {
          return false;
        }
      }
      // Is there a virtual border on our axis?
      if (z_axis_size_offset > 0)
      {
        // Are we a virtual border cell?
        if ((virtual_border_grid_index.Z() == 0)
            || (virtual_border_grid_index.Z() == (num_z_cells - 1)))
        {
          return false;
        }
      }
      const GridIndex real_grid_index(
            virtual_border_grid_index.X() - x_axis_query_offset,
            virtual_border_grid_index.Y() - y_axis_query_offset,
            virtual_border_grid_index.Z() - z_axis_query_offset);
      return is_filled_fn(real_grid_index);
    };
    // Make both SDFs
    const common_robotics_utilities::voxel_grid::GridSizes enlarged_sizes(
        grid.GetCellSizes().x(), num_x_cells, num_y_cells, num_z_cells);
    auto free_sdf_result
        = ExtractSignedDistanceField<T>(
            grid.GetOriginTransform(), enlarged_sizes, free_is_filled_fn,
            oob_value, frame, use_parallel);
    auto filled_sdf_result
        = ExtractSignedDistanceField<T>(
            grid.GetOriginTransform(), enlarged_sizes, filled_is_filled_fn,
            oob_value, frame, use_parallel);
    // Combine to make a single SDF
    SignedDistanceField<SDFBackingStore> combined_sdf(
          grid.GetOriginTransform(), frame, grid.GetGridSizes(), oob_value);
    for (int64_t x_idx = 0; x_idx < combined_sdf.GetNumXCells(); x_idx++)
    {
      for (int64_t y_idx = 0; y_idx < combined_sdf.GetNumYCells(); y_idx++)
      {
        for (int64_t z_idx = 0; z_idx < combined_sdf.GetNumZCells(); z_idx++)
        {
          const int64_t query_x_idx = x_idx + x_axis_query_offset;
          const int64_t query_y_idx = y_idx + y_axis_query_offset;
          const int64_t query_z_idx = z_idx + z_axis_query_offset;
          const float free_sdf_value
              = free_sdf_result.DistanceField().GetImmutable(
                  query_x_idx, query_y_idx, query_z_idx).Value();
          const float filled_sdf_value
              = filled_sdf_result.DistanceField().GetImmutable(
                  query_x_idx, query_y_idx, query_z_idx).Value();
          if (free_sdf_value >= 0.0)
          {
            combined_sdf.SetValue(x_idx, y_idx, z_idx, free_sdf_value);
          }
          else if (filled_sdf_value <= -0.0)
          {
            combined_sdf.SetValue(x_idx, y_idx, z_idx, filled_sdf_value);
          }
          else
          {
            combined_sdf.SetValue(x_idx, y_idx, z_idx, 0.0f);
          }
        }
      }
    }
    // Get the combined max/min values
    return MakeSignedDistanceFieldResult<SDFBackingStore>(
        combined_sdf, free_sdf_result.Maximum(), filled_sdf_result.Minimum());
  }
}

template<typename T, typename BackingStore=std::vector<T>,
         typename SDFBackingStore=std::vector<float>>
inline SignedDistanceFieldResult<SDFBackingStore> ExtractSignedDistanceField(
    const common_robotics_utilities::voxel_grid
        ::VoxelGridBase<T, BackingStore>& grid,
    const std::function<bool(const T&)>& is_filled_fn,
    const float oob_value, const std::string& frame,
    const bool use_parallel)
{
  if (!grid.HasUniformCellSize())
  {
    throw std::invalid_argument("Grid must have uniform resolution");
  }
  const std::function<bool(const GridIndex&)> real_is_filled_fn =
      [&] (const GridIndex& index)
  {
    const T& stored = grid.GetImmutable(index).Value();
    // If it matches an object to use OR there are no objects supplied
    if (is_filled_fn(stored))
    {
      // Mark as filled
      return true;
    }
    else
    {
      // Mark as free space
      return false;
    }
  };
  return ExtractSignedDistanceField<T, BackingStore, SDFBackingStore>(
        grid, real_is_filled_fn, oob_value, frame, use_parallel, false);
}
}  // namespace signed_distance_field_generation
}  // namespace voxelized_geometry_tools


//=======================================================================
// Copyright (c) 2013 Piotr Wygocki
//               2014 Piotr Godlewski
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file steiner_network_oracle.hpp
 * @brief
 * @author Piotr Wygocki, Piotr Godlewski
 * @version 1.0
 * @date 2013-06-24
 */
#ifndef PAAL_STEINER_NETWORK_ORACLE_HPP
#define PAAL_STEINER_NETWORK_ORACLE_HPP

#include "paal/iterative_rounding/min_cut.hpp"

#include <boost/range/as_array.hpp>

namespace paal {
namespace ir {

/**
 * @class steiner_network_violation_checker
 * @brief Violations checker for the separation oracle
 *      in the steiner network problem.
 */
class steiner_network_violation_checker {
    using AuxVertex = min_cut_finder::Vertex;
    using Violation = double;

  public:
    using Candidate = std::pair<AuxVertex, AuxVertex>;

    /**
     * Checks if any solution to the problem exists.
     */
    template <typename Problem>
    bool check_if_solution_exists(Problem &problem) {
        auto const &g = problem.get_graph();
        auto const &index = problem.get_index();
        m_min_cut.init(num_vertices(g));

        for (auto e : boost::as_array(edges(g))) {
            auto u = get(index, source(e, g));
            auto v = get(index, target(e, g));
            m_min_cut.add_edge_to_graph(u, v, 1, 1);
        }

        for (auto res : problem.get_restrictions_vec()) {
            if (check_violation(res, problem)) {
                return false;
            }
        }

        return true;
    }

    /**
     * Returns an iterator range of violated constraint candidates.
     */
    template <typename Problem, typename LP>
    auto get_violation_candidates(const Problem &problem, const LP &lp)
        ->decltype(problem.get_restrictions_vec()) {

        fill_auxiliary_digraph(problem, lp);
        return problem.get_restrictions_vec();
    }

    /**
     * Checks if the given constraint candidate is violated an if it is,
     * returns the violation value and violated constraint ID.
     */
    template <typename Problem>
    Violation check_violation(Candidate candidate, const Problem &problem) {
        double violation =
            find_violation(candidate.first, candidate.second, problem);
        if (problem.get_compare().g(violation, 0)) {
            return violation;
        } else {
            return Violation{};
        }
    }

    /**
     * Adds a violated constraint to the LP.
     */
    template <typename Problem, typename LP>
    void add_violated_constraint(Candidate violation, const Problem &problem,
                                 LP &lp) {
        if (violation != m_min_cut.get_last_cut()) {
            find_violation(violation.first, violation.second, problem);
        }

        auto const &g = problem.get_graph();
        auto const &index = problem.get_index();
        auto restriction =
            problem.get_max_restriction(violation.first, violation.second);

        for (auto const &e : problem.get_edges_in_solution()) {
            if (is_edge_in_violating_cut(e, g, index)) {
                --restriction;
            }
        }

        lp::linear_expression expr;
        for (auto const &e : problem.get_edge_map()) {
            if (is_edge_in_violating_cut(e.second, g, index)) {
                expr += e.first;
            }
        }

        lp.add_row(std::move(expr) >= restriction);
    }

  private:

    /**
     * Checks if a given edge belongs to the cut given by the current violating
     * set.
     */
    template <typename Edge, typename Graph, typename Index>
    bool is_edge_in_violating_cut(Edge edge, const Graph &g,
                                  const Index &index) {
        auto u = get(index, source(edge, g));
        auto v = get(index, target(edge, g));
        return m_min_cut.is_in_source_set(u) != m_min_cut.is_in_source_set(v);
    }

    /**
     * Creates the auxiliary directed graph used for feasibility testing.
     */
    template <typename Problem, typename LP>
    void fill_auxiliary_digraph(Problem &problem, const LP &lp) {
        auto const &g = problem.get_graph();
        auto const &index = problem.get_index();
        m_min_cut.init(num_vertices(g));

        for (auto const &e : problem.get_edge_map()) {
            lp::col_id col_idx = e.first;
            double col_val = lp.get_col_value(col_idx);

            if (problem.get_compare().g(col_val, 0)) {
                auto u = get(index, source(e.second, g));
                auto v = get(index, target(e.second, g));
                m_min_cut.add_edge_to_graph(u, v, col_val, col_val);
            }
        }

        for (auto const &e : problem.get_edges_in_solution()) {
            auto u = get(index, source(e, g));
            auto v = get(index, target(e, g));
            m_min_cut.add_edge_to_graph(u, v, 1, 1);
        }
    }

    /**
     * Finds the most violated set of vertices containing \c src and not
     * containing \c trg and returns its violation value.
     * @param src vertex to be contained in the violating set
     * @param trg vertex not to be contained in the violating set
     * @param problem problem object
     * @return violation of the found set
     */
    template <typename Problem>
    double find_violation(AuxVertex src, AuxVertex trg,
                          const Problem &problem) {
        double min_cut_weight = m_min_cut.find_min_cut(src, trg);
        double restriction = problem.get_max_restriction(src, trg);
        return restriction - min_cut_weight;
    }

    min_cut_finder m_min_cut;
};

} //! ir
} //! paal
#endif // PAAL_STEINER_NETWORK_ORACLE_HPP


/* ============================================================================
 * Copyright (c) 2009-2019 BlueQuartz Software, LLC
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright notice, this
 * list of conditions and the following disclaimer in the documentation and/or
 * other materials provided with the distribution.
 *
 * Neither the name of BlueQuartz Software, the US Air Force, nor the names of its
 * contributors may be used to endorse or promote products derived from this software
 * without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
 * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * The code contained herein was partially funded by the followig contracts:
 *    United States Air Force Prime Contract FA8650-07-D-5800
 *    United States Air Force Prime Contract FA8650-10-D-5210
 *    United States Air Force Prime Contract FA8650-15-D-5231
 *    United States Prime Contract Navy N00173-07-C-2068
 *
 * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

#include "RotateSampleRefFrame.h"

#include <cmath>

#ifdef SIMPL_USE_PARALLEL_ALGORITHMS
#include <tbb/blocked_range3d.h>
#include <tbb/parallel_for.h>
#include <tbb/partitioner.h>
#endif

#include <QtCore/QTextStream>

#include <Eigen/Dense>

#include "SIMPLib/SIMPLibVersion.h"
#include "SIMPLib/Common/Constants.h"
#include "SIMPLib/DataContainers/DataContainer.h"
#include "SIMPLib/DataContainers/DataContainerArray.h"
#include "SIMPLib/FilterParameters/AbstractFilterParametersReader.h"
#include "SIMPLib/FilterParameters/AttributeMatrixSelectionFilterParameter.h"
#include "SIMPLib/FilterParameters/DynamicTableFilterParameter.h"
#include "SIMPLib/FilterParameters/FloatFilterParameter.h"
#include "SIMPLib/FilterParameters/FloatVec3FilterParameter.h"
#include "SIMPLib/FilterParameters/LinkedChoicesFilterParameter.h"
#include "SIMPLib/FilterParameters/SeparatorFilterParameter.h"
#include "SIMPLib/Geometry/ImageGeom.h"
#include "SIMPLib/Math/MatrixMath.h"

namespace
{
struct RotateArgs
{
  int64_t xp = 0;
  int64_t yp = 0;
  int64_t zp = 0;
  float xRes = 0.0f;
  float yRes = 0.0f;
  float zRes = 0.0f;
  int64_t xpNew = 0;
  int64_t ypNew = 0;
  int64_t zpNew = 0;
  float xResNew = 0.0f;
  float yResNew = 0.0f;
  float zResNew = 0.0f;
  float xMinNew = 0.0f;
  float yMinNew = 0.0f;
  float zMinNew = 0.0f;
};

using Matrix3fR = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>;

constexpr float k_Threshold = 0.0001f;

const Eigen::Vector3f k_XAxis = Eigen::Vector3f::UnitX();
const Eigen::Vector3f k_YAxis = Eigen::Vector3f::UnitY();
const Eigen::Vector3f k_ZAxis = Eigen::Vector3f::UnitZ();

// Requires table to be 3 x 3
Matrix3fR tableToMatrix(const std::vector<std::vector<double>>& table)
{
  Matrix3fR matrix;

  for(size_t i = 0; i < table.size(); i++)
  {
    const auto& row = table[i];
    for(size_t j = 0; j < row.size(); j++)
    {
      matrix(i, j) = row[j];
    }
  }

  return matrix;
}

void determineMinMax(const Matrix3fR& rotationMatrix, const FloatVec3Type& spacing, size_t col, size_t row, size_t plane, float& xMin, float& xMax, float& yMin, float& yMax, float& zMin, float& zMax)
{
  Eigen::Vector3f coords(static_cast<float>(col) * spacing[0], static_cast<float>(row) * spacing[1], static_cast<float>(plane) * spacing[2]);

  Eigen::Vector3f newCoords = rotationMatrix * coords;

  xMin = std::min(newCoords[0], xMin);
  xMax = std::max(newCoords[0], xMax);

  yMin = std::min(newCoords[1], yMin);
  yMax = std::max(newCoords[1], yMax);

  zMin = std::min(newCoords[2], zMin);
  zMax = std::max(newCoords[2], zMax);
}

float cosBetweenVectors(const Eigen::Vector3f& a, const Eigen::Vector3f& b)
{
  float normA = a.norm();
  float normB = b.norm();

  if(normA == 0.0f || normB == 0.0f)
  {
    return 1.0f;
  }

  return a.dot(b) / (normA * normB);
}

float determineSpacing(const FloatVec3Type& spacing, const Eigen::Vector3f& axisNew)
{
  float xAngle = std::abs(cosBetweenVectors(k_XAxis, axisNew));
  float yAngle = std::abs(cosBetweenVectors(k_YAxis, axisNew));
  float zAngle = std::abs(cosBetweenVectors(k_ZAxis, axisNew));

  std::array<float, 3> axes = {xAngle, yAngle, zAngle};

  auto iter = std::max_element(axes.cbegin(), axes.cend());

  size_t index = std::distance(axes.cbegin(), iter);

  return spacing[index];
}

RotateArgs createRotateParams(const ImageGeom& imageGeom, const Matrix3fR& rotationMatrix)
{
  const SizeVec3Type origDims = imageGeom.getDimensions();
  const FloatVec3Type spacing = imageGeom.getSpacing();
  // const FloatVec3Type origin = imageGeom.getOrigin();

  float xMin = std::numeric_limits<float>::max();
  float xMax = std::numeric_limits<float>::min();
  float yMin = std::numeric_limits<float>::max();
  float yMax = std::numeric_limits<float>::min();
  float zMin = std::numeric_limits<float>::max();
  float zMax = std::numeric_limits<float>::min();

  const std::vector<std::vector<size_t>> coords{{0, 0, 0},
                                                {origDims[0] - 1, 0, 0},
                                                {0, origDims[1] - 1, 0},
                                                {origDims[0] - 1, origDims[1] - 1, 0},
                                                {0, 0, origDims[2] - 1},
                                                {origDims[0] - 1, 0, origDims[2] - 1},
                                                {0, origDims[1] - 1, origDims[2] - 1},
                                                {origDims[0] - 1, origDims[1] - 1, origDims[2] - 1}};

  for(const auto& item : coords)
  {
    determineMinMax(rotationMatrix, spacing, item[0], item[1], item[2], xMin, xMax, yMin, yMax, zMin, zMax);
  }

  Eigen::Vector3f xAxisNew = rotationMatrix * k_XAxis;
  Eigen::Vector3f yAxisNew = rotationMatrix * k_YAxis;
  Eigen::Vector3f zAxisNew = rotationMatrix * k_ZAxis;

  float xResNew = determineSpacing(spacing, xAxisNew);
  float yResNew = determineSpacing(spacing, yAxisNew);
  float zResNew = determineSpacing(spacing, zAxisNew);

  MeshIndexType xpNew = static_cast<int64_t>(std::nearbyint((xMax - xMin) / xResNew) + 1);
  MeshIndexType ypNew = static_cast<int64_t>(std::nearbyint((yMax - yMin) / yResNew) + 1);
  MeshIndexType zpNew = static_cast<int64_t>(std::nearbyint((zMax - zMin) / zResNew) + 1);

  RotateArgs params;

  params.xp = origDims[0];
  params.xRes = spacing[0];
  params.yp = origDims[1];
  params.yRes = spacing[1];
  params.zp = origDims[2];
  params.zRes = spacing[2];

  params.xpNew = xpNew;
  params.xResNew = xResNew;
  params.xMinNew = xMin;
  params.ypNew = ypNew;
  params.yResNew = yResNew;
  params.yMinNew = yMin;
  params.zpNew = zpNew;
  params.zResNew = zResNew;
  params.zMinNew = zMin;

  return params;
}

void updateGeometry(ImageGeom& imageGeom, const RotateArgs& params)
{
  FloatVec3Type origin = imageGeom.getOrigin();

  imageGeom.setSpacing(params.xResNew, params.yResNew, params.zResNew);
  imageGeom.setDimensions(params.xpNew, params.ypNew, params.zpNew);
  origin[0] += params.xMinNew;
  origin[1] += params.yMinNew;
  origin[2] += params.zMinNew;
  imageGeom.setOrigin(origin);
}

/**
 * @brief The RotateSampleRefFrameImpl class implements a threaded algorithm to do the
 * actual computation of the rotation by applying the rotation to each Euler angle
 */
class SampleRefFrameRotator
{
  DataArray<int64_t>::Pointer m_NewIndicesPtr;
  float m_RotMatrixInv[3][3] = {{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}};
  bool m_SliceBySlice = false;
  RotateArgs m_Params;

public:
  SampleRefFrameRotator(DataArray<int64_t>::Pointer newindices, const RotateArgs& args, const Matrix3fR& rotationMatrix, bool sliceBySlice)
  : m_NewIndicesPtr(newindices)
  , m_SliceBySlice(sliceBySlice)
  , m_Params(args)
  {
    // We have to inline the 3x3 Maxtrix transpose here because of the "const" nature of the 'convert' function
    Matrix3fR transpose = rotationMatrix.transpose();
    // Need to use row based Eigen matrix so that the values get mapped to the right place in the raw array
    // Raw array is faster than Eigen
    Eigen::Map<Matrix3fR>(&m_RotMatrixInv[0][0], transpose.rows(), transpose.cols()) = transpose;
  }

  ~SampleRefFrameRotator() = default;

  void convert(int64_t zStart, int64_t zEnd, int64_t yStart, int64_t yEnd, int64_t xStart, int64_t xEnd) const
  {
    int64_t* newindicies = m_NewIndicesPtr->getPointer(0);

    for(int64_t k = zStart; k < zEnd; k++)
    {
      int64_t ktot = (m_Params.xpNew * m_Params.ypNew) * k;
      for(int64_t j = yStart; j < yEnd; j++)
      {
        int64_t jtot = (m_Params.xpNew) * j;
        for(int64_t i = xStart; i < xEnd; i++)
        {
          int64_t index = ktot + jtot + i;
          newindicies[index] = -1;

          float coords[3] = {0.0f, 0.0f, 0.0f};
          float coordsNew[3] = {0.0f, 0.0f, 0.0f};

          coords[0] = (static_cast<float>(i) * m_Params.xResNew) + m_Params.xMinNew;
          coords[1] = (static_cast<float>(j) * m_Params.yResNew) + m_Params.yMinNew;
          coords[2] = (static_cast<float>(k) * m_Params.zResNew) + m_Params.zMinNew;

          MatrixMath::Multiply3x3with3x1(m_RotMatrixInv, coords, coordsNew);

          int64_t colOld = static_cast<int64_t>(std::nearbyint(coordsNew[0] / m_Params.xRes));
          int64_t rowOld = static_cast<int64_t>(std::nearbyint(coordsNew[1] / m_Params.yRes));
          int64_t planeOld = static_cast<int64_t>(std::nearbyint(coordsNew[2] / m_Params.zRes));

          if(m_SliceBySlice)
          {
            planeOld = k;
          }

          if(colOld >= 0 && colOld < m_Params.xp && rowOld >= 0 && rowOld < m_Params.yp && planeOld >= 0 && planeOld < m_Params.zp)
          {
            newindicies[index] = (m_Params.xp * m_Params.yp * planeOld) + (m_Params.xp * rowOld) + colOld;
          }
        }
      }
    }
  }

#ifdef SIMPL_USE_PARALLEL_ALGORITHMS
  void operator()(const tbb::blocked_range3d<int64_t, int64_t, int64_t>& r) const
  {
    convert(r.pages().begin(), r.pages().end(), r.rows().begin(), r.rows().end(), r.cols().begin(), r.cols().end());
  }
#endif
};

} // namespace

struct RotateSampleRefFrame::Impl
{
  Matrix3fR m_RotationMatrix = Matrix3fR::Zero();
  RotateArgs m_Params;

  void reset()
  {
    m_RotationMatrix.setZero();

    m_Params = RotateArgs();
  }
};

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
RotateSampleRefFrame::RotateSampleRefFrame()
: p_Impl(std::make_unique<Impl>())
{
  std::vector<std::vector<double>> defaultTable{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}};

  m_RotationTable.setTableData(defaultTable);
  m_RotationTable.setDynamicRows(false);
  m_RotationTable.setDynamicCols(false);
  m_RotationTable.setDefaultColCount(3);
  m_RotationTable.setDefaultRowCount(3);
  m_RotationTable.setMinCols(3);
  m_RotationTable.setMinRows(3);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
RotateSampleRefFrame::~RotateSampleRefFrame() = default;

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void RotateSampleRefFrame::setupFilterParameters()
{
  FilterParameterVectorType parameters;

  {
    LinkedChoicesFilterParameter::Pointer parameter = LinkedChoicesFilterParameter::New();
    parameter->setHumanLabel("Rotation Representation");
    parameter->setPropertyName("RotationRepresentationChoice");
    parameter->setSetterCallback(SIMPL_BIND_SETTER(RotateSampleRefFrame, this, RotationRepresentationChoice));
    parameter->setGetterCallback(SIMPL_BIND_GETTER(RotateSampleRefFrame, this, RotationRepresentationChoice));
    std::vector<QString> choices{"Axis Angle", "Rotation Matrix"};
    parameter->setChoices(choices);
    std::vector<QString> linkedProps{"RotationAngle", "RotationAxis", "RotationTable"};
    parameter->setLinkedProperties(linkedProps);
    parameter->setEditable(false);
    parameter->setCategory(FilterParameter::Category::Parameter);
    parameters.push_back(parameter);
  }

  // Axis Angle Parameters

  parameters.push_back(SIMPL_NEW_FLOAT_FP("Rotation Angle (Degrees)", RotationAngle, FilterParameter::Category::Parameter, RotateSampleRefFrame, 0));
  parameters.push_back(SIMPL_NEW_FLOAT_VEC3_FP("Rotation Axis (ijk)", RotationAxis, FilterParameter::Category::Parameter, RotateSampleRefFrame, 0));

  // Rotation Matrix Parameters

  parameters.push_back(SIMPL_NEW_DYN_TABLE_FP("Rotation Matrix", RotationTable, FilterParameter::Category::Parameter, RotateSampleRefFrame, 1));

  // Required Arrays

  parameters.push_back(SeparatorFilterParameter::Create("Cell Data", FilterParameter::Category::RequiredArray));
  {
    AttributeMatrixSelectionFilterParameter::RequirementType req = AttributeMatrixSelectionFilterParameter::CreateRequirement(AttributeMatrix::Type::Cell, IGeometry::Type::Image);
    parameters.push_back(SIMPL_NEW_AM_SELECTION_FP("Cell Attribute Matrix", CellAttributeMatrixPath, FilterParameter::Category::RequiredArray, RotateSampleRefFrame, req));
  }

  setFilterParameters(parameters);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void RotateSampleRefFrame::readFilterParameters(AbstractFilterParametersReader* reader, int index)
{
  reader->openFilterGroup(this, index);
  setCellAttributeMatrixPath(reader->readDataArrayPath("CellAttributeMatrixPath", getCellAttributeMatrixPath()));
  setRotationAxis(reader->readFloatVec3("RotationAxis", getRotationAxis()));
  setRotationAngle(reader->readValue("RotationAngle", getRotationAngle()));
  reader->closeFilterGroup();
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void RotateSampleRefFrame::initialize()
{
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void RotateSampleRefFrame::dataCheck()
{
  clearErrorCode();
  clearWarningCode();

  p_Impl->reset();

  if(getErrorCode() < 0)
  {
    return;
  }

  if(!isRotationRepresentationValid(m_RotationRepresentationChoice))
  {
    QString ss = QObject::tr("Invalid rotation representation");
    setErrorCondition(-45001, ss);
    return;
  }

  getDataContainerArray()->getPrereqGeometryFromDataContainer<ImageGeom>(this, getCellAttributeMatrixPath().getDataContainerName());
  getDataContainerArray()->getPrereqAttributeMatrixFromPath(this, getCellAttributeMatrixPath(), -301);
  if(getErrorCode() < 0)
  {
    return;
  }

  DataContainer::Pointer m = getDataContainerArray()->getDataContainer(getCellAttributeMatrixPath().getDataContainerName());

  if(m == nullptr)
  {
    QString ss = QObject::tr("Failed to get DataContainer '%1'").arg(getCellAttributeMatrixPath().getDataContainerName());
    setErrorCondition(-45002, ss);
    return;
  }

  ImageGeom::Pointer imageGeom = m->getGeometryAs<ImageGeom>();

  if(imageGeom == nullptr)
  {
    QString ss = QObject::tr("Failed to get Image Geometry from '%1'").arg(getCellAttributeMatrixPath().getDataContainerName());
    setErrorCondition(-45002, ss);
    return;
  }

  const RotationRepresentation representation = getRotationRepresentation();

  switch(representation)
  {
  case RotationRepresentation::AxisAngle: {
    const Eigen::Vector3f rotationAxis(m_RotationAxis.data());
    float norm = rotationAxis.norm();
    if(!SIMPLibMath::closeEnough(rotationAxis.norm(), 1.0f, k_Threshold))
    {
      QString ss = QObject::tr("Axis angle is not normalized (norm is %1). Filter will automatically normalize the value.").arg(norm);
      setWarningCondition(-45003, ss);
    }

    float rotationAngleRadians = m_RotationAngle * SIMPLib::Constants::k_DegToRadD;

    Eigen::AngleAxisf axisAngle(rotationAngleRadians, rotationAxis.normalized());

    p_Impl->m_RotationMatrix = axisAngle.toRotationMatrix();
  }
  break;
  case RotationRepresentation::RotationMatrix: {
    auto rotationMatrixTable = m_RotationTable.getTableData();

    if(rotationMatrixTable.size() != 3)
    {
      QString ss = QObject::tr("Rotation Matrix must be 3 x 3");
      setErrorCondition(-45004, ss);
      return;
    }

    for(const auto& row : rotationMatrixTable)
    {
      if(row.size() != 3)
      {
        QString ss = QObject::tr("Rotation Matrix must be 3 x 3");
        setErrorCondition(-45005, ss);
        return;
      }
    }

    Matrix3fR rotationMatrix = tableToMatrix(rotationMatrixTable);

    float determinant = rotationMatrix.determinant();

    if(!SIMPLibMath::closeEnough(determinant, 1.0f, k_Threshold))
    {
      QString ss = QObject::tr("Rotation Matrix must have a determinant of 1 (is %1)").arg(determinant);
      setErrorCondition(-45006, ss);
      return;
    }

    Matrix3fR transpose = rotationMatrix.transpose();
    Matrix3fR inverse = rotationMatrix.inverse();

    if(!transpose.isApprox(inverse, k_Threshold))
    {
      QString ss = QObject::tr("Rotation Matrix's inverse and transpose must be equal");
      setErrorCondition(-45007, ss);
      return;
    }

    p_Impl->m_RotationMatrix = rotationMatrix;
  }
  break;
  default: {
    QString ss = QObject::tr("Invalid rotation representation");
    setErrorCondition(-45008, ss);
    return;
  }
  }

  p_Impl->m_Params = createRotateParams(*imageGeom, p_Impl->m_RotationMatrix);

  updateGeometry(*imageGeom, p_Impl->m_Params);

  // Resize attribute matrix

  std::vector<size_t> tDims(3);
  tDims[0] = p_Impl->m_Params.xpNew;
  tDims[1] = p_Impl->m_Params.ypNew;
  tDims[2] = p_Impl->m_Params.zpNew;
  QString attrMatName = getCellAttributeMatrixPath().getAttributeMatrixName();
  m->getAttributeMatrix(attrMatName)->resizeAttributeArrays(tDims);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void RotateSampleRefFrame::execute()
{
  dataCheck();
  if(getErrorCode() < 0)
  {
    return;
  }

  DataContainer::Pointer m = getDataContainerArray()->getDataContainer(getCellAttributeMatrixPath().getDataContainerName());

  if(m == nullptr)
  {
    QString ss = QObject::tr("Failed to get DataContainer '%1'").arg(getCellAttributeMatrixPath().getDataContainerName());
    setErrorCondition(-45101, ss);
    return;
  }

  int64_t newNumCellTuples = p_Impl->m_Params.xpNew * p_Impl->m_Params.ypNew * p_Impl->m_Params.zpNew;

  DataArray<int64_t>::Pointer newIndiciesPtr = DataArray<int64_t>::CreateArray(newNumCellTuples, std::string("_INTERNAL_USE_ONLY_RotateSampleRef_NewIndicies"), true);
  newIndiciesPtr->initializeWithValue(-1);
  int64_t* newindicies = newIndiciesPtr->getPointer(0);

#ifdef SIMPL_USE_PARALLEL_ALGORITHMS
  tbb::parallel_for(tbb::blocked_range3d<int64_t, int64_t, int64_t>(0, p_Impl->m_Params.zpNew, 0, p_Impl->m_Params.ypNew, 0, p_Impl->m_Params.xpNew),
                    SampleRefFrameRotator(newIndiciesPtr, p_Impl->m_Params, p_Impl->m_RotationMatrix, m_SliceBySlice), tbb::auto_partitioner());
#else
  {
    SampleRefFrameRotator serial(newIndiciesPtr, p_Impl->m_Params, p_Impl->m_RotationMatrix, m_SliceBySlice);
    serial.convert(0, p_Impl->m_Params.zpNew, 0, p_Impl->m_Params.ypNew, 0, p_Impl->m_Params.xpNew);
  }
#endif

  // This could technically be parallelized also where each thread takes an array to adjust. Except
  // that the DataContainer is NOT thread safe or re-entrant so that would actually be a BAD idea.

  QString attrMatName = getCellAttributeMatrixPath().getAttributeMatrixName();
  QList<QString> voxelArrayNames = m->getAttributeMatrix(attrMatName)->getAttributeArrayNames();

  for(const auto& attrArrayName : voxelArrayNames)
  {
    IDataArray::Pointer p = m->getAttributeMatrix(attrMatName)->getAttributeArray(attrArrayName);

    // Make a copy of the 'p' array that has the same name. When placed into
    // the data container this will over write the current array with
    // the same name.

    IDataArray::Pointer data = p->createNewArray(newNumCellTuples, p->getComponentDimensions(), p->getName());
    int64_t newIndicies_I = 0;
    for(size_t i = 0; i < static_cast<size_t>(newNumCellTuples); i++)
    {
      newIndicies_I = newindicies[i];
      if(newIndicies_I >= 0)
      {
        if(!data->copyFromArray(i, p, newIndicies_I, 1))
        {
          QString ss = QObject::tr("copyFromArray Failed: ");
          QTextStream out(&ss);
          out << "Source Array Name: " << p->getName() << " Source Tuple Index: " << newIndicies_I << "\n";
          out << "Dest Array Name: " << data->getName() << "  Dest. Tuple Index: " << i << "\n";
          setErrorCondition(-45102, ss);
          return;
        }
      }
      else
      {
        int var = 0;
        data->initializeTuple(i, &var);
      }
    }
    m->getAttributeMatrix(attrMatName)->insertOrAssign(data);
  }
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
AbstractFilter::Pointer RotateSampleRefFrame::newFilterInstance(bool copyFilterParameters) const
{
  RotateSampleRefFrame::Pointer filter = RotateSampleRefFrame::New();
  if(copyFilterParameters)
  {
    copyFilterParameterInstanceVariables(filter.get());
  }
  return filter;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString RotateSampleRefFrame::getCompiledLibraryName() const
{
  return Core::CoreBaseName;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString RotateSampleRefFrame::getBrandingString() const
{
  return "SIMPLib Core Filter";
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString RotateSampleRefFrame::getFilterVersion() const
{
  QString version;
  QTextStream vStream(&version);
  vStream << SIMPLib::Version::Major() << "." << SIMPLib::Version::Minor() << "." << SIMPLib::Version::Patch();
  return version;
}
// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString RotateSampleRefFrame::getGroupName() const
{
  return SIMPL::FilterGroups::SamplingFilters;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QUuid RotateSampleRefFrame::getUuid() const
{
  return QUuid("{e25d9b4c-2b37-578c-b1de-cf7032b5ef19}");
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString RotateSampleRefFrame::getSubGroupName() const
{
  return SIMPL::FilterSubGroups::RotationTransformationFilters;
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
QString RotateSampleRefFrame::getHumanLabel() const
{
  return "Rotate Sample Reference Frame";
}

// -----------------------------------------------------------------------------
RotateSampleRefFrame::Pointer RotateSampleRefFrame::NullPointer()
{
  return Pointer(static_cast<Self*>(nullptr));
}

// -----------------------------------------------------------------------------
std::shared_ptr<RotateSampleRefFrame> RotateSampleRefFrame::New()
{
  struct make_shared_enabler : public RotateSampleRefFrame
  {
  };
  std::shared_ptr<make_shared_enabler> val = std::make_shared<make_shared_enabler>();
  val->setupFilterParameters();
  return val;
}

// -----------------------------------------------------------------------------
QString RotateSampleRefFrame::getNameOfClass() const
{
  return QString("RotateSampleRefFrame");
}

// -----------------------------------------------------------------------------
QString RotateSampleRefFrame::ClassName()
{
  return QString("RotateSampleRefFrame");
}

// -----------------------------------------------------------------------------
void RotateSampleRefFrame::setCellAttributeMatrixPath(const DataArrayPath& value)
{
  m_CellAttributeMatrixPath = value;
}

// -----------------------------------------------------------------------------
DataArrayPath RotateSampleRefFrame::getCellAttributeMatrixPath() const
{
  return m_CellAttributeMatrixPath;
}

// -----------------------------------------------------------------------------
void RotateSampleRefFrame::setRotationAxis(const FloatVec3Type& value)
{
  m_RotationAxis = value;
}

// -----------------------------------------------------------------------------
FloatVec3Type RotateSampleRefFrame::getRotationAxis() const
{
  return m_RotationAxis;
}

// -----------------------------------------------------------------------------
void RotateSampleRefFrame::setRotationAngle(float value)
{
  m_RotationAngle = value;
}

// -----------------------------------------------------------------------------
float RotateSampleRefFrame::getRotationAngle() const
{
  return m_RotationAngle;
}

// -----------------------------------------------------------------------------
void RotateSampleRefFrame::setSliceBySlice(bool value)
{
  m_SliceBySlice = value;
}

// -----------------------------------------------------------------------------
bool RotateSampleRefFrame::getSliceBySlice() const
{
  return m_SliceBySlice;
}

// -----------------------------------------------------------------------------
void RotateSampleRefFrame::setRotationTable(const DynamicTableData& value)
{
  m_RotationTable = value;
}

// -----------------------------------------------------------------------------
DynamicTableData RotateSampleRefFrame::getRotationTable() const
{
  return m_RotationTable;
}

// -----------------------------------------------------------------------------
void RotateSampleRefFrame::setRotationRepresentationChoice(int value)
{
  m_RotationRepresentationChoice = value;
}

// -----------------------------------------------------------------------------
int RotateSampleRefFrame::getRotationRepresentationChoice() const
{
  return m_RotationRepresentationChoice;
}

// -----------------------------------------------------------------------------
RotateSampleRefFrame::RotationRepresentation RotateSampleRefFrame::getRotationRepresentation() const
{
  return static_cast<RotationRepresentation>(m_RotationRepresentationChoice);
}

// -----------------------------------------------------------------------------
void RotateSampleRefFrame::setRotationRepresentation(RotationRepresentation value)
{
  m_RotationRepresentationChoice = static_cast<int>(value);
}

// -----------------------------------------------------------------------------
bool RotateSampleRefFrame::isRotationRepresentationValid(int value) const
{
  return (value >= 0) && (value <= 1);
}


#ifndef DFG_PHYSICS_DIDNITUL
#define DFG_PHYSICS_DIDNITUL

#include "dfgDefs.hpp"
#include "physics/constants.hpp"
#include "math/constants.hpp"
#include "math/pow.hpp"
#include "build/utils.hpp"
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/temperature.hpp>
#include <boost/units/systems/si/frequency.hpp>
#include <boost/units/systems/si/energy.hpp>
#include <boost/units/systems/si/temperature.hpp>
#include <boost/units/systems/temperature/celsius.hpp>
#include <boost/units/systems/temperature/fahrenheit.hpp>

DFG_ROOT_NS_BEGIN { DFG_SUB_NS(physics) {

class CelsiusType
{
public:
	explicit CelsiusType(double val) :
	  m_val(val)
	{}

	double value() const {return m_val;}

	double m_val;
};

const auto kelvin		= boost::units::si::kelvin;
const auto celsius		= boost::units::celsius::degrees;
const auto fahrenheit	= boost::units::fahrenheit::degrees;

class InvalidType {};

inline double photonEnergy_RawSi(const double& fInHz)
{
	return const_hPlanck_rawSiValue * fInHz;
}

inline double photonEnergyVacuumWavelength_RawSi(const double& waveLengthInM)
{
	return photonEnergy_RawSi(const_speedOfLight_rawSiValue / waveLengthInM);
}

template <class FromQuantity_T, class ToUnit_T> inline InvalidType convertTo(const FromQuantity_T&, const ToUnit_T&)
{
	DFG_BUILD_GENERATE_FAILURE_IF_INSTANTIATED(FromQuantity_T,
												DFG_CURRENT_FUNCTION_NAME ": not implemented. Note that e.g. for temperatures "
												"either convertToAbsolute or convertToDifference should be used.");
	return InvalidType();
}

template <class FromQuantity_T, class ToUnit_T> inline InvalidType convertToAbsolute(const FromQuantity_T&, const ToUnit_T&)
{
	DFG_BUILD_GENERATE_FAILURE_IF_INSTANTIATED(FromQuantity_T, DFG_CURRENT_FUNCTION_NAME ": not implemented.");
	return InvalidType();
}

template <class FromQuantity_T, class ToUnit_T> inline InvalidType convertToDifference(const FromQuantity_T&, const ToUnit_T&)
{
	DFG_BUILD_GENERATE_FAILURE_IF_INSTANTIATED(FromQuantity_T, DFG_CURRENT_FUNCTION_NAME ": not implemented.");
	return InvalidType();
}

// Celsius -> Kelvin
auto inline convertToAbsolute(const decltype(1.0 * celsius)& from, const decltype(kelvin)&) -> decltype(from.value() * kelvin)
{
	return (from.value() + 273.15) * kelvin;
}

// Kelvin -> Celsius
auto inline convertToAbsolute(const decltype(1.0 * kelvin)& from, const decltype(celsius)&) -> decltype(from.value() * celsius)
{
	return (from.value() - 273.15) * celsius;
}

auto inline convertToDifference(const decltype(1.0 * celsius)& from, const decltype(kelvin)&) -> decltype(from.value() * kelvin)
{
	return from.value() * kelvin;
}

auto inline convertToDifference(const decltype(1.0 * kelvin)& from, const decltype(celsius)&) -> decltype(from.value() * celsius)
{
	return from.value() * celsius;
}

// Wikipedia http://en.wikipedia.org/wiki/Planck%27s_law
// TODO: test
inline double planckBlackBodySpectralEnergyDensityByFrequency_RawSi(const double TinK, const double fInHz)
{
	const double factor = 8.0 * DFG_SUB_NS_NAME(math)::const_pi * const_hPlanck_rawSiValue / DFG_SUB_NS_NAME(math)::pow3(const_speedOfLight_rawSiValue);
	const double beta = 1.0 / (const_k_B_rawSiValue * TinK);
	return factor * DFG_SUB_NS_NAME(math)::pow3(fInHz) / (exp(photonEnergy_RawSi(fInHz) * beta) - 1);
}

// Wikipedia http://en.wikipedia.org/wiki/Planck%27s_law
// TODO: test
inline double planckBlackBodySpectralEnergyDensityByWaveLength_RawSi(const double TinK, const double wavelengthInM)
{
	const double factor = 8.0 * DFG_SUB_NS_NAME(math)::const_pi * const_hPlanck_rawSiValue * const_speedOfLight_rawSiValue;
	const double beta = 1.0 / (const_k_B_rawSiValue * TinK);
	return factor * (1.0 / DFG_SUB_NS_NAME(math)::pow5(wavelengthInM)) * (1.0 / (exp(photonEnergyVacuumWavelength_RawSi(wavelengthInM) * beta) - 1));
}

}} // module physics

#endif // include guard


/*
 * func_ver.cpp
 *
 *  Created on: 04.07.2012
 *      Author: stephaniebayer
 */

#include "func_ver.h"
#include<vector>
#include "Cipher_elg.h"
#include "G_q.h"
#include "Mod_p.h"
#include "Functions.h"
#include "ElGammal.h"
#include "multi_expo.h"
#include <fstream>

#include <time.h>
#include <NTL/ZZ.h>
NTL_CLIENT

extern G_q G;
extern G_q H;
extern Pedersen Ped;
extern ElGammal El;

func_ver::func_ver() {
	// TODO Auto-generated constructor stub

}

func_ver::~func_ver() {
	// TODO Auto-generated destructor stub
}

int func_ver::check_Dh(vector<Mod_p>* c_Dh, vector<ZZ>* chal, vector<ZZ>* D_h_bar, ZZ r_Dh_bar){
	long i;
	Mod_p t_Dh, co_Dh, temp;
	long m = c_Dh->size();

	t_Dh =c_Dh->at(0);
	for (i=1; i<m;i++){
		Mod_p::expo(temp,c_Dh->at(i),chal->at(i-1));
		Mod_p::mult(t_Dh,t_Dh,temp);
	}
	co_Dh = Ped.commit(D_h_bar,r_Dh_bar);
/*	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"D_h "<<t_Dh<<" "<<co_Dh<<endl;*/
	if (t_Dh == co_Dh){
		return 1;
	}
	return 0;
}

int func_ver::check_D(Mod_p c_D0, Mod_p c_z, vector<Mod_p>* c_A, vector<Mod_p>* c_B, vector<ZZ>* chal_1, ZZ chal_2, vector<ZZ>* A_bar, ZZ r_A_bar, long n){
	int i;
	Mod_p t_D, co_D, temp, inv;
	long m = c_A->size()-1;
	ZZ ord = H.get_ord();
	vector<ZZ>* v_1 = new vector<ZZ>(n);

	t_D = c_D0;
	Mod_p::inv(inv, c_z);
	for (i=1; i<m;i++){
		Mod_p::expo(temp,c_A->at(i),chal_2);
		Mod_p::mult(temp, temp,c_B->at(i));
		Mod_p::mult(temp,temp,inv);
		Mod_p::expo(temp, temp, chal_1->at(i-1));
		Mod_p::mult(t_D,t_D,temp);
	}
	for(i=0; i<n;i++){
		NegateMod(v_1->at(i),to_ZZ(1),ord);
	}
	temp=Ped.commit(v_1,to_ZZ(0));
	Mod_p::expo(temp, temp, chal_1->at(m-1));
	Mod_p::mult(t_D,t_D,temp);
	co_D = Ped.commit(A_bar, r_A_bar);

	/*string name = "example.txt";
	ofstream s;
	ost.open(name.c_str(),ios::app);
	ost<<"D "<<t_D<<" "<<co_D<<endl;*/
	delete v_1;
	if(t_D == co_D){
		return 1;
	}
	return 0;
}

int func_ver::check_Ds(vector<Mod_p>* c_Ds, vector<Mod_p>* c_Dh, Mod_p c_Dm, vector<ZZ>* chal_1, vector<ZZ>* chal_2, vector<ZZ>* Ds_bar, ZZ r_Ds_bar){
	long i,l;
	long m= c_Ds->size()-1;
	Mod_p t_Ds, co_Ds, temp, temp_1;

	l=m-1;
	for(i=0; i<l; i++){
		Mod_p::expo(c_Ds->at(i),c_Dh->at(i), chal_1->at(i));
	}
	if(m>1){
		Mod_p::expo(temp, c_Dh->at(1), chal_1->at(0));
		for(i=1;i<l; i++){
			Mod_p::expo(temp_1,c_Dh->at(i+1), chal_1->at(i));
			Mod_p::mult(temp,temp,temp_1);
		}
		c_Ds->at(l)=temp;
	}
	else{
		c_Ds->at(l)=Mod_p(1,G.get_mod());
	}

	c_Ds->at(m)=c_Dm;
	Mod_p::expo(t_Ds, c_Ds->at(0),chal_2->at(m-1));
	for(i=1; i<m; i++){
		Mod_p::expo(temp, c_Ds->at(i), chal_2->at(m-1-i));
		Mod_p::mult(t_Ds, t_Ds,temp);
	}
	Mod_p::mult(t_Ds,t_Ds,c_Ds->at(m));
	co_Ds = Ped.commit(Ds_bar, r_Ds_bar);

/*	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"Ds "<<t_Ds<<" "<<co_Ds<<endl;*/
	if(t_Ds == co_Ds){
		return 1;
	}
	return 0;
}

int func_ver::check_Dl(vector<Mod_p>* c_Dl, vector<ZZ>* chal_1, vector<ZZ>* A, vector<ZZ>* B, vector<ZZ>*  chal_2, ZZ r_Dl_bar){
	long i;
	Mod_p t_Dl, co_Dl, temp;
	ZZ temp_1;
	long l = c_Dl->size();
	long pos = (l-1)/2+1;
	ZZ mod = G.get_mod();
	t_Dl = c_Dl->at(0);
	for(i=1; i<l; i++){
		Mod_p::expo(temp, c_Dl->at(i),chal_1->at(i-1));
		Mod_p::mult(t_Dl, t_Dl, temp);
	}
	temp_1=Functions::bilinearMap(A,B,chal_2);
	co_Dl =Ped.commit(temp_1,r_Dl_bar);

/*	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"Dl "<<t_Dl<<" "<<co_Dl<<endl;*/

	temp = Mod_p(1,mod);
	if(t_Dl==co_Dl & c_Dl->at(pos) ==temp){
		return 1;
	}
	return 0;
}

int func_ver::check_d(vector<Mod_p>* c_Dh, Mod_p c_d, vector<ZZ>* chal, vector<ZZ>* d_bar, ZZ r_d_bar){
	Mod_p t_d, co_d, temp;
	long m = c_Dh->size();

	Mod_p::expo(temp, c_Dh->at(m-1), chal->at(0));
	Mod_p::mult(t_d, temp, c_d);
		co_d = Ped.commit(d_bar, r_d_bar);

	/*	string name = "example.txt";
		ofstream ost;
		ost.open(name.c_str(),ios::app);
		ost<<"d "<<t_d<<" "<<co_d<<endl;*/
	if(t_d==co_d){
		return 1;
	}
	return 0;
}

int func_ver::check_Delta(Mod_p c_dh, Mod_p c_Delta, vector<ZZ>* chal, vector<ZZ>* Delta_bar, vector<ZZ>* d_bar, ZZ r_Delta_bar, ZZ chal_1, ZZ chal_2, ZZ chal_3){
	long i,j;
	Mod_p t_Delta, co_Delta, temp;
	ZZ t_1, t_2, t_3, prod, chal_temp;
	ZZ ord = H.get_ord();
	long m = (chal->size()-1)/2;
	long n = Delta_bar->size();
	vector<ZZ>* Delta_temp=0;

	Mod_p::expo(temp, c_dh, chal->at(0));
	Mod_p::mult(t_Delta, temp, c_Delta);

	Delta_temp = new vector<ZZ>(n-1);
	t_3= chal->at(0);
	for(i=0; i<n-1; i++){
		MulMod(t_1, Delta_bar->at(i), d_bar->at(i+1), ord);
		MulMod(t_2, t_3, Delta_bar->at(i+1),ord);
		SubMod(Delta_temp->at(i), t_2, t_1, ord);
	}

	co_Delta = Ped.commit(Delta_temp, r_Delta_bar);

	delete Delta_temp;
/*	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"Delta "<<t_Delta<<" "<<c_Delta<<endl;*/

	prod = to_ZZ(1);
	chal_temp =to_ZZ(1);
	for(i=1; i<=m; i++){
		for(j=1; j<=n; j++){
			MulMod(chal_temp, chal_temp, chal_1,ord);
			SubMod(t_1, chal_temp, chal_2,ord);
			t_3 = n*(i-1)+j;
			MulMod(t_3,t_3, chal_3, ord);
			AddMod(t_1,t_1, t_3, ord);
			MulMod(prod,prod, t_1, ord);
		}
	}
	MulMod(prod, prod, chal->at(0), ord);

	//	ost<<"prod "<<prod<<" "<<Delta_bar->at(n-1)<<endl;
	//ost<<d_bar->at(0)<<" "<<Delta_bar->at(0)<<endl;
	if(t_Delta ==co_Delta)
		if(prod ==Delta_bar->at(n-1) & d_bar->at(0)==Delta_bar->at(0)){
		return 1;
	}
	return 0;
}

int func_ver::check_B(vector<Mod_p>* c_B, Mod_p c_B0, vector<ZZ>* chal, vector<ZZ>* B_bar, ZZ r_B_bar){
	long i;
	Mod_p t_B, co_B, temp, temp_1;
	long m = c_B->size();
	//check for correctness of the committed B
	temp = c_B0;
	for(i = 0; i<m; i++){
		Mod_p::expo(temp_1,c_B->at(i), chal->at(i));
		Mod_p::mult(temp,temp,temp_1);
	}
	t_B = temp;
	co_B = Ped.commit(B_bar, r_B_bar);
/*	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"B "<<t_B<<" "<<co_B<<endl;*/
	if(t_B==co_B){
		return 1;
	}
	return 0;
}

int func_ver::check_a(vector<Mod_p>* c_a, vector<Mod_p>* c_Dl, vector<ZZ>* chal, ZZ a_bar, ZZ r_a_bar){
	long i,l;
	Mod_p t_a, co_a, temp, temp_1;
	long m = c_a->size()/2;

	//Check that the random values are used right
	temp = c_a->at(0);
	l=2*m-1;
	for(i = 1; i<=l; i++){
		Mod_p::expo(temp_1,c_a->at(i), chal->at(i-1));
		Mod_p::mult(temp,temp,temp_1);
	}
	t_a = temp;
	co_a = Ped.commit(a_bar, r_a_bar);
/*	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"a "<<t_a<<" "<<co_a<<endl;
*/
	if(t_a==co_a & c_a->at(m)==c_Dl->at(m+1)){//both commitments should be com(0,0), so equal
		return 1;
	}
	return 0;
}

int func_ver::check_c(vector<vector<Cipher_elg>* >* enc, vector<Cipher_elg>* E, ZZ chal){
	long i,j;
	ZZ chal_temp;
	ZZ ord = H.get_ord();
	Cipher_elg c, temp;
	long m = enc->size();
	long n = enc->at(0)->size();

	chal_temp = to_ZZ(1);
	c = Cipher_elg(1,1,H.get_mod());
 	for(i = 0; i<m;i++){
		for(j = 0; j<n;j++){
			MulMod(chal_temp, chal_temp, chal, ord);
			Cipher_elg::expo(temp,enc->at(i)->at(j),chal_temp);
			Cipher_elg::mult(c,c,temp);
		}
	}
 	if(E->at(m)==c){
 		return 1;
 	}
 	return 0;
}

int func_ver::check_E(vector<vector<Cipher_elg>* >* C, vector<Cipher_elg>* E, vector<ZZ>* chal, vector<ZZ>* B_bar, ZZ a_bar, ZZ rho_bar ){
	long i,j,l;
	Cipher_elg t_E, co_E, temp, temp_1, temp_2;
	Mod_p gen,t;
	long m = C->size();
	long n = C->at(0)->size();
	ZZ te;
	ZZ ord = H.get_ord();
	ZZ mod = H.get_mod();

 	temp = E->at(0);
 	l=2*m;
	for(i = 1; i<l; i++){
		Cipher_elg::expo(temp_1,E->at(i), chal->at(i-1));
		Cipher_elg::mult(temp,temp,temp_1);
	}
	t_E = temp;

	gen = H.get_gen();
	t = Mod_p::expo(gen,a_bar);
	temp = El.encrypt(t, rho_bar);
	temp_1=Cipher_elg(1,1,mod);
	l=m-1;
	for(i = 0; i<l;i++){
		for(j = 0; j<n;j++){
			MulMod(te , B_bar->at(j),chal->at(m-i-2),ord);
			Cipher_elg::expo(temp_2,C->at(i)->at(j),te);
			Cipher_elg::mult(temp_1,temp_1,temp_2);
		}
	}
	for(j = 0; j<n;j++){
		te =B_bar->at(j);
		Cipher_elg::expo(temp_2,C->at(m-1)->at(j),te);
		Cipher_elg::mult(temp_1,temp_1,temp_2);
	}
	Cipher_elg::mult(co_E ,temp_1, temp);

/*	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"E"<<t_E<<" "<<co_E<<endl;*/
	if(t_E==co_E){
		return 1;
	}
	return 0;
}

int func_ver::check_Dh_op(vector<Mod_p>* c_Dh, vector<ZZ>* chal, vector<ZZ>* D_h_bar, ZZ r_Dh_bar, long win_LL){
	Mod_p t_Dh, co_Dh;

	multi_expo::multi_expo_LL(t_Dh,c_Dh, chal, win_LL);
	co_Dh = Ped.commit_opt(D_h_bar,r_Dh_bar);

	//cout<<"D_h "<<t_Dh<<" "<<co_Dh<<endl;
	if (t_Dh == co_Dh){
		return 1;
	}
	return 0;
}

int func_ver::check_D_op(Mod_p c_D0, Mod_p c_z, vector<Mod_p>* c_A, vector<Mod_p>* c_B, vector<ZZ>* chal_1, ZZ chal_2, vector<ZZ>* A_bar, ZZ r_A_bar, long n){
	int i;
	Mod_p t_D, co_D, temp, inv;
	long m = c_A->size()-1;
	ZZ ord = H.get_ord();
	vector<ZZ>* v_1 = new vector<ZZ>(n);

	t_D = c_D0;
	Mod_p::inv(inv, c_z);
	for (i=1; i<m;i++){
		Mod_p::expo(temp,c_A->at(i),chal_2);
		Mod_p::mult(temp, temp,c_B->at(i));
		Mod_p::mult(temp,temp,inv);
		Mod_p::expo(temp, temp, chal_1->at(i-1));
		Mod_p::mult(t_D,t_D,temp);
	}
	for(i=0; i<n;i++){
		NegateMod(v_1->at(i),to_ZZ(1),ord);
	}
	temp=Ped.commit_opt(v_1,to_ZZ(0));
	Mod_p::expo(temp, temp, chal_1->at(m-1));
	Mod_p::mult(t_D,t_D,temp);
	co_D = Ped.commit_opt(A_bar, r_A_bar);
	//cout<<"D "<<t_D<<" "<<co_D<<endl;
	delete v_1;
	if(t_D == co_D){
		return 1;
	}
	return 0;
}

int func_ver::check_Ds_op(vector<Mod_p>* c_Ds, vector<Mod_p>* c_Dh, Mod_p c_Dm, vector<ZZ>* chal_1, vector<ZZ>* chal_2, vector<ZZ>* Ds_bar, ZZ r_Ds_bar){
	long i,l;
	long m= c_Ds->size()-1;
	Mod_p t_Ds, co_Ds, temp, temp_1;

	l=m-1;
	for(i=0; i<l; i++){
		Mod_p::expo(c_Ds->at(i),c_Dh->at(i), chal_1->at(i));
	}
	if(m>1){
		Mod_p::expo(temp, c_Dh->at(1), chal_1->at(0));
		for(i=1;i<l; i++){
			Mod_p::expo(temp_1,c_Dh->at(i+1), chal_1->at(i));
			Mod_p::mult(temp,temp,temp_1);
		}
		c_Ds->at(l)=temp;
	}
	else{
		c_Ds->at(l)=Mod_p(1,G.get_mod());
	}

	c_Ds->at(m)=c_Dm;
	Mod_p::expo(t_Ds, c_Ds->at(0),chal_2->at(m-1));
	for(i=1; i<m; i++){
		Mod_p::expo(temp, c_Ds->at(i), chal_2->at(m-1-i));
		Mod_p::mult(t_Ds, t_Ds,temp);
	}
	Mod_p::mult(t_Ds,t_Ds,c_Ds->at(m));
	co_Ds = Ped.commit_opt(Ds_bar, r_Ds_bar);
	//cout<<"Ds "<<t_Ds<<" "<<co_Ds<<endl;
	if(t_Ds == co_Ds){
		return 1;
	}
	return 0;
}

int func_ver::check_Dl_op(vector<Mod_p>* c_Dl, vector<ZZ>* chal, vector<ZZ>* A_bar, vector<ZZ>* Ds_bar, vector<ZZ>*  chal_1, ZZ r_Dl_bar){
	long i;
	Mod_p t_Dl, co_Dl, temp;
	ZZ temp_1;
	long l = c_Dl->size();
	long pos = (l-1)/2+1;
	ZZ mod = G.get_mod();

	t_Dl = c_Dl->at(0);
	for(i=1; i<l; i++){
		Mod_p::expo(temp, c_Dl->at(i),chal->at(i-1));
		//Mod_p::mult(t_Dl,t_Dl, temp);
		t_Dl = t_Dl*temp;
	}
	temp_1=Functions::bilinearMap(A_bar,Ds_bar,chal_1);
	co_Dl =Ped.commit_sw(temp_1,r_Dl_bar);
	//cout<<"Dl "<<t_Dl<<" "<<co_Dl<<endl;

	temp= Mod_p(1, mod);
	if(t_Dl==co_Dl & c_Dl->at(pos)==temp){
		return 1;
	}
	return 0;
}

int func_ver::check_d_op(vector<Mod_p>* c_Dh, Mod_p c_d, vector<ZZ>* chal, vector<ZZ>* d_bar, ZZ r_d_bar){
	Mod_p t_d, co_d, temp;
	long m = c_Dh->size();
	Mod_p::expo(temp, c_Dh->at(m-1), chal->at(0));
	Mod_p::mult(t_d, temp, c_d);
	co_d = Ped.commit_opt(d_bar, r_d_bar);
	//cout<<"d "<<t_d<<" "<<co_d<<endl;
	if(t_d==co_d){
		return 1;
	}
	return 0;
}

int func_ver::check_Delta_op(Mod_p c_dh, Mod_p c_Delta, vector<ZZ>* chal, vector<ZZ>* Delta_bar, vector<ZZ>* d_bar, ZZ r_Delta_bar, ZZ chal_1, ZZ chal_2, ZZ chal_3){
	long i,j;
	Mod_p t_Delta, co_Delta, temp;
	ZZ t_1, t_2, t_3, prod, chal_temp;
	ZZ ord = H.get_ord();
	long m = (chal->size()-1)/2;
	long n = Delta_bar->size();
	vector<ZZ>* Delta_temp=0;

	Mod_p::expo(temp, c_dh, chal->at(0));
	Mod_p::mult(t_Delta, temp, c_Delta);

	Delta_temp = new vector<ZZ>(n-1);
	t_3= chal->at(0);
	for(i=0; i<n-1; i++){
		MulMod(t_1, Delta_bar->at(i), d_bar->at(i+1), ord);
		MulMod(t_2, t_3, Delta_bar->at(i+1),ord);
		SubMod(Delta_temp->at(i), t_2, t_1, ord);
	}

	co_Delta = Ped.commit_opt(Delta_temp, r_Delta_bar);

	delete Delta_temp;
	//cout<<"Delta "<<t_Delta<<" "<<co_Delta<<endl;

	prod = to_ZZ(1);
	chal_temp =to_ZZ(1);
	for(i=1; i<=m; i++){
		for(j=1; j<=n; j++){
			MulMod(chal_temp, chal_temp, chal_1,ord);
			SubMod(t_1, chal_temp, chal_2,ord);
			t_3 = n*(i-1)+j;
			MulMod(t_3,t_3, chal_3, ord);
			AddMod(t_1,t_1, t_3, ord);
			MulMod(prod,prod, t_1, ord);
		}
	}
	MulMod(prod, prod, chal->at(0), ord);

	//cout<<"prod "<<prod<<" "<<Delta_bar->at(n-1)<<endl;
	//cout<<d_bar->at(0)<<" "<<Delta_bar->at(0)<<endl;
	if(t_Delta ==co_Delta)
		if(prod ==Delta_bar->at(n-1) & d_bar->at(0)==Delta_bar->at(0)){
		return 1;
	}
	return 0;
}

int func_ver::check_B_op(vector<Mod_p>* c_B, Mod_p c_B0, vector<ZZ>* chal, vector<ZZ>* B_bar, ZZ r_B_bar, long win_LL){
	long i;
	Mod_p t_B, co_B, temp, temp_1;
	long m = c_B->size();
	vector<Mod_p>* B_temp = new vector<Mod_p>(m+1);
	vector<ZZ>* chal_mult = new vector<ZZ>(m+1);

	//check for correctness of the committed B
	B_temp ->at(0)= c_B0;
	chal_mult->at(0)=1;
	for(i = 0; i<m; i++){
		B_temp->at(i+1)=c_B->at(i);
		chal_mult->at(i+1)=chal->at(i);
	}
	multi_expo::multi_expo_LL(t_B, B_temp, chal_mult, win_LL);

	co_B = Ped.commit_opt(B_bar, r_B_bar);
	//cout<<"B "<<t_B<<" "<<co_B<<endl;
	delete B_temp;
	delete chal_mult;
	if(t_B==co_B){
		return 1;
	}
	return 0;
}

int func_ver::check_a_op(vector<Mod_p>* c_a, vector<Mod_p>* c_Dl, vector<ZZ>* chal, ZZ a_bar, ZZ r_a_bar){
	long i,l;
	Mod_p t_a, co_a, temp, temp_1;
	long m = c_a->size()/2;

	//Check that the random values are used right
	temp = c_a->at(0);
	l=2*m-1;
	for(i = 1; i<=l; i++){
		Mod_p::expo(temp_1,c_a->at(i), chal->at(i-1));
		Mod_p::mult(temp,temp,temp_1);
	}
	t_a = temp;
	co_a = Ped.commit_sw(a_bar, r_a_bar);
	//cout<<"a "<<t_a<<" "<<co_a<<endl;

	if(t_a==co_a & c_a->at(m)==c_Dl->at(m+1)){//both commitments should be com(0,0), so equal
		return 1;
	}
	return 0;
}

int func_ver::check_c_op(vector<vector<Cipher_elg>* >* enc, vector<Cipher_elg>* E, ZZ chal, long omega){
	long i,j;
	ZZ chal_temp;
	ZZ ord = H.get_ord();
	Cipher_elg c, temp;
	vector<ZZ>* v_chal = 0;
	long m = enc->size();
	long n = enc->at(0)->size();

	chal_temp = to_ZZ(1);
	c = Cipher_elg(1,1,H.get_mod());
	v_chal = new vector<ZZ>(n);
 	for(i = 0; i<m;i++){
		for(j = 0; j<n;j++){
			MulMod(chal_temp, chal_temp, chal, ord);
			v_chal->at(j)=chal_temp;
		}
		multi_expo::expo_mult(temp, enc->at(i), v_chal, omega);
		Cipher_elg::mult(c,c,temp);
	}
 	delete v_chal;
 	//cout<<"c "<<E->at(m)<<" "<<c<<endl;
 	if(E->at(m)==c){
 		return 1;
 	}
 	return 0;
}

int func_ver::check_E_op(vector<vector<Cipher_elg>* >* C, vector<Cipher_elg>* E, vector<ZZ>* chal, vector<ZZ>* B_bar, ZZ a_bar, ZZ rho_bar , long omega){
	long i,l;
	Cipher_elg t_E, co_E, temp, temp_1;
	Mod_p gen,t;
	long m = C->size();
	ZZ ord = H.get_ord();
	ZZ mod = H.get_mod();
	long num_b = NumBits(ord);
	l=2*m;
	vector<vector<long>* >* basis_chal = new vector<vector<long>* >(l);

	basis_chal ->at(0)= multi_expo::to_basis(to_ZZ(1), num_b, omega);
	basis_chal->at(1) = multi_expo::to_basis(chal->at(0), num_b, omega);
	for(i=2; i<l; i++){
		basis_chal->at(i) = multi_expo::to_basis(chal->at(i-1), num_b, omega);
	}

	multi_expo::expo_mult(t_E, E, basis_chal, omega);

	vector<ZZ>* chal_temp = new vector<ZZ>(m);
	l=m-1;
	for(i=0; i<l; i++){
		chal_temp->at(i)= chal->at(m-i-2);
	}
	chal_temp->at(l)=1;

	gen = H.get_gen();
	t = Mod_p::expo(gen,a_bar);
	temp = El.encrypt(t, rho_bar);
	multi_expo::expo_mult(temp_1, C, chal_temp, B_bar, omega);

	Cipher_elg::mult(co_E ,temp_1, temp);

	Functions::delete_vector(basis_chal);
	delete(chal_temp);
	//cout<<"E"<<t_E<<" "<<co_E<<endl;
	if(t_E==co_E){
		return 1;
	}

	return 0;

}

void func_ver::fill_vector(vector<ZZ>* t){
	long i,l;
	ZZ temp;
	ZZ ord = H.get_ord();

	l= t->size();
	temp = RandomBnd(ord);
	t->at(0)=temp;
	for(i=1; i<l; i++){
		MulMod(t->at(i),t->at(i-1),temp, ord);
	}
}

void func_ver::fill_x8(vector<ZZ>* chal_x8, vector<vector<long>* >* basis_chal_x8, vector<ZZ>* mul_chal_x8, long omega){
	long i, l;
	ZZ chal;
	ZZ ord = H.get_ord();
	long num_b= NumBits(ord);

	l= chal_x8->size();
	chal = RandomBnd(ord);

	chal_x8->at(0)= chal;
	basis_chal_x8->at(0) = multi_expo::to_basis(to_ZZ(1),num_b, omega);
	basis_chal_x8->at(1) = multi_expo::to_basis(chal_x8->at(0),num_b, omega);

	mul_chal_x8->at(0) =1;
	mul_chal_x8->at(1) =chal_x8->at(0);

	for (i = 1; i<l; i++){
		 MulMod(chal_x8->at(i),chal, chal_x8->at(i-1), ord);
		 basis_chal_x8->at(i+1) = multi_expo::to_basis(chal_x8->at(i),num_b, omega);
		 mul_chal_x8->at(i+1) = chal_x8->at(i);
	}
}