// This file is part of VIO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// VIO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// VIO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#include <algorithm>
#include <vio/vision.h>
#include <boost/bind.hpp>
#include <boost/math/distributions/normal.hpp>
#include <vio/global.h>
#include <vio/global_optimizer.h>
#include <vio/frame.h>
#include <vio/point.h>
#include <vio/feature.h>
#include <vio/config.h>
#include <g2o/solvers/structure_only/structure_only_solver.h>
#if VIO_DEBUG
#include <sys/types.h>
#include <sys/stat.h>
#endif
namespace vio {

    BA_Glob::BA_Glob(Map& map) :map_(map),thread_(NULL)
    {
        optimizer_=std::make_unique<g2o::SparseOptimizer>();
        optimizer_->setVerbose(false);
        /*g2o::BlockSolver_6_3::LinearSolverType * linearSolver=new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();*/
        std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver=g2o::make_unique<g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>>();
        std::unique_ptr<g2o::OptimizationAlgorithm> solver(new g2o::OptimizationAlgorithmLevenberg(
                g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))));
        optimizer_->setAlgorithm(std::move(solver));
        // setup camera
        cam_params_ =std::make_shared<g2o::CameraParameters>(1.0, Vector2d(0.,0.), 0.);
        cam_params_->setId(0);
        if (!optimizer_->addParameter(cam_params_)) {
            assert(false && "Camera initialization in BA");
        }
#if VIO_DEBUG
        log_ =fopen((std::string(PROJECT_DIR)+"/loop_closure_log.txt").c_str(),"w+");
        assert(log_);
        chmod((std::string(PROJECT_DIR)+"/loop_closure_log.txt").c_str(), ACCESSPERMS);
#endif
    }

    BA_Glob::~BA_Glob()
    {
        stopThread();
    }

    void BA_Glob::startThread()
    {
        thread_ = new boost::thread(&BA_Glob::updateLoop, this);
    }

    void BA_Glob::stopThread()
    {

        if(thread_ != NULL)
        {
            thread_->interrupt();
            usleep(5000);
            thread_->join();
            thread_ = NULL;
        }
#if VIO_DEBUG
        fclose(log_);
#endif
    }
    void BA_Glob::updateLoop()
    {
        while(!boost::this_thread::interruption_requested())
        {
            boost::unique_lock< boost::mutex > lk( mtx_);
            while(map_.keyframes_.empty() || new_keyframe_ == false)
                cond_.wait(lk);
#if VIO_DEBUG
            fprintf(log_,"[%s] BA loop run \n",
                    vio::time_in_HH_MM_SS_MMM().c_str());
#endif
            new_keyframe_=false;
            // init g2o
            g2o::OptimizableGraph::VertexContainer points;
            ba_mux_.lock();
            // Go through all Keyframes
            v_id_ = 0;
            auto end=map_.keyframes_.end();
            auto end_1=end--;
            for(auto it_kf=map_.keyframes_.begin();it_kf!=map_.keyframes_.end();++it_kf)
            {
                // New Keyframe Vertex
                if(it_kf !=map_.keyframes_.end() && it_kf !=end && it_kf !=end_1){
                    (*it_kf)->v_kf_ = createG2oFrameSE3(*it_kf,true);
                }else{
                    (*it_kf)->v_kf_ = createG2oFrameSE3(*it_kf,false);
                }
                optimizer_->addVertex((*it_kf)->v_kf_);
                for(auto& it_ftr:(*it_kf)->fts_)
                {
                    if(it_ftr->point==NULL)continue;
                    if(it_ftr->point->type_ != vio::Point::TYPE_GOOD)continue;
                    // for each keyframe add edges to all observed mapoints
                    if(it_ftr->point->pos_.hasNaN())continue;
                    if(it_ftr->point->pos_.norm()==0.)continue;
                    if(it_ftr->point->v_pt_ == NULL)
                    {
                        // mappoint-vertex doesn't exist yet. create a new one:
                        it_ftr->point->v_pt_ = createG2oPoint(it_ftr->point->pos_);
                        optimizer_->addVertex(it_ftr->point->v_pt_);
                    }
                    optimizer_->addEdge(createG2oEdgeSE3((*it_kf)->v_kf_, it_ftr->point->v_pt_, vk::project2d(it_ftr->f),
                                                                         true,
                                                                         Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth()));
                }
            }
            for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer_->vertices().begin();
                 it != optimizer_->vertices().end(); ++it) {
                auto v = std::static_pointer_cast<g2o::OptimizableGraph::Vertex>(it->second);
                if (v->dimension() == 3 && v->edges().size()>2) points.push_back(v);
            }
            // Optimization
            if(points.empty()){
                optimizer_->clear();
                ba_mux_.unlock();
                continue;
            }
            optimizer_->initializeOptimization();
            optimizer_->computeActiveErrors();
            g2o::StructureOnlySolver<3> structure_only_ba;
            structure_only_ba.calc(points, vio::Config::lobaNumIter());

#if VIO_DEBUG
            fprintf(log_,"[%s] init error: %f \n",
                    vio::time_in_HH_MM_SS_MMM().c_str(),optimizer_->activeChi2());
#endif
            if(optimizer_->optimize(vio::Config::lobaNumIter())<1){
                optimizer_->clear();
                ba_mux_.unlock();
                continue;
            }
#if VIO_DEBUG
            fprintf(log_,"[%s] end error: %f \n",
                    vio::time_in_HH_MM_SS_MMM().c_str(),optimizer_->activeChi2());
#endif
            // Update Keyframe and MapPoint Positions
            for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
                it_kf != map_.keyframes_.end();++it_kf)
            {
                (*it_kf)->T_f_w_ = SE2_5(SE3((*it_kf)->v_kf_->estimate().rotation().toRotationMatrix(),
                                        (*it_kf)->v_kf_->estimate().translation()));
                for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
                {
                    if((*it_ftr)->point == NULL)
                        continue;
                    if((*it_ftr)->point->v_pt_ == NULL)
                        continue;       // mp was updated before
                    (*it_ftr)->point->pos_ = (*it_ftr)->point->v_pt_->estimate();
                    (*it_ftr)->point->v_pt_.reset();
                }
            }
            optimizer_->clear();
            ba_mux_.unlock();
        }
    }

   std::shared_ptr<g2o::VertexSE3Expmap>
   BA_Glob::createG2oFrameSE3(FramePtr frame, bool state)
   {
       std::shared_ptr<g2o::VertexSE3Expmap> v= std::make_shared<g2o::VertexSE3Expmap>();
       ++v_id_;
       v->setId(v_id_);
       // not all frames are fixed
       v->setFixed(state);
       v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation()));
       return v;
   }

   std::shared_ptr<g2o::VertexPointXYZ>
   BA_Glob::createG2oPoint(Vector3d pos)
   {
       ++v_id_;
       std::shared_ptr<g2o::VertexPointXYZ> v =std::make_shared<g2o::VertexPointXYZ>();
       v->setId(v_id_);
       //v->setFixed(false);
       v->setMarginalized(true);
       v->setEstimate(pos);
       return v;

   }

    std::shared_ptr<g2o::EdgeProjectXYZ2UV> BA_Glob::createG2oEdgeSE3(
            std::shared_ptr<g2o::VertexSE3Expmap> v_frame,
            std::shared_ptr<g2o::VertexPointXYZ> v_point,
                     const Vector2d& f_up,
                     bool robust_kernel,
                     double huber_width,
                     double weight)
   {
       std::shared_ptr<g2o::EdgeProjectXYZ2UV> e= std::make_shared<g2o::EdgeProjectXYZ2UV>();
       e->vertices()[0]=v_point;
       e->vertices()[1]=v_frame;
       e->setMeasurement(f_up);
       e->information() = weight*Eigen::Matrix2d::Identity();
       g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
       rk->setDelta(huber_width);
       e->setRobustKernel(rk);
       e->setParameterId(0, 0); //old: e->setId(v_point->id());
       return e;
   }
   void BA_Glob::reset_map(){
        for(auto&& f:map_.keyframes_){
            f->v_kf_.reset();
            for(auto p:f->fts_)
                if(p->point!=NULL)
                    p->point->v_pt_.reset();
        }
    }

} // namespace vio


/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, Daichi Yoshikawa
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Daichi Yoshikawa 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 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.
 *
 * Author: Daichi Yoshikawa
 *
 *********************************************************************/

#include <mutex>
#include <Eigen/Dense>
#include <ros/ros.h>
#include <ahl_gazebo_interface/gazebo_interface.hpp>
#include <ahl_gazebo_interface/exception.hpp>
#include <ahl_robot/ahl_robot.hpp>
#include <ahl_utils/exception.hpp>
#include <ahl_utils/scoped_lock.hpp>
#include <ahl_robot_controller/robot_controller.hpp>
#include <ahl_robot_controller/tasks.hpp>

using namespace ahl_robot;
using namespace ahl_ctrl;

std::mutex mutex;
RobotPtr robot;
RobotControllerPtr controller;
bool updated = false;
TfPublisherPtr tf_pub;
TaskPtr gravity_compensation;
TaskPtr damping;
TaskPtr joint_control;
TaskPtr joint_limit;
TaskPtr position_control;
TaskPtr orientation_control;
ahl_gazebo_if::GazeboInterfacePtr gazebo_interface;
bool initialized = false;
bool joint_updated = false;

void updateModel(const ros::TimerEvent&)
{
  try
  {
    ahl_utils::ScopedLock lock(mutex);
    if(joint_updated)
    {
      robot->computeJacobian("mnp");
      robot->computeMassMatrix("mnp");
      controller->updateModel();
      updated = true;
      tf_pub->publish(robot);
    }
  }
  catch(ahl_utils::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
  catch(ahl_gazebo_if::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
}

void control(const ros::TimerEvent&)
{
  try
  {
    ahl_utils::ScopedLock lock(mutex);

    if(gazebo_interface->subscribed())
    {
      Eigen::VectorXd q = gazebo_interface->getJointStates();
      robot->update(q);
      joint_updated = true;
    }

    if(updated)
    {
      static long cnt = 0;

      if(initialized == false)
      {
        Eigen::VectorXd qd = Eigen::VectorXd::Constant(robot->getDOF("mnp"), M_PI / 4.0);
        double sin_val = 1.0;//std::abs(sin(2.0 * M_PI * 0.1 * cnt * 0.001));
        qd = sin_val * qd;
        joint_control->setGoal(qd);

        static int32_t reached = 0;
        if(robot->reached("mnp", qd, 2.2))
        {
          ++reached;

          if(reached > 500)
          {
            std::cout << "switch to task space control." << std::endl;

            initialized = true;
            controller->clearTask();
            controller->addTask(joint_control, 0);
            controller->addTask(gravity_compensation, 10);
            controller->addTask(position_control, 10);
            controller->addTask(orientation_control, 5);
            //controller->addTask(joint_limit, 100);

            joint_control->setGoal(qd);

            Eigen::Vector3d xd;
            xd << 0.8, 0.35, 1.0;
            position_control->setGoal(xd);

            Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
            double rad = 0.0;
            R << cos(rad), 0, sin(rad),
              0, 1, 0,
              -sin(rad), 0, cos(rad);
            orientation_control->setGoal(R);
          }
        }
        else
        {
          reached = 0;
        }
      }
      else
      {
        Eigen::Vector3d xd;
        xd << 0.8, 0.35, 1.0;
        xd.coeffRef(0) += 0.2 * cos(2.0 * M_PI * 0.2 * cnt * 0.001);
        xd.coeffRef(1) += 0.2 * sin(2.0 * M_PI * 0.2 * cnt * 0.001);
        //xd.coeffRef(2) += 0.2 * cos(2.0 * M_PI * 0.2 * cnt * 0.001);

        Eigen::VectorXd qd = Eigen::VectorXd::Constant(robot->getDOF("mnp"), M_PI / 4.0);
        qd[3] = -qd[3];
        qd[7] = -qd[7];
        joint_control->setGoal(qd);

        position_control->setGoal(xd);
        Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
        double rad = 0.0; //M_PI / 4.0 * sin(2.0 * M_PI * 0.2 * cnt * 0.001) + M_PI / 4;
        R << cos(rad), 0, sin(rad),
          0, 1, 0,
          -sin(rad), 0, cos(rad);
        orientation_control->setGoal(R);
      }

      ++cnt;

      Eigen::VectorXd tau(robot->getDOF("mnp"));
      controller->computeGeneralizedForce(tau);

      gazebo_interface->applyJointEfforts(tau);
    }

  }
  catch(ahl_utils::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
  catch(ahl_gazebo_if::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "red_arm_server");
  ros::NodeHandle nh;

  ros::Timer timer_update_model = nh.createTimer(ros::Duration(0.01), updateModel);
  ros::Timer timer_control = nh.createTimer(ros::Duration(0.001), control);

  robot = std::make_shared<Robot>("red_arm2");
  ParserPtr parser = std::make_shared<Parser>();

  std::string path = "/home/daichi/Work/catkin_ws/src/ahl_ros_pkg/ahl_robot/ahl_robot/yaml/red_arm2.yaml";
  parser->load(path, robot);

  controller = std::make_shared<RobotController>();
  controller->init(robot);

  using namespace ahl_gazebo_if;
  gazebo_interface = std::make_shared<GazeboInterface>();
  gazebo_interface->addJoint("red_arm2::joint1");
  gazebo_interface->addJoint("red_arm2::joint2");
  gazebo_interface->addJoint("red_arm2::joint3");
  gazebo_interface->addJoint("red_arm2::joint4");
  gazebo_interface->addJoint("red_arm2::joint5");
  gazebo_interface->addJoint("red_arm2::joint6");
  gazebo_interface->addJoint("red_arm2::joint7");
  gazebo_interface->addJoint("red_arm2::joint8");
  gazebo_interface->addJoint("red_arm2::joint9");
  gazebo_interface->addJoint("red_arm2::joint10");
  gazebo_interface->addJoint("red_arm2::joint11");
  gazebo_interface->connect();

  tf_pub = std::make_shared<TfPublisher>();

  ManipulatorPtr mnp = robot->getManipulator("mnp");

  gravity_compensation = std::make_shared<GravityCompensation>(robot);
  damping = std::make_shared<Damping>(robot);
  joint_control = std::make_shared<JointControl>(mnp);
  joint_limit = std::make_shared<JointLimit>(mnp, 0.087);
  position_control = std::make_shared<PositionControl>(mnp, "gripper", 0.001);
  orientation_control = std::make_shared<OrientationControl>(mnp, "gripper", 0.001);

  controller->addTask(gravity_compensation, 0);
  controller->addTask(joint_control, 0);

  ros::MultiThreadedSpinner spinner;
  spinner.spin();

  return 0;
}


/*
 * Copyright (C) 2019 Intel Corporation
 *
 * SPDX-License-Identifier: MIT
 *
 */

#include "vme_wpp/vme_wpp.hpp"

#include <algorithm>
#include <fstream>
#include <iostream>
#include <sstream>
#include <stdexcept>
#include <vector>

#include <boost/program_options.hpp>

#include <boost/compute/core.hpp>
#include <boost/compute/image.hpp>
#include <boost/compute/utility.hpp>

#include <CL/cl_ext_intel.h>

#include "align_utils/align_utils.hpp"
#include "timer/timer.hpp"
#include "ocl_utils/ocl_utils.hpp"
#include "logging/logging.hpp"

namespace au = compute_samples::align_utils;
namespace po = boost::program_options;
namespace compute = boost::compute;

namespace compute_samples {

VmeWppApplication::Arguments VmeWppApplication::parse_command_line(
    const std::vector<std::string> &command_line) {
  Arguments args;

  po::options_description desc("Allowed options");
  auto options = desc.add_options();
  options("help,h", "show help message");
  options("output-bmp,b",
          po::value<bool>(&args.output_bmp)
              ->default_value(false)
              ->implicit_value(true),
          "output to bmp images for each frame");
  options("input-yuv,i",
          po::value<std::string>(&args.input_yuv_path)
              ->default_value("goal_1280x720.yuv"),
          "path to input yuv file");
  options("output-yuv,o",
          po::value<std::string>(&args.output_yuv_path)
              ->default_value("output_goal_1280x720.yuv"),
          "path to output yuv with motion vectors");
  options("qp,q", po::value<int>(&args.qp)->default_value(49),
          "quantization parameter value to use for estimation heuristics"
          "(higher for faster motion frames)");
  options("width,w", po::value<int>(&args.width)->default_value(1280),
          "width of input yuv");
  options("height,h", po::value<int>(&args.height)->default_value(720),
          "height of input yuv");
  options("frames,f", po::value<int>(&args.frames)->default_value(0),
          "number of frame to use for motion estimation (0 represents entire "
          "yuv sequence)");

  po::positional_options_description p;
  p.add("input-yuv", 1);
  p.add("output-yuv", 1);

  po::variables_map vm;
  po::store(
      po::command_line_parser(command_line).options(desc).positional(p).run(),
      vm);

  if (vm.count("help") != 0u) {
    std::cout << desc;
    args.help = true;
    return args;
  }

  po::notify(vm);

  if (args.qp > 51) {
    throw std::invalid_argument("Invalid argument for qp. Valid range (0-51).");
  }

  return args;
}

#define MV_PER_DIM (2)
#define DIM_TO_MB_SZ(X, Y) (au::align_units(au::align_units(X, Y), 16))
#define DIM_TO_MV_SZ(X, Y) (DIM_TO_MB_SZ(X, Y) * MV_PER_DIM)

Application::Status
VmeWppApplication::run_implementation(std::vector<std::string> &command_line) {
  const Arguments args = parse_command_line(command_line);
  if (args.help) {
    return Status::SKIP;
  }

  compute::device device = compute::system::default_device();
  LOG_INFO << "OpenCL device: " << device.name();

  if (!device.supports_extension(
          "cl_intel_device_side_avc_motion_estimation")) {
    LOG_ERROR
        << "The selected device doesn't support device-side motion estimation.";
    return Status::SKIP;
  }

  if (!device.check_version(2, 0)) {
    LOG_ERROR << "The selected device doesn't support OpenCL 2.0.";
    return Status::SKIP;
  }

  LOG_INFO << "Input yuv path: " << args.input_yuv_path;
  LOG_INFO << "Frame size: " << args.width << "x" << args.height << " pixels";

  Timer timer_total;

  compute::context context(device);
  compute::command_queue queue(context, device);

  Timer timer;
  compute::program program =
      build_program(context, "vme_wpp.cl", "-cl-std=CL2.0");
  timer.print("Program created");

  compute::kernel ds_kernel = program.create_kernel("downsample_3_tier");
  compute::kernel hme_n_kernel = program.create_kernel("tier_n_hme");
  compute::kernel wpp_kernel = program.create_kernel("vme_wpp");
  timer.print("Kernels created");

  YuvCapture capture(args.input_yuv_path, args.width, args.height, args.frames);
  const int frame_count =
      (args.frames) != 0 ? args.frames : capture.get_num_frames();
  YuvWriter writer(args.width, args.height, frame_count, args.output_bmp);

  PlanarImage planar_image(args.width, args.height);
  capture.get_sample(0, planar_image);
  timer.print("Read YUV frame 0 from disk to CPU linear memory.");

  writer.append_frame(planar_image);

  compute::image_format format(CL_R, CL_UNORM_INT8);
  compute::image2d ref_image(context, args.width, args.height, format);
  compute::image2d src_image(context, args.width, args.height, format);
  compute::image2d src_image_8x(context, au::align_units(args.width, 8),
                                au::align_units(args.height, 8), format);
  compute::image2d ref_image_8x(context, au::align_units(args.width, 8),
                                au::align_units(args.height, 8), format);
  compute::image2d src_image_4x(context, au::align_units(args.width, 4),
                                au::align_units(args.height, 4), format);
  compute::image2d ref_image_4x(context, au::align_units(args.width, 4),
                                au::align_units(args.height, 4), format);
  compute::image2d src_image_2x(context, au::align_units(args.width, 2),
                                au::align_units(args.height, 2), format);
  compute::image2d ref_image_2x(context, au::align_units(args.width, 2),
                                au::align_units(args.height, 2), format);
  timer.print("Created opencl mem objects for downsample_3_tier kernel");

  size_t origin[] = {0, 0, 0};
  size_t region[] = {static_cast<size_t>(args.width),
                     static_cast<size_t>(args.height), 1};
  queue.enqueue_write_image(src_image, origin, region, planar_image.get_y(),
                            planar_image.get_pitch_y());
  timer.print("Copied frame 0 to tiled memory.");

  ds_kernel.set_args(src_image, src_image_2x, src_image_4x, src_image_8x);
  queue.enqueue_nd_range_kernel(
      ds_kernel, 2, nullptr,
      compute::dim(au::align16(au::align_units(args.width, 4)),
                   au::align_units(args.height, 16))
          .data(),
      compute::dim(16, 1).data());
  timer.print("Enqueued downsample_3_tier kernel for frame 0");

  for (int k = 1; k < frame_count; k++) {
    LOG_INFO << "Processing frame " << k << "...";
    run_vme_wpp(args, device, context, queue, ds_kernel, hme_n_kernel,
                wpp_kernel, capture, planar_image, src_image, ref_image,
                src_image_2x, ref_image_2x, src_image_4x, ref_image_4x,
                src_image_8x, ref_image_8x, k);
    writer.append_frame(planar_image);
  }

  LOG_INFO << "Wrote " << frame_count << " frames with overlaid "
           << "motion vectors to " << args.output_yuv_path << " .";
  writer.write_to_file(args.output_yuv_path.c_str());

  timer_total.print("Total");
  return Status::OK;
}

void VmeWppApplication::run_vme_wpp(
    const VmeWppApplication::Arguments &args, compute::device &device,
    compute::context &context, compute::command_queue &queue,
    compute::kernel &ds_kernel, compute::kernel &hme_n_kernel,
    compute::kernel &wpp_kernel, YuvCapture &capture, PlanarImage &planar_image,
    compute::image2d &src_image, compute::image2d &ref_image,
    compute::image2d &src_2x_image, compute::image2d &ref_2x_image,
    compute::image2d &src_4x_image, compute::image2d &ref_4x_image,
    compute::image2d &src_8x_image, compute::image2d &ref_8x_image,
    int frame_idx) const {
  Timer timer;

  int width = args.width;
  int height = args.height;

  std::swap(ref_image, src_image);
  std::swap(ref_2x_image, src_2x_image);
  std::swap(ref_4x_image, src_4x_image);
  std::swap(ref_8x_image, src_8x_image);

  capture.get_sample(frame_idx, planar_image);
  timer.print("Read next YUV frame from disk to CPU linear memory.");

  size_t origin[] = {0, 0, 0};
  size_t region[] = {static_cast<size_t>(width), static_cast<size_t>(height),
                     1};
  queue.enqueue_write_image(src_image, origin, region, planar_image.get_y(),
                            planar_image.get_pitch_y());
  timer.print("Copied next frame to GPU tiled memory.");

  ds_kernel.set_args(src_image, src_2x_image, src_4x_image, src_8x_image);
  queue.enqueue_nd_range_kernel(
      ds_kernel, 2, nullptr,
      compute::dim(au::align16(au::align_units(width, 4)),
                   au::align_units(height, 16))
          .data(),
      compute::dim(16, 1).data());
  timer.print("Enqueued downsample_3_tier kernel for next frame");

  uint32_t mv_8x_count = DIM_TO_MV_SZ(width, 8) * DIM_TO_MV_SZ(height, 8);
  au::PageAlignedVector<cl_short2> predictors_4x(au::align64(mv_8x_count));
  uint32_t mv_4x_count = DIM_TO_MV_SZ(width, 4) * DIM_TO_MV_SZ(height, 4);
  au::PageAlignedVector<cl_short2> predictors_2x(au::align64(mv_4x_count));
  uint32_t mv_2x_count = DIM_TO_MV_SZ(width, 2) * DIM_TO_MV_SZ(height, 2);
  au::PageAlignedVector<cl_short2> predictors(au::align64(mv_2x_count));

  compute::buffer pred_4x_buffer(
      context, au::align64(mv_8x_count * sizeof(cl_short2)),
      CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR, predictors_4x.data());
  compute::buffer pred_2x_buffer(
      context, au::align64(mv_4x_count * sizeof(cl_short2)),
      CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR, predictors_2x.data());
  compute::buffer pred_buffer(
      context, au::align64(mv_2x_count * sizeof(cl_short2)),
      CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR, predictors.data());
  timer.print("Created opencl mem objects for tier_n_hme kernel");

  hme_n_kernel.set_arg(0, src_8x_image);
  hme_n_kernel.set_arg(1, ref_8x_image);
  hme_n_kernel.set_arg(2, sizeof(cl_mem), nullptr);
  hme_n_kernel.set_arg(3, pred_4x_buffer);
  queue.enqueue_nd_range_kernel(
      hme_n_kernel, 2, nullptr,
      compute::dim(au::align16(au::align_units(width, 8)),
                   DIM_TO_MB_SZ(height, 8))
          .data(),
      compute::dim(16, 1).data());
  timer.print("Enqueued tier 3 hme kernel for next frame");

  hme_n_kernel.set_args(src_4x_image, ref_4x_image, pred_4x_buffer,
                        pred_2x_buffer);
  queue.enqueue_nd_range_kernel(
      hme_n_kernel, 2, nullptr,
      compute::dim(au::align16(au::align_units(width, 4)),
                   DIM_TO_MB_SZ(height, 4))
          .data(),
      compute::dim(16, 1).data());
  timer.print("Enqueued tier 2 hme kernel for next frame");

  hme_n_kernel.set_args(src_2x_image, ref_2x_image, pred_2x_buffer,
                        pred_buffer);
  queue.enqueue_nd_range_kernel(
      hme_n_kernel, 2, nullptr,
      compute::dim(au::align16(au::align_units(width, 2)),
                   DIM_TO_MB_SZ(height, 2))
          .data(),
      compute::dim(16, 1).data());
  timer.print("Enqueued tier 1 hme kernel for next frame");

  int mb_image_width = au::align_units(width, 16);
  int mb_image_height = au::align_units(height, 16);
  int mb_count = mb_image_width * mb_image_height;
  int mv_image_width = mb_image_width * 4;
  int mv_image_height = mb_image_height * 4;
  int mv_count = mv_image_width * mv_image_height;

  uint32_t num_eu = device.compute_units();
  uint32_t num_threads_per_eu = 7;
  uint32_t max_threads = num_eu * num_threads_per_eu;
  uint32_t width_mb_sz = au::align_units(width, 16);
  uint32_t num_blocks = (width_mb_sz < max_threads) ? width_mb_sz : max_threads;
  uint32_t simd_size = 16;

  au::PageAlignedVector<cl_short2> mvs(au::align64(mv_count));
  au::PageAlignedVector<cl_ushort> residuals(au::align64(mv_count));
  au::PageAlignedVector<cl_uchar2> shapes(au::align64(mb_count));
  au::PageAlignedVector<cl_int> scoreboard(au::align64(width_mb_sz), 0);

  compute::buffer scoreboard_buffer(
      context, au::align64(width_mb_sz * sizeof(uint32_t)),
      CL_MEM_WRITE_ONLY | CL_MEM_USE_HOST_PTR, scoreboard.data());
  compute::buffer mv_buffer(context, au::align64(mv_count * sizeof(cl_short2)),
                            CL_MEM_WRITE_ONLY | CL_MEM_USE_HOST_PTR,
                            mvs.data());
  compute::buffer residual_buffer(
      context, au::align64(mv_count * sizeof(cl_ushort)),
      CL_MEM_WRITE_ONLY | CL_MEM_USE_HOST_PTR, residuals.data());
  compute::buffer shape_buffer(
      context, au::align64(mb_count * sizeof(cl_uchar2)),
      CL_MEM_WRITE_ONLY | CL_MEM_USE_HOST_PTR, shapes.data());
  timer.print("Created opencl mem objects for tier 0 wpp kernel");

  auto qp = static_cast<cl_uchar>(args.qp);
  cl_uchar sad_adjustment = CL_AVC_ME_SAD_ADJUST_MODE_NONE_INTEL;
  cl_uchar pixel_mode = CL_AVC_ME_SUBPIXEL_MODE_QPEL_INTEL;
  wpp_kernel.set_args(src_image, ref_image, pred_buffer, mv_buffer,
                      residual_buffer, shape_buffer, scoreboard_buffer, qp,
                      sad_adjustment, pixel_mode);
  size_t local_size = 16;
  size_t global_size = num_blocks * simd_size;
  queue.enqueue_nd_range_kernel(wpp_kernel, 1, nullptr, &global_size,
                                &local_size);
  timer.print("Enquequed tier 0 vme_wpp kernel");

  queue.finish();
  timer.print("Kernel finished.");

  planar_image.overlay_vectors(reinterpret_cast<motion_vector *>(mvs.data()),
                               reinterpret_cast<inter_shape *>(shapes.data()));
}
} // namespace compute_samples


#ifndef AUTONOMY_SCRIPT_COMMANDS_HPP
#define AUTONOMY_SCRIPT_COMMANDS_HPP
#include <boost/random.hpp>

#include <autonomy/script_instruction.hpp>
#include <autonomy/entity/scripted_drone.hpp>

namespace autonomy {
    //! \brief Move command.
    //! Takes 2 "directions", up and right.
    //! A direction is either positive, 0 or negative
    struct move : public script_instruction_base<move>
    {
        friend class boost::serialization::access;
        static std::string name() { return "move"; }
        unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<move>);
            }
    };

    struct get_x : public script_instruction_base<get_x>
    {
        friend class boost::serialization::access;
        static std::string name() { return "get_x"; }
        unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<get_x>);
            }
    };

    struct get_y : public script_instruction_base<get_y>
    {
        friend class boost::serialization::access;
        static std::string name() { return "get_y"; }
        unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<get_y>);
            }
    };

    //! \brief scan command
    //! Takes an x and a y and returns an "object type"
    struct scan : public script_instruction_base<scan>
    {
        friend class boost::serialization::access;
        static std::string name() { return "scan"; }
        unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<scan>);
            }
    };

    struct is_drone : public script_instruction_base<is_drone>
    {
        friend class boost::serialization::access;
        static std::string name() { return "is_drone"; }
        unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<is_drone>);
            }
    };

    struct is_asteroid : public script_instruction_base<is_asteroid>
    {
        friend class boost::serialization::access;
        static std::string name() { return "is_asteroid"; }
        unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<is_asteroid>);
            }
    };

    struct is_base : public script_instruction_base<is_base>
    {
        friend class boost::serialization::access;
        static std::string name() { return "is_base"; }
        unsigned int execute(size_t which_queue, entity::scripted_drone & drone );
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<is_base>);
            }
    };

    //! \brief Random direction command
    //! Returns a random "direction" which is either -1, 0, or 1
    struct rand_dir : public script_instruction_base<rand_dir>
    {
        friend class boost::serialization::access;
        rand_dir()
        {}
        static std::string name() { return "rand_dir"; }
        unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        static boost::mt19937 rng;
        static boost::uniform_int<> dir;
        static boost::variate_generator<boost::mt19937&, boost::uniform_int<> > _rand_dir;
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<rand_dir>);
            }
    };

    //! \brief Mine command
    //! Takes an x, and a y and mines an asteroid some set amount if there is one there
    struct mine : public script_instruction_base<mine>
    {
        friend class boost::serialization::access;
        static std::string name() { return "mine"; }
        unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<mine>);
            }
    };

    //! \brief Unload command
    //! Takes an x, and a y and unloads some set amount to a basestation
    struct unload : public script_instruction_base<unload>
    {
        friend class boost::serialization::access;
        static std::string name() { return "unload"; }
        unsigned int execute(size_t which_queue, entity::scripted_drone & drone);
        private:
        template<class Archive>
            void serialize(Archive & ar, const unsigned int version)
            {
                ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(script_instruction_base<unload>);
            }
    };
}
#endif


#pragma once

#include <protozero/pbf_writer.hpp>
#include <protozero/varint.hpp>

#include <boost/geometry.hpp>

#include "web_mercator.hpp"
#include "vector_tile.hpp"
#include "common.hpp"

namespace util { namespace tile {

typedef boost::geometry::model::point<std::int32_t, 2, boost::geometry::cs::cartesian> tile_point_t;
typedef boost::geometry::model::linestring<tile_point_t> tile_linestring_t;
typedef boost::geometry::model::box<tile_point_t> tile_box_t;

typedef boost::geometry::model::point<double, 2, boost::geometry::cs::cartesian> mercator_point_t;
typedef boost::geometry::model::linestring<mercator_point_t> mercator_linestring_t;
typedef boost::geometry::model::box<mercator_point_t> mercator_box_t;
typedef boost::geometry::model::multi_linestring<mercator_linestring_t> mercator_multi_linestring_t;

const static tile_box_t tile_clip_box(tile_point_t(-util::vector_tile::BUFFER, -util::vector_tile::BUFFER),
                                      tile_point_t(util::vector_tile::EXTENT + util::vector_tile::BUFFER,
                                                   util::vector_tile::EXTENT + util::vector_tile::BUFFER));

struct tile_point_hash {
    std::size_t operator()(const tile_point_t &key) const {
        const auto a = std::hash<std::int32_t>()(key.get<0>());
        const auto b = std::hash<std::int32_t>()(key.get<1>());
        // From boost::hash_combine
        return a ^ (b + 0x9e3779b9 + (a << 6) + (a >> 2));
    }
};

struct tile_point_equal {
    bool operator()(const tile_point_t &a, const tile_point_t &b) const {
        return a.get<0>() == b.get<0>() && a.get<1>() == b.get<1>();
    }
};

// from mapnik-vector-tile
// Encodes a linestring using protobuf zigzag encoding
inline bool encodeLinestring(const tile_linestring_t  &line,
                             protozero::packed_field_uint32 &geometry,
                             std::int32_t &start_x,
                             std::int32_t &start_y)
{
    const std::size_t line_size = line.size();
    if (line_size < 2)
    {
        return false;
    }

    const unsigned LINETO_count = static_cast<const unsigned>(line_size) - 1;

    auto pt = line.begin();
    const constexpr int MOVETO_COMMAND = 9;
    geometry.add_element(MOVETO_COMMAND); // move_to | (1 << 3)
    geometry.add_element(protozero::encode_zigzag32(pt->get<0>() - start_x));
    geometry.add_element(protozero::encode_zigzag32(pt->get<1>() - start_y));
    start_x = pt->get<0>();
    start_y = pt->get<1>();
    // This means LINETO repeated N times
    // See: https://github.com/mapbox/vector-tile-spec/tree/master/2.1#example-command-integers
    geometry.add_element((LINETO_count << 3u) | 2u);
    // Now that we've issued the LINETO REPEAT N command, we append
    // N coordinate pairs immediately after the command.
    for (++pt; pt != line.end(); ++pt)
    {
        const std::int32_t dx = pt->get<0>() - start_x;
        const std::int32_t dy = pt->get<1>() - start_y;
        geometry.add_element(protozero::encode_zigzag32(dx));
        geometry.add_element(protozero::encode_zigzag32(dy));
        start_x = pt->get<0>();
        start_y = pt->get<1>();
    }
    return true;
}

inline tile_linestring_t segmentToTileLine(const wgs84_segment_t &segment,
                                           const mercator_box_t &tile_bbox)
{
    wgs84_linestring_t geo_line;
    geo_line.push_back(segment.first);
    geo_line.push_back(segment.second);

    mercator_linestring_t unclipped_line;

    auto wgs84_to_tile = [&tile_bbox](const wgs84_point_t &wgs84_point) {
        // Convert lon/lat to global mercator coordinates
        double mercator_x = wgs84_point.get<0>() * util::web_mercator::DEGREE_TO_PX;
        double mercator_y = util::web_mercator::latToY(wgs84_point.get<1>()) *
                            util::web_mercator::DEGREE_TO_PX;

        // Convert global mercator coordinates to relative positions on
        // the provide mercator tile
        // convert lon/lat to tile coordinates
        const auto box_width = tile_bbox.max_corner().get<0>() - tile_bbox.min_corner().get<0>();
        const auto box_height = tile_bbox.max_corner().get<1>() - tile_bbox.min_corner().get<1>();
        const auto tile_x = std::round(
            ((mercator_x - tile_bbox.min_corner().get<0>()) * util::web_mercator::TILE_SIZE / box_width) *
            util::vector_tile::EXTENT / util::web_mercator::TILE_SIZE);
        const auto tile_y = std::round(
            ((tile_bbox.max_corner().get<1>() - mercator_y) * util::web_mercator::TILE_SIZE / box_height) *
            util::vector_tile::EXTENT / util::web_mercator::TILE_SIZE);

        return mercator_point_t(tile_x, tile_y);
    };

    boost::geometry::append(unclipped_line, wgs84_to_tile(segment.first));
    boost::geometry::append(unclipped_line, wgs84_to_tile(segment.second));

    mercator_multi_linestring_t clipped_line;

    boost::geometry::intersection(tile_clip_box, unclipped_line, clipped_line);

    tile_linestring_t tile_line;

    // b::g::intersection might return a line with one point if the
    // original line was very short and coords were dupes
    if (!clipped_line.empty() && clipped_line[0].size() == 2)
    {
        if (clipped_line[0].size() == 2)
        {
            for (const auto &p : clipped_line[0])
            {
                tile_line.emplace_back(p.get<0>(), p.get<1>());
            }
        }
    }

    return tile_line;
}

} }


//
// Created by markus on 20.04.21.
//

#include <random>
#include "ParticleOperator.h"
#include "BasicWithNovelty.h"
#include "../lib/helpers.h"
#include <boost/math/distributions/exponential.hpp>
#include <chrono>

using namespace std::chrono;

ParticleOperator::ParticleOperator(RobotControlInterface *robot, std::string map_filename, bool benchmarkMode) : RobotOperator(robot), particleFilter(robot, map_filename) {
    if (benchmarkMode) std::cout << "Benchmark mode support has been removed from the particle filter class. If required implement it on Scenario/Operator level" << std::endl;
    this->secondOperator = new BasicWithNovelty(robot);
}

/** loop function which will be called once every tick
 *
 */
void ParticleOperator::update() {
    // using another operator for the actual movement, so this one can focus on particle filter
    this->secondOperator->update();
    this->particleFilter.update();

}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Regression

#include <fstream>
#include <iostream>

#include <boost/test/unit_test.hpp>

#include <xolotl/core/flux/W100FitFluxHandler.h>
#include <xolotl/core/network/PSIReactionNetwork.h>
#include <xolotl/options/Options.h>
#include <xolotl/test/CommandLine.h>
#include <xolotl/util/MPIUtils.h>

using namespace std;
using namespace xolotl;
using namespace core;
using namespace flux;

using Kokkos::ScopeGuard;
BOOST_GLOBAL_FIXTURE(ScopeGuard);

/**
 * The test suite is responsible for testing the W100FitFluxHandler.
 */
BOOST_AUTO_TEST_SUITE(W100FitFluxHandlerTester_testSuite)

BOOST_AUTO_TEST_CASE(checkComputeIncidentFlux)
{
	// Create the option to create a network
	xolotl::options::Options opts;
	// Create a good parameter file
	std::string parameterFile = "param.txt";
	std::ofstream paramFile(parameterFile);
	paramFile << "netParam=9 0 0 0 0" << std::endl;
	paramFile.close();

	// Create a fake command line to read the options
	test::CommandLine<2> cl{{"fakeXolotlAppNameForTests", parameterFile}};
	util::mpiInit(cl.argc, cl.argv);
	opts.readParams(cl.argc, cl.argv);

	std::remove(parameterFile.c_str());

	// Create a grid
	std::vector<double> grid;
	for (int l = 0; l < 7; l++) {
		grid.push_back((double)l * 1.25);
	}
	// Specify the surface position
	int surfacePos = 0;

	// Create the network
	using NetworkType =
		network::PSIReactionNetwork<network::PSIFullSpeciesList>;
	NetworkType::AmountType maxV = opts.getMaxV();
	NetworkType::AmountType maxI = opts.getMaxI();
	NetworkType::AmountType maxHe = opts.getMaxImpurity();
	NetworkType::AmountType maxD = opts.getMaxD();
	NetworkType::AmountType maxT = opts.getMaxT();
	NetworkType network({maxHe, maxD, maxT, maxV, maxI}, grid.size(), opts);
	network.syncClusterDataOnHost();
	network.getSubpaving().syncZones(plsm::onHost);
	// Get its size
	const int dof = network.getDOF();

	// Create the W100 flux handler
	auto testFitFlux = make_shared<W100FitFluxHandler>(opts);
	// Set the flux amplitude
	testFitFlux->setFluxAmplitude(1.0);
	// Initialize the flux handler
	testFitFlux->initializeFluxHandler(network, surfacePos, grid);

	// Create a time
	double currTime = 1.0;

	// The array of concentration
	double newConcentration[5 * dof];

	// Initialize their values
	for (int i = 0; i < 5 * dof; i++) {
		newConcentration[i] = 0.0;
	}

	// The pointer to the grid point we want
	double* updatedConc = &newConcentration[0];
	double* updatedConcOffset = updatedConc + dof;

	// Update the concentrations at some grid points
	testFitFlux->computeIncidentFlux(
		currTime, updatedConcOffset, 1, surfacePos);
	updatedConcOffset = updatedConc + 2 * dof;
	testFitFlux->computeIncidentFlux(
		currTime, updatedConcOffset, 2, surfacePos);
	updatedConcOffset = updatedConc + 3 * dof;
	testFitFlux->computeIncidentFlux(
		currTime, updatedConcOffset, 3, surfacePos);

	// Check the value at some grid points
	BOOST_REQUIRE_CLOSE(newConcentration[9], 0.444777, 0.01);
	BOOST_REQUIRE_CLOSE(newConcentration[18], 0.247638, 0.01);
	BOOST_REQUIRE_CLOSE(newConcentration[27], 0.10758, 0.01);

	return;
}

BOOST_AUTO_TEST_CASE(checkComputeIncidentFluxNoGrid)
{
	// Create the option to create a network
	xolotl::options::Options opts;
	// Create a good parameter file
	std::string parameterFile = "param.txt";
	std::ofstream paramFile(parameterFile);
	paramFile << "netParam=9 0 0 0 0" << std::endl;
	paramFile.close();

	// Create a fake command line to read the options
	test::CommandLine<2> cl{{"fakeXolotlAppNameForTests", parameterFile}};
	opts.readParams(cl.argc, cl.argv);

	std::remove(parameterFile.c_str());

	// Create a grid
	std::vector<double> grid;
	// Specify the surface position
	int surfacePos = 0;

	// Create the network
	using NetworkType =
		network::PSIReactionNetwork<network::PSIFullSpeciesList>;
	NetworkType::AmountType maxV = opts.getMaxV();
	NetworkType::AmountType maxI = opts.getMaxI();
	NetworkType::AmountType maxHe = opts.getMaxImpurity();
	NetworkType::AmountType maxD = opts.getMaxD();
	NetworkType::AmountType maxT = opts.getMaxT();
	NetworkType network({maxHe, maxD, maxT, maxV, maxI}, grid.size(), opts);
	network.syncClusterDataOnHost();
	network.getSubpaving().syncZones(plsm::onHost);
	// Get its size
	const int dof = network.getDOF();

	// Create the W100 flux handler
	auto testFitFlux = make_shared<W100FitFluxHandler>(opts);
	// Set the flux amplitude
	testFitFlux->setFluxAmplitude(1.0);
	// Initialize the flux handler
	testFitFlux->initializeFluxHandler(network, surfacePos, grid);

	// Create a time
	double currTime = 1.0;

	// The array of concentration
	double newConcentration[dof];

	// Initialize their values
	for (int i = 0; i < dof; i++) {
		newConcentration[i] = 0.0;
	}

	// The pointer to the grid point we want
	double* updatedConc = &newConcentration[0];
	double* updatedConcOffset = updatedConc;

	// Update the concentrations at some grid points
	testFitFlux->computeIncidentFlux(
		currTime, updatedConcOffset, 0, surfacePos);

	// Check the value at some grid points
	BOOST_REQUIRE_CLOSE(newConcentration[0], 1.0, 0.01);

	return;
}

BOOST_AUTO_TEST_CASE(checkFluence)
{
	// Create the option to create a network
	xolotl::options::Options opts;
	// Create a good parameter file
	std::string parameterFile = "param.txt";
	std::ofstream paramFile(parameterFile);
	paramFile << "netParam=9 0 0 0 0" << std::endl;
	paramFile.close();

	// Create a fake command line to read the options
	test::CommandLine<2> cl{{"fakeXolotlAppNameForTests", parameterFile}};
	opts.readParams(cl.argc, cl.argv);

	std::remove(parameterFile.c_str());

	// Create a grid
	std::vector<double> grid;
	for (int l = 0; l < 7; l++) {
		grid.push_back((double)l * 1.25);
	}
	// Specify the surface position
	int surfacePos = 0;

	// Create the network
	using NetworkType =
		network::PSIReactionNetwork<network::PSIFullSpeciesList>;
	NetworkType::AmountType maxV = opts.getMaxV();
	NetworkType::AmountType maxI = opts.getMaxI();
	NetworkType::AmountType maxHe = opts.getMaxImpurity();
	NetworkType::AmountType maxD = opts.getMaxD();
	NetworkType::AmountType maxT = opts.getMaxT();
	NetworkType network({maxHe, maxD, maxT, maxV, maxI}, grid.size(), opts);
	network.syncClusterDataOnHost();
	network.getSubpaving().syncZones(plsm::onHost);
	// Get its size
	const int dof = network.getDOF();

	// Create the W100 flux handler
	auto testFitFlux = make_shared<W100FitFluxHandler>(opts);
	// Set the flux amplitude
	testFitFlux->setFluxAmplitude(1.0);
	// Initialize the flux handler
	testFitFlux->initializeFluxHandler(network, surfacePos, grid);

	// Check that the fluence is 0.0 at the beginning
	BOOST_REQUIRE_EQUAL(testFitFlux->getFluence(), 0.0);

	// Increment the fluence
	testFitFlux->incrementFluence(1.0e-8);

	// Check that the fluence is not 0.0 anymore
	BOOST_REQUIRE_EQUAL(testFitFlux->getFluence(), 1.0e-8);

	return;
}

BOOST_AUTO_TEST_CASE(checkFluxAmplitude)
{
	// Create the option to create a network
	xolotl::options::Options opts;
	// Create a good parameter file
	std::string parameterFile = "param.txt";
	std::ofstream paramFile(parameterFile);
	paramFile << "netParam=9 0 0 0 0" << std::endl;
	paramFile.close();

	// Create a fake command line to read the options
	test::CommandLine<2> cl{{"fakeXolotlAppNameForTests", parameterFile}};
	opts.readParams(cl.argc, cl.argv);

	std::remove(parameterFile.c_str());

	// Create a grid
	std::vector<double> grid;
	for (int l = 0; l < 7; l++) {
		grid.push_back((double)l * 1.25);
	}
	// Specify the surface position
	int surfacePos = 0;

	// Create the network
	using NetworkType =
		network::PSIReactionNetwork<network::PSIFullSpeciesList>;
	NetworkType::AmountType maxV = opts.getMaxV();
	NetworkType::AmountType maxI = opts.getMaxI();
	NetworkType::AmountType maxHe = opts.getMaxImpurity();
	NetworkType::AmountType maxD = opts.getMaxD();
	NetworkType::AmountType maxT = opts.getMaxT();
	NetworkType network({maxHe, maxD, maxT, maxV, maxI}, grid.size(), opts);
	network.syncClusterDataOnHost();
	network.getSubpaving().syncZones(plsm::onHost);
	// Get its size
	const int dof = network.getDOF();

	// Create the W100 flux handler
	auto testFitFlux = make_shared<W100FitFluxHandler>(opts);

	// Set the factor to change the flux amplitude
	testFitFlux->setFluxAmplitude(2.5);
	// Initialize the flux handler
	testFitFlux->initializeFluxHandler(network, surfacePos, grid);

	// Check the value of the flux amplitude
	BOOST_REQUIRE_EQUAL(testFitFlux->getFluxAmplitude(), 2.5);

	// Create a time
	double currTime = 1.0;

	// The array of concentration
	double newConcentration[5 * dof];

	// Initialize their values
	for (int i = 0; i < 5 * dof; i++) {
		newConcentration[i] = 0.0;
	}

	// The pointer to the grid point we want
	double* updatedConc = &newConcentration[0];
	double* updatedConcOffset = updatedConc + dof;

	// Update the concentrations at some grid points
	testFitFlux->computeIncidentFlux(
		currTime, updatedConcOffset, 1, surfacePos);
	updatedConcOffset = updatedConc + 2 * dof;
	testFitFlux->computeIncidentFlux(
		currTime, updatedConcOffset, 2, surfacePos);
	updatedConcOffset = updatedConc + 3 * dof;
	testFitFlux->computeIncidentFlux(
		currTime, updatedConcOffset, 3, surfacePos);

	// Check the value at some grid points
	BOOST_REQUIRE_CLOSE(newConcentration[9], 1.111943, 0.01);
	BOOST_REQUIRE_CLOSE(newConcentration[18], 0.619095, 0.01);
	BOOST_REQUIRE_CLOSE(newConcentration[27], 0.268961, 0.01);

	return;
}

BOOST_AUTO_TEST_CASE(checkTimeProfileFlux)
{
	// Create the option to create a network
	xolotl::options::Options opts;
	// Create a good parameter file
	std::string parameterFile = "param.txt";
	std::ofstream paramFile(parameterFile);
	paramFile << "netParam=9 0 0 0 0" << std::endl;
	paramFile.close();

	// Create a fake command line to read the options
	test::CommandLine<2> cl{{"fakeXolotlAppNameForTests", parameterFile}};
	opts.readParams(cl.argc, cl.argv);

	std::remove(parameterFile.c_str());

	// Create a grid
	std::vector<double> grid;
	for (int l = 0; l < 7; l++) {
		grid.push_back((double)l * 1.25);
	}
	// Specify the surface position
	int surfacePos = 0;

	// Create the network
	using NetworkType =
		network::PSIReactionNetwork<network::PSIFullSpeciesList>;
	NetworkType::AmountType maxV = opts.getMaxV();
	NetworkType::AmountType maxI = opts.getMaxI();
	NetworkType::AmountType maxHe = opts.getMaxImpurity();
	NetworkType::AmountType maxD = opts.getMaxD();
	NetworkType::AmountType maxT = opts.getMaxT();
	NetworkType network({maxHe, maxD, maxT, maxV, maxI}, grid.size(), opts);
	network.syncClusterDataOnHost();
	network.getSubpaving().syncZones(plsm::onHost);
	// Get its size
	const int dof = network.getDOF();

	// Create a file with a time profile for the flux
	// First column with the time and the second with
	// the amplitude (in He/nm2/s) at that time.
	std::string fluxFile = "fluxFile.dat";
	std::ofstream writeFluxFile(fluxFile);
	writeFluxFile << "0.0 1000.0 \n"
					 "1.0 4000.0 \n"
					 "2.0 2000.0 \n"
					 "3.0 3000.0 \n"
					 "4.0 0.0";
	writeFluxFile.close();

	auto testFitFlux = make_shared<W100FitFluxHandler>(opts);
	// Initialize the time profile for the flux handler
	testFitFlux->initializeTimeProfile(fluxFile);
	// Initialize the flux handler
	testFitFlux->initializeFluxHandler(network, surfacePos, grid);

	// Create a time
	double currTime = 0.5;

	// The array of concentration
	double newConcentration[5 * dof];

	// Initialize their values
	for (int i = 0; i < 5 * dof; i++) {
		newConcentration[i] = 0.0;
	}

	// The pointer to the grid point we want
	double* updatedConc = &newConcentration[0];
	double* updatedConcOffset = updatedConc + dof;

	// Update the concentrations at some grid points
	testFitFlux->computeIncidentFlux(
		currTime, updatedConcOffset, 1, surfacePos);
	updatedConcOffset = updatedConc + 2 * dof;
	testFitFlux->computeIncidentFlux(
		currTime, updatedConcOffset, 2, surfacePos);
	updatedConcOffset = updatedConc + 3 * dof;
	testFitFlux->computeIncidentFlux(
		currTime, updatedConcOffset, 3, surfacePos);

	// Check the value at some grid points
	BOOST_REQUIRE_CLOSE(newConcentration[9], 1111.94, 0.01);
	BOOST_REQUIRE_CLOSE(newConcentration[18], 619.095, 0.01);
	BOOST_REQUIRE_CLOSE(newConcentration[27], 268.961, 0.01);
	// Check the value of the flux amplitude
	BOOST_REQUIRE_EQUAL(testFitFlux->getFluxAmplitude(), 2500.0);

	// Change the current time
	currTime = 3.5;

	// Reinitialize their values
	for (int i = 0; i < 5 * dof; i++) {
		newConcentration[i] = 0.0;
	}

	// Update the concentrations at some grid points
	updatedConcOffset = updatedConc + dof;
	testFitFlux->computeIncidentFlux(
		currTime, updatedConcOffset, 1, surfacePos);
	updatedConcOffset = updatedConc + 2 * dof;
	testFitFlux->computeIncidentFlux(
		currTime, updatedConcOffset, 2, surfacePos);
	updatedConcOffset = updatedConc + 3 * dof;
	testFitFlux->computeIncidentFlux(
		currTime, updatedConcOffset, 3, surfacePos);

	// Check the value at some grid points
	BOOST_REQUIRE_CLOSE(newConcentration[9], 667.166, 0.01);
	BOOST_REQUIRE_CLOSE(newConcentration[18], 371.457, 0.01);
	BOOST_REQUIRE_CLOSE(newConcentration[27], 161.377, 0.01);
	// Check the value of the flux amplitude
	BOOST_REQUIRE_EQUAL(testFitFlux->getFluxAmplitude(), 1500.0);

	// Remove the created file
	std::remove(fluxFile.c_str());

	// Finalize MPI
	MPI_Finalize();

	return;
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/**
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================

#ifndef BOOST_SIMD_FUNCTION_SCALAR_EXPRECNEGC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_EXPRECNEGC_HPP_INCLUDED

#include <boost/simd/function/definition/exprecnegc.hpp>
#include <boost/simd/arch/common/scalar/function/exprecnegc.hpp>

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_SSE2_HMSB_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_SSE2_HMSB_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT

#include <boost/simd/reduction/functions/hmsb.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/dispatch/attributes.hpp>
#include <cstddef>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( hmsb_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_ < type8_<A0>
                                              , boost::simd::tag::sse_
                                              >
                                      ))
                                    )
  {
    typedef std::size_t result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return _mm_movemask_epi8(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( hmsb_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_ < type32_<A0>
                                              , boost::simd::tag::sse_
                                              >
                                      ))
                                    )
  {
    typedef std::size_t result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      typedef typename dispatch::meta::as_floating<A0>::type type;
      return _mm_movemask_ps(bitwise_cast<type>(a0));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( hmsb_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_ < type64_<A0>
                                              , boost::simd::tag::sse_
                                              >
                                      ))
                      )
  {
    typedef std::size_t result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      typedef typename dispatch::meta::as_floating<A0>::type type;
      return _mm_movemask_pd(bitwise_cast<type>(a0));
    }
  };

} } }

#endif
#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE threshold_test

#include <nil/crypto3/pubkey/modes/algorithm/sign.hpp>
#include <nil/crypto3/pubkey/modes/algorithm/verify.hpp>
#include <nil/crypto3/pubkey/modes/algorithm/part_verify.hpp>
#include <nil/crypto3/pubkey/modes/algorithm/aggregate.hpp>
#include <nil/crypto3/pubkey/algorithm/deal_shares.hpp>
#include <nil/crypto3/pubkey/modes/algorithm/create_key.hpp>

#include <nil/crypto3/pubkey/modes/threshold.hpp>

#include <nil/crypto3/pubkey/bls.hpp>

#include <nil/crypto3/pubkey/secret_sharing.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <iostream>
#include <string>
#include <cassert>
#include <unordered_map>

using namespace nil::crypto3::algebra;
using namespace nil::crypto3::hashes;
using namespace nil::crypto3::pubkey;

template<typename FieldParams>
void print_field_element(std::ostream &os, const typename fields::detail::element_fp<FieldParams> &e) {
    os << e.data << std::endl;
}

template<typename FpCurveGroupElement>
void print_fp_curve_group_element(std::ostream &os, const FpCurveGroupElement &e) {
    os << std::hex << "( " << e.X.data << " : " << e.Y.data << " : " << e.Z.data << " )" << std::endl;
}

template<typename Fp2CurveGroupElement>
void print_fp2_curve_group_element(std::ostream &os, const Fp2CurveGroupElement &e) {
    os << std::hex << "(" << e.X.data[0].data << " , " << e.X.data[1].data << ") : (" << e.Y.data[0].data << " , "
       << e.Y.data[1].data << ") : (" << e.Z.data[0].data << " , " << e.Z.data[1].data << ")" << std::endl;
}

namespace boost {
    namespace test_tools {
        namespace tt_detail {
            template<>
            struct print_log_value<typename curves::bls12<381>::g1_type::value_type> {
                void operator()(std::ostream &os, typename curves::bls12<381>::g1_type::value_type const &e) {
                    print_fp_curve_group_element(os, e);
                }
            };

            template<>
            struct print_log_value<typename curves::bls12<381>::g2_type::value_type> {
                void operator()(std::ostream &os, typename curves::bls12<381>::g2_type::value_type const &e) {
                    print_fp2_curve_group_element(os, e);
                }
            };

            template<template<typename, typename> class P, typename K, typename V>
            struct print_log_value<P<K, V>> {
                void operator()(std::ostream &, P<K, V> const &) {
                }
            };
        }    // namespace tt_detail
    }        // namespace test_tools
}    // namespace boost

const std::string msg_str = "hello foo";
const std::vector<std::uint8_t> msg(msg_str.begin(), msg_str.end());

BOOST_AUTO_TEST_SUITE(threshold_self_test_suite)

BOOST_AUTO_TEST_CASE(threshold_bls_feldman_self_test) {
    using curve_type = curves::bls12_381;
    using hash_type = sha2<256>;
    using bls_variant = bls_mps_ro_variant<curve_type, hash_type>;
    using base_scheme_type = bls<bls_variant, bls_basic_scheme>;
    using mode_type = modes::threshold<base_scheme_type, feldman_sss, nop_padding>;
    using scheme_type = typename mode_type::scheme_type;
    using privkey_type = private_key<scheme_type>;
    using pubkey_type = public_key<scheme_type>;
    using no_key_type = no_key_ops<scheme_type>;
    using sss_pubkey_no_key_type = typename privkey_type::sss_public_key_no_key_ops_type;

    std::size_t n = 20;
    std::size_t t = 10;

    //===========================================================================
    // dealer creates participants keys and its public key
    typename sss_pubkey_no_key_type::coeffs_type coeffs = sss_pubkey_no_key_type::get_poly(t, n);
    typename sss_pubkey_no_key_type::public_coeffs_type public_coeffs =
        sss_pubkey_no_key_type::get_public_coeffs(coeffs);
    typename sss_pubkey_no_key_type::public_coeffs_type public_coeffs_wrong(public_coeffs.begin(),
                                                                            public_coeffs.end() - 1);
    auto [PK, privkeys] = nil::crypto3::create_key<scheme_type>(coeffs, n);

    //===========================================================================
    // participants should check received shares before key creating
    std::vector<privkey_type> verified_privkeys;
    typename sss_pubkey_no_key_type::shares_type verified_shares =
        nil::crypto3::deal_shares<typename privkey_type::sss_public_key_group_type>(coeffs, n);
    for (auto &s : verified_shares) {
        verified_privkeys.emplace_back(nil::crypto3::create_key<scheme_type>(public_coeffs, s, n));
        BOOST_CHECK(verified_privkeys.back().verify_key(public_coeffs));
        BOOST_CHECK(!verified_privkeys.back().verify_key(public_coeffs_wrong));
    }

    //===========================================================================
    // participants sign messages and verify its signatures
    std::vector<typename privkey_type::part_signature_type> part_signatures;
    for (auto &sk : privkeys) {
        part_signatures.emplace_back(nil::crypto3::sign<mode_type>(msg, sk));
        BOOST_CHECK(static_cast<bool>(nil::crypto3::part_verify<mode_type>(msg, part_signatures.back(), sk)));
    }

    //===========================================================================
    // threshold number of participants aggregate partial signatures
    typename no_key_type::signature_type sig =
        nil::crypto3::aggregate<mode_type>(part_signatures.begin(), part_signatures.begin() + t);
    BOOST_CHECK(static_cast<bool>(nil::crypto3::verify<mode_type>(msg, sig, PK)));

    //===========================================================================
    // less than threshold number of participants cannot aggregate partial signatures
    typename no_key_type::signature_type wrong_sig =
        nil::crypto3::aggregate<mode_type>(part_signatures.begin(), part_signatures.begin() + t - 1);
    BOOST_CHECK(!static_cast<bool>(nil::crypto3::verify<mode_type>(msg, wrong_sig, PK)));
}

BOOST_AUTO_TEST_CASE(threshold_bls_pedersen_self_test) {
    using curve_type = curves::bls12_381;
    using hash_type = sha2<256>;
    using bls_variant = bls_mps_ro_variant<curve_type, hash_type>;
    using base_scheme_type = bls<bls_variant, bls_basic_scheme>;
    using mode_type = modes::threshold<base_scheme_type, pedersen_dkg, nop_padding>;
    using scheme_type = typename mode_type::scheme_type;
    using privkey_type = private_key<scheme_type>;
    using pubkey_type = public_key<scheme_type>;
    using no_key_type = no_key_ops<scheme_type>;
    using sss_pubkey_group_type = typename privkey_type::sss_public_key_group_type;
    using sss_pubkey_no_key_type = typename privkey_type::sss_public_key_no_key_ops_type;

    std::size_t n = 20;
    std::size_t t = 10;

    //===========================================================================
    // every participant generates polynomial

    std::vector<typename sss_pubkey_no_key_type::coeffs_type> P_polys;
    std::generate_n(std::back_inserter(P_polys), n, [t, n]() { return sss_pubkey_no_key_type::get_poly(t, n); });

    //===========================================================================
    // each participant calculates public values representing coefficients of its polynomial,
    // then he broadcasts these values

    std::vector<typename sss_pubkey_no_key_type::public_coeffs_type> P_public_polys;
    std::transform(P_polys.begin(), P_polys.end(), std::back_inserter(P_public_polys),
                   [](const auto &poly_i) { return sss_pubkey_no_key_type::get_public_coeffs(poly_i); });

    //===========================================================================
    // every participant generates shares for each participant in group,
    // which he then transmits to the intended parties

    std::vector<typename sss_pubkey_no_key_type::shares_type> P_generated_shares;
    std::transform(P_polys.begin(), P_polys.end(), std::back_inserter(P_generated_shares), [n, t](const auto &poly_i) {
        return static_cast<typename sss_pubkey_no_key_type::shares_type>(
            nil::crypto3::deal_shares<sss_pubkey_group_type>(poly_i, n));
    });

    std::vector<std::vector<typename sss_pubkey_no_key_type::share_type>> P_received_shares(n);
    for (auto &i_generated_shares : P_generated_shares) {
        for (auto it = i_generated_shares.begin(); it != i_generated_shares.end(); it++) {
            P_received_shares.at(it->first - 1).emplace_back(*it);
        }
    }

    //===========================================================================
    // each participant check received share and create key

    std::vector<pubkey_type> PKs;
    std::vector<privkey_type> privkeys;
    for (auto &shares : P_received_shares) {
        auto [PK_temp, privkey] = nil::crypto3::create_key<scheme_type>(P_public_polys, shares, n);
        PKs.emplace_back(PK_temp);
        privkeys.emplace_back(privkey);
    }

    //===========================================================================
    // participants sign messages and verify its signatures
    std::vector<typename privkey_type::part_signature_type> part_signatures;
    for (auto &sk : privkeys) {
        part_signatures.emplace_back(nil::crypto3::sign<mode_type>(msg, sk));
        BOOST_CHECK(static_cast<bool>(nil::crypto3::part_verify<mode_type>(msg, part_signatures.back(), sk)));
    }

    //===========================================================================
    // threshold number of participants aggregate partial signatures
    typename no_key_type::signature_type sig =
        nil::crypto3::aggregate<mode_type>(part_signatures.begin(), part_signatures.begin() + t);
    BOOST_CHECK(static_cast<bool>(nil::crypto3::verify<mode_type>(msg, sig, PKs.back())));

    //===========================================================================
    // less than threshold number of participants cannot aggregate partial signatures
    typename no_key_type::signature_type wrong_sig =
        nil::crypto3::aggregate<mode_type>(part_signatures.begin(), part_signatures.begin() + t - 1);
    BOOST_CHECK(!static_cast<bool>(nil::crypto3::verify<mode_type>(msg, wrong_sig, PKs.back())));
}

BOOST_AUTO_TEST_CASE(threshold_bls_weighted_shamir_test) {
    using curve_type = curves::bls12_381;
    using hash_type = sha2<256>;
    using bls_variant = bls_mps_ro_variant<curve_type, hash_type>;
    using base_scheme_type = bls<bls_variant, bls_basic_scheme>;
    using mode_type = modes::threshold<base_scheme_type, weighted_shamir_sss, nop_padding>;
    using scheme_type = typename mode_type::scheme_type;
    using privkey_type = private_key<scheme_type>;
    using pubkey_type = public_key<scheme_type>;
    using no_key_type = no_key_ops<scheme_type>;
    using sss_pubkey_no_key_type = typename privkey_type::sss_public_key_no_key_ops_type;

    std::size_t n = 20;
    std::size_t t = 10;

    auto i = 1;
    auto j = 1;
    typename privkey_type::sss_public_key_no_key_ops_type::weights_type weights;
    std::generate_n(std::inserter(weights, weights.end()), n, [&i, &j, &t]() {
        j = j >= t ? 1 : j;
        return typename privkey_type::sss_public_key_no_key_ops_type::weight_type(i++, j++);
    });

    //===========================================================================
    // dealer creates participants keys and its public key
    typename sss_pubkey_no_key_type::coeffs_type coeffs = sss_pubkey_no_key_type::get_poly(t, n);
    auto [PK, privkeys] = nil::crypto3::create_key<scheme_type>(coeffs, weights);

    //===========================================================================
    // participants sign messages and verify its signatures
    std::vector<typename privkey_type::part_signature_type> part_signatures;
    for (auto &sk : privkeys) {
        part_signatures.emplace_back(
            nil::crypto3::sign<mode_type>(msg.begin(), msg.end(), weights.begin(), weights.end(), sk));
        BOOST_CHECK(static_cast<bool>(nil::crypto3::part_verify<mode_type>(msg.begin(), msg.end(), weights.begin(),
                                                                           weights.end(), part_signatures.back(), sk)));
    }

    //===========================================================================
    // confirmed group of participants aggregate partial signatures
    typename no_key_type::signature_type sig =
        nil::crypto3::aggregate<mode_type>(part_signatures.begin(), part_signatures.end());
    BOOST_CHECK(static_cast<bool>(nil::crypto3::verify<mode_type>(msg, sig, PK)));

    //===========================================================================
    // not confirmed group of participants cannot aggregate partial signatures
    typename no_key_type::signature_type wrong_sig =
        nil::crypto3::aggregate<mode_type>(part_signatures.begin(), part_signatures.end() - 1);
    BOOST_CHECK(!static_cast<bool>(nil::crypto3::verify<mode_type>(msg, wrong_sig, PK)));

    //===========================================================================
    // threshold number of participants sign messages and verify its signatures

    std::vector<typename privkey_type::part_signature_type> part_signatures_t;
    typename privkey_type::sss_public_key_no_key_ops_type::weights_type confirmed_weights;
    std::vector<privkey_type> confirmed_keys;
    auto it_weight_t = privkeys.begin();
    auto weight = 0;
    while (true) {
        weight += it_weight_t->get_weight();
        if (weight >= t) {
            confirmed_keys.emplace_back(*it_weight_t);
            confirmed_weights.emplace(it_weight_t->get_index(), weights.at(it_weight_t->get_index()));
            it_weight_t++;
            break;
        }
        confirmed_keys.emplace_back(*it_weight_t);
        confirmed_weights.emplace(it_weight_t->get_index(), weights.at(it_weight_t->get_index()));
        it_weight_t++;
    }

    for (auto &sk : confirmed_keys) {
        part_signatures_t.emplace_back(nil::crypto3::sign<mode_type>(msg.begin(), msg.end(), confirmed_weights.begin(),
                                                                     confirmed_weights.end(), sk));
        BOOST_CHECK(static_cast<bool>(nil::crypto3::part_verify<mode_type>(
            msg.begin(), msg.end(), confirmed_weights.begin(), confirmed_weights.end(), part_signatures_t.back(), sk)));
    }

    //===========================================================================
    // threshold number of participants aggregate partial signatures

    typename no_key_type::signature_type sig_t =
        nil::crypto3::aggregate<mode_type>(part_signatures_t.begin(), part_signatures_t.end());
    BOOST_CHECK(static_cast<bool>(nil::crypto3::verify<mode_type>(msg, sig_t, PK)));

    //===========================================================================
    // less than threshold number of participants sign messages and verify its signatures

    std::vector<typename privkey_type::part_signature_type> part_signatures_less_t;
    typename privkey_type::sss_public_key_no_key_ops_type::weights_type confirmed_weights_less_t;
    std::vector<privkey_type> confirmed_keys_less_t;
    auto it_weight_less_t = privkeys.begin();
    auto weight_less_t = 0;
    while (true) {
        weight_less_t += it_weight_less_t->get_weight();
        if (weight_less_t >= t) {
            break;
        }
        confirmed_keys_less_t.emplace_back(*it_weight_less_t);
        confirmed_weights_less_t.emplace(it_weight_less_t->get_index(), weights.at(it_weight_less_t->get_index()));
        it_weight_t++;
    }

    for (auto &sk : confirmed_keys_less_t) {
        part_signatures_less_t.emplace_back(nil::crypto3::sign<mode_type>(
            msg.begin(), msg.end(), confirmed_weights_less_t.begin(), confirmed_weights_less_t.end(), sk));
        BOOST_CHECK(static_cast<bool>(
            nil::crypto3::part_verify<mode_type>(msg.begin(), msg.end(), confirmed_weights_less_t.begin(),
                                                 confirmed_weights_less_t.end(), part_signatures_less_t.back(), sk)));
    }

    //===========================================================================
    // less than threshold number of participants cannot aggregate partial signatures

    typename no_key_type::signature_type sig_less_t =
        nil::crypto3::aggregate<mode_type>(part_signatures_less_t.begin(), part_signatures_less_t.end());
    BOOST_CHECK(!static_cast<bool>(nil::crypto3::verify<mode_type>(msg, sig_less_t, PK)));
}

BOOST_AUTO_TEST_SUITE_END()