#include "geometric-vision/relative-non-central-pnp.h"

#include <cstdlib>
#include <memory>
#include <vector>

#include <Eigen/Core>
#include <aslam/cameras/camera-pinhole.h>
#include <aslam/cameras/camera.h>
#include <aslam/cameras/distortion-fisheye.h>
#include <aslam/cameras/distortion.h>
#include <aslam/cameras/ncamera.h>
#include <aslam/matcher/match.h>
#include <aslam/matcher/matching-engine-exclusive.h>
#include <aslam/matcher/matching-problem-frame-to-frame.h>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/quaternion-math.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>
#include <opengv/test/experiment_helpers.hpp>
#include <opengv/test/random_generators.hpp>
#include <opengv/test/time_measurement.hpp>
#include <simulation/generic-path-generator.h>
#include <simulation/visual-nframe-simulator-channels.h>
#include <simulation/visual-nframe-simulator.h>

class RelativeNonCentralPnpTest : public ::testing::Test {
 public:
  typedef aslam::FisheyeDistortion DistortionType;
  typedef aslam::PinholeCamera CameraType;
};

// Compares relative non-central PnP estimate from nframe interface
// with ground truth and with openGV PnP result.
TEST_F(RelativeNonCentralPnpTest, testInterfaceWithOpengvSetup) {
  // Force NOT to use a random seed.
  srand(1);

  // Set experiment parameters.
  // Solve problem with only one camera?
  constexpr bool kAsCentral = false;
  constexpr bool kRandomSeed = false;
  constexpr double kNoise = 0.0;
  constexpr double kOutlierFraction = 0.1;
  constexpr size_t kNumCams = 4;
  constexpr size_t kNumNFrames = 2;
  // Maximal number of correspondences between nframe 0 and nframe 1. In
  // general, the number of correspondences is (a lot) lower since not all
  // keypoints are seen from both nframes. Makes sure that the unit test
  // does not fail due to insufficient observation pairs.
  constexpr size_t kPointsPerCam = 5000;
  constexpr size_t kViewpoint0 = 0;
  constexpr size_t kViewpoint1 = 1;

  // Generate a "random"/arbitrary pose for viewpoint 0.
  opengv::translation_t position_nframe_0 = Eigen::Vector3d::Zero();
  opengv::rotation_t rotation_nframe_0 = Eigen::Matrix3d::Identity();

  // Generate a "random"/arbitrary pose for viewpoint 1.
  opengv::translation_t position_nframe_1 =
      opengv::generateRandomTranslation(2.0);
  opengv::rotation_t rotation_nframe_1 = opengv::generateRandomRotation(0.5);

  // Create a "random"/arbitrary camera system.
  opengv::translations_t camera_offsets;
  opengv::rotations_t camera_rotations;
  opengv::generateRandomCameraSystem(
      kNumCams, camera_offsets, camera_rotations);

  // Derive correspondences based on random point-cloud.
  std::vector<std::shared_ptr<opengv::bearingVectors_t> >
      multi_bearing_vectors_nframe_0;
  std::vector<std::shared_ptr<opengv::bearingVectors_t> >
      multi_bearing_vectors_nframe_1;
  std::vector<std::shared_ptr<Eigen::MatrixXd> > keypoints_ground_truth;

  opengv::generateMulti2D2DCorrespondences(
      position_nframe_0, rotation_nframe_0, position_nframe_1,
      rotation_nframe_1, camera_offsets, camera_rotations, kPointsPerCam,
      kNoise, kOutlierFraction, multi_bearing_vectors_nframe_0,
      multi_bearing_vectors_nframe_1, keypoints_ground_truth);

  // Extract the relative pose
  opengv::translation_t relative_translation_ground_truth;
  opengv::rotation_t relative_rotation_ground_truth;
  opengv::extractRelativePose(
      position_nframe_0, position_nframe_1, rotation_nframe_0,
      rotation_nframe_1, relative_translation_ground_truth,
      relative_rotation_ground_truth, false);

  // Define the camera parameters.
  constexpr double distortion_param = 0.0;
  constexpr double fu = 200.0;
  constexpr double fv = 200.0;
  constexpr size_t ru = 640;
  constexpr size_t rv = 480;
  constexpr double cu = ru / 2.0;
  constexpr double cv = rv / 2.0;

  // Set the camera parameters.
  Eigen::VectorXd distortion_params(1);
  distortion_params << distortion_param;
  Eigen::VectorXd intrinsics(4);
  intrinsics << fu, fv, cu, cv;

  // Create a vector with all cameras.
  std::vector<aslam::Camera::Ptr> camera_vector;

  // Transformation vector for all cameras.
  aslam::TransformationVector T_C_B_vector;

  // Fill the camera vector.
  for (size_t camera_idx = 0; camera_idx < kNumCams; ++camera_idx) {
    aslam::Distortion::UniquePtr distortion(
        new DistortionType(distortion_params));
    aslam::Camera::Ptr camera = std::shared_ptr<CameraType>(
        new CameraType(intrinsics, ru, rv, distortion));
    CHECK(camera);
    aslam::CameraId cam_id;
    cam_id.randomize();
    camera->setId(cam_id);
    camera_vector.push_back(camera);

    // Extrinsics
    Eigen::Matrix4d T_B_C;
    T_B_C.block<3, 3>(0, 0) = camera_rotations[camera_idx];
    T_B_C.block<3, 1>(0, 3) = camera_offsets[camera_idx];
    pose::Transformation T_B_C_(T_B_C);
    T_C_B_vector.push_back(T_B_C_.inverse());
  }

  // Create the camera rig.
  // Construct the ID object.
  aslam::NCameraId camera_rig_id;
  // Randomize the ID.
  camera_rig_id.randomize();
  aslam::NCamera::Ptr camera_rig;
  camera_rig.reset(
      new aslam::NCamera(
          camera_rig_id, T_C_B_vector, camera_vector, "pose_estimator_test"));

  // Convert the openGV simulation data to nframes to match the interface.
  std::vector<std::vector<size_t> > nframe_0_indices;
  std::vector<std::vector<size_t> > nframe_1_indices;
  aslam::VisualNFrame::Ptr nframe_0, nframe_1;
  for (size_t nframe_idx = 0; nframe_idx < kNumNFrames; ++nframe_idx) {
    aslam::NFramesId nframe_id;
    nframe_id.randomize();
    if (nframe_idx == kViewpoint0) {
      nframe_0 = aligned_shared<aslam::VisualNFrame>(nframe_id, camera_rig);
    } else if (nframe_idx == kViewpoint1) {
      nframe_1 = aligned_shared<aslam::VisualNFrame>(nframe_id, camera_rig);
    }
    // Fill in a frame for each camera in the rig.
    for (size_t camera_idx = 0; camera_idx < kNumCams; ++camera_idx) {
      // Sets up the frame with id, assigns the cameras, etc.
      aslam::VisualFrame::Ptr frame(new aslam::VisualFrame);
      aslam::FrameId frame_id;
      frame_id.randomize();
      frame->setId(frame_id);
      aslam::Camera::ConstPtr camera = camera_rig->getCameraShared(camera_idx);
      CHECK(camera);
      frame->setCameraGeometry(camera);

      // Add the measurement in nframe 0 and nframe 1.
      std::vector<Eigen::Matrix<double, 3, 1>,
                  Eigen::aligned_allocator<Eigen::Matrix<double, 3, 1> > >
          bearing_vector;
      if (nframe_idx == kViewpoint0) {
        bearing_vector = *(multi_bearing_vectors_nframe_0[camera_idx]);
      } else if (nframe_idx == kViewpoint1) {
        bearing_vector = *(multi_bearing_vectors_nframe_1[camera_idx]);
      }
      size_t num_measurements = bearing_vector.size();
      Eigen::Matrix2Xd keypoints;
      keypoints.resize(Eigen::NoChange, num_measurements);

      std::vector<size_t> keypoints_indices;
      // Project the bearing vectors in the cameras of both nframes.
      for (size_t i = 0; i < num_measurements; ++i) {
        Eigen::Vector2d projection;
        // Make sure that keypoint is visible.
        if (camera->project3(bearing_vector[i], &projection) ==
            aslam::ProjectionResult::Status::KEYPOINT_VISIBLE) {
          keypoints_indices.push_back(i);
        }
        keypoints(0, i) = projection(0);
        keypoints(1, i) = projection(1);
      }
      frame->setKeypointMeasurements(keypoints);
      if (nframe_idx == kViewpoint0) {
        nframe_0_indices.push_back(keypoints_indices);
        nframe_0->setFrame(camera_idx, frame);
      } else if (nframe_idx == kViewpoint1) {
        nframe_1_indices.push_back(keypoints_indices);
        nframe_1->setFrame(camera_idx, frame);
      }
    }  // Loop over all cameras.
  }    // Loop over both nframes.

  // Create matches pairs
  aslam::FrameToFrameMatchesList matches_paired(kNumCams);
  for (size_t camera_idx = 0; camera_idx < kNumCams; ++camera_idx) {
    // Fix index of first frame...
    for (size_t keypoint_idx_nframe_0 = 0;
         keypoint_idx_nframe_0 < nframe_0_indices[camera_idx].size();
         ++keypoint_idx_nframe_0) {
      // loop over indices in second frame ...
      for (size_t keypoint_idx_nframe_1 = 0;
           keypoint_idx_nframe_1 < nframe_1_indices[camera_idx].size();
           ++keypoint_idx_nframe_1) {
        // ... and see if it was found in the second frame.
        size_t kp_0 = nframe_0_indices[camera_idx][keypoint_idx_nframe_0];
        size_t kp_1 = nframe_1_indices[camera_idx][keypoint_idx_nframe_1];
        if (kp_0 == kp_1) {
          matches_paired[camera_idx].emplace_back(kp_0, kp_1);
        }
      }
    }
  }

  // Setup and run the PNP pose estimator.
  constexpr bool kNonlinearRefinement = false;
  geometric_vision::MultiNonCentralRelativeSacPnp::Algorithm solver =
      geometric_vision::MultiNonCentralRelativeSacPnp::Algorithm::SEVENTEENPT;
  geometric_vision::RelativeNonCentralPnp pose_estimator(
      kNonlinearRefinement, solver, kRandomSeed);
  const double pixel_sigma = sqrt(2);
  const size_t kRansacMaxIters = 100;
  pose::Transformation T_B0_B1;
  size_t num_inliers = 0;
  pose_estimator.computePinhole(
      nframe_0, nframe_1, matches_paired, pixel_sigma, kRansacMaxIters,
      &T_B0_B1, &num_inliers);

  // Compare ground truth relative pose to PNP estimates.
  EXPECT_NEAR_EIGEN(
      relative_translation_ground_truth, T_B0_B1.getPosition(), 1e-5);
  EXPECT_NEAR_EIGEN(
      relative_rotation_ground_truth, T_B0_B1.getRotationMatrix(), 1e-10);

  // Now run opengv PnP directly.
  // Remove observations that are not inside image box.
  std::vector<std::shared_ptr<opengv::bearingVectors_t> >
      multi_bearing_vectors_nframe_0_only_visible_landmarks;
  std::vector<std::shared_ptr<opengv::bearingVectors_t> >
      multi_bearing_vectors_nframe_1_only_visible_landmarks;
  for (size_t camera_idx = 0; camera_idx < kNumCams; ++camera_idx) {
    std::shared_ptr<opengv::bearingVectors_t> bearing_vectors_0(
        new opengv::bearingVectors_t());
    std::shared_ptr<opengv::bearingVectors_t> bearing_vectors_1(
        new opengv::bearingVectors_t());
    for (size_t i = 0; i < matches_paired[camera_idx].size(); ++i) {
      const size_t keypoint_idx_nframe_0 =
          matches_paired[camera_idx][i].getKeypointIndexAppleFrame();
      const size_t keypoint_idx_nframe_1 =
          matches_paired[camera_idx][i].getKeypointIndexBananaFrame();

      opengv::bearingVector_t bearing_vector_0 =
          (*multi_bearing_vectors_nframe_0[camera_idx])[keypoint_idx_nframe_0];
      opengv::bearingVector_t bearing_vector_1 =
          (*multi_bearing_vectors_nframe_1[camera_idx])[keypoint_idx_nframe_1];

      bearing_vectors_0->push_back(bearing_vector_0);
      bearing_vectors_1->push_back(bearing_vector_1);
    }
    multi_bearing_vectors_nframe_0_only_visible_landmarks.push_back(
        bearing_vectors_0);
    multi_bearing_vectors_nframe_1_only_visible_landmarks.push_back(
        bearing_vectors_1);
  }

  // Create a non-central relative multi-adapter.
  opengv::relative_pose::NoncentralRelativeMultiAdapter adapter(
      multi_bearing_vectors_nframe_0_only_visible_landmarks,
      multi_bearing_vectors_nframe_1_only_visible_landmarks, camera_offsets,
      camera_rotations);

  // Create a MultiNoncentralRelativePoseSacProblem and Ransac.
  opengv::sac::MultiRansac<geometric_vision::MultiNonCentralRelativeSacPnp>
      ransac(kRandomSeed);
  std::shared_ptr<geometric_vision::MultiNonCentralRelativeSacPnp> problem_ptr(
      new geometric_vision::MultiNonCentralRelativeSacPnp(
          adapter, geometric_vision::MultiNonCentralRelativeSacPnp::SEVENTEENPT,
          kAsCentral, kRandomSeed));

  ransac.sac_model_ = problem_ptr;
  const double focal_length = (fu + fv) / 2.0;
  ransac.threshold_ = 1.0 - cos(atan(pixel_sigma / focal_length));
  ransac.max_iterations_ = kRansacMaxIters;

  // Run the ransac experiment.
  ransac.computeModel();

  Eigen::Vector3d relative_translation_opengv =
      ransac.model_coefficients_.block<3, 1>(0, 3);
  Eigen::Matrix3d relative_rotation_opengv =
      ransac.model_coefficients_.block<3, 3>(0, 0);

  // Compare the direct opengv PNP result to PNP result from interface.
  EXPECT_NEAR_EIGEN(relative_translation_opengv, T_B0_B1.getPosition(), 1e-3);
  EXPECT_NEAR_EIGEN(
      relative_rotation_opengv, T_B0_B1.getRotationMatrix(), 1e-3);
}

// *************************************************************
// The test goes as follows:
// [1] Generate the cameras.
// [2] Generate the path and landmarks.
// [3] Setup and run the visual nframe simulator.
// [4] Feed the visual nframes to the matcher.
// [5] Convert aslam::MatchingProblemFrameToFrame::Matches to
// geometric_vision::Matches
// [6] Setup and run the PNP pose estimator.
// [7] Validate the PNP pose estimates.
// *************************************************************
TEST_F(RelativeNonCentralPnpTest, testInterfaceWithGeometricVisionSetup) {
  // **************************************************
  // [1] Generate the cameras.
  // **************************************************
  const size_t kNumCams = 4;
  aslam::NCamera::Ptr n_camera = aslam::NCamera::createTestNCamera(kNumCams);

  // **************************************************
  // [2] Generate the path and landmarks.
  // **************************************************
  test_trajectory_gen::PathAndLandmarkSettings settings;
  settings.mode = test_trajectory_gen::Path::kCircular;
  settings.imu_sigmas.acc_bias_random_walk_noise_density = 0.0;
  settings.imu_sigmas.acc_noise_density = 0.0;
  settings.gravity_meter_by_second2 = 9.81;
  settings.imu_sigmas.gyro_bias_random_walk_noise_density = 0.0;
  settings.imu_sigmas.gyro_noise_density = 0.0;
  settings.imu_noise_bias_seed = 10u;
  settings.num_of_path_constraints = 8;
  settings.landmark_seed = 5u;
  settings.landmark_variance_meter = 0.25;
  settings.num_of_landmarks = 1000;
  settings.sampling_time_second = 0.1;
  settings.distance_to_keypoints_meter = 7.5;

  std::shared_ptr<test_trajectory_gen::GenericPathGenerator> path_generator;
  path_generator =
      aligned_shared<test_trajectory_gen::GenericPathGenerator>(settings);
  path_generator->generatePath();
  path_generator->generateLandmarks();

  // Get ground truth transformations between frames.
  aslam::TransformationVector true_T_G_B;
  path_generator->getGroundTruthTransformations(&true_T_G_B);

  // **************************************************
  // [3] Setup and run visual nframe simulator.
  // **************************************************
  simulation::VisualNFrameSimulator nframe_simulator;
  aslam::VisualNFrame::PtrVector nframes;
  const bool kAddNoiseToKeypoints = false;
  const size_t kNumBitsToFlip = 0u;
  const double kKeypointSigmaPixel = 0.8;

  aslam::TransformationVector T_G_Bs;
  path_generator->getGroundTruthTransformations(&T_G_Bs);
  aslam::TransformationVector poses_without_keypoints;
  nframe_simulator.simulateVisualNFrames(
      path_generator->getTimestampsInSeconds(), T_G_Bs,
      path_generator->getLandmarks(), n_camera, kKeypointSigmaPixel,
      kAddNoiseToKeypoints, kNumBitsToFlip, &nframes, &poses_without_keypoints);
  ASSERT_GT(nframes.size(), 0u) << "Zero simulated nframes. Aborting.";
  CHECK(poses_without_keypoints.empty());

  // **************************************************
  // [4] Feed the visual nframes to the matcher.
  // **************************************************
  // Choose two consecutive (arbitrary) nframes with some translation in
  // between.
  constexpr size_t kFrame0Idx = 3;
  constexpr size_t kFrame1Idx = 4;

  const aslam::Transformation& T_G_B0 = true_T_G_B[kFrame0Idx];
  const aslam::Transformation& T_G_B1 = true_T_G_B[kFrame1Idx];

  // Set matcher parameters.
  constexpr double kImageSpaceDistanceThreshold = 500.0;
  constexpr size_t kDescriptorDistanceThreshold = 5;

  aslam::MatchingEngineExclusive<aslam::MatchingProblemFrameToFrame>
      matching_engine;
  aslam::MatchingProblemFrameToFrame::MatchesList matches_0_1;
  // Start for-loop cameras.
  for (size_t camera_idx = 0; camera_idx < kNumCams; ++camera_idx) {
    // Previous frame == frame 0.
    aslam::VisualFrame::Ptr frame0 =
        nframes[kFrame0Idx]->getFrameShared(camera_idx);
    CHECK(frame0);
    CHECK_EQ(nframes[kFrame0Idx]->getNumFrames(), kNumCams);

    // Current frame == frame 1.
    aslam::VisualFrame::Ptr frame1 =
        nframes[kFrame1Idx]->getFrameShared(camera_idx);
    CHECK(frame1);
    CHECK_EQ(nframes[kFrame1Idx]->getNumFrames(), kNumCams);

    // Get the ground truth rotation between the first and second frame.
    const aslam::Transformation& T_C_B0 =
        nframes[kFrame0Idx]->get_T_C_B(camera_idx);
    const aslam::Transformation& T_C_B1 =
        nframes[kFrame1Idx]->get_T_C_B(camera_idx);
    const aslam::Transformation T_C0_C1 =
        T_C_B0 * T_G_B0.inverse() * T_G_B1 * T_C_B1.inverse();
    aslam::MatchingProblemFrameToFrame matching_problem(
        *frame0, *frame1, T_C0_C1.getRotation(), kImageSpaceDistanceThreshold,
        kDescriptorDistanceThreshold);

    aslam::MatchingProblemFrameToFrame::Matches matches;
    matching_engine.match(&matching_problem, &matches);
    matches_0_1.push_back(matches);
  }  // End for-loop cameras.

  // **************************************************
  // [5] Setup and run the PNP pose estimator.
  // **************************************************
  // Force deterministic SAC problem.
  constexpr bool kNonlinearRefinement = false;
  constexpr bool kRandomSeed = false;
  geometric_vision::MultiNonCentralRelativeSacPnp::Algorithm solver =
      geometric_vision::MultiNonCentralRelativeSacPnp::Algorithm::SEVENTEENPT;
  geometric_vision::RelativeNonCentralPnp pose_estimator(
      kNonlinearRefinement, solver, kRandomSeed);
  constexpr double kPixelSigma = 0.8;
  constexpr size_t kRansacMaxIters = 100;
  pose::Transformation pnp_T_B0_B1;
  size_t num_inliers = 0;
  pose_estimator.computePinhole(
      nframes[kFrame0Idx], nframes[kFrame1Idx], matches_0_1, kPixelSigma,
      kRansacMaxIters, &pnp_T_B0_B1, &num_inliers);

  // **************************************************
  // [6] Validate the PNP pose estimates.
  // **************************************************
  const aslam::Transformation true_T_B1_B0 =
      true_T_G_B[kFrame1Idx].inverse() * true_T_G_B[kFrame0Idx];
  const aslam::Transformation pnp_T_B1_B0 = pnp_T_B0_B1.inverse();

  EXPECT_NEAR_EIGEN(
      true_T_B1_B0.getPosition().normalized(),
      pnp_T_B1_B0.getPosition().normalized(), 1.0e-4);
  EXPECT_NEAR_EIGEN(
      true_T_B1_B0.getRotationMatrix(), pnp_T_B1_B0.getRotationMatrix(),
      1.0e-8);
}

MAPLAB_UNITTEST_ENTRYPOINT


#pragma once

#include <Eigen/Dense>
#include <vector>

#include <Configuration.h>

class DracoStateProvider;
class RobotSystem;
class BasicAccumulation;
class DracoSensorData;
class filter;

class DracoStateEstimator {
 public:
  DracoStateEstimator(RobotSystem* robot);
  ~DracoStateEstimator();

  void initialization(DracoSensorData*);
  void update(DracoSensorData*);

 protected:
  double joint_velocity_filter_freq_;
  double angular_velocity_filter_freq_;

  double initial_height_;
  int fixed_foot_;
  Eigen::Vector3d foot_pos_;
  DracoStateProvider* sp_;
  RobotSystem* robot_;

  Eigen::VectorXd curr_config_;
  Eigen::Vector3d global_linear_offset_;
  Eigen::VectorXd prev_config_;

  Eigen::VectorXd curr_qdot_;
  Eigen::VectorXd prev_qdot_;
  Eigen::Vector3d global_body_euler_zyx_;
  Eigen::Quaternion<double> global_body_quat_;
  Eigen::Vector3d global_body_euler_zyx_dot_;
  Eigen::Vector3d prev_body_euler_zyx_dot_;
  Eigen::VectorXd virtual_q_;
  Eigen::VectorXd virtual_qdot_;

  BasicAccumulation* ori_est_;
  filter* x_vel_est_;
  filter* y_vel_est_;
  filter* z_vel_est_;

  void _JointUpdate(DracoSensorData* data);
  void _ConfigurationAndModelUpdate();
  void _FootContactUpdate(DracoSensorData* data);
  void _UpdateDCM();
  void MapToTorso_(const Eigen::VectorXd& imu_acc,
                   const Eigen::VectorXd& imu_angvel,
                   std::vector<double>& torso_acc,
                   std::vector<double>& torso_angvel);

  // clamp helper function
  double clamp_value(double in, double min, double max);

  // compute alpha from break frequency
  double computeAlphaGivenBreakFrequency(double hz, double dt);
};


// FILE: src/packet_handler.cpp  Date Modified: March 17, 2015
// PacketGenie
// Copyright 2014 The Regents of the University of Michigan
// Dong-Hyeon Park, Ritesh Parikh, Valeria Bertacco
//
// PacketGenie is licensed under the Apache License, Version 2.0
// (the "License"); you may not use this file except in compliance
// with the License. You may obtain a copy of the License at
//     http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.

#include "packet_handler.hpp"
#include <cstdlib>
#include <boost/lexical_cast.hpp>
#include <math.h>

ReplyPacketHandler* ReplyPacketHandler::New(vector<string> *str_params)
{
    string new_id, new_type;
    size_t new_size, path_delay;
    ReplyPacket* new_pckt;
    if (str_params->size() < 3)
        return NULL;

    new_id = str_params->at(0);
    new_size = boost::lexical_cast<size_t>(str_params->at(2));
    new_pckt = new ReplyPacket(new_id, new_size, 0,
                               NULL, NULL, NULL, "UNSPECIFIED");

    new_type = str_params->at(1);
    path_delay = 0;

    // Instantiate the appropriate Packet Handler
    ReplyPacketHandler* new_handler;
    if (new_type == "erlang")
        new_handler = new ReplyPacketHandlerErlang(new_pckt, path_delay);
    else if (new_type == "dram")
        new_handler = new ReplyPacketHandlerDram(new_pckt, path_delay);
    else
        new_handler = new ReplyPacketHandler("fixed", new_pckt, path_delay);

    int error = new_handler->init(str_params);
    if (error)
        return NULL;
    else
        return new_handler;

}

int ReplyPacketHandler::init(vector<string> *str_params)
{
    m_process_delay = boost::lexical_cast<double>(str_params->at(3));
    return SUCCESS;
}

// NOTE: ready_to_reply should be getting invoked based on the period of
//       node's internal clock, not the global clock
bool ReplyPacketHandler::ready_to_reply(size_t node_clock,
                                        ReplyJob *test_job,
                                        receivers_t *replier_pool)
{
    if(test_job->get_wait_count() == (m_path_delay+m_process_delay))
        return true; // Consumed.

    test_job->inc_count();
    return false;
}

ReplyJob* ReplyPacketHandler::create_reply_job(RequestPacket* request_packet)
{
    // ReplyPacket gets created (allocated to heap) here,
    // deleted at traffic_report_handler.
    return ReplyJob::New(this, request_packet, m_packet->clone(), "Default");
}

int ReplyPacketHandlerErlang::init(vector<string> *str_params)
{
    //    -packet_type_reply <name> <type> <size>
    //                       <arrival_rate> <service_time>
    if ( str_params->size() < 5)
        return ERROR;

    m_arrival_rate =  boost::lexical_cast<double>(str_params->at(3));
    m_service_time =  boost::lexical_cast<double>(str_params->at(4));

    m_erlangC = erlang_delay_constant(m_arrival_rate, m_service_time);
    return SUCCESS;
}

bool ReplyPacketHandlerErlang::ready_to_reply(size_t node_clock,
                                              ReplyJob *test_job,
                                              receivers_t *replier_pool)
{
    size_t packet_age;
    double rand_val, thresh;
    packet_age = test_job->get_wait_count();

    if (packet_age >= m_path_delay)
    {
        rand_val = get_rand_decimal();
        thresh = erlang_prob_wait(packet_age-m_path_delay,
                                  m_arrival_rate, m_service_time,
                                  m_erlangC);

        if (rand_val > thresh)
            return true;
    }

    test_job->inc_count();
    return false;
}

ReplyJob* ReplyPacketHandlerDram::create_reply_job(RequestPacket* request_packet)
{
    assert((request_packet->get_dest())->is_dram());
    return ReplyJob::New(this, request_packet, m_packet->clone(), "Dram");
}

bool ReplyPacketHandlerDram::ready_to_reply(size_t node_clock,
                                            ReplyJob *test_job,
                                            receivers_t *replier_pool)
{
    assert(test_job->get_type() == "Dram");
    ReplyJobDram* dram_job = (ReplyJobDram*)test_job;

    DramJobStatus status;
    size_t packet_age;
    Node* mc;
    bool change_status = false;

    status = dram_job->get_status();
    packet_age = dram_job->get_wait_count();
    mc = dram_job->get_replier();

    if (status == IN_NETWORK)
    {
        // If the upcoming increment will change the status
        if (packet_age+1 >= m_path_delay)
            change_status = true;
    }
    else if (status == WAITING_BANK)
    {
        change_status = mc->bank_ready(dram_job->get_bank_count());
        if (change_status)
            dram_job->set_access_time(mc->generate_access_time());
    }
    else if (status == WAITING_ACCESS)
    {
        change_status = dram_job->check_access_status();
    }
    else if (status == DONE_ACCESS)
    {
        return true;
    }

    // Increment the wait counter
    dram_job->inc_count();

    // Because of how inc_count is setup in ReplyJobDram,
    // we need to change status after the count.
    if (change_status)
        dram_job->advance_status();

    return false;
}


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   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.

   Author: Moritz Dannhauer, Spencer Frisby
   Date:   September 2014
*/

///@file SetupRHSforTDCSandTMS
///@brief
/// This module sets up TDCS problem by providing the right hand side vector (controlled by GUI) and
/// inputs for the modules: AddKnownsToLinearSystem, BuildTDCSMatrix.
///
///@author
/// Moritz Dannhauer, Spencer Frisby
///
///@details
/// .The module gets 4 inputs the tetrahedral mesh (first), triangle surfaces (as part of the first
/// input) for the scalp (second) and electrodes sponges (third) as well as the a matrix (output of
/// ElectrodeCoilSetup)
///  that contains the center location and thickness of the electrode sponge. The first output
///  contains the knowns (reference zeros node) of the linear system (third input of
///  AddKnownsToLinearSystem). The zero potential node is set to be the first mesh node by default,
///  which can be chosen arbitrarily in the GUI by the user. The second until fifth output prepares
///  the inputs for the BuildTDCSMatrix module that combines the FEM stiffness matrix with the
///  boundary conditions of the complete electrode model. The electrical impedances can be provided
///  in the GUI for each electrode individually. The module needs to identify the surface of the
///  sponge which makes up the major part of the algorithmic module implementation. The sixth output
///  creates the right hand side (RHS) vector that can be provided by the GUI. The electrical
///  charges (ingoing and outgoing current intensities) need to sum up zero. If you execute the
///  module once the module will graphically only present the number of electrodes defined in the
///  input data. The complete electrode model solves the nodal and electrode potentials in an
///  agumented linear system the potential solution (output of SolveLinearSystem) needs to
///  adjuested. Therefore, a seventh output is provided that contains the relevant indeces to be
///  used for the module 'SelectSubMatrix'. Since the identification of the contact surface between
///  electrode sponge and the electrode itself (the sponge surface top is assumed to be that) is a
///  difficult geometrical process the eighth output provides a triangle mesh that can be used for
///  visual inspection (->ShowField->ViewScene).

#include <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/Algorithms/Base/VariableHelper.h>
#include <Core/Algorithms/BrainStimulator/SetupRHSforTDCSandTMSAlgorithm.h>
#include <Core/Algorithms/Legacy/Fields/DomainFields/SplitFieldByDomainAlgo.h>
#include <Core/Algorithms/Legacy/Fields/MeshDerivatives/SplitByConnectedRegion.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/Legacy/Field/Field.h>
#include <Core/Datatypes/Legacy/Field/FieldInformation.h>
#include <Core/Datatypes/Legacy/Field/VField.h>
#include <Core/Datatypes/Legacy/Field/VMesh.h>
#include <Core/Datatypes/Matrix.h>
#include <Core/Datatypes/MatrixComparison.h>
#include <Core/Datatypes/MatrixTypeConversions.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Datatypes/SparseRowMatrixFromMap.h>
#include <Core/GeometryPrimitives/Vector.h>
#include <boost/lexical_cast.hpp>

using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core::Algorithms::BrainStimulator;
using namespace SCIRun::Core::Geometry;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Algorithms::Fields;
using namespace SCIRun;

ALGORITHM_PARAMETER_DEF(BrainStimulator, ElectrodeTableValues);
ALGORITHM_PARAMETER_DEF(BrainStimulator, ImpedanceTableValues);
ALGORITHM_PARAMETER_DEF(BrainStimulator, SurfaceAreaValues);
ALGORITHM_PARAMETER_DEF(BrainStimulator, ELECTRODE_VALUES);
ALGORITHM_PARAMETER_DEF(BrainStimulator, IMPEDANCE_VALUES);
ALGORITHM_PARAMETER_DEF(BrainStimulator, refnode);
ALGORITHM_PARAMETER_DEF(BrainStimulator, normal_dot_product_bound);
ALGORITHM_PARAMETER_DEF(BrainStimulator, pointdistancebound);
ALGORITHM_PARAMETER_DEF(BrainStimulator, number_of_electrodes);
ALGORITHM_PARAMETER_DEF(BrainStimulator, GetContactSurface);

AlgorithmInputName SetupTDCSAlgorithm::MESH("MESH");
AlgorithmInputName SetupTDCSAlgorithm::SCALP_TRI_SURF_MESH("SCALP_TRI_SURF_MESH");
AlgorithmInputName SetupTDCSAlgorithm::ELECTRODE_TRI_SURF_MESH("ELECTRODE_TRI_SURF_MESH");
AlgorithmInputName SetupTDCSAlgorithm::ELECTRODE_SPONGE_LOCATION_AVR(
    "ELECTRODE_SPONGE_LOCATION_AVR");
AlgorithmOutputName SetupTDCSAlgorithm::ELECTRODE_ELEMENT("ELECTRODE_ELEMENT");
AlgorithmOutputName SetupTDCSAlgorithm::ELECTRODE_ELEMENT_TYPE("ELECTRODE_ELEMENT_TYPE");
AlgorithmOutputName SetupTDCSAlgorithm::ELECTRODE_ELEMENT_DEFINITION(
    "ELECTRODE_ELEMENT_DEFINITION");
AlgorithmOutputName SetupTDCSAlgorithm::ELECTRODE_CONTACT_IMPEDANCE("ELECTRODE_CONTACT_IMPEDANCE");
AlgorithmOutputName SetupTDCSAlgorithm::RHS("RHS");
AlgorithmOutputName SetupTDCSAlgorithm::LHS_KNOWNS("LHS_KNOWNS");
AlgorithmOutputName SetupTDCSAlgorithm::ELECTRODE_SPONGE_SURF("ELECTRODE_SPONGE_SURF");
AlgorithmOutputName SetupTDCSAlgorithm::SELECTMATRIXINDECES("SELECTMATRIXINDECES");

AlgorithmParameterName SetupTDCSAlgorithm::ElectrodeParameterName(int i)
{
  return AlgorithmParameterName(Name("elc" + boost::lexical_cast<std::string>(i)));
}
AlgorithmParameterName SetupTDCSAlgorithm::ElectrodeImpedanceParameterName(int i)
{
  return AlgorithmParameterName(Name("imp_elc" + boost::lexical_cast<std::string>(i)));
}

const double SetupTDCSAlgorithm::electode_current_summation_bound = 1e-6;
const int SetupTDCSAlgorithm::special_label = 4321;
const int SetupTDCSAlgorithm::max_number_of_electrodes = 512;

SetupTDCSAlgorithm::SetupTDCSAlgorithm()
{
  addParameter(Parameters::SurfaceAreaValues, 0);
  addParameter(
      Parameters::ELECTRODE_VALUES, 0);  // just a default value, will be replaced with vector
  addParameter(Parameters::IMPEDANCE_VALUES, 0);
  addParameter(Parameters::number_of_electrodes, max_number_of_electrodes);
  addParameter(Parameters::refnode, 0);
  addParameter(Parameters::normal_dot_product_bound, 0.7);
  addParameter(Parameters::pointdistancebound, 0.0001);
  addParameter(Parameters::GetContactSurface, false);
}

AlgorithmOutput SetupTDCSAlgorithm::run(const AlgorithmInput& input) const
{
  auto mesh = input.get<Field>(MESH);

  auto all_elc_values = get(Parameters::ELECTRODE_VALUES).toVector();
  auto all_imp_elc_values = get(Parameters::IMPEDANCE_VALUES).toVector();

  if (all_elc_values.size() != all_imp_elc_values.size())
  {
    THROW_ALGORITHM_PROCESSING_ERROR(
        "Internal Error: electrode information could not retrieved from GUI.");
  }

  /// consistency checks to validate the GUI<->Algorithm data transfer
  for (int i = 0; i < all_elc_values.size(); i++)
  {
    auto elecName = all_elc_values[i].nameForXml();
    // auto elecValue = all_elc_values[i].getDouble(); // why is not that used?
    auto expectedElecName = ElectrodeParameterName(i);  // ElecrodeParameterName(i);
    if (elecName.name_.compare(expectedElecName.name_) !=
        0)  // if so, electrodes are being stored out of order.
      THROW_ALGORITHM_PROCESSING_ERROR("Values are being stored out of order!");

    auto impelecName = all_imp_elc_values[i].nameForXml();
    auto expectedImpElecName = ElectrodeImpedanceParameterName(i);
    if (elecName.name_.compare(expectedElecName.name_) !=
        0)  // if so, electrodes are being stored out of order.
      THROW_ALGORITHM_PROCESSING_ERROR("Impedance values are being stored out of order!");
  }

  auto scalp_tri_surf = input.get<Field>(SCALP_TRI_SURF_MESH);
  auto elc_tri_surf = input.get<Field>(ELECTRODE_TRI_SURF_MESH);

  if (!elc_tri_surf)
  {
    THROW_ALGORITHM_PROCESSING_ERROR("Input Error: electrode triangle surface is not allocated.");
  }

  if (!scalp_tri_surf)
  { THROW_ALGORITHM_PROCESSING_ERROR("Input Error: Scalp triangle surface is not allocated."); }

  // obtaining number of electrodes
  DenseMatrixHandle elc_sponge_location =
      convertMatrix::toDense(input.get<Matrix>(ELECTRODE_SPONGE_LOCATION_AVR));
  if (!elc_sponge_location)
  {
    THROW_ALGORITHM_PROCESSING_ERROR(
        "Electrode sponges matrix (center locations) is not allocated.");
  }
  if (elc_sponge_location->ncols() != 4 || elc_sponge_location->nrows() < 2)
  {
    THROW_ALGORITHM_PROCESSING_ERROR(
        "Electrode sponges matrix needs to have dimension #sponges x 4 (#sponges>=2)");
  }

  int num_of_elc = elc_sponge_location->nrows();

  AlgorithmOutput output;

  DenseMatrixHandle lhs_knowns, elc_element, elc_element_typ, elc_element_def, elc_contact_imp, rhs,
      selectmatrixind;
  FieldHandle elec_sponge_surf;
  std::vector<double> electrode_sponge_areas;
  boost::tie(lhs_knowns, elc_element, elc_element_typ, elc_element_def, elc_contact_imp, rhs,
      elec_sponge_surf, selectmatrixind, electrode_sponge_areas) = run(mesh, all_elc_values,
      all_imp_elc_values, num_of_elc, scalp_tri_surf, elc_tri_surf, elc_sponge_location);

  output[LHS_KNOWNS] = lhs_knowns;
  output[ELECTRODE_ELEMENT] = elc_element;
  output[ELECTRODE_ELEMENT_TYPE] = elc_element_typ;
  output[ELECTRODE_ELEMENT_DEFINITION] = elc_element_def;
  output[ELECTRODE_CONTACT_IMPEDANCE] = elc_contact_imp;
  output[RHS] = rhs;
  output[SELECTMATRIXINDECES] = selectmatrixind;
  output[ELECTRODE_SPONGE_SURF] = elec_sponge_surf;

  Variable::List surface_area;
  for (long i = 0; i < electrode_sponge_areas.size(); i++)
  {
    surface_area.push_back(makeVariable(
        "surf_area_" + boost::lexical_cast<std::string>(i), electrode_sponge_areas[i]));
  }
  VariableHandle var_surface_area(new Variable(Name("surf_areas"), surface_area));
  output.setAdditionalAlgoOutput(var_surface_area);
  return output;
}

boost::tuple<DenseMatrixHandle, DenseMatrixHandle, DenseMatrixHandle, DenseMatrixHandle,
    DenseMatrixHandle, DenseMatrixHandle, FieldHandle, std::vector<double>>
SetupTDCSAlgorithm::create_lhs(FieldHandle mesh, const std::vector<Variable>& impelc,
    FieldHandle scalp_tri_surf, FieldHandle elc_tri_surf,
    DenseMatrixHandle elc_sponge_location) const
{
  FieldInformation fi1(mesh);
  if (!fi1.is_tetvolmesh())
  {
    THROW_ALGORITHM_PROCESSING_ERROR(
        " The mesh (first module input, field, MESH) must be a tetrahedral mesh. ");
  }

  FieldInformation fi2(scalp_tri_surf);
  if (!fi2.is_trisurfmesh())
  {
    THROW_ALGORITHM_PROCESSING_ERROR(
        " The scalp surface (second module input, field, SCALP_TRI_SURF_MESH) must be a triangle "
        "mesh. ");
  }

  VMesh* mesh_vmesh = mesh->vmesh();
  VMesh::size_type mesh_num_nodes = mesh_vmesh->num_nodes();
  mesh_vmesh->synchronize(Mesh::NODE_LOCATE_E);
  DenseMatrixHandle lhs_knows, elc_elem, elc_elem_typ, elc_elem_def, elc_con_imp;
  std::vector<double> electrode_sponge_areas;
  index_type refnode_number = get(Parameters::refnode).toInt();
  bool getsurf = get(Parameters::GetContactSurface).toBool();
  double normal_dot_product_bound_ = get(Parameters::normal_dot_product_bound).toDouble();
  double identical_node_location_differce = get(Parameters::pointdistancebound).toDouble();

  if (identical_node_location_differce <= 0 || refnode_number < 0 ||
      normal_dot_product_bound_ < 0)  /// check for valid input data
  {
    THROW_ALGORITHM_PROCESSING_ERROR(
        " Gui values (refnode, point dis., surf shrink) should be > 0. ");
  }

  FieldInformation fi4(elc_tri_surf);
  if (!(fi4.is_trisurfmesh() || fi4.is_pointcloudmesh()))
  {
    THROW_ALGORITHM_PROCESSING_ERROR(
        " The electrode surface mesh (third module input, field, ELECTRODE_TRI_SURF) must be a "
        "triangle surface mesh or a input field with points (pointcloudmesh). ");
  }

  if (refnode_number > mesh_num_nodes || refnode_number < 0)
  { THROW_ALGORITHM_PROCESSING_ERROR(" Reference node exceeds number of FEM nodes. "); }
  /// prepare LHS_KNOWNS which is the an input to addknownstolinearsystem (called x) besides the
  /// stiffness matrix
  lhs_knows = makeShared<DenseMatrix>(mesh_num_nodes, 1);
  for (VMesh::Node::index_type idx = 0; idx < mesh_num_nodes; idx++)
  { (*lhs_knows)(idx, 0) = std::numeric_limits<double>::quiet_NaN(); }
  (*lhs_knows)(refnode_number, 0) = 0;

  if (fi4.is_pointcloudmesh())
  {
    std::ostringstream ostr4;
    ostr4 << " The third module input appears as a point electrode definition. The complete "
             "electrode model is not used in that case and therefore 2-5th and 7th will not "
             "contain output data. "
          << std::endl;
    remark(ostr4.str());

    VMesh* mesh_elc_tri_surf = elc_tri_surf->vmesh();
    mesh_elc_tri_surf->synchronize(Mesh::NODE_LOCATE_E);
    std::vector<double> fvalues;
    FieldInformation fieldinfo(
        "PointCloudMesh", static_cast<int>(databasis_info_type::LINEARDATA_E), "double");
    FieldHandle point_electrodes = CreateField(fieldinfo);
    VMesh* point_electrodes_mesh = point_electrodes->vmesh();

    for (VMesh::Node::index_type l = 0; l < mesh_elc_tri_surf->num_nodes(); l++)
    {
      Point p, q;
      mesh_elc_tri_surf->get_center(p, l);
      double distance = -1;
      VMesh::Node::index_type ind;
      mesh_vmesh->find_closest_node(distance, q, ind, p);
      point_electrodes_mesh->add_point(q);
      fvalues.push_back(0);
      electrode_sponge_areas.push_back(std::numeric_limits<double>::quiet_NaN());
    }
    VField* point_electrodes_vfld = point_electrodes->vfield();
    point_electrodes_vfld->resize_values();
    point_electrodes_vfld->set_values(fvalues);

    DenseMatrixHandle empty;

    return boost::make_tuple(empty, lhs_knows, elc_elem, elc_elem_typ, elc_elem_def, elc_con_imp,
        point_electrodes, electrode_sponge_areas);
  }

  SplitFieldByConnectedRegionAlgo algo;
  algo.set(Fields::Parameters::SortDomainBySize, false);
  algo.set(Fields::Parameters::SortAscending, false);
  std::vector<FieldHandle> result = algo.run(elc_tri_surf);

  if (result.size() <= 0)
  { THROW_ALGORITHM_PROCESSING_ERROR(" Splitting input mesh into connected regions failed. "); }

  if (elc_sponge_location->ncols() != 4)
  {
    THROW_ALGORITHM_PROCESSING_ERROR(
        " ELECTRODE_SPONGE_LOCATION_AVR (4th module input) needs to have 4 columns (x,y,z,elc. "
        "sponge height).");
  }

  if (result.size() != elc_sponge_location->nrows())
  {
    THROW_ALGORITHM_PROCESSING_ERROR(
        " The number of electrode sponges (4th module input) does not match number of a splitted "
        "electrode surfaces (3rd module input).");
  }

  if (result.size() != impelc.size())
  {
    THROW_ALGORITHM_PROCESSING_ERROR(
        " Number of electrode contact impadances (GUI) do not match up with the number of "
        "electrodes (modules third input).  ");
  }

  if (elc_sponge_location->nrows() != impelc.size())
  {
    THROW_ALGORITHM_PROCESSING_ERROR(
        " Internal error: Number of electrode contact impadances do not match up with the number "
        "of electrodes (GUI row table)!?!? ");
  }

  FieldInformation fieldinfo(
      "TriSurfMesh", static_cast<int>(databasis_info_type::CONSTANTDATA_E), "int");
  FieldHandle elc_sponge_surf = CreateField(fieldinfo);
  VMesh* elc_sponge_surf_vmesh = elc_sponge_surf->vmesh();
  VField* elc_sponge_surf_vfld = elc_sponge_surf->vfield();
  std::vector<double> field_values, impedances;

  /// map the electrode sponge center (CreateElectrodeCoil) to generated tDCS electrode geometry
  /// (Cleaver), this mapping is meant to map the GUI inputs to actual electrode geometry by having
  /// a lookup table
  int electrode_sponges = elc_sponge_location->nrows();
  DenseMatrixHandle lookup(new DenseMatrix(electrode_sponges, 1));
  DenseMatrixHandle distances(new DenseMatrix(electrode_sponges, 1));
  VMesh::Node::index_type didx;
  double distance = 0;

  for (long i = 0; i < electrode_sponges; i++)
  {
    Point elc(
        (*elc_sponge_location)(i, 0), (*elc_sponge_location)(i, 1), (*elc_sponge_location)(i, 2)),
        r;
    double min_dis = std::numeric_limits<double>::infinity();
    long found_index = std::numeric_limits<double>::quiet_NaN();
    for (long j = 0; j < result.size(); j++)
    {
      VMesh* tmp_mesh = result[j]->vmesh();
      tmp_mesh->synchronize(Mesh::NODE_LOCATE_E);

      tmp_mesh->find_closest_node(distance, r, didx, elc);

      if (distance < min_dis)
      {
        min_dis = distance;
        found_index = j;
      }
    }
    (*lookup)(i, 0) = found_index;
    (*distances)(i, 0) = min_dis;
  }

  /// throw error if the geometry is too far away from predicted location - further away than sponge
  /// thickness
  for (long i = 0; i < elc_sponge_location->nrows(); i++)
  {
    if ((*distances)(i, 0) > (*elc_sponge_location)(i, 3))
    {
      std::ostringstream ostr1;
      ostr1 << " distance to electrode " << i << " sponge  = " << (*distances)(i, 0)
            << " exceeds defined limit of " << (*elc_sponge_location)(i, 3) << std::endl;
      THROW_ALGORITHM_PROCESSING_ERROR(ostr1.str());
    }
  }

  /// determine intersection of electrode sponge and scalp; as well as center of electrode
  /// sponge/scalp surface
  FieldInformation fi("PointCloudMesh", 0, "double");
  FieldHandle sponge_geometry_centers = CreateField(fi);
  VMesh* mesh_sponge_geometry_centers = sponge_geometry_centers->vmesh();

  VMesh* mesh_scalp_tri_surf = scalp_tri_surf->vmesh();
  Point p, q;
  Vector norm;
  VMesh::Node::index_type node_ind;
  distance = std::numeric_limits<double>::infinity();
  DenseMatrixHandle sponge_center_pojected_onto_scalp_index(new DenseMatrix(result.size(), 1));
  DenseMatrixHandle sponge_center_pojected_onto_scalp_normal(new DenseMatrix(result.size(), 3));
  DenseMatrixHandle sponge_center_pojected_onto_scalp(new DenseMatrix(result.size(), 3));

  mesh_scalp_tri_surf->synchronize(Mesh::NODE_LOCATE_E);
  for (long i = 0; i < result.size(); i++)
  {
    VMesh* tmp_mesh = result[i]->vmesh();
    tmp_mesh->synchronize(Mesh::NODE_LOCATE_E);
    double avr_x = 0, avr_y = 0, avr_z = 0;
    long number_electrode_nodes = tmp_mesh->num_nodes();

    FieldInformation fi2("PointCloudMesh", 0, "double");
    FieldHandle tmp_field = CreateField(fi2);
    VMesh* tmp_field_mesh = tmp_field->vmesh();

    for (VMesh::Node::index_type idx = 0; idx < number_electrode_nodes; idx++)
    {
      tmp_mesh->get_center(p, idx);
      avr_x += p.x();
      avr_y += p.y();
      avr_z += p.z();
      if (mesh_scalp_tri_surf->find_closest_node(distance, q, node_ind, p))
      {
        if (distance < identical_node_location_differce) { tmp_field_mesh->add_point(q); }
      }
    }

    avr_x /= (number_electrode_nodes);
    avr_y /= (number_electrode_nodes);
    avr_z /= (number_electrode_nodes);

    Point avr_sponge_geometry(avr_x, avr_y, avr_z);
    mesh_sponge_geometry_centers->add_point(avr_sponge_geometry);
    tmp_field_mesh->synchronize(Mesh::NODE_LOCATE_E);
    tmp_field_mesh->find_closest_node(distance, q, node_ind, avr_sponge_geometry);

    (*sponge_center_pojected_onto_scalp)(i, 0) = q.x();
    (*sponge_center_pojected_onto_scalp)(i, 1) = q.y();
    (*sponge_center_pojected_onto_scalp)(i, 2) = q.z();

    mesh_scalp_tri_surf->find_closest_node(distance, p, node_ind, q);
    (*sponge_center_pojected_onto_scalp_index)(i, 0) = node_ind;
  }
  /// determine normal of scalp/electrode sponge inferface center
  mesh_scalp_tri_surf->synchronize(Mesh::NORMALS_E);
  VMesh::Elem::array_type ca;
  for (long j = 0; j < result.size(); j++)
  {
    VMesh::Node::index_type ii((*sponge_center_pojected_onto_scalp_index)(j, 0));
    mesh_scalp_tri_surf->get_normal(norm, ii);
    double x = norm.x(), y = norm.y(), z = norm.z();

    double normal_mag = sqrt(x * x + y * y + z * z);  /// normalize vector normal
    x /= normal_mag;
    y /= normal_mag;
    z /= normal_mag;

    (*sponge_center_pojected_onto_scalp_normal)(j, 0) = x;
    (*sponge_center_pojected_onto_scalp_normal)(j, 1) = y;
    (*sponge_center_pojected_onto_scalp_normal)(j, 2) = z;
  }

  VMesh* mesh_elc_tri_surf = scalp_tri_surf->vmesh();
  mesh_elc_tri_surf->synchronize(Mesh::NODE_LOCATE_E);

  int nr_elc_sponge_triangles = 0;
  FieldInformation fi3("PointCloudMesh", 0, "double");
  FieldHandle estimated_sponge_top_center_points = CreateField(fi3);
  VMesh* estimated_sponge_top_center_points_vmesh = estimated_sponge_top_center_points->vmesh();
  /// retrieve electrode thickness from last input by using the lookup table
  for (int k = 0; k < sponge_center_pojected_onto_scalp_normal->nrows(); k++)
  {
    VMesh* tmp_mesh = result[k]->vmesh();
    VField* tmp_fld = result[k]->vfield();
    if (getsurf)
    {
      VMesh::Face::size_type nrows;
      tmp_mesh->size(nrows);
      for (VMesh::Face::index_type idx = 0; idx < nrows; idx++)
      {
        Point o1;
        double dis1 = std::numeric_limits<double>::quiet_NaN();
        Point sponge_tri_center, scalp_tri_center;
        tmp_mesh->get_center(sponge_tri_center, idx);
        VMesh::Elem::index_type node_tri_ind = 0;
        mesh_scalp_tri_surf->synchronize(Mesh::ELEM_LOCATE_E);
        mesh_scalp_tri_surf->find_closest_elem(dis1, o1, node_tri_ind, sponge_tri_center);
        mesh_scalp_tri_surf->synchronize(Mesh::NODE_LOCATE_E);
        mesh_scalp_tri_surf->get_center(scalp_tri_center, node_tri_ind);

        /// check if its really a scalp triangle -> compare if centers of found triangle (scalp,
        /// sponge) is the same
        if (std::fabs(o1.x() - scalp_tri_center.x()) < identical_node_location_differce &&
            std::fabs(o1.y() - scalp_tri_center.y()) < identical_node_location_differce &&
            std::fabs(o1.z() - scalp_tri_center.z()) < identical_node_location_differce)
        {
          VMesh::Node::array_type onodes(3);  /// create scalp surf that touches sponge/electrode
          Point o1, o2, o3;
          mesh_scalp_tri_surf->get_nodes(onodes, node_tri_ind);
          mesh_scalp_tri_surf->get_center(o1, onodes[0]);
          mesh_scalp_tri_surf->get_center(o2, onodes[1]);
          mesh_scalp_tri_surf->get_center(o3, onodes[2]);
          elc_sponge_surf_vmesh->add_point(o1);  /// duplicated nodes are ok! they are gonna be
                                                 /// deleted in last step of this function
          elc_sponge_surf_vmesh->add_point(o2);
          elc_sponge_surf_vmesh->add_point(o3);
          node_tri_ind = 0;  /// get the first triangle to determine the orientation of the mesh
          mesh_scalp_tri_surf->get_nodes(onodes, node_tri_ind);
          double omin = std::numeric_limits<double>::quiet_NaN(),
                 omax = std::numeric_limits<double>::quiet_NaN();  // determine tri ordering from
                                                                   // scalp mesh
          int imin = -1, imax = -1;
          for (int q = 0; q < 3; q++)
          {
            if (onodes[q] < omin || IsNan(omin))
            {
              omin = onodes[q];
              imin = q;
            }
            if (onodes[q] > omax || IsNan(omax))
            {
              omax = onodes[q];
              imax = q;
            }
          }
          if (imin == imax)  // an triangle that has nodes that are the same (sliver element?) -
                             // that shoud not happend
          {
            std::ostringstream ostr1;
            ostr1 << " Triangular definition of scalp surface element contains duplicated node "
                     "number in electrode/sponge - scalp contact surface. "
                  << std::endl;
            THROW_ALGORITHM_PROCESSING_ERROR(ostr1.str());
          }

          double ordering[3];
          for (int q = 0; q < 3; q++)
          {
            if (q == imin)
              ordering[q] = 0;
            else if (q == imax)
              ordering[q] = 2;
            else
              ordering[q] = 1;
          }

          onodes[0] = ordering[0] + nr_elc_sponge_triangles;
          onodes[1] = ordering[1] + nr_elc_sponge_triangles;
          onodes[2] = ordering[2] + nr_elc_sponge_triangles;

          nr_elc_sponge_triangles += 3;
          impedances.push_back(impelc[k].toDouble());
          elc_sponge_surf_vmesh->add_elem(onodes);
          field_values.push_back(k);
        }
      }
    }
    else
    {
      bool normal_traveling_direction_needs_to_be_positive = true;
      double elc_thickness = 0;
      bool found = false;

      for (long j = 0; j < lookup->nrows(); j++)
      {
        if ((*lookup)(j, 0) == k)
        {
          elc_thickness = (*elc_sponge_location)((*lookup)(j, 0), 3);
          found = true;
        }
      }

      if (!found)
      {
        THROW_ALGORITHM_PROCESSING_ERROR(
            "Internal error: Lookup of mesh defined electrode (first input) and definition (fourth "
            "input) do not match - check your inputs!");
      }
      /// determine the outgoing normal of the scalp to find sponge/electrode surface
      Point o1((*sponge_center_pojected_onto_scalp)(k, 0) +
                   (*sponge_center_pojected_onto_scalp_normal)(k, 0) * elc_thickness / 2,
          (*sponge_center_pojected_onto_scalp)(k, 1) +
              (*sponge_center_pojected_onto_scalp_normal)(k, 1) * elc_thickness / 2,
          (*sponge_center_pojected_onto_scalp)(k, 2) +
              (*sponge_center_pojected_onto_scalp_normal)(k, 2) * elc_thickness / 2);
      Point o2((*sponge_center_pojected_onto_scalp)(k, 0) -
                   (*sponge_center_pojected_onto_scalp_normal)(k, 0) * elc_thickness / 2,
          (*sponge_center_pojected_onto_scalp)(k, 1) -
              (*sponge_center_pojected_onto_scalp_normal)(k, 1) * elc_thickness / 2,
          (*sponge_center_pojected_onto_scalp)(k, 2) -
              (*sponge_center_pojected_onto_scalp_normal)(k, 2) * elc_thickness / 2);

      Vector sp_o1(o1.x() - (*sponge_center_pojected_onto_scalp)(k, 0),
          o1.y() - (*sponge_center_pojected_onto_scalp)(k, 1),
          o1.z() - (*sponge_center_pojected_onto_scalp)(k, 2));
      Vector sp_o2(o2.x() - (*sponge_center_pojected_onto_scalp)(k, 0),
          o2.y() - (*sponge_center_pojected_onto_scalp)(k, 1),
          o2.z() - (*sponge_center_pojected_onto_scalp)(k, 2));
      Vector current_scalp_normal_at_elec((*sponge_center_pojected_onto_scalp_normal)(k, 0),
          (*sponge_center_pojected_onto_scalp_normal)(k, 1),
          (*sponge_center_pojected_onto_scalp_normal)(k, 2));

      double dot_sp_o1 = Dot(sp_o1, current_scalp_normal_at_elec);
      double dot_sp_o2 = Dot(sp_o2, current_scalp_normal_at_elec);
      double x = 0, y = 0, z = 0;
      if ((dot_sp_o1 < 0 && dot_sp_o2 < 0) || (dot_sp_o1 > 0 && dot_sp_o2 > 0))
      {
        THROW_ALGORITHM_PROCESSING_ERROR(
            "Internal error: Criteria to find sponge top and bottom failed!");
      }
      else if (dot_sp_o1 > 0 && dot_sp_o2 < 0)
      {
        normal_traveling_direction_needs_to_be_positive = true;
        x = (*sponge_center_pojected_onto_scalp)(k, 0) +
            (*sponge_center_pojected_onto_scalp_normal)(k, 0) * elc_thickness;
        y = (*sponge_center_pojected_onto_scalp)(k, 1) +
            (*sponge_center_pojected_onto_scalp_normal)(k, 1) * elc_thickness;
        z = (*sponge_center_pojected_onto_scalp)(k, 2) +
            (*sponge_center_pojected_onto_scalp_normal)(k, 2) * elc_thickness;
      }
      else if (dot_sp_o1 < 0 && dot_sp_o2 > 0)
      {
        normal_traveling_direction_needs_to_be_positive = false;
        x = (*sponge_center_pojected_onto_scalp)(k, 0) -
            (*sponge_center_pojected_onto_scalp_normal)(k, 0) * elc_thickness;
        y = (*sponge_center_pojected_onto_scalp)(k, 1) -
            (*sponge_center_pojected_onto_scalp_normal)(k, 1) * elc_thickness;
        z = (*sponge_center_pojected_onto_scalp)(k, 2) -
            (*sponge_center_pojected_onto_scalp_normal)(k, 2) * elc_thickness;
      }

      tmp_mesh->synchronize(Mesh::NORMALS_E);

      Point o3(x, y, z), o4;  // electrode surface point = additional criterial to identify the
                              // sponge top/electrode surface
      tmp_mesh->synchronize(Mesh::NODE_LOCATE_E);
      tmp_mesh->find_closest_node(distance, o4, node_ind, o3);
      estimated_sponge_top_center_points_vmesh->add_point(o4);

      Vector sponge_outwards_normal;
      if (normal_traveling_direction_needs_to_be_positive)  /// force normals to point towards
                                                            /// electrode
      {
        sponge_outwards_normal.x((*sponge_center_pojected_onto_scalp_normal)(k, 0));
        sponge_outwards_normal.y((*sponge_center_pojected_onto_scalp_normal)(k, 1));
        sponge_outwards_normal.z((*sponge_center_pojected_onto_scalp_normal)(k, 2));
      }
      else
      {
        sponge_outwards_normal.x(-(*sponge_center_pojected_onto_scalp_normal)(k, 0));
        sponge_outwards_normal.y(-(*sponge_center_pojected_onto_scalp_normal)(k, 1));
        sponge_outwards_normal.z(-(*sponge_center_pojected_onto_scalp_normal)(k, 2));
      }

      /// determine which sponge electrode triangles make up sponge top (using the scalp outgoing
      /// normal) the critia to control this can be adjusted in GUI "elc. normal criteria", -1..1,
      /// smaller values more electrode surface
      VMesh::coords_type center;
      VMesh::Face::size_type nrows;
      tmp_mesh->size(nrows);

      for (VMesh::Face::index_type idx = 0; idx < nrows; idx++)
      {
        tmp_mesh->get_element_center(center);
        tmp_mesh->get_normal(norm, center, VMesh::Elem::index_type(idx));
        double dot_product = Dot(norm, sponge_outwards_normal);

        if (dot_product >= normal_dot_product_bound_)
        {
          Point o1, o2, o3, o0;
          VMesh::Node::array_type onodes(3);
          tmp_mesh->get_nodes(onodes, idx);

          tmp_mesh->get_center(o1, onodes[0]);
          tmp_mesh->get_center(o2, onodes[1]);
          tmp_mesh->get_center(o3, onodes[2]);
          double dis1 = std::numeric_limits<double>::quiet_NaN(),
                 dis2 = std::numeric_limits<double>::quiet_NaN(),
                 dis3 = std::numeric_limits<double>::quiet_NaN();
          mesh_scalp_tri_surf->find_closest_node(dis1, o0, node_ind, o1);
          mesh_scalp_tri_surf->find_closest_node(dis2, o0, node_ind, o2);
          mesh_scalp_tri_surf->find_closest_node(dis3, o0, node_ind, o3);

          if (dis1 > identical_node_location_differce && dis2 > identical_node_location_differce &&
              dis3 > identical_node_location_differce)
          { tmp_fld->set_value(static_cast<int>(special_label), idx); }
        }
        else
        {
          tmp_fld->set_value(static_cast<int>(0), idx);
        }
      }
      /// use SplitFieldByDomainAlgo to find only labeled (based on elc. normal criteria) surfaces
      VMesh::Elem::index_type c_ind = 0;
      SplitFieldByDomainAlgo algo2;
      algo2.setLogger(getLogger());
      FieldList output;
      algo2.set(Fields::Parameters::SortBySize, true);
      algo2.set(Fields::Parameters::SortAscending, false);
      algo2.runImpl(result[k], output);

      bool found_sponge_surface = false;
      int electrode_surface = -1;

      mesh_scalp_tri_surf->synchronize(Mesh::NODE_LOCATE_E);
      /// find out which of the selected triangles (from previous step) do not coincide with scalp
      /// -> those should belong to the electrode sponge top
      for (long o = 0; o < output.size(); o++)
      {
        VField* tmp_splitbydomain_tri_field = output[o]->vfield();

        int tmp_val = 0;
        tmp_splitbydomain_tri_field->get_value(tmp_val, c_ind);

        if (tmp_val == special_label)
        {
          found_sponge_surface = true;
          electrode_surface = o;
        }
      }

      /// use SplitFieldByConnectedRegionAlgo to make sure we have the right triangle surface
      if (!found_sponge_surface || electrode_surface == -1)
      {
        remark(
            " At least for one electrode the electrode/sponge interface could not be identified.");
        continue;
      }
      else
      {
        SplitFieldByConnectedRegionAlgo algo3;
        algo3.set(Fields::Parameters::SortDomainBySize, true);
        algo3.set(Fields::Parameters::SortAscending, false);
        std::vector<FieldHandle> result3 = algo3.run(output[electrode_surface]);
        if (result3.size() > 1)
        {
          remark(
              " This module found two electrode sponge surfaces that are not connected and took "
              "the one that has more elements. Visualize the 8th (Field) output to make sure "
              "otherwise the simulation could be wrong!");
        }
        if (result3.size() == 0)
        {
          THROW_ALGORITHM_PROCESSING_ERROR(
              "Internal error: Cannot find any electrode sponge surface. ");
        }

        /// create eightht output
        FieldHandle tri_mesh = result3[0];  /// use largest surface
        VMesh* tri_mesh_vmesh = tri_mesh->vmesh();
        VMesh::Node::array_type onodes(3);
        for (VMesh::Node::index_type l = 0; l < tri_mesh_vmesh->num_nodes(); l++)
        {
          Point point;
          tri_mesh_vmesh->get_center(p, l);
          elc_sponge_surf_vmesh->add_point(p);
        }

        for (VMesh::Elem::index_type l = 0; l < tri_mesh_vmesh->num_elems(); l++)
        {
          VMesh::Node::array_type onodes(3);
          tri_mesh_vmesh->get_nodes(onodes, l);
          onodes[0] += nr_elc_sponge_triangles;
          onodes[1] += nr_elc_sponge_triangles;
          onodes[2] += nr_elc_sponge_triangles;
          elc_sponge_surf_vmesh->add_elem(onodes);
          field_values.push_back(k);
          impedances.push_back(impelc[k].toDouble());
        }
        nr_elc_sponge_triangles += tri_mesh_vmesh->num_nodes();
      }
    }
  }
  /// create matrix outputs
  elc_sponge_surf_vfld->resize_values();
  elc_sponge_surf_vfld->set_values(field_values);

  elc_elem =
      std::make_shared<DenseMatrix>(DenseMatrix::Zero(elc_sponge_surf_vmesh->num_elems(), 1));
  elc_elem_def =
      makeShared<DenseMatrix>(DenseMatrix::Zero(elc_sponge_surf_vmesh->num_elems(), 4));
  elc_elem_typ =
      makeShared<DenseMatrix>(DenseMatrix::Zero(elc_sponge_surf_vmesh->num_elems(), 1));
  elc_con_imp =
      makeShared<DenseMatrix>(DenseMatrix::Zero(elc_sponge_surf_vmesh->num_elems(), 1));

  if (impedances.size() != elc_sponge_surf_vmesh->num_elems())
  {
    THROW_ALGORITHM_PROCESSING_ERROR(
        "Internal error: Number of impedances and electrode contact surface elements do not match "
        "up. ");
  }

  if (elc_sponge_surf_vmesh->num_elems() != field_values.size())
  { THROW_ALGORITHM_PROCESSING_ERROR("Internal error: internal field definition is "); }

  double area = 0.0, prev_elc = 0, tmp_fld_val = 0;
  for (VMesh::Elem::index_type l = 0; l < elc_sponge_surf_vmesh->num_elems(); l++)
  {
    (*elc_elem_typ)(l, 0) = 2;  // define triangles to incject currents in TDCS simulations
    VMesh::Node::array_type onodes(3);
    elc_sponge_surf_vmesh->get_nodes(onodes, l);
    elc_sponge_surf_vmesh->get_center(p, onodes[0]);
    mesh_vmesh->find_closest_node(distance, q, node_ind, p);
    if (node_ind == refnode_number)  /// check if ref_node is part of electrode definition throw
                                     /// error, first triangle node of definition
    {
      THROW_ALGORITHM_PROCESSING_ERROR(
          "Reference node is part of electrode definition. Choose another reference node for given "
          "electrode surface mesh. ");
    }
    (*elc_elem_def)(l, 0) = node_ind;
    Point pos;
    mesh_vmesh->get_point(pos, node_ind);
    double x1 = pos.x(), y1 = pos.y(), z1 = pos.z();

    elc_sponge_surf_vmesh->get_center(p, onodes[1]);
    mesh_vmesh->find_closest_node(distance, q, node_ind, p);
    if (node_ind == refnode_number)  /// check if ref_node is part of electrode definition throw
                                     /// error, second triangle node of definition
    {
      THROW_ALGORITHM_PROCESSING_ERROR(
          "Reference node is part of electrode definition. Choose another reference node for given "
          "electrode surface mesh. ");
    }
    (*elc_elem_def)(l, 1) = node_ind;
    mesh_vmesh->get_point(pos, node_ind);
    double x2 = pos.x(), y2 = pos.y(), z2 = pos.z();
    elc_sponge_surf_vmesh->get_center(p, onodes[2]);
    mesh_vmesh->find_closest_node(distance, q, node_ind, p);
    if (node_ind == refnode_number)  /// check if ref_node is part of electrode definition throw
                                     /// error, third triangle node of definition
    {
      THROW_ALGORITHM_PROCESSING_ERROR(
          "Reference node is part of electrode definition. Choose another reference node for given "
          "electrode surface mesh. ");
    }
    (*elc_elem_def)(l, 2) = node_ind;
    mesh_vmesh->get_point(pos, node_ind);
    double x3 = pos.x(), y3 = pos.y(), z3 = pos.z();

    (*elc_elem_def)(l, 3) = 0;
    (*elc_elem)(l, 0) = field_values[l];

    /// compute surface area of electrode/scalp interface
    double area_tmp = y1 * z2 + z1 * y3 + y2 * z3 - z2 * y3 - z1 * y2 - y1 * z3;
    double area_tmp1 = z1 * x2 + x1 * z3 + z2 * x3 - x2 * z3 - x1 * z2 - z1 * x3;
    double area_tmp2 = x1 * y2 + y1 * x3 + x2 * y3 - y2 * x3 - y1 * x2 - x1 * y3;
    double triangle_area =
        0.5 * sqrt(area_tmp * area_tmp + area_tmp1 * area_tmp1 + area_tmp2 * area_tmp2);
    tmp_fld_val = 0;
    elc_sponge_surf_vfld->get_value(tmp_fld_val, l);

    if (prev_elc != tmp_fld_val)
    {
      area *= 1e-6;  /// convert electrode/sponge surface area from mm^2 to m^2
      electrode_sponge_areas.push_back(area);
      area = 0.0;
      prev_elc = tmp_fld_val;
    }
    else
    {
      area += triangle_area;
    }
  }

  area *= 1e-6;

  electrode_sponge_areas.push_back(area);

  for (VMesh::Elem::index_type l = 0; l < elc_sponge_surf_vmesh->num_elems();
       l++)  /// the impedance (Ohm * m^2) that was provided by the user needs to be defined related
             /// to the electrode area. For convenience we do that for the user here.
  {
    elc_sponge_surf_vfld->get_value(tmp_fld_val, l);
    if (tmp_fld_val >= electrode_sponge_areas.size())
    {
      THROW_ALGORITHM_PROCESSING_ERROR(
          "Internal ERROR (should not happen): could not scale contact impedance by electrode "
          "surface area (index out of bound). ");
    }
    (*elc_con_imp)(l, 0) = impedances[tmp_fld_val] * electrode_sponge_areas[tmp_fld_val];
  }

  DenseMatrixHandle selectmatrixind(
      new DenseMatrix(mesh_vmesh->num_nodes(), 1));  /// create indeces for SelectSubMatrix
  for (long i = 0; i < mesh_vmesh->num_nodes(); i++) { (*selectmatrixind)(i, 0) = i; }

  return boost::make_tuple(selectmatrixind, lhs_knows, elc_elem, elc_elem_typ, elc_elem_def,
      elc_con_imp, elc_sponge_surf, electrode_sponge_areas);
}

DenseMatrixHandle SetupTDCSAlgorithm::create_rhs(FieldHandle mesh, FieldHandle elc_tri_surf,
    const std::vector<Variable>& elcs, int num_of_elc) const
{
  /// get the right-hand-side from GUI
  std::vector<Variable, std::allocator<Variable>> elcs_wanted;
  for (int i = 0; i < num_of_elc; i++) elcs_wanted.push_back(elcs[i]);

  VField* vfield = mesh->vfield();
  VMesh* vmesh = mesh->vmesh();
  // making sure current magnitudes of the electrodes summed are greater than 10e-6
  double min_current = 0;
  for (int i = 0; i < elcs_wanted.size(); i++)
  {
    double temp = elcs_wanted[i].toDouble();
    min_current += temp;
  }

  if (std::fabs(min_current) > electode_current_summation_bound)
    THROW_ALGORITHM_INPUT_ERROR(
        "Summed electrode current intensities are greater than 1e-6 mA. The sum should be close to "
        "0 mA !!! ");

  FieldInformation fi(elc_tri_surf);
  if (fi.is_pointcloudmesh())
  {
    auto output(makeShared<DenseMatrix>(vmesh->num_nodes(), 1));

    auto mesh_elc_tri_surf = elc_tri_surf->vmesh();
    mesh_elc_tri_surf->synchronize(Mesh::NODE_LOCATE_E);
    vmesh->synchronize(Mesh::NODE_LOCATE_E);
    for (VMesh::Node::index_type l = 0; l < vmesh->num_nodes(); l++) { (*output)(l, 0) = 0; }
    double min_dis = get(Parameters::pointdistancebound).toDouble();
    for (VMesh::Node::index_type l = 0; l < mesh_elc_tri_surf->num_nodes(); l++)
    {
      Point p, q;
      mesh_elc_tri_surf->get_center(p, l);
      double distance = -1;
      VMesh::Node::index_type ind;
      vmesh->find_closest_node(distance, q, ind, p);
      (*output)(ind, 0) = elcs_wanted[l].toDouble() / 1000.0;
      if (min_dis < distance)
      {
        std::ostringstream ostr4;
        ostr4 << " The electrode locations (4th module input) are further away from them mesh as "
                 "provided by the GUI (min. distance bound). "
              << std::endl;
        remark(ostr4.str());
      }
    }

    return output;
  }
  else
  {
    int node_elements = vfield->vmesh()->num_nodes();
    int total_elements = node_elements + elcs_wanted.size();

    DenseMatrixHandle output(makeShared<DenseMatrix>(total_elements, 1));
    int cnt = 0;
    double module_half_done = total_elements * 2;  /// half of the runtime is reserved for rhs

    /// su
    for (int i = 0; i < total_elements; i++)
    {
      if (i < node_elements)
        (*output)(i, 0) = 0.0;  // for the nodes
      else
        (*output)(i, 0) = elcs_wanted[i - node_elements].toDouble() /
                          1000.0;  /// for electrodes ~ converting to Amps

      cnt++;
      if (cnt == total_elements / 4)
      {
        cnt = 0;
        update_progress_max(i, module_half_done);  /// progress bar is devided in 2 parts; first
                                                   /// part = create rhs and second for lhs
      }
    }
    return output;
  }
}

boost::tuple<DenseMatrixHandle, DenseMatrixHandle, DenseMatrixHandle, DenseMatrixHandle,
    DenseMatrixHandle, DenseMatrixHandle, FieldHandle, DenseMatrixHandle, std::vector<double>>
SetupTDCSAlgorithm::run(FieldHandle mesh, const std::vector<Variable>& elcs,
    const std::vector<Variable>& impelc, int num_of_elc, FieldHandle scalp_tri_surf,
    FieldHandle elc_tri_surf, DenseMatrixHandle elc_sponge_location) const
{
  if (num_of_elc > max_number_of_electrodes)
  {
    THROW_ALGORITHM_INPUT_ERROR("Number of electrodes (>512) given exceeds what is possible ");
  }  /// number of  possible electrodes is currently bound to 512 electrodes in default setting
  if (num_of_elc < 0) { THROW_ALGORITHM_INPUT_ERROR("Negative number of electrodes given "); }
  if (!mesh) THROW_ALGORITHM_INPUT_ERROR("Input field (mesh) was not allocated ");
  if (!scalp_tri_surf)
    THROW_ALGORITHM_INPUT_ERROR("Input field (scalp triangle surface) was not allocated ");
  if (!elc_tri_surf)
    THROW_ALGORITHM_INPUT_ERROR("Input field (electrode triangle surface) was not allocated ");
  if (!elc_sponge_location)
    THROW_ALGORITHM_INPUT_ERROR("Input field (electrode triangle surface) was not allocated ");
  DenseMatrixHandle rhs =
      create_rhs(mesh, elc_tri_surf, elcs, num_of_elc);  /// get the right-hand-side

  DenseMatrixHandle lhs_knowns, elc_element, elc_element_typ, elc_element_def, elc_contact_imp,
      selectmatrixind;
  FieldHandle elec_sponge_surf;
  std::vector<double> electrode_sponge_areas;
  boost::tie(selectmatrixind, lhs_knowns, elc_element, elc_element_typ, elc_element_def,
      elc_contact_imp, elec_sponge_surf, electrode_sponge_areas) = create_lhs(mesh, impelc,
      scalp_tri_surf, elc_tri_surf, elc_sponge_location);  /// get various left-hand-side outputs

  return boost::make_tuple(lhs_knowns, elc_element, elc_element_typ, elc_element_def,
      elc_contact_imp, rhs, elec_sponge_surf, selectmatrixind, electrode_sponge_areas);
}


/*
 * FootholdFinderTest.cpp
 *
 *  Created on: Sep 18, 2014
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, Autonomous Systems Lab
 */

#include "grid_map_lib/FootholdFinder.hpp"

// gtest
#include <gtest/gtest.h>

// Eigen
#include <Eigen/Core>

// Kindr
#include <kindr/rotations/RotationEigen.hpp>

using namespace std;
using namespace Eigen;
using namespace grid_map_lib;

TEST(matchToSurfaceNormal, Standard)
{
//  Eigen::Vector3d normal;
//  normal << 0.0, 0.0, 1.0;
//  kindr::rotations::eigen_impl::EulerAnglesZyxPD rpy(0.0, 0.0, 0.0);
//  kindr::rotations::eigen_impl::RotationQuaternionPD rotation(rpy);
//  matchToSurfaceNormal(normal, rotation);
//  rpy(rotation);
//  rpy = rpy.getUnique();
//  EXPECT_EQ(0.0, rpy.roll() / M_PI * 180.0);
//  EXPECT_EQ(0.0, rpy.pitch() / M_PI * 180.0);
//  EXPECT_EQ(0.0, rpy.yaw() / M_PI * 180.0);

//  normal << 0.0, 0.707, 0.707;
//  kindr::rotations::eigen_impl::EulerAnglesZyxPD rpy(-M_PI_2, 0.0, 0.0);
//  std::cout << "roll: " << rpy.roll() / M_PI * 180.0 << endl;
//  std::cout << "pitch: " << rpy.pitch() / M_PI * 180.0 << endl;
//  std::cout << "yaw: " << rpy.yaw() / M_PI * 180.0 << endl;
//  rotation(rpy);
//  std::cout << "normal: " << normal.transpose() << std::endl;
//  std::cout << "rotation: " << rotation << std::endl;
//  matchToSurfaceNormal(normal, rotation);
//  std::cout << "rotation after: " << rotation << std::endl;
//  rpy(rotation);
//  rpy = rpy.getUnique();
//  std::cout << "roll: " << rpy.roll() / M_PI * 180.0 << endl;
//  std::cout << "pitch: " << rpy.pitch() / M_PI * 180.0 << endl;
//  std::cout << "yaw: " << rpy.yaw() / M_PI * 180.0 << endl;

//  Eigen::Vector3d normal(0.0, 0.70710678118, 0.70710678118);
//  Rotation rotation(kindr::rotations::eigen_impl::EulerAnglesYprPD(-90.0 / 180.0 * M_PI, 0.0, -22.5 / 180.0 * M_PI));
//  matchToSurfaceNormal(normal, rotation);

  // TODO Make this a unit test.
//  Eigen::Vector3d referenceTest = Eigen::Vector3d::UnitZ();
//  referenceTest = rotation.inverseRotate(referenceTest);
//  std::cout << "Reference test: " << referenceTest.transpose() << std::endl;
//  double differenceTest = acos(referenceTest.dot(normal) / (referenceTest.norm() * referenceTest.norm())) / M_PI * 180.0;
//  std::cout << "Angle [deg] between normal and reference test: " << differenceTest << std::endl;

}


/*
 * Prover.cpp
 *
 *  Created on: 26.10.2010
 *      Author: stephaniebayer
 */

#include "Prover.h"

#include<vector>
#include "Cipher_elg.h"
#include "G_q.h"
#include "Mod_p.h"
#include "Functions.h"
#include "ElGammal.h"
#include "func_pro.h"
#include "fft.h"
#include<fstream>

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

extern G_q H;
extern ElGammal El;

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

}

Prover::Prover(vector<vector<Cipher_elg>* >* Cin, vector<vector<ZZ>* >* Rin, vector<vector<vector<long>* >* >* piin, vector<long> num, ZZ genq){

	// set the dimensions of the row and columns according to the user input
	m = num[1]; //number of rows
	n = num[2]; //number of columns
	C = Cin; //sets the reencrypted cipertexts to the input
	R = Rin; //sets the random elements to the input
	pi = piin; // sets the permutation to the input

	fft *fft_new = new fft;
	//o is a root of unity
	o = fft_new->r_o_u(genq, m, H.get_ord());

	A = new vector<vector<ZZ>* >(m);
	func_pro::set_A(A,pi, m, n);

	//Allocate the storage needed for the vectors
	chal_x6 = new vector<ZZ>(2*m); //x6, x6^2, ... challenges from round 6
	chal_y6 = new vector<ZZ>(n); //y6, y6^2, ... challenges form round 6
	chal_x8 = new vector<ZZ>(2*m +1); //x8, x8^2, ... challenges from round 8

	//Allocate the storage needed for the vectors
	c_A = new vector<Mod_p>(m+1); //commitments to the rows in A
	r_A = new vector<ZZ>(m+1); //random elements used for the commitments

	D = new vector<vector<ZZ>* >(m+1); //vector containing in the first row random values and in all others y*A(ij) + B(ij)-z
	D_h = new vector<vector<ZZ>* >(m); //Vector of the Hadamare products of the rows in D
	D_s = new vector<vector<ZZ>* >(m+1); //Shifted rows of D_h
	d = new vector<ZZ>(n); //containing random elements to proof product of D_hm
	Delta = new vector<ZZ>(n); //containing random elements to proof product of D_hm
	d_h = new vector<ZZ>(n); // vector containing the last row of D-h

	r_D_h = new vector<ZZ>(m);//random elements for commitments to D_h
	c_D_h = new vector<Mod_p>(m+2);//commitments to the rows in D_h

	B = new vector<vector<ZZ>* >(m);//matrix of permuted exponents, exponents are x2^i, i=1, ..N
	B_0 = new vector<ZZ>(n); //vector containing random exponents
	r_B = new vector<ZZ>(m); //random elements used to commit to T
	c_B = new vector<Mod_p>(m); //vector of commitments to rows in T
	a = new vector<ZZ>(2*m); //elements used for reencryption in round 5
	r_a = new vector<ZZ>(2*m); //random elements to commit to elements in a
	c_a = new vector<Mod_p>(2*m); //commitments to elements a
	E = new vector<Cipher_elg>(2*m); //
	rho_a = new vector<ZZ>(2*m); //

	Dl = new vector<ZZ>(2*m+1); //bilinear_map(Y_pi, U, chal_t)
	r_Dl = new vector<ZZ>(2*m+1); //random elements to commit to the C_ls
	c_Dl = new vector<Mod_p>(2*m +1); //commitments to the C_ls

	d_bar = new vector<ZZ>(n);// chal_x8*D_h(m-1) +d
	Delta_bar = new vector<ZZ>(n);//chal_x8*d_h+Delta
	D_h_bar = new vector<ZZ>(n);//sum over the rows in D_h

	B_bar  = new vector<ZZ>(n); // sum over the rows in T multiplied by chal^i

	A_bar = new vector<ZZ>(n); //sum over the rows in Y times the challenges
	D_s_bar = new vector<ZZ>(n); // sum over the rows in U times thes challenges

}
//Destructor deletes all pointers and frees the storage
Prover::~Prover() {
	delete chal_x6;
	delete chal_y6;
	delete chal_x8;
	delete c_A;
	delete r_A;

	Functions::delete_vector(D);
	Functions::delete_vector(D_h);
	Functions::delete_vector(D_s);
	delete d;
	delete Delta;
	delete d_h;

	delete r_D_h;
	delete c_D_h;
	Functions::delete_vector(B);
	delete B_0;
	delete r_B;
	delete c_B;
	delete a;
	delete r_a;
	delete c_a;
	delete rho_a;
	delete Dl;
	delete r_Dl;
	delete c_Dl;

	delete D_h_bar;
	delete d_bar;
	delete Delta_bar;
	delete B_bar;
	delete A_bar;
	delete D_s_bar;
}

//round_1 picks random elements and commits to the rows of Y
string Prover::round_1(){
	long i;
	string name;
	time_t rawtime;
	time ( &rawtime );

	//calculates commitments to rows of Y
	name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"commitments to A and random values "<<endl;
	ost.close();
	Functions::commit(A,r_A, c_A);

	//write commitments in the file name
	name = "round_1 ";
	name = name + ctime(&rawtime);
	ost.open(name.c_str());
	for (i=0; i<m; i++){
		ost << c_A->at(i)<< " ";
	}
	return name;

}

//round_3, permuted the exponents in s,  picks random elements and commits to values
string Prover::round_3(string in_name){
	long i;
	ZZ x2;
	vector<vector<ZZ>* >* chal_x2 = new vector<vector<ZZ>* >(m);

	string name;
	time_t rawtime;
	time ( &rawtime );

	//reads in values of s
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	ist >> x2;
	name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"x ist "<<x2<<endl;
	//creates a matrix with entries x2,..., x2^N
	func_pro::set_x2(chal_x2,x2, m,n);

	//permutes chal_x2 according pi to create B
	func_pro::set_B(B, chal_x2, pi);

	//commits to the rows in B
	ost<<"commitment to B "<<endl;
	Functions::commit(B,r_B,c_B);
	ost.close();

	name = "round_3 ";
	name = name + ctime(&rawtime);

	//write data in the file name
	 ost.open(name.c_str());
	for (i=0; i<m;i++){
		ost << c_B->at(i) <<" ";
	}

	Functions::delete_vector(chal_x2);
	return name;
}

//round_5a calculates Y_pi and the commitments to the vectors alpha, W
void Prover::round_5a(){
	long i;
	ZZ temp, t; //temporary variables
	vector<ZZ>* r = new vector<ZZ>(n);
	vector<ZZ>* v_z = new vector<ZZ>(n); //row containing the challenge alpha
	ZZ ord = H.get_ord();
	time_t rawtime;
	time ( &rawtime );

	//calculate for each value in the first m rows in D: y* A_ij + A_ij -z
	func_pro::set_D(D, A,B, chal_z4, chal_y4);

	//Set the matrix D_h as the Hadamard product of the rows in D
	func_pro::set_D_h(D_h, D);

	for( i=0; i<n;i++){
		v_z->at(i) = chal_z4; //fills the vector z with the challenge z
		NegateMod(r->at(i),to_ZZ(1),ord);
	}

	//Sets the additional row in D to contain -1
	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"Last row in D ist -1 "<<endl;
	D->at(m) = r;
	//random number to commit to last row in A
	ost<<"r_A to commit to last row in A ist "<<0<<endl;
	r_A->at(m) = 0;

	//calculate commitment to
	ost<<"commitment  to vector z"<<endl;
	Functions::commit(v_z, r_z, c_z);
	//calculate commitment to the rows in D_h
	ost<<"Commitments to Hadamard product over D "<<endl;
	Functions::commit(D_h,r_D_h,c_D_h);

	delete v_z;
}

void Prover::round_5b(){

	//picks random values to set B0 and commits to it
	func_pro::commit_B0(B_0, r_B0, c_B0);
	//picks random values to set A and commits to them;
	func_pro::commit_a(a, r_a, c_a);

}

void Prover::round_5c(){
	vector<Cipher_elg>*  e = 0;
	double tstart, tstop, ttime;

	//calculates the value R_t as sum(T(ij)*r(ij));
	func_pro::set_Rb(B,R,R_b);

	tstart = (double)clock()/CLOCKS_PER_SEC;
		e= calculate_e();
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	//cout << "To calculate the di's took " << ttime << " second(s)." << endl;

	calculate_E(e);
	delete e;
}

string Prover::round_5(string in_name ){
	long i;
	string name;
	time_t rawtime;
	time ( &rawtime );

	//reads the values out of the file
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	ist>> chal_z4;
	ist >> chal_y4;

	 name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"z "<<chal_z4<<" y "<<chal_y4<<endl;
	ost.close();
	round_5a();
	round_5b();
	round_5c();
	//Set name of the output file and open stream
	name = "round_5 ";
	name = name + ctime(&rawtime);

	ost.open(name.c_str());

	//writes the commitments in the file
	ost<<  c_z<< "\n";
	for (i = 0; i<m ; i++){
		ost << c_D_h ->at(i)<< " ";
	}
	ost << c_B0<< "\n ";
	for (i = 0; i<2*m ; i++){
		ost << c_a ->at(i)<< " ";
	}
	ost << "\n";
	for (i = 0; i<2*m ; i++){
		ost << E ->at(i)<< " ";
	}

	delete E;
	return name;
}

void Prover::round_7a(){
	//Set the rows in D_s as D_s(i) = chal_t_1^i+1*D_h(i) for i<m-1 and D_s(m-1) = sum(chal_x6^i+1 * D_s(i+1) and set last row of D_s to random values and also D(0)

	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"matrix D_s = chal_t_1^i+1*D_h(i) for i<m-1 and D_s(m-1) = sum(chal_x6^i+1 * D_s(i+1) "<<endl;
	func_pro::set_D_s(D_s,D_h,D,chal_x6,r_Dl_bar);

	//calculate the values Dls as Dl(l) = sum(D(i)*D_s->at(i)*chal_y6) for j=n+i-l and commits to the values
	func_pro::commit_Dl(c_Dl,Dl, r_Dl, D, D_s, chal_y6, o);

	//commitments to D(0) and D_s(m)
	ost<<"commitment to D(0) and D_s(m) "<<endl;
	Functions::commit(D->at(0),r_D0,c_D0);
	Functions::commit(D_s->at(m), r_Dm, c_Dm);

	//commitments to prove that the product over the elements in D_h->at(m) is the desired product of n *y + x2n -z
	ost<<"commitment to d "<<endl;
	func_pro::commit_d(d,r_d,c_d);
	func_pro::commit_Delta(Delta, d, r_Delta, c_Delta);
	func_pro::commit_d_h(D_h,d_h,d,Delta, r_d_h, c_d_h);
}

void Prover::round_7b(){
	//calculate B_bar = sum(chal_x6^i B(i)), opening to prove knowledge of B
	func_pro::calculate_B_bar(B_0, B,chal_x6, B_bar );

	//calculate r_B_bar= sum(chal_x6^i r_B(i)), opening to prove knowledge of B
	func_pro::calculate_r_B_bar(r_B, chal_x6,r_B0, r_B_bar );

	//calculate a_bar = sum(chal_x6 ^i a(i)), opening to prove reencryption
	func_pro::calculate_a_bar(a, chal_x6, a_bar);

	//calculate r_a_bar= sum(chal_x6 ^i r_a(i)), opening to prove reencryption
	func_pro::calculate_r_a_bar(r_a, chal_x6, r_a_bar);

	//calculate rho_a_bar = sum(chal_x6 ^i rho_a(i)), opening to prove reencryption
	func_pro::calculate_rho_a_bar(rho_a, chal_x6, rho_bar);
}

string Prover::round_7(string in_name){
	long i;
	long l=2*m;
	string name;
	time_t rawtime;
	time ( &rawtime );
	//reads the values out of the file
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	//reads the vector t_1 and t
	for(i=0; i<l; i++){
		ist>>chal_x6->at(i);
	}
	for (i = 0; i<n; i++){
		ist >> chal_y6->at(i);
	}

	round_7a();
	round_7b();

	//Set name of the output file and open stream
	name = "round_7 ";
	name = name + ctime(&rawtime);

	ofstream ost(name.c_str());
	for (i = 0; i<=l ; i++){
		ost << c_Dl ->at(i)<< " ";
	}
	ost << "\n";
	ost<<c_D0<<"\n";
	ost <<c_Dm<<"\n";
	ost<<c_d<<"\n";
	ost<<c_Delta<<"\n";
	ost<<c_d_h<<"\n";
	for (i = 0; i<n; i++){
		ost << B_bar->at(i)<<" ";
	}
	ost <<"\n";

	ost<< r_B_bar;
	ost <<"\n";

	ost<< a_bar;
	ost <<"\n";

	ost<< r_a_bar;
	ost <<"\n";

	ost<< rho_bar;

	return name;
}

void Prover::round_9a(){

	//Calculate D_h_bar = sum(chal^i*D_h(row(i)))
	func_pro::calculate_D_h_bar(D_h_bar,D_h,chal_x8);

	//calculate r_Dh_bar = sum(chal^i*r_Dh_bar(i)), opening to prove correctness of D_h
	func_pro::calculate_r_Dh_bar(r_D_h, chal_x8, r_Dh_bar);

	//calculate d_bar, r_d_bar, Delta_bar, r_Delta_bar, openings to prove product over elements in D_h->at(m-1)
	func_pro::calculate_dbar_rdbar(D_h, chal_x8, d_bar,d,r_D_h, r_d, r_d_bar);
	func_pro::calculate_Deltabar_rDeltabar(d_h, chal_x8, Delta_bar, Delta, r_d_h, r_Delta, r_Delta_bar);
}

void  Prover::round_9b(){
	//A_bar and r_A_bar, openings to prove permutation in D
	func_pro::calculate_A_bar(D, A_bar, chal_x8);
	func_pro::calculate_r_A_bar(r_D0, r_A, r_B, chal_x8, r_z, chal_y4, r_A_bar);

	//D_s_bar and r_Ds_bar, openings to prove correctness of D_s
	func_pro::calculate_D_s_bar(D_s, D_s_bar, chal_x8);
	func_pro::calculate_r_Ds_bar(r_D_h, chal_x6, chal_x8, r_Ds_bar, r_Dm);

	//sum of the random values used to commit to the Dl's, to prover correctness of them
	func_pro::calculate_r_Dl_bar(r_Dl, chal_x8, r_Dl_bar);
}

string Prover::round_9(string in_name){
	long i;
	string name;
	time_t rawtime;
	time ( &rawtime );

	//reads the values out of the file
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	//reads the vector e
	for (i = 0; i<(signed) chal_x8->size(); i++){
		ist >> chal_x8->at(i);
	}

	name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);

	ost<<"chal_x8 "<< chal_x8->at(0)<<endl;
	ost.close();

	round_9a();
	round_9b();

	//Set name of the output file and open stream
	name = "round_9 ";
	name = name + ctime(&rawtime);

	ost.open(name.c_str());

	for (i = 0; i<n; i++){
		ost << D_h_bar->at(i)<<" ";
	}
	ost <<"\n";
	ost<< r_Dh_bar;
	ost <<"\n";

	for(i=0; i<n;i++){
		ost<<d_bar->at(i)<<" ";
	}
	ost<<"\n";
	ost<< r_d_bar <<"\n";

	for(i=0; i<n; i++){
		ost<<Delta_bar->at(i)<<" ";
	}
	ost<<"\n";
	ost<<r_Delta_bar <<"\n";

	for (i = 0; i<n; i++){
		ost << A_bar->at(i)<<" ";
	}
	ost<<"\n";
	ost<<r_A_bar<<"\n";
	for(i=0; i<n; i++){
		ost<<D_s_bar->at(i)<<" ";
	}
	ost<<"\n";
	ost<<r_Ds_bar<<"\n";
	ost<<r_Dl_bar<<"\n";

	return name;
}

Cipher_elg Prover::calculate_e_1(long pos){
	long i;
	Cipher_elg temp, temp_1;

	Cipher_elg::expo(temp, C->at(pos)->at(0), B_0->at(0));
	for (i = 1; i<n; i++){
		Cipher_elg::expo(temp_1, C->at(pos)->at(i), B_0->at(i));
		Cipher_elg::mult(temp,temp, temp_1);
	}

	return temp;
}

Cipher_elg Prover::calculate_e_2(long k){
	long i,j,l;
	Cipher_elg temp, temp_1;
	ZZ mod = H.get_mod();

	temp=Cipher_elg(1,1,mod);
	for (i = 1; i<=m; i++){
		l = k-m+i;
		if((l>0) & (l<=m)){
			for(j = 0; j<n;j++){
				Cipher_elg::expo(temp_1,C->at(i-1)->at(j), B->at(l-1)->at(j));
				Cipher_elg::mult(temp,temp,temp_1);
			}
		}
	}

	return temp;
}

vector<Cipher_elg>* Prover::calculate_e(){
	long k,l;
	Cipher_elg temp, temp_1;
	vector<Cipher_elg>* e = new vector<Cipher_elg>(2*m);

	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"calculate the product of the diagonlas, values E "<<endl;
	e->at(0)= calculate_e_1(m-1);
	ost<<e->at(0)<<" ";

	l=m-1;
	for (k =1; k<=l; k++){
		temp_1 = calculate_e_1(m-k-1);
		temp = calculate_e_2(k);
		Cipher_elg::mult(temp,temp,temp_1);
		e->at(k) = temp;
		ost<<e->at(k)<<" ";
	}
	l=2*m-1;
	for (k = m; k<=l; k++){
		e->at(k) = calculate_e_2(k);
		ost<<e->at(k)<<" ";
	}
	ost<<endl;
	return e;
}

void Prover::calculate_E(vector<Cipher_elg>* e){
	long i,l;
	Mod_p t;
	Mod_p gen = H.get_gen();
	ZZ ord = H.get_ord();
	l=2*m;
	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"reencrypt values in e, pick random rho "<<endl;
	for (i = 0; i<l; i++){
		rho_a->at(i)= RandomBnd(ord);
		ost<<rho_a->at(i)<<" ";
	}
	ost<<endl;
	NegateMod(rho_a->at(m),R_b,ord);
	ost<<"rho(m) = -R_b  m"<<rho_a->at(m)<<" "<<m<<endl;
	ost<<"t=g^a encrypt(t, rho)e "<<endl;
	for (i = 0; i<l; i++){
		 t = gen.expo(a->at(i));
		 E->at(i) = El.encrypt(t,rho_a->at(i))*e->at(i);
		 ost<<t<<" "<<E->at(i)<<endl;
	}
}


// This file is part of DM-HEOM (https://github.com/noma/dm-heom)
//
// Copyright (c) 2015-2019 Matthias Noack, Zuse Institute Berlin
//
// Licensed under the 3-clause BSD License, see accompanying LICENSE,
// CONTRIBUTORS.md, and README.md for further information.

#include <exception>
#include <iostream>

#include <boost/format.hpp>
#include <noma/num/meta_stepper.hpp>

#include "heom/command_line.hpp"
#include "heom/common.hpp"
#include "heom/handle_main_exception.hpp"
#include "heom/hierarchy_graph.hpp"
#include "heom/hierarchy_mask.hpp"
#include "heom/hierarchy_norm.hpp"
#include "heom/instance.hpp"
#include "heom/make_file_observer_list.hpp"
#include "heom/ocl_config.hpp"
#include "heom/ode.hpp"
#include "heom/population_dynamics_config.hpp"
#include "heom/population_dynamics_solver.hpp"

namespace bmt = ::heom::bmt;
namespace num = ::heom::num;
namespace ocl = ::heom::ocl;
using num::int_t;
using num::real_t;

int main(int argc, char* argv[])
{
	// start runtime measurement
	bmt::timer app_timer;

	// output compile-time configuration
	std::cout << "-------------------- Compile-Time Configuration ------------" << std::endl;
	heom::write_compile_config(std::cout);
	std::cout << "------------------------------------------------------------" << std::endl;

	std::exception_ptr eptr;
	try {
		// command line parsing
		heom::command_line command_line { argc, argv };

		// parse config files
		std::cout << "Parsing OpenCL configuration from file: " << command_line.ocl_config_filename() << std::endl;
		heom::ocl_config ocl_config(command_line.ocl_config_filename());
		std::cout << "Parsing HEOM configuration from file: " << command_line.heom_config_filename() << std::endl;
		heom::population_dynamics_config heom_config(command_line.heom_config_filename());

		heom::hierarchy_graph complete_graph(heom_config.baths_number(), heom_config.baths_matsubaras(), heom_config.system_ado_depth());

		heom::instance heom_instance(heom_config, heom::sites_to_states_mode_t::identity, complete_graph);
		heom_instance.set_hierarchy_top(heom_config.population_dynamics_rho_init().data());

		using stepper_t = num::meta_stepper; // uses solver_stepper_type from config
		using solver_t = heom::population_dynamics_solver<heom::ode, stepper_t, heom::hierarchy_norm>;

		// create a solver from configuration and instance
		const ocl::nd_range ocl_nd_range {
				{}, // offset
		        {1, static_cast<std::uint64_t>(heom_instance.matrices())}, // global size
		        {1, 1} // local size
			};

		solver_t solver(ocl_config, ocl_nd_range, heom_config, heom_instance);
		std::cout << "-------------------- OpenCL Runtime Configuration ----------" << std::endl;
		solver.write_ocl_runtime_config(std::cout);
		std::cout << "------------------------------------------------------------" << std::endl;

		// with create_hierarchy_mask we create mask corresponding to configured filtering strategy, then update solver
		// NOTE: .data() returns pointer to mask_t array
		heom::hierarchy_mask hierarchy_mask = heom::create_hierarchy_mask(heom_config.filtering_strategy(), complete_graph, heom_config.baths_matsubaras(), heom_config.filtering_first_layer());
		solver.update_hierarchy_mask(hierarchy_mask.data());

		std::cout << "-------------------- Hierarchy Mask Counter ----------------" << std::endl;
		heom::write_hierarchy_mask_stats(hierarchy_mask, std::cout);
		std::cout << "------------------------------------------------------------" << std::endl;

		// setup output
		heom::observer_list observer_list = heom::make_file_observer_list(heom_config, complete_graph, solver);
		observer_list.observe(0.0);

		// run the solver, print output every n steps
		int_t iterations = heom_config.solver_steps() / heom_config.program_observe_steps();
		int_t steps_per_iteration = heom_config.program_observe_steps();
		int_t total_steps = iterations * heom_config.program_observe_steps();

		for (int_t i = 0; i < iterations; ++i) {
			// propagate
			solver.step_forward(steps_per_iteration);

			// after propagation, we are at:
			const auto current_step = (i + 1) * steps_per_iteration;
			const real_t current_time = current_step * heom_config.solver_step_size();

			// observe
//			solver.get_top_result(result_buffer_top);
			observer_list.observe(current_time);

			// update status
			heom::write_progress(current_step - 1, total_steps, "Calculation population dynamics: ", std::cout);
		}

		// output benchmark results
		std::cout << "-------------------- Solver Runtime Summary ----------------" << std::endl;
		solver.write_runtime_summary(std::cout);
		std::cout << "------------------------------------------------------------" << std::endl;
		std::cout << "-------------------- Memory Summary ------------------------" << std::endl;
		std::cout << "local hierarchy buffer size:            "
		          << boost::format("%11.2f MiB\n") % (heom_instance.size_hierarchy_byte() / 1024.0 / 1024.0);
		std::cout << "local instance size:                    "
		          << boost::format("%11.2f MiB\n") % (heom_instance.allocated_byte() / 1024.0 / 1024.0);
		std::cout << "max. heap via aligned::allocate(..):    "
		          << boost::format("%11.2f MiB") % (heom::memory::aligned::instance().allocated_byte_max() / 1024.0 / 1024.0)
		          << " ("
		          << boost::format("count: %6i") % heom::memory::aligned::instance().allocations_max()
		          << ')' << std::endl;
		std::cout << "max. OpenCL buffer allocations:         "
		          << boost::format("%11.2f MiB") % (solver.ocl_helper().allocated_byte_max() / 1024.0 / 1024.0)
		          << " ("
			      << boost::format("count: %6i") % solver.ocl_helper().allocations_max()
			      << ')' << std::endl;
		std::cout << "------------------------------------------------------------" << std::endl;

	} catch (...) {
		eptr = std::current_exception();
	}
	int ret = heom::handle_main_exception(eptr);

	// print application runtime
	std::cout << "main(): application runtime: " << boost::format("%11.2f") % std::chrono::duration_cast<bmt::seconds>(bmt::duration(app_timer.elapsed())).count() << " s" << std::endl;

	return ret;
}