// Copyright (c) 2018 Graphcore Ltd. All rights reserved.

#include "PoolPlan.hpp"
#include "../poplin/ConvPlan.hpp"
#include "PerformanceEstimation.hpp"
#include "PoolVertices.hpp"
#include "poplibs_support/VectorUtils.hpp"
#include "poplibs_support/gcd.hpp"
#include "poplibs_support/print.hpp"
#include "poplin/ConvUtil.hpp"
#include "poputil/VarStructure.hpp"
#include <poplibs_support/Memoize.hpp>
#include <popsolver/Model.hpp>

#include <boost/range/adaptor/reversed.hpp>

#include <unordered_set>

using namespace poputil;

namespace popnn {
namespace pooling {

// Constraint variables that represent how variables are partitioned per tile
struct PartitionVariables {
  popsolver::Variable batchSplit;
  popsolver::Variable chanGroupsSplit;
  popsolver::Variable chansPerGroup;
  std::vector<popsolver::Variable> fieldSplit;
  PartitionVariables() = default;
};

// Create Partition from the solution to the Pooling Plan solver
static Partition makePartition(const popsolver::Solution &solution,
                               const PartitionVariables &vars) {
  Partition partition;
  partition.chansPerGroup = solution[vars.chansPerGroup].getAs<unsigned>();
  partition.batch = solution[vars.batchSplit].getAs<unsigned>();
  partition.chanGroups = solution[vars.chanGroupsSplit].getAs<unsigned>();
  partition.field.reserve(vars.fieldSplit.size());
  partition.kernel.reserve(vars.fieldSplit.size());
  for (unsigned i = 0; i < vars.fieldSplit.size(); i++) {
    partition.field.push_back(solution[vars.fieldSplit[i]].getAs<unsigned>());

    // currently kernel is not split. set it to 1
    partition.kernel.push_back(1);
  }
  return partition;
}

// Create Partition from component vector
static Partition makePartition(const std::vector<unsigned> &values,
                               const unsigned numFieldDims) {
  Partition partition;
  partition.batch = values[0];
  partition.chanGroups = values[1];
  partition.chansPerGroup = values[2];
  partition.field.insert(partition.field.begin(), values.begin() + 3,
                         values.begin() + 3 + numFieldDims);
  partition.kernel.insert(partition.kernel.begin(),
                          values.begin() + 3 + numFieldDims,
                          values.begin() + 3 + (2 * numFieldDims));
  return partition;
}

// dim is given as an index into an activation shaped tensor
// [N][....F..][C]
static bool canFlattenDim(const poplin::ConvParams &params, unsigned dim) {
  if (dim >= params.getNumFieldDims() + 1) {
    return false;
  }
  if (dim == 0) {
    return true;
  }
  if (params.kernelShape[dim - 1] != 1) {
    return false;
  }
  // With striding in this dimension this doesn't make
  // a lot of sense.
  if (params.outputTransform.stride[dim - 1] != 1 ||
      params.inputTransform.dilation[dim - 1] != 1) {
    return false;
  }
  return true;
}

static Transform getTransform(const poplar::Graph &graph,
                              const poplin::ConvParams &params,
                              const poplar::Tensor &in,
                              std::size_t chanGrainSize) {
  Transform transform;
  // If we don't have enough channels, prefer to take some elements
  // of another dimension to introducing padding.
  const std::size_t numChans = params.getNumInputChans();
  if (numChans < chanGrainSize) {
    std::size_t currentFactor = 1;
    const auto desiredFactor = lcm(numChans, chanGrainSize) / numChans;

    auto groupings = detectDimGroupings(graph, in);

    // Flatten independent spatial dimensions into channels.
    //
    // Try to flatten spatial dimensions with a detectable grouping first.
    std::unordered_set<std::size_t> transformedDims;
    for (const auto &entry : groupings) {
      const auto d = entry.first;
      const std::size_t grouping = entry.second;
      if (canFlattenDim(params, d) && transformedDims.count(d) == 0 &&
          currentFactor < desiredFactor) {
        const auto f =
            gcd(desiredFactor / currentFactor, gcd(in.dim(d), grouping));
        transform.flattenDims.push_back(std::make_pair(d, f));
        transformedDims.emplace(d);
        currentFactor *= f;
      }
    }
    // Flatten any remaining dims possible, innermost spatial dimension first.
    for (int d = params.getNumFieldDims(); d >= 0; --d) {
      if (canFlattenDim(params, d) && transformedDims.count(d) == 0 &&
          currentFactor < desiredFactor) {
        const auto f = gcd(desiredFactor / currentFactor, in.dim(d));
        transform.flattenDims.push_back(std::make_pair(d, f));
        transformedDims.emplace(d);
        currentFactor *= f;
      }
    }
  }
  return transform;
}
// Functions to cache cycle estimates using a map. We need std::hash and ==
// operator to be implemented in order to memoize

class EstimateCache {
public:
  decltype(poplibs_support::memoize(
      poolVertexCycleEstimate)) mPoolVertexCycleEstimate;
  EstimateCache() : mPoolVertexCycleEstimate(poolVertexCycleEstimate) {}
};

static std::uint64_t getNumberOfOperations(const poplin::ConvParams &params) {
  // The operations used in pooling can be calculated in a similar way to
  // convolution, except that each channel is independent.
  auto convParams = params;
  convParams.inputChannelsPerConvGroup = 1;

  return getNumberOfMACs(convParams);
}

static std::size_t getBytesRequiredPerTile(const Partition &perTile,
                                           const poplin::ConvParams &params,
                                           const PoolConfig &poolCfg,
                                           unsigned bytesPerInputElement) {
  // In each dimension that the kernel moves in, each new output needs a
  // contiguous slice of input of size "stride" plus the amount the kernel adds
  // which is "kernelSize-stride"
  std::size_t inFieldElementsPerTile = 1;
  for (unsigned i = 0; i < perTile.field.size(); i++) {
    const auto factor = perTile.field[i] * params.outputTransform.stride[i] +
                        perTile.kernel[i] - params.outputTransform.stride[i];
    inFieldElementsPerTile *= factor;
  }
  const std::size_t inBytesPerTile =
      inFieldElementsPerTile * perTile.batch * perTile.chanGroups *
      perTile.chansPerGroup * bytesPerInputElement;
  const std::size_t outBytesPerTile =
      product(perTile.field) * perTile.batch * perTile.chanGroups *
      perTile.chansPerGroup * bytesPerInputElement;
  // Some vertices require more inputs.
  if (poolCfg.pass == PoolPass::POOL_BWD && poolCfg.type == PoolingType::MAX) {
    // 3 inputs: `fwdActsIn` is the size of the output field, `fwdActsOut` and
    // `in` are the size of the input field
    return 2 * inBytesPerTile + outBytesPerTile;
  }
  if (poolCfg.pass == PoolPass::POOL_FWD && poolCfg.type == PoolingType::MAX &&
      poolCfg.scaledGradient) {
    // 2 inputs: `fwdActsOut` is the size of the output field, `in` is the size
    // of the input field
    return inBytesPerTile + outBytesPerTile;
  }
  return inBytesPerTile;
}

// Build a popsolver model with the appropriate Pooling Planning variables and
// constraints. Return the "cycle" cost as the derived variable which needs to
// be optimized.
static popsolver::Variable constructModel(
    popsolver::Model &m, const poplar::Target &target, PartitionVariables &vars,
    const PoolConfig &poolCfg, const poplin::ConvParams &params,
    const unsigned minGrainsPerChanGroup, const unsigned maxGrainsPerChanGroup,
    const std::size_t chanGrainSize, const std::size_t numChannels,
    const std::size_t detChansPerGroup, const std::size_t minChannelsPerGroup,
    EstimateCache &cache) {

  auto numTiles = target.getNumTiles();
  auto fieldShape = params.getOutputFieldShape();
  auto kernelShape = params.kernelShape;
  auto nChGrain = m.addVariable(minGrainsPerChanGroup,
                                std::min(maxGrainsPerChanGroup, numTiles));
  auto nCh = m.addConstant(numChannels);
  auto chGrainSize = m.addConstant(chanGrainSize);

  // Sweep Channels Per Group
  vars.chansPerGroup = m.product({nChGrain, chGrainSize});

  // Channel-Group split sweep
  auto nChGroupsMax = m.ceildiv(nCh, vars.chansPerGroup);
  auto nTiles = m.addConstant(numTiles);
  vars.chanGroupsSplit = m.addVariable(1, numTiles);
  m.lessOrEqual(vars.chanGroupsSplit, nChGroupsMax);

  // Batch split sweep
  vars.batchSplit = m.addVariable(1, params.batchSize);

  // Sweep across each field dimension
  vars.fieldSplit.reserve(fieldShape.size());
  for (auto dimSize : fieldShape) {
    vars.fieldSplit.push_back(m.addVariable(1, dimSize));
  }

  // Constrain splits to not exceed the given number of tiles
  std::vector<popsolver::Variable> splits = {vars.chanGroupsSplit,
                                             vars.batchSplit};
  splits.insert(splits.end(), vars.fieldSplit.begin(), vars.fieldSplit.end());
  auto usedTiles = m.product(splits);
  m.lessOrEqual(usedTiles, nTiles);

  // Constrain channels to be >= minChannelsPerGroup
  m.lessOrEqual(m.addConstant(minChannelsPerGroup), vars.chansPerGroup);

  // Work out the size of each partition after applying the split
  std::vector<popsolver::Variable> fieldVar;
  std::vector<popsolver::Variable> kernelVar;
  fieldVar.reserve(vars.fieldSplit.size());
  kernelVar.reserve(vars.fieldSplit.size());
  auto nChGroups = m.ceildiv(nChGroupsMax, vars.chanGroupsSplit);
  auto batchSize = m.addConstant(params.batchSize);
  auto nBatch = m.ceildiv(batchSize, vars.batchSplit);

  for (unsigned i = 0; i < vars.fieldSplit.size(); i++) {
    auto fieldDim = m.addConstant(fieldShape[i]);
    fieldVar.push_back(m.ceildiv(fieldDim, vars.fieldSplit[i]));
    kernelVar.push_back(m.addConstant(kernelShape[i]));
  }

  // Compute lower bound on cycles - define some constants
  auto totalOperations = m.addConstant(getNumberOfOperations(params));

  auto vertexCyclesPerInnerLoop = m.addConstant(poolVertexCyclesPerVector(
      poolCfg.type == PoolingType::MAX, poolCfg.pass == PoolPass::POOL_BWD));
  auto vectorWidth = m.addConstant(target.getVectorWidth(params.inputType));
  auto vertexCyclesPerRow = m.addConstant(poolVertexCyclesPerRow());

  // Compute lower bound on cycles, rounding down as it is a lower bound
  auto rowsPerTile = m.floordiv(m.product(fieldVar), fieldVar.back());
  auto rowOverheadPerTile = m.product({rowsPerTile, vertexCyclesPerRow});
  auto operationsPerTile = m.floordiv(totalOperations, usedTiles);
  auto minCyclesPerTile = m.sum(
      {rowOverheadPerTile, m.product({m.ceildiv(operationsPerTile, vectorWidth),
                                      vertexCyclesPerInnerLoop})});

  // Evaluate the cycle-cost for each possible partition
  std::vector<popsolver::Variable> splitVars = {
      nBatch,
      nChGroups,
      vars.chansPerGroup,
  };
  splitVars.insert(splitVars.end(), fieldVar.begin(), fieldVar.end());
  splitVars.insert(splitVars.end(), kernelVar.begin(), kernelVar.end());

  auto cycles = m.call<unsigned>(
      splitVars, [&target, poolCfg, &params, fieldShape, detChansPerGroup,
                  &cache](const std::vector<unsigned> &values) {
        // Note that "perTile" is not a partition per se, but it contains
        // the variables after they were partitioned for each tile:
        //      perTile->batch = batchSize / batchSplit
        //      perTile->chanGroups = numChannels / channelSplit
        //      perTile->field[d] = ConvParams::fieldShape[d] / fieldSplit[d]
        //      perTile->kernel[d] = ConvParams::kernelShape[d] / kernelSplit[d]
        Partition perTile = makePartition(values, fieldShape.size());

        const unsigned strideX = params.inputTransform.dilation.back();
        const unsigned numKernelPositions = product(params.kernelShape);
        const unsigned inputVectorWidth =
            params.inputType == poplar::HALF ? 4 : 2;
        const unsigned workers = target.getNumWorkerContexts();
        auto computeCost = cache.mPoolVertexCycleEstimate(
            perTile, poolCfg, strideX, numKernelPositions, inputVectorWidth,
            workers);
        // Where the field was divided it was rounded up.  For plans where
        // that is not exact, the tiles which take the rounded down field
        // pieces can use more cycles.  This could be true for any dimension, so
        // we try them all and allow the planning cost to determine which is
        // best rather than coding assumptions about the planning cost here. It
        // could be that the slowest tile has a combination of rounded down dims
        // to deal with, but that is not necessarily going to happen and adds
        // many more calls to the estimator, so is not implemented.
        for (unsigned i = 0; i < fieldShape.size(); i++) {
          if (fieldShape[i] % perTile.field[i]) {
            auto altPerTile = perTile;
            altPerTile.field[i] -= 1;
            computeCost = std::max(computeCost, cache.mPoolVertexCycleEstimate(
                                                    altPerTile, poolCfg,
                                                    strideX, numKernelPositions,
                                                    inputVectorWidth, workers));
          }
        }
        // But the partitioned field size is the output, we'll be exchanging
        // and rearranging the input - scale to find the input needed to
        // produce the tile's partitioned output.  In some cases there are
        // multiple inputs
        // exchange cost: assume everything brought onto tile
        const auto bytesPerTile = getBytesRequiredPerTile(
            perTile, params, poolCfg, target.getTypeSize(params.inputType));
        const unsigned exchangeBytesPerCycle =
            target.getExchangeBytesPerCycle();
        uint64_t exchangeCost = bytesPerTile / exchangeBytesPerCycle;

        // Penalise for changing from detected group
        std::uint64_t rearrangementCost = 0;
        if (detChansPerGroup != perTile.chansPerGroup) {
          rearrangementCost += bytesPerTile / 4;
        }
        return popsolver::DataType{computeCost + exchangeCost +
                                   rearrangementCost};
      });

  // Constrain so that the min cycles estimate per tile is < cycles
  // which avoids calling the costly cycle estimator in cases where it is clear
  // that the cycles estimate will be large
  m.lessOrEqual(minCyclesPerTile, cycles);

  return cycles;
}

poplin::ConvParams applyTransform(poplin::ConvParams params,
                                  const Transform &transform,
                                  const std::vector<poplar::Tensor *> &as) {
  for (const auto &entry : transform.flattenDims) {
    const auto d = entry.first;
    const auto f = entry.second;
    // Spatial dimension 0 means batch size
    if (d == 0) {
      assert(params.batchSize % f == 0);
      params.inputChannelsPerConvGroup *= f;
      params.outputChannelsPerConvGroup *= f;
      params.batchSize /= f;
    } else {
      assert(params.inputFieldShape[d - 1] % f == 0);
      params.inputChannelsPerConvGroup *= f;
      params.outputChannelsPerConvGroup *= f;
      params.inputFieldShape[d - 1] /= f;
      // N.B. This is only possible if kernel size in this dimension is
      // 1 so we don't touch kernel shape.
      assert(params.kernelShape[d - 1] == 1);
    }
    for (poplar::Tensor *a : as) {
      if (a != nullptr) {
        *a = a->dimRoll(d, a->rank() - 2)
                 .reshapePartial(a->rank() - 2, a->rank(),
                                 {a->dim(d) / f, a->dim(a->rank() - 1) * f})
                 .dimRoll(a->rank() - 2, d);
      }
    }
  }
  return params;
}

void applyTransformInverse(const poplin::ConvParams &params,
                           const Transform &transform,
                           const std::vector<poplar::Tensor *> &as) {
  for (const auto &entry : boost::adaptors::reverse(transform.flattenDims)) {
    const auto d = entry.first;
    const auto f = entry.second;
    for (poplar::Tensor *a : as) {
      if (a != nullptr) {
        *a = a->dimRoll(d, a->rank() - 2)
                 .reshapePartial(a->rank() - 2, a->rank(),
                                 {a->dim(d) * f, a->dim(a->rank() - 1) / f})
                 .dimRoll(a->rank() - 2, d);
      }
    }
  }
}

// Get plan based on compute and exchange cost. As a further improvement, the
// plan could incorporate introspection. For now, keep it simple.
// Fwd and Bwd plans are kept separate as there is possibly no benefit for
// doing a joint one.
PlanResult getPlan(const poplar::Graph &graph, const PoolConfig &poolCfg,
                   const TransformedInput &input,
                   const TransformedInput &inputGrouped) {
  Plan plan;

  // Don't use getTypeSize here because IpuModel will report something
  // different to what it actually uses.
  // We can change this once T6380 is fixed.
  const auto typeSize = (input.params.inputType == poplar::HALF ? 2 : 4);
  const auto chanGrainSize = 8UL / typeSize;

  plan.transform = getTransform(graph, input.params, input.in, chanGrainSize);

  // Apply any transform to the parameters and input then work
  // out partitioning.
  poplar::Tensor in = input.in;
  const auto transformedParams =
      applyTransform(input.params, plan.transform, {&in});
  const auto inShape = in.shape();
  auto chansPerGroupDet = detectInnermostGrouping(graph, in);
  auto numChannels = inShape[inShape.size() - 1];

  // Do not allow a large number of grains as memory cost of exchanging and
  // rearranging is significant
  auto minGrainsPerChanGroup = 1UL;
  auto maxGrainsPerChanGroup =
      std::min((chansPerGroupDet + chanGrainSize - 1) / chanGrainSize,
               (numChannels + chanGrainSize - 1) / chanGrainSize);
  maxGrainsPerChanGroup =
      std::max(std::min(maxGrainsPerChanGroup, 8UL), minGrainsPerChanGroup);

  // Construct model with variables and constraints
  popsolver::Model m;
  PartitionVariables vars;
  EstimateCache cache;

  auto cycles =
      constructModel(m, graph.getTarget(), vars, poolCfg, transformedParams,
                     minGrainsPerChanGroup, maxGrainsPerChanGroup,
                     chanGrainSize, numChannels, chansPerGroupDet, 1, cache);

  // Optimise within constraints
  auto s = m.minimize({cycles});
  assert(s.validSolution());
  plan.partition = makePartition(s, vars);
  auto sResult = *s[cycles];

  // Consider a second plan, constrained to a minimum number of channels, as
  // operations that use the output can benefit from this.  Allow the pooling
  // cycles to degrade by a fairly large percentage in order to achieve the
  // minimum number of channels as pooling is a relatively cheap operation,
  // and the other operations that it may affect are more expensive.

  // Use the grouped input for this plan, as the input may have been transformed
  // so that channels were combined into the spatial dimensions no longer
  // giving enough channels to meet the preferred channel grouping.
  const auto minChannelsPerGroup =
      getPreferredChannelGrouping(inputGrouped.params.inputType, poolCfg.type);
  auto numChannelsGrouped = inputGrouped.in.shape().back();
  if (plan.partition.chansPerGroup < minChannelsPerGroup &&
      numChannelsGrouped >= minChannelsPerGroup) {
    maxGrainsPerChanGroup =
        (minChannelsPerGroup + chanGrainSize - 1) / chanGrainSize;
    Plan plan;
    plan.transform = getTransform(graph, inputGrouped.params, inputGrouped.in,
                                  chanGrainSize);
    const auto transformedParams =
        applyTransform(inputGrouped.params, plan.transform, {&in});
    auto chansPerGroupDet = detectInnermostGrouping(graph, inputGrouped.in);

    popsolver::Model mConstrained;
    PartitionVariables varsConstrained;
    auto cyclesConstrained =
        constructModel(mConstrained, graph.getTarget(), varsConstrained,
                       poolCfg, transformedParams, minGrainsPerChanGroup,
                       maxGrainsPerChanGroup, chanGrainSize, numChannelsGrouped,
                       chansPerGroupDet, minChannelsPerGroup, cache);

    // Optimise within constraints of minChannelsPerGroup.  There may not be a
    // solution for all targets
    auto sConstrained = mConstrained.minimize({cyclesConstrained});
    if (sConstrained.validSolution()) {
      auto sConstrainedResult = *sConstrained[cyclesConstrained];
      if (sConstrainedResult < (sResult * 4) / 3) {
        plan.partition = makePartition(sConstrained, varsConstrained);
        return {plan, sConstrainedResult, true};
      }
    }
  }
  return {plan, sResult, false};
}

std::ostream &operator<<(std::ostream &os, const Partition &p) {
  os << "Partition:\n";
  os << "        Batch split              " << p.batch << "\n";
  os << "        Channel split            " << p.chanGroups << "\n";
  os << "        Chans per group          " << p.chansPerGroup << "\n";
  os << "        Kernel split             ";
  printContainer(p.kernel, os);
  os << "\n";
  os << "        Field split              ";
  printContainer(p.field, os);
  os << "\n";
  return os;
}

std::ostream &operator<<(std::ostream &o, const Transform &t) {
  o << "Transform:\n"
    << "        Flatten dims ";
  printContainer(t.flattenDims, o);
  o << "\n";
  return o;
}

std::ostream &operator<<(std::ostream &o, const Plan &p) {
  o << p.transform << p.partition;
  return o;
}

} // namespace pooling
} // namespace popnn


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2007-2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_UNIT_SYSTEMS_METRIC_BARN_HPP_INCLUDED
#define BOOST_UNIT_SYSTEMS_METRIC_BARN_HPP_INCLUDED

#include <boost/units/conversion.hpp>
#include <boost/units/systems/si/area.hpp>

BOOST_UNITS_DEFINE_BASE_UNIT_WITH_CONVERSIONS(metric, barn, "barn", "b", 1.0e-28, si::area, 11);

#endif // BOOST_UNIT_SYSTEMS_METRIC_BARN_HPP_INCLUDED


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

   The MIT License

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

   License for the specific language governing rights and limitations under
   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.
*/

#include <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/GeometryPrimitives/Point.h>
#include <Core/GeometryPrimitives/Vector.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Algorithms/BrainStimulator/ElectrodeCoilSetupAlgorithm.h>
#include <Core/Algorithms/Legacy/Fields/DistanceField/CalculateSignedDistanceField.h>
#include <Core/Algorithms/Legacy/Fields/FieldData/ConvertFieldBasisType.h>
#include <Core/Algorithms/Legacy/Fields/FieldData/GetFieldData.h>
#include <Core/Algorithms/Legacy/Fields/MeshDerivatives/SplitByConnectedRegion.h>
#include <Core/Algorithms/Legacy/Fields/DomainFields/SplitFieldByDomainAlgo.h>
#include <Core/Algorithms/Legacy/Fields/MeshDerivatives/GetFieldBoundaryAlgo.h>
#include <Core/Algorithms/Legacy/Fields/MeshData/GetMeshNodes.h>
#include <Core/Datatypes/Legacy/Field/Field.h>
#include <Core/Datatypes/Legacy/Field/VField.h>
#include <Core/Datatypes/Legacy/Field/VMesh.h>
#include <Core/Datatypes/Legacy/Field/FieldInformation.h>
#include <Core/Datatypes/Matrix.h>
#include <Core/Datatypes/MatrixTypeConversions.h>
#include <Core/Datatypes/MatrixComparison.h>
#include <string>
#include <boost/range/algorithm/count.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
#include <boost/format.hpp>
#include <boost/assign.hpp>
#include <math.h>
#include <Core/Math/MiscMath.h>
#include <iostream>

using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core::Algorithms::BrainStimulator;
using namespace SCIRun::Core::Algorithms::Fields;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Geometry;
using namespace SCIRun;
using namespace boost;
using namespace boost::assign;
using namespace  Eigen;

ALGORITHM_PARAMETER_DEF(BrainStimulator, NumberOfPrototypes);
ALGORITHM_PARAMETER_DEF(BrainStimulator, TableValues);
ALGORITHM_PARAMETER_DEF(BrainStimulator, ProtoTypeInputCheckbox);
ALGORITHM_PARAMETER_DEF(BrainStimulator, AllInputsTDCS);
ALGORITHM_PARAMETER_DEF(BrainStimulator, ProtoTypeInputComboBox);
ALGORITHM_PARAMETER_DEF(BrainStimulator, ElectrodethicknessCheckBox);
ALGORITHM_PARAMETER_DEF(BrainStimulator, ElectrodethicknessSpinBox);

const AlgorithmOutputName ElectrodeCoilSetupAlgorithm::FINAL_ELECTRODES_FIELD("FINAL_ELECTRODES_FIELD");
const AlgorithmOutputName ElectrodeCoilSetupAlgorithm::MOVED_ELECTRODES_FIELD("MOVED_ELECTRODES_FIELD");
const AlgorithmInputName ElectrodeCoilSetupAlgorithm::SCALP_SURF("SCALP_SURF");
const AlgorithmInputName ElectrodeCoilSetupAlgorithm::ELECTRODECOILPROTOTYPES("ELECTRODECOILPROTOTYPES");
const AlgorithmOutputName ElectrodeCoilSetupAlgorithm::ELECTRODE_SPONGE_LOCATION_AVR("ELECTRODE_SPONGE_LOCATION_AVR");
const AlgorithmOutputName ElectrodeCoilSetupAlgorithm::COILS_FIELD("COILS_FIELD");
const AlgorithmInputName ElectrodeCoilSetupAlgorithm::LOCATIONS("LOCATIONS");

const int ElectrodeCoilSetupAlgorithm::unknown_stim_type = 0;
const int ElectrodeCoilSetupAlgorithm::TMS_stim_type     = 1;
const int ElectrodeCoilSetupAlgorithm::tDCS_stim_type    = 2;
const double ElectrodeCoilSetupAlgorithm::direction_bound = -0.99;

const AlgorithmParameterName ElectrodeCoilSetupAlgorithm::columnNames[] =
{ Name("Input #"),
Name("Type"),
Name("X"),
Name("Y"),
Name("Z"),
Name("Angle"),
Name("NX"),
Name("NY"),
Name("NZ"),
Name("thickness")
};

ElectrodeCoilSetupAlgorithm::ElectrodeCoilSetupAlgorithm()
{
 using namespace Parameters;
 {
  addParameter(MOVED_ELECTRODES_FIELD, 0);
  addParameter(NumberOfPrototypes, 0);
  addParameter(TableValues, 0);
  addParameter(ProtoTypeInputCheckbox, false);
  addParameter(ProtoTypeInputComboBox, false);
  addParameter(AllInputsTDCS, false);
  addParameter(ElectrodethicknessCheckBox, false);
  addParameter(ElectrodethicknessSpinBox, 1.0);
 }
}

VariableHandle ElectrodeCoilSetupAlgorithm::fill_table(FieldHandle scalp, DenseMatrixHandle locations, const std::vector<FieldHandle>& input) const
{
  Variable::List table;
  if (locations->ncols()!=3)
  {
   THROW_ALGORITHM_PROCESSING_ERROR(" LOCATIONS needs to have dimensions such as: (#CoilsOrElectrodes) x 3 ");
  }

  auto tab_values = get(Parameters::TableValues).toVector();

  for (int i=0;i<locations->nrows();i++)
  {

   Variable::List tmp;

   if (tab_values.size()<locations->nrows())
   {
   tmp +=
     Variable(columnNames[0], boost::str(boost::format("%s") % "0")),
     Variable(columnNames[1], boost::str(boost::format("%s") % "0")),
     Variable(columnNames[2], boost::str(boost::format("%.3f") % (* locations)(i,0))),
     Variable(columnNames[3], boost::str(boost::format("%.3f") % (* locations)(i,1))),
     Variable(columnNames[4], boost::str(boost::format("%.3f") % (* locations)(i,2))),
     Variable(columnNames[5], boost::str(boost::format("???"))),
     Variable(columnNames[6], std::string("???")),
     Variable(columnNames[7], std::string("???")),
     Variable(columnNames[8], std::string("???")),
     Variable(columnNames[9], std::string("???"));

   } else
   {
      auto col = tab_values[i].toVector();

     if (col.size()==number_of_columns)
     {
      std::string str1=col[0].toString();
      std::string str2=col[1].toString();
      std::string str3=col[2].toString();
      std::string str4=col[3].toString();
      std::string str5=col[4].toString();
      std::string str6=col[5].toString();
      std::string str7=col[6].toString();
      std::string str8=col[7].toString();
      std::string str9=col[8].toString();
      std::string str10=col[9].toString();

      Variable var1=makeVariable("Input #", boost::str(boost::format("%s") % str1));
      Variable var2=makeVariable("Type",   boost::str(boost::format("%s") % str2));

      Variable var3,var4,var5;

      if (str3.compare("???")==0)
       var3=makeVariable("X",     boost::str(boost::format("%.3f") % (* locations)(i,0)));
        else
         var3=makeVariable("X",   boost::str(boost::format("%s") % str3));

      if (str4.compare("???")==0)
       var4=makeVariable("Y",     boost::str(boost::format("%.3f") % (* locations)(i,1)));
	else
	 var4=makeVariable("Y",   boost::str(boost::format("%s") % str4));

      if (str5.compare("???")==0)
       var5=makeVariable("Y",     boost::str(boost::format("%.3f") % (* locations)(i,2)));
	else
         var5=makeVariable("Z",   boost::str(boost::format("%s") % str5));

      Variable var6=makeVariable("Angle",  boost::str(boost::format("%s") % str6));

      ///if this table row was selected as tDCS -> project point to scalp surface and put its normal in table (NX,NY,NZ)
      Variable var7=makeVariable("NX", boost::str(boost::format("%s") % str7));
      Variable var8=makeVariable("NY", boost::str(boost::format("%s") % str8));
      Variable var9=makeVariable("NZ", boost::str(boost::format("%s") % str9));
      Variable var10=makeVariable("thickness",boost::str(boost::format("%s") % str10));
      tmp += var1,var2,var3,var4,var5,var6,var7,var8,var9,var10;

     }
   }

   table.push_back(makeVariable("row" + boost::lexical_cast<std::string>(i), tmp));
  }
  VariableHandle output(new Variable(Name("Table"), table));

  return output;
}

DenseMatrixHandle ElectrodeCoilSetupAlgorithm::make_rotation_matrix(const double angle, const std::vector<double>& normal) const
{
 DenseMatrixHandle result(new DenseMatrix(3, 3));

  double normal_vector_norm=sqrt(normal[0]*normal[0]+normal[1]*normal[1]+normal[2]*normal[2]);
  Vector3d normal_vector((double)normal[0]/normal_vector_norm, (double)normal[1]/normal_vector_norm, (double)normal[2]/normal_vector_norm);

  Vector3d tan_vector1, tan_vector2;
  if (normal_vector(0)!=0 || normal_vector(2)!=0)
  {
   tan_vector1(0)= normal_vector(2);
   tan_vector1(1)= 0.0;
   if (normal_vector(0)==0)
    tan_vector1(2) = 0.0;
     else
       tan_vector1(2)= -1*normal_vector(0);
   } else
  {
   tan_vector1(0)= normal_vector(1);
   tan_vector1(1)= 0.0;
   tan_vector1(2)= 0.0;
  }

 normal_vector_norm=sqrt(tan_vector1(0)*tan_vector1(0)+tan_vector1(1)*tan_vector1(1)+tan_vector1(2)*tan_vector1(2));
 tan_vector1(0)/=normal_vector_norm; tan_vector1(1)/=normal_vector_norm; tan_vector1(2)/=normal_vector_norm;
 tan_vector2 = normal_vector.cross(tan_vector1);
 normal_vector_norm=sqrt(tan_vector2(0)*tan_vector2(0)+tan_vector2(1)*tan_vector2(1)+tan_vector2(2)*tan_vector2(2));
 tan_vector2(0)/=normal_vector_norm; tan_vector2(1)/=normal_vector_norm; tan_vector2(2)/=normal_vector_norm;
 DenseMatrixHandle rotation_matrix2;
 Matrix3d rotation_matrix(3,3), rotation_matrix1(3,3);

 (*result)(0,2)=normal_vector(0);
 (*result)(1,2)=normal_vector(1);
 (*result)(2,2)=normal_vector(2);
 (*result)(0,1)=tan_vector2(0);
 (*result)(1,1)=tan_vector2(1);
 (*result)(2,1)=tan_vector2(2);
 (*result)(0,0)=tan_vector1(0);
 (*result)(1,0)=tan_vector1(1);
 (*result)(2,0)=tan_vector1(2);

 return result;
}

DenseMatrixHandle ElectrodeCoilSetupAlgorithm::make_rotation_matrix_around_axis(double angle, std::vector<double>& axis_vector) const
{
   DenseMatrixHandle result(new DenseMatrix(3, 3));

   angle = angle * M_PI / 180.0;
   double cos_angle = cos(angle);
   double sin_angle = sin(angle);
   double ux=axis_vector[0],uy=axis_vector[1], uz=axis_vector[2];
   (*result)(0,0)=cos_angle+ux*ux*(1-cos_angle);
   (*result)(0,1)=ux*uy*(1-cos_angle)-uz*sin_angle;
   (*result)(0,2)=ux*uz*(1-cos_angle)+uy*sin_angle;
   (*result)(1,0)=uy*ux*(1-cos_angle)+uz*sin_angle;
   (*result)(1,1)=cos_angle+uy*uy*(1-cos_angle);
   (*result)(1,2)=uy*uz*(1-cos_angle)-ux*sin_angle;
   (*result)(2,0)=uz*ux*(1-cos_angle)-uy*sin_angle;
   (*result)(2,1)=uz*uy*(1-cos_angle)+ux*sin_angle;
   (*result)(2,2)=cos_angle+uz*uz*(1-cos_angle);

  return result;
}

FieldHandle ElectrodeCoilSetupAlgorithm::make_tms(FieldHandle scalp, const std::vector<FieldHandle>& elc_coil_proto, const std::vector<double>& coil_prototyp_map, const std::vector<double>& coil_x, const std::vector<double>& coil_y, const std::vector<double>& coil_z, const std::vector<double>& coil_angle_rotation, const std::vector<double>& coil_nx, const std::vector<double>& coil_ny, const std::vector<double>& coil_nz) const
{
  FieldInformation fieldinfo("PointCloudMesh", 0, "Vector");
  FieldHandle tms_coils_field = CreateField(fieldinfo);
  VMesh* tms_coils_vmesh = tms_coils_field->vmesh();
  VField* tms_coils_vfld  = tms_coils_field->vfield();
  std::vector<Point> tms_coils_field_values;

  for (int i=0; i<coil_prototyp_map.size(); i++)
  {
   if (coil_prototyp_map[i]<=elc_coil_proto.size() && coil_prototyp_map[i]>=0)
   {
    if( !(coil_x.size()-1>=i && coil_y.size()-1>=i && coil_z.size()-1>=i))
     {
       THROW_ALGORITHM_PROCESSING_ERROR("Internal error: definition of coil (x,y,z) seems to be empty.");
     }

    /// 1) move coil to predetermined position and orientation
    FieldHandle coil_fld = elc_coil_proto[coil_prototyp_map[i]-1];
    if (!coil_fld)
    {
      THROW_ALGORITHM_PROCESSING_ERROR("Internal error: coil field .");
    }

    FieldInformation fi(coil_fld);
    if(fi.is_pointcloudmesh())
    {
     GetFieldDataAlgo algo_getfielddata;
     DenseMatrixHandle fielddata;
     try
     {
      fielddata  = algo_getfielddata.run(coil_fld);
     }
     catch (...)
     {
     }

     GetMeshNodesAlgo algo_getfieldnodes;
     DenseMatrixHandle fieldnodes;
     try
     {
      algo_getfieldnodes.run(coil_fld,fieldnodes);
     }
     catch (...)
     {
      THROW_ALGORITHM_PROCESSING_ERROR("Internal error: could not retrieve positions from  ");
     }

     DenseMatrixHandle magnetic_dipoles(boost::make_shared<DenseMatrix>(fielddata->nrows(),3));

     /// subtract the mean from the coil positions to move them accourding to GUI table entries
     double mean_loc_x=0,mean_loc_y=0,mean_loc_z=0;
     for(int j=0; j<fieldnodes->nrows(); j++)
     {
      mean_loc_x+=(*fieldnodes)(j,0);
      mean_loc_y+=(*fieldnodes)(j,1);
      mean_loc_z+=(*fieldnodes)(j,2);
     }
     mean_loc_x/=fieldnodes->nrows();
     mean_loc_y/=fieldnodes->nrows();
     mean_loc_z/=fieldnodes->nrows();

     for(int j=0; j<fieldnodes->nrows(); j++)
     {
      (*fieldnodes)(j,0)-=mean_loc_x;
      (*fieldnodes)(j,1)-=mean_loc_y;
      (*fieldnodes)(j,2)-=mean_loc_z;
     }

     /// 2) create normals and rotate if needed
     if (coil_nx.size()-1>=i && coil_ny.size()-1>=i && coil_nz.size()-1>=i && coil_angle_rotation.size()-1>=i)
     {
      double angle = coil_angle_rotation[i];
      DenseMatrixHandle rotation_matrix,rotation_matrix1,rotation_matrix2;
      // 2.1) create rotation matrices
      std::vector<double> coil_vector;
      coil_vector.push_back(coil_nx[i]);
      coil_vector.push_back(coil_ny[i]);
      coil_vector.push_back(coil_nz[i]);

      rotation_matrix1 = make_rotation_matrix(angle, coil_vector);

      if (angle!=0) /// test it !
      {
       std::vector<double> axis;
       axis.push_back(coil_nx[i]);
       axis.push_back(coil_ny[i]);
       axis.push_back(coil_nz[i]);
       rotation_matrix2 = make_rotation_matrix_around_axis(angle, axis);
       rotation_matrix = boost::make_shared<DenseMatrix>((*rotation_matrix2) * (*rotation_matrix1));
      }

      /// 2.2) apply rotation and move points
      for(int j=0; j<fieldnodes->nrows(); j++)
      {
       if(coil_x.size()-1>=i && coil_y.size()-1>=i && coil_z.size()-1>=i)
       {
        DenseMatrixHandle pos_vec (boost::make_shared<DenseMatrix>(3,1));

       (*pos_vec)(0,0)=(*fieldnodes)(j,0);
       (*pos_vec)(1,0)=(*fieldnodes)(j,1);
       (*pos_vec)(2,0)=(*fieldnodes)(j,2);

       DenseMatrixHandle rotated_positions;

       if (angle==0)
        rotated_positions = boost::make_shared<DenseMatrix>((*rotation_matrix1) * (*pos_vec));
	 else
	   rotated_positions = boost::make_shared<DenseMatrix>((*rotation_matrix) * (*pos_vec));

       (*fieldnodes)(j,0)=(*rotated_positions)(0,0)+coil_x[i];
       (*fieldnodes)(j,1)=(*rotated_positions)(1,0)+coil_y[i];
       (*fieldnodes)(j,2)=(*rotated_positions)(2,0)+coil_z[i];

      } else
      {
       THROW_ALGORITHM_PROCESSING_ERROR("Internal error: definition of coil (x,y,z) seems to be empty.");
      }
    }

   /// 2.3) use normal as magnetic dipole orientation if there are no normals defined at prototyp
   if(coil_nx.size()-1>=i && coil_ny.size()-1>=i && coil_nz.size()-1>=i)
    {
     if(fielddata->ncols()==1) /// if there are no dipoles but only scalar values use GUI normal
     {
      for (int j=0; j<fielddata->nrows(); j++)
      {
       (*magnetic_dipoles)(j,0)=(*fielddata)(j,0)*coil_nx[i];
       (*magnetic_dipoles)(j,1)=(*fielddata)(j,0)*coil_ny[i];
       (*magnetic_dipoles)(j,2)=(*fielddata)(j,0)*coil_nz[i];
      }
     } else
      if(fielddata->ncols()==3) /// roatate magnetic dipoles
      {
       for (int j=0; j<fielddata->nrows(); j++)
       {
    	 DenseMatrixHandle pos_vec(boost::make_shared<DenseMatrix>(3,1));
         (*pos_vec)(0,0)=(*fielddata)(j,0);
         (*pos_vec)(1,0)=(*fielddata)(j,1);
         (*pos_vec)(2,0)=(*fielddata)(j,2);

	 DenseMatrixHandle rotated_positions;
	 if (angle==0 || IsNan(angle))
	 {
          rotated_positions = boost::make_shared<DenseMatrix>((*rotation_matrix1) * (*pos_vec));
	 }
	   else
	     rotated_positions = boost::make_shared<DenseMatrix>((*rotation_matrix) * (*pos_vec));

 	(*magnetic_dipoles)(j,0)=(*rotated_positions)(0,0);
	(*magnetic_dipoles)(j,1)=(*rotated_positions)(1,0);
	(*magnetic_dipoles)(j,2)=(*rotated_positions)(2,0);
       }
      }
      if (fielddata->ncols()!=3)
      {
       std::ostringstream ostr4;
       ostr4 << " Trying to generate magnetic dipoles for TMS coil defined in table row " << i+1 << " could not find any prototyp normals - using (NX,NY,NZ) from GUI instead! " << std::endl;
       remark(ostr4.str());
      }
    }
   } else
   {
     THROW_ALGORITHM_PROCESSING_ERROR("Internal error: coil normals or coil prototype or coil angle rotation did not make it to algorithm. ");
   }

    /// 4) join coil to output coil Field
    for (VMesh::index_type j=0; j<fieldnodes->nrows(); j++)
    {
      Point p((*fieldnodes)(j,0),(*fieldnodes)(j,1),(*fieldnodes)(j,2));
      tms_coils_vmesh->add_point(p);
      Point vec((*magnetic_dipoles)(j,0),(*magnetic_dipoles)(j,1),(*magnetic_dipoles)(j,2));
      tms_coils_field_values.push_back(vec);
    }
   } else
   {
     std::ostringstream ostr3;
     ostr3 << " TMS coil definition for " << i+1 << ". row needs be a point cloud (with defined scalar or vectors data)." << std::endl;
     remark(ostr3.str());
     continue;
   }
  } else
  {
     THROW_ALGORITHM_PROCESSING_ERROR("Internal error: coil definition inconsistent. ");
  }
 }

  VMesh::Node::iterator it, it_end;
  tms_coils_vmesh->begin(it);
  tms_coils_vmesh->end(it_end);
  index_type j = 0;
  tms_coils_vfld->resize_values();
  while (it!=it_end)
  {
   Vector vec(tms_coils_field_values[j].x(),tms_coils_field_values[j].y(),tms_coils_field_values[j].z());
   tms_coils_vfld->set_value(vec,*it);
   j++;
   ++it;
  }

  return tms_coils_field;
}

boost::tuple<Variable::List,double, double, double> ElectrodeCoilSetupAlgorithm::make_table_row(int i, double x, double y, double z, double nx, double ny, double nz) const
{
 Variable::List tmp;
 double out_nx, out_ny, out_nz;
 auto tab_values = get(Parameters::TableValues).toVector();
 auto col = tab_values[i].toVector();
 std::string str1=col[0].toString();
 std::string str2=col[1].toString();
 std::string str3=col[2].toString();
 std::string str4=col[3].toString();
 std::string str5=col[4].toString();
 std::string str6=col[5].toString();

 std::string str7=col[6].toString();
 std::string str8=col[7].toString();
 std::string str9=col[8].toString();

 std::string str10=col[9].toString();

 if (str7.compare("???")==0)
 {
  str7 = boost::str(boost::format("%.3f") % nx);
  out_nx=nx;
 } else
 {
  try
  {
   out_nx = boost::lexical_cast<double>(col[6].toString());
  } catch(bad_lexical_cast&) { }
 }

 if (str8.compare("???")==0)
 {
  str8 = boost::str(boost::format("%.3f") % ny);
  out_ny=ny;
 } else
 {
  try
  {
   out_ny = boost::lexical_cast<double>(col[7].toString());
  } catch(bad_lexical_cast&) { }
 }

 if (str9.compare("???")==0)
 {
  str9 = boost::str(boost::format("%.3f") % nz);
  out_nz=nz;
 } else
 {
  try
  {
   out_nz = boost::lexical_cast<double>(col[8].toString());
  } catch(bad_lexical_cast&) { }
 }

 Variable var1=makeVariable("Input #", boost::str(boost::format("%s") % str1));
 Variable var2=makeVariable("Type",   boost::str(boost::format("%s") % str2));

 Variable var3,var4,var5;

 if (str3.compare("???")==0)
  var3=makeVariable("X",     boost::str(boost::format("%.3f") % x));
  else
    var3=makeVariable("X",   boost::str(boost::format("%s") % str3));

 if (str4.compare("???")==0)
  var4=makeVariable("Y",     boost::str(boost::format("%.3f") % y));
   else
    var4=makeVariable("Y",   boost::str(boost::format("%s") % str4));

 if (str5.compare("???")==0)
  var5=makeVariable("Y",     boost::str(boost::format("%.3f") % z));
   else
    var5=makeVariable("Z",   boost::str(boost::format("%s") % str5));

 Variable var6=makeVariable("Angle",  boost::str(boost::format("%s") % str6));

 ///if this table row was selected as tDCS -> project point to scalp surface and put its normal in table (NX,NY,NZ)
 Variable var7=makeVariable("NX", boost::str(boost::format("%s") % str7));
 Variable var8=makeVariable("NY", boost::str(boost::format("%s") % str8));
 Variable var9=makeVariable("NZ", boost::str(boost::format("%s") % str9));

 Variable var10=makeVariable("thickness",boost::str(boost::format("%s") % str10));
 tmp += var1,var2,var3,var4,var5,var6,var7,var8,var9,var10;

 return boost::make_tuple(tmp, out_nx, out_ny, out_nz);
}

boost::tuple<DenseMatrixHandle, FieldHandle, FieldHandle, VariableHandle> ElectrodeCoilSetupAlgorithm::make_tdcs_electrodes(FieldHandle scalp, const std::vector<FieldHandle>& elc_coil_proto, const std::vector<double>& elc_prototyp_map, const std::vector<double>& elc_x, const std::vector<double>& elc_y, const std::vector<double>& elc_z, const std::vector<double>& elc_angle_rotation, const std::vector<double>& elc_thickness, VariableHandle table) const
{
 int nr_elc_sponge_triangles=0, num_valid_electrode_definition=0, nr_elc_sponge_triangles_on_scalp=0;
 std::vector<double> field_values, field_values_elc_on_scalp;
 std::vector<int> valid_electrode_definition;
 FieldInformation fieldinfo("TriSurfMesh", CONSTANTDATA_E, "int");
 FieldHandle electrode_field = CreateField(fieldinfo);
 VMesh* tdcs_vmesh = electrode_field->vmesh();
 VField* tdcs_vfld  = electrode_field->vfield();
 FieldInformation fieldinfo3("TriSurfMesh", CONSTANTDATA_E, "int");
 FieldHandle output = CreateField(fieldinfo3);
 VMesh* output_vmesh = output->vmesh();
 VField* output_vfld = output->vfield();
 Variable::List new_table;
 DenseMatrixHandle elc_sponge_locations;
 auto tab_values = get(Parameters::TableValues).toVector();
 bool flip_normal=false;
 if (tab_values.size()==elc_prototyp_map.size() && elc_thickness.size()==elc_prototyp_map.size() && elc_x.size()==elc_prototyp_map.size() && elc_y.size()==elc_prototyp_map.size() && elc_z.size()==elc_prototyp_map.size() && elc_angle_rotation.size()==elc_prototyp_map.size())
 {
  VMesh* scalp_vmesh = scalp->vmesh();
  VField* scalp_vfld = scalp->vfield();
  valid_electrode_definition.resize(elc_prototyp_map.size());
  for (int i=0; i<elc_prototyp_map.size(); i++)
  {
   if (elc_thickness[i]<=0 || IsNan(elc_thickness[i]))
   {
    continue;
   }
   double distance=0;
   bool skip_current_iteration=false;
   VMesh::Node::index_type didx;
   Point elc(elc_x[i],elc_y[i],elc_z[i]),r;
   scalp_vmesh->synchronize(Mesh::NODE_LOCATE_E);
   scalp_vmesh->find_closest_node(distance,r,didx,elc);  /// project GUI (x,y,z) onto scalp and ...
   Vector norm;
   scalp_vmesh->synchronize(Mesh::NORMALS_E);
   scalp_vmesh->get_normal(norm,didx); /// ... get its normal
   //update GUI table normals
   double nx,ny,nz;
   Variable::List new_row;
   boost::tie(new_row,nx,ny,nz)=make_table_row(i,elc_x[i],elc_y[i],elc_z[i],norm.x(),norm.y(),norm.z());
   new_table.push_back(makeVariable("row" + boost::lexical_cast<std::string>(i), new_row));
   if (!(nx==0.0 && ny==0.0 && nz==0.0))
   {
    Vector norm2(nx,ny,nz);
    double dot_product=Dot(norm, norm2);
    norm = Vector(nx,ny,nz);
    if (dot_product<direction_bound)
    {
      flip_normal=true;
    }
   }

   /// move coil prototype to projected location
   /// first, compute the transfer matrices
   std::vector<double> axis;
   axis.push_back(norm[0]);
   axis.push_back(norm[1]);
   axis.push_back(norm[2]);
   double angle=elc_angle_rotation[i];
   DenseMatrixHandle rotation_matrix,rotation_matrix1,rotation_matrix2;
   rotation_matrix1 = make_rotation_matrix(angle, axis);
   if (elc_angle_rotation[i]!=0)
   {
    rotation_matrix2 = make_rotation_matrix_around_axis(angle, axis);
    rotation_matrix = boost::make_shared<DenseMatrix>((*rotation_matrix2) * (*rotation_matrix1));
   }
   FieldHandle prototype = elc_coil_proto[elc_prototyp_map[i]-1];
   FieldInformation fi(prototype);
   if(fi.is_trisurfmesh())
   {
    GetMeshNodesAlgo algo_getfieldnodes;
    DenseMatrixHandle fieldnodes;
    try
    {
     algo_getfieldnodes.run(prototype,fieldnodes);
    }
    catch (...)
    {
     THROW_ALGORITHM_PROCESSING_ERROR("Internal error: could not retrieve positions from assigned prototype ");
    }

    if (fieldnodes->nrows()<=0) // put this to tms as well
    {
     THROW_ALGORITHM_PROCESSING_ERROR("Internal error: could not retrieve positions from assigned prototype ");
    }

    ///second, subtract the mean of the prototyp positions to center it in origin
    double mean_loc_x=0,mean_loc_y=0,mean_loc_z=0;
    for(int j=0; j<fieldnodes->nrows(); j++)
    {
     mean_loc_x+=(*fieldnodes)(j,0);
     mean_loc_y+=(*fieldnodes)(j,1);
     mean_loc_z+=(*fieldnodes)(j,2);
    }
     mean_loc_x/=fieldnodes->nrows();
     mean_loc_y/=fieldnodes->nrows();
     mean_loc_z/=fieldnodes->nrows();

     for(int j=0; j<fieldnodes->nrows(); j++)
     {
      (*fieldnodes)(j,0)-=mean_loc_x;
      (*fieldnodes)(j,1)-=mean_loc_y;
      (*fieldnodes)(j,2)-=mean_loc_z;
     }

     DenseMatrixHandle rotated_positions;

     for(int j=0; j<fieldnodes->nrows(); j++)
     {
      DenseMatrixHandle pos_vec (boost::make_shared<DenseMatrix>(3,1));

      (*pos_vec)(0,0)=(*fieldnodes)(j,0);
      (*pos_vec)(1,0)=(*fieldnodes)(j,1);
      (*pos_vec)(2,0)=(*fieldnodes)(j,2);

      if (angle==0)
       rotated_positions = boost::make_shared<DenseMatrix>((*rotation_matrix1) * (*pos_vec));
	 else
	  rotated_positions = boost::make_shared<DenseMatrix>((*rotation_matrix) * (*pos_vec));

      (*fieldnodes)(j,0)=(*rotated_positions)(0,0)+elc_x[i];
      (*fieldnodes)(j,1)=(*rotated_positions)(1,0)+elc_y[i];
      (*fieldnodes)(j,2)=(*rotated_positions)(2,0)+elc_z[i];
     }

     VMesh* prototype_vmesh = prototype->vmesh();

     FieldInformation fieldinfo("TriSurfMesh", CONSTANTDATA_E, "double"); /// this is the final moved prototype for elc i
     FieldHandle tmp_tdcs_elc = CreateField(fieldinfo);
     VMesh*  tmp_tdcs_elc_vmesh = tmp_tdcs_elc->vmesh();
     VField* tmp_tdcs_elc_vfld  = tmp_tdcs_elc->vfield();

     Point p;
     for (int l=0; l<fieldnodes->nrows(); l++)
     {
      Point p((*fieldnodes)(l,0),(*fieldnodes)(l,1),(*fieldnodes)(l,2));
      tdcs_vmesh->add_point(p);
      tmp_tdcs_elc_vmesh->add_point(p);
     }

     for (VMesh::Elem::index_type l=0; l<prototype_vmesh->num_elems(); l++)
     {
      VMesh::Node::array_type onodes(3);
      prototype_vmesh->get_nodes(onodes, l);
      tmp_tdcs_elc_vmesh->add_elem(onodes);
      onodes[0]+=nr_elc_sponge_triangles;
      onodes[1]+=nr_elc_sponge_triangles;
      onodes[2]+=nr_elc_sponge_triangles;
      tdcs_vmesh->add_elem(onodes);
      field_values.push_back(i);
     }
    nr_elc_sponge_triangles+=fieldnodes->nrows();
    valid_electrode_definition[i]=1;
    num_valid_electrode_definition++;

    tmp_tdcs_elc_vfld->set_all_values(0.0);

    /// since the protoype has to be centered around coordinate origin
    /// it will envelop the scalp/electrode sponge surface at its final location (it is now!)
    /// scalp needs to have data stored on nodes for clipping to prevent having frayed electrode sponge corners
    FieldHandle scalp_linear_data;
    ConvertFieldBasisTypeAlgo convert_field_basis;
    if (!scalp_vfld->is_lineardata())
    {
     using namespace SCIRun::Core::Algorithms::Fields::Parameters;
     {
      convert_field_basis.set_option(OutputType, "Linear");
      convert_field_basis.set(BuildBasisMapping, false);
      convert_field_basis.runImpl(scalp, scalp_linear_data);
     }
    }

    CalculateSignedDistanceFieldAlgo algo_sdf;
    FieldHandle sdf_output;
    if(scalp_vfld->is_lineardata())
     algo_sdf.run(scalp, tmp_tdcs_elc, sdf_output);
       else
         algo_sdf.run(scalp_linear_data, tmp_tdcs_elc, sdf_output); /// assumed that CalculateSignedDistanceFieldAlgo output has always values defined on nodes

    VField* tmp_sdf_vfld  = sdf_output->vfield();
    VMesh*  tmp_sdf_vmsh  = sdf_output->vmesh();
    if (!tmp_sdf_vfld || tmp_sdf_vfld->num_values()<=0)
    {
     std::ostringstream ostr3;
     ostr3 << " Electrode sponge/scalp surface could not be found for " << i+1 << ". electrode. Make sure that prototype encapsulated scalp." << std::endl;
     remark(ostr3.str());
     skip_current_iteration=true;
     continue;/// in that case go to the next electrode -> leave the for loop thats iterating over i
    }

    std::vector<double> tmp_field_bin_values;
    bool found_elc_surf=false;
    tmp_field_bin_values.resize(tmp_sdf_vfld->num_values());
    for (VMesh::Node::index_type l=0; l<tmp_sdf_vfld->num_values(); l++) /// find out which nodes are inside the
    {
     double tmp_sdf_fld_val=std::numeric_limits<double>::quiet_NaN(); /// store binary classification of which scalp nodes are inside prototype and which are outside
     tmp_sdf_vfld->get_value(tmp_sdf_fld_val,l);
     if (tmp_sdf_fld_val <= 0.0)
     {
      tmp_field_bin_values[l]=1.0;
      found_elc_surf=true;
     } else
       tmp_field_bin_values[l]=0.0;
    }
    FieldHandle final_electrode_sponge_surf;

    if(!found_elc_surf)
    {
     std::ostringstream ostr3;
     ostr3 << " Electrode sponge/scalp surface could not be found for " << i+1 << ". electrode (after sdf binarization). Make sure that prototype encapsulates scalp." << std::endl;
     remark(ostr3.str());
     skip_current_iteration=true;
     continue;/// in that case go to the next electrode -> leave the for loop thats iterating over
    }

    using namespace SCIRun::Core::Algorithms::Fields::Parameters;  /// convert the data values (zero's) to elements
    {
     convert_field_basis.set_option(OutputType, "Constant");
     convert_field_basis.set(BuildBasisMapping, false);
     convert_field_basis.runImpl(sdf_output, final_electrode_sponge_surf);
    }
    VField* final_electrode_sponge_surf_fld = final_electrode_sponge_surf->vfield();
    VMesh*  final_electrode_sponge_surf_msh = final_electrode_sponge_surf->vmesh();

    if (!final_electrode_sponge_surf_fld || final_electrode_sponge_surf_fld->num_values()<=0 || !final_electrode_sponge_surf_msh || final_electrode_sponge_surf_msh->num_elems()<=0)
    {
     std::ostringstream ostr3;
     ostr3 << " Electrode sponge/scalp surface could not be found for " << i+1 << ". electrode (conversion to constant data storage). Make sure that prototype encapsulates scalp." << std::endl;
     remark(ostr3.str());
     skip_current_iteration=true;
     continue;/// in that case go to the next electrode -> leave the for loop thats iterating over i
    }

    final_electrode_sponge_surf_fld->set_all_values(0.0); /// Precaution: set data values (defined at elements) to zero
    for (VMesh::Elem::index_type l=0; l<final_electrode_sponge_surf_msh->num_elems(); l++)
    {
      VMesh::Node::array_type onodes(3);
      tmp_sdf_vmsh->get_nodes(onodes, l);
      if (tmp_field_bin_values[onodes[0]]==1.0 && tmp_field_bin_values[onodes[1]]==1.0 && tmp_field_bin_values[onodes[2]]==1.0)
      {
        final_electrode_sponge_surf_fld->set_value(1.0,l);
	found_elc_surf=true;
      }
        else
          final_electrode_sponge_surf_fld->set_value(0.0,l);
    }

    if (!found_elc_surf)
    {
     std::ostringstream ostr3;
     ostr3 << " Electrode sponge/scalp surface could not be found for " << i+1 << ". electrode (conversion to constant data storage). Make sure that prototype encapsulates scalp." << std::endl;
     remark(ostr3.str());
     skip_current_iteration=true;
     continue;/// in that case go to the next electrode -> leave the for loop thats iterating over i
    }
     /// are there multiple not connected scalp surfaces that are inside the prototype
     /// use projected point r (that was projected on scalp surface) to differentiate which surface is the one to use
     /// but first splitbydomain to get the surface with tha value 1.0
     SplitFieldByDomainAlgo algo_splitfieldbydomain;
     algo_splitfieldbydomain.setLogger(getLogger());
     FieldList final_electrode_sponge_surf_domainsplit;
     algo_splitfieldbydomain.set(SplitFieldByDomainAlgo::SortBySize, true);
     algo_splitfieldbydomain.set(SplitFieldByDomainAlgo::SortAscending, false);
     algo_splitfieldbydomain.runImpl(final_electrode_sponge_surf, final_electrode_sponge_surf_domainsplit);
     found_elc_surf=false;
     FieldHandle find_elc_surf;
     VMesh::Elem::index_type c_ind=0;
     for(long l=0;l<final_electrode_sponge_surf_domainsplit.size();l++)
     {
      VField* final_electrode_sponge_surf_domainsplit_fld = final_electrode_sponge_surf_domainsplit[l]->vfield();
      double tmp_val=std::numeric_limits<double>::quiet_NaN();
      final_electrode_sponge_surf_domainsplit_fld->get_value(tmp_val,c_ind);
      if(tmp_val==1.0)
      {
       find_elc_surf=final_electrode_sponge_surf_domainsplit[l];
       found_elc_surf=true;
      }
     }

     if(!found_elc_surf)
     {
       std::ostringstream ostr3;
       ostr3 << " Electrode sponge/scalp surface could not be found for " << i+1 << ". electrode (after domainsplit). Make sure that prototype encapsulates scalp." << std::endl;
       remark(ostr3.str());
       found_elc_surf=false;
       continue;/// in that case go to the next electrode -> leave the for loop thats iterating over i
     }

     SplitFieldByConnectedRegionAlgo algo_splitbyconnectedregion;
     algo_splitbyconnectedregion.set(SplitFieldByConnectedRegionAlgo::SortDomainBySize(), true);
     algo_splitbyconnectedregion.set(SplitFieldByConnectedRegionAlgo::SortAscending(), false);
     std::vector<FieldHandle> result = algo_splitbyconnectedregion.run(find_elc_surf);
     found_elc_surf=false;
     double distance=std::numeric_limits<double>::quiet_NaN();
     VMesh::Node::index_type didx;
     for(int l=0;l<result.size();l++)
     {
      FieldHandle tmp_fld = result[l];
      VMesh*  tmp_fld_msh  = tmp_fld->vmesh();

      tmp_fld_msh->synchronize(Mesh::NODE_LOCATE_E);
      tmp_fld_msh->find_closest_node(distance,p,didx,r);

      if (distance==0)
      {
       found_elc_surf=true;
       scalp_vmesh->synchronize(Mesh::NODE_LOCATE_E);
       Point q;
       Vector norm;

       /// create scalp/electrode sponge triangle nodes
       for (VMesh::Node::index_type k=0; k<tmp_fld_msh->num_nodes(); k++)
       {
        tmp_fld_msh->get_center(p,k);
	output_vmesh->add_point(p);
       }
       /// create electrode sponge top triangle nodes
       long count_pts=0;
       for (VMesh::Node::index_type k=0; k<tmp_fld_msh->num_nodes(); k++)
       {
        tmp_fld_msh->get_center(p,k);
	scalp_vmesh->find_closest_node(distance,q,didx,p);
	if (distance==0)
	{
	 scalp_vmesh->synchronize(Mesh::NORMALS_E);
	 scalp_vmesh->get_normal(norm,didx);
	 double x=norm.x(),y=norm.y(),z=norm.z();
	 double normal_mag=sqrt(x*x+y*y+z*z);
	 x/=normal_mag;
         y/=normal_mag;
         z/=normal_mag;
	 Point tmp_pt;
	 if (!flip_normal)
	   tmp_pt=Point(q.x()+x*elc_thickness[i],q.y()+y*elc_thickness[i],q.z()+z*elc_thickness[i]);
	     else
	      tmp_pt=Point(q.x()-x*elc_thickness[i],q.y()-y*elc_thickness[i],q.z()-z*elc_thickness[i]);

	 output_vmesh->add_point(tmp_pt);
	 count_pts++;
	}
       }
       int offset=nr_elc_sponge_triangles_on_scalp;
       for (int iter_tmp=0;iter_tmp<2;iter_tmp++)
       {
        for (VMesh::Elem::index_type k=0; k<tmp_fld_msh->num_elems(); k++)
        {
         VMesh::Node::array_type onodes(3);
         tmp_fld_msh->get_nodes(onodes, k);
         onodes[0]+=offset;
         onodes[1]+=offset;
         onodes[2]+=offset;
         output_vmesh->add_elem(onodes);
         field_values_elc_on_scalp.push_back(i);
        }
	offset+=tmp_fld_msh->num_nodes();
       }

       MatrixHandle mapping;
       GetFieldBoundaryAlgo algo_getfldbnd;
       FieldHandle boundary;
       algo_getfldbnd.run(tmp_fld, boundary, mapping);
       VMesh* boundary_msh  = boundary->vmesh();
       /// connect electrode sponge surfaces
       if (boundary->vmesh()->is_curvemesh())
       {
         tmp_fld_msh->synchronize(Mesh::NODE_LOCATE_E);
	VMesh::Edge::iterator meshEdgeIter;
        VMesh::Edge::iterator meshEdgeEnd;
        VMesh::Node::array_type nodesFromEdge(2);
	boundary_msh->end(meshEdgeEnd);
	for (boundary_msh->begin(meshEdgeIter); meshEdgeIter != meshEdgeEnd; ++meshEdgeIter)
        {
          VMesh::Edge::index_type edgeID = *meshEdgeIter;
	  boundary_msh->get_nodes(nodesFromEdge, edgeID);
	  Point p0, p1;
	  boundary_msh->get_point(p0, nodesFromEdge[0]);
          boundary_msh->get_point(p1, nodesFromEdge[1]);
	  VMesh::Node::index_type idx1;
	  tmp_fld_msh->find_closest_node(distance,r,idx1,p0);
	  if (distance!=0)
	  {
	   std::ostringstream ostr3;
           ostr3 << " Electrode sponge/scalp surface could not be found for " << i+1 << ". electrode (could not find scalp bnd fragment node in scalp fragment). Make sure that prototype encapsulates scalp." << std::endl;
           remark(ostr3.str());
           break;
	  }
	  VMesh::Node::index_type idx2;
	  tmp_fld_msh->find_closest_node(distance,r,idx2,p1);
	  if (distance!=0)
	  {
	   std::ostringstream ostr3;
           ostr3 << " Electrode sponge/scalp surface could not be found for " << i+1 << ". electrode (could not find scalp bnd fragment node in scalp fragment). Make sure that prototype encapsulates scalp." << std::endl;
           remark(ostr3.str());
           break;
	  }
	  long i1=(long)idx1, i2=(long)idx2;
	  long i3=i1+tmp_fld_msh->num_nodes(), i4=i2+tmp_fld_msh->num_nodes();
	  VMesh::Node::array_type onodes(3);
	  onodes[0]=i3+nr_elc_sponge_triangles_on_scalp;
	  onodes[1]=i1+nr_elc_sponge_triangles_on_scalp;
	  onodes[2]=i4+nr_elc_sponge_triangles_on_scalp;
	  output_vmesh->add_elem(onodes);
	  field_values_elc_on_scalp.push_back(i);
	  onodes[0]=i4+nr_elc_sponge_triangles_on_scalp;
	  onodes[1]=i1+nr_elc_sponge_triangles_on_scalp;
	  onodes[2]=i2+nr_elc_sponge_triangles_on_scalp;
	  output_vmesh->add_elem(onodes);
	  field_values_elc_on_scalp.push_back(i);
        }

        nr_elc_sponge_triangles_on_scalp+=tmp_fld_msh->num_nodes()+count_pts;
       } else
       {
         std::ostringstream ostr3;
         ostr3 << " Trying to get boundary for scalp peace usind to create electrode " << i+1 << ". Make sure that prototype encapsulates scalp." << std::endl;
         remark(ostr3.str());
         continue;
       }

      }
     }

   } else
   {
    std::ostringstream ostr3;
    ostr3 << " tDCS electrode definition for " << i+1 << ". row needs be a point cloud (with defined scalar or vectors data)." << std::endl;
    remark(ostr3.str());
    skip_current_iteration=true;
    continue;
   }

  tdcs_vfld->resize_values();
  tdcs_vfld->set_values(field_values);
  output_vfld->resize_values();
  output_vfld->set_values(field_values_elc_on_scalp);
  elc_sponge_locations = boost::make_shared<DenseMatrix>(DenseMatrix::Zero(num_valid_electrode_definition,4));
  int count=0;
  for(int j=0;j<valid_electrode_definition.size();j++)
  {
   if(valid_electrode_definition[j]==1)
   {
    if(count<elc_sponge_locations->nrows())
    {
     (*elc_sponge_locations)(count,0)=elc_x[i];
     (*elc_sponge_locations)(count,1)=elc_y[i];
     (*elc_sponge_locations)(count,2)=elc_z[i];
     (*elc_sponge_locations)(count,3)=elc_thickness[i];
     count++;
    }
   }
  }
 }
 } else
 {
  std::ostringstream ostr3;
  ostr3 << " Internal error: tDCS electrode could not be generated. " << std::endl;
  remark(ostr3.str());
 }

 VariableHandle table2(new Variable(Name("Table"), new_table));

 return boost::make_tuple(elc_sponge_locations, electrode_field, output, table2);
}

boost::tuple<VariableHandle, DenseMatrixHandle, FieldHandle, FieldHandle, FieldHandle> ElectrodeCoilSetupAlgorithm::run(const FieldHandle scalp, const DenseMatrixHandle locations, const std::vector<FieldHandle>& elc_coil_proto) const
{
 VariableHandle table_output = fill_table(scalp, locations, elc_coil_proto);
 DenseMatrixHandle elc_sponge_locations;
 FieldHandle electrodes_field, coils_field, final_electrodes_field;
 auto table = table_output->toVector();

 /// check GUI inputs:
 /// 1) Is there any valid row in the GUI table, so at least one row where both ComboBoxes are set
 ///
 std::vector<double> elc_prototyp_map;
 std::vector<double> elc_thickness;
 std::vector<double> elc_angle_rotation;
 std::vector<double> elc_x;
 std::vector<double> elc_y;
 std::vector<double> elc_z;

 std::vector<double> coil_prototyp_map;
 std::vector<double> coil_angle_rotation;
 std::vector<double> coil_x;
 std::vector<double> coil_y;
 std::vector<double> coil_z;
 std::vector<double> coil_nx;
 std::vector<double> coil_ny;
 std::vector<double> coil_nz;

 /// The rest of the run function checks the validity of the GUI inputs. If there are not valid (="???") it tries to use the prototype inputs and if valid it calls functions make_tdcs_electrodes or make_tms
 for(int i=0;i<table.size();i++)
 {
  auto row = (table[i]).toVector();

  /// the c* integer variables refine the column vales (1,2,3,...,10) for the current row
  int c1=std::numeric_limits<double>::quiet_NaN(),c2=std::numeric_limits<double>::quiet_NaN();
  double c3=std::numeric_limits<double>::quiet_NaN(),c4=std::numeric_limits<double>::quiet_NaN(),c5=std::numeric_limits<double>::quiet_NaN(),
  c6=std::numeric_limits<double>::quiet_NaN(),c7=std::numeric_limits<double>::quiet_NaN(),c8=std::numeric_limits<double>::quiet_NaN(),
  c9=std::numeric_limits<double>::quiet_NaN(),c10=std::numeric_limits<double>::quiet_NaN();

  bool valid_position=true, valid_normal=true, row_valid=true;

  try
  {
   c1 = lexical_cast<int>(row[0].toString());
  } catch(bad_lexical_cast &)
  {
   c1=std::numeric_limits<double>::quiet_NaN();
   row_valid=false;
  }

  try
  {
   c2 = lexical_cast<int>(row[1].toString());
  } catch(bad_lexical_cast &)
  {
   c2=std::numeric_limits<double>::quiet_NaN();
   row_valid=false;
  }

  auto str_x = row[2].toString();
  if (str_x.compare("???")==0)
  {
    valid_position=false;
  } else
  {
   try
   {
     c3 = lexical_cast<double>(str_x);
   } catch(bad_lexical_cast &)
   {
     c3=std::numeric_limits<double>::quiet_NaN();
     valid_position=false;
   }
  }

  auto str_y = row[3].toString();
  if (str_y.compare("???")==0)
  {
   valid_position=false;
  } else
  {
   try  ///get the electrode thickness from GUI
   {
     c4 = lexical_cast<double>(str_y);
   } catch(bad_lexical_cast &)
   {
     c4=std::numeric_limits<double>::quiet_NaN();
     valid_position=false;
   }
  }

  auto str_z = row[4].toString();
  if (str_z.compare("???")==0)
  {
     valid_position=false;
  }  else
  {
   try  ///get the electrode thickness from GUI
   {
     c5 = lexical_cast<double>(str_z);
   } catch(bad_lexical_cast &)
   {
     c5=std::numeric_limits<double>::quiet_NaN();
     valid_position=false;
   }
  }

  c6=-1;
  auto angle = row[5].toString();
  if (angle.compare("???")!=0)
  {
   try
   {
    c6 = lexical_cast<double>(row[5].toString());
   } catch(bad_lexical_cast &)
   {
    c6=std::numeric_limits<double>::quiet_NaN();
   }
  } else
    c6=std::numeric_limits<double>::quiet_NaN();

  if (IsNan(c6))
  {
   c6=0;
  }

  if(row_valid && c1>0 && c2>0)   ///both combo boxes are set up, so this could be a valid row but first check if ...
                     /// its a tDCS electrode and if so if the thickness is provided in the GUI
		     /// or if its a TMS coil check if the prototype has normals - if so lets put the normal in the GUI
  {

     FieldHandle prototyp = elc_coil_proto[c1-1];

     if (!prototyp)
     {
       std::ostringstream ostr1;
       ostr1 << "Module input " << (c1+2) << " seems to be empty" << std::endl;
       THROW_ALGORITHM_PROCESSING_ERROR(ostr1.str());
     }

     GetFieldDataAlgo algo_getfielddata;
     DenseMatrixHandle fielddata;
     try
     {
      fielddata  = algo_getfielddata.run(prototyp);
     }
     catch (...)
     {

     }

     GetMeshNodesAlgo algo_getfieldnodes;
     DenseMatrixHandle fieldnodes;
     try
     {
      algo_getfieldnodes.run(prototyp,fieldnodes);
     }
     catch (...)
     {

     }

     if ( (fieldnodes->nrows()!=fielddata->nrows()) )
     {
      if( !((fieldnodes->nrows()==3 || fieldnodes->nrows()==1) && (fielddata->nrows()==3 || fielddata->nrows()==1)) )
      {
       std::ostringstream ostr_;
       ostr_ << "Module input " << (c1+2) << " (assigned tms coil prototype) does not contain same number of nodes as data values (linear basis)." << std::endl;
       THROW_ALGORITHM_PROCESSING_ERROR(ostr_.str());
      }
     }

     /// get Coil normal from GUI
     /// get NX
     try
     {
       c7 = lexical_cast<double>(row[6].toString());
     } catch(bad_lexical_cast &)
     {
      c7=std::numeric_limits<double>::quiet_NaN();
      valid_normal=false;
     }

     /// get NY
     try
     {
       c8 = lexical_cast<double>(row[7].toString());
     } catch(bad_lexical_cast &)
     {
      c8=std::numeric_limits<double>::quiet_NaN();
      valid_normal=false;
     }

     /// get NZ
     try
     {
       c9 = lexical_cast<double>(row[8].toString());
     } catch(bad_lexical_cast &)
     {
      c9=std::numeric_limits<double>::quiet_NaN();
      valid_normal=false;
     }

     /// get electrode thickness
     try
     {
       c10 = lexical_cast<double>(row[9].toString());
     } catch(bad_lexical_cast &)
     {
      c10=std::numeric_limits<double>::quiet_NaN();
     }

     if (!valid_normal && fielddata->ncols()==3 && fielddata->nrows()>=1) /// use FIRST(!) mag. dipole direction to infer coil orientation. Note that the determined direction could be anti parallel
     {
       double norm=sqrt((*fielddata)(0,0)*(*fielddata)(0,0)+(*fielddata)(0,1)*(*fielddata)(0,1)+(*fielddata)(0,2)*(*fielddata)(0,2)); /// normalize mag. dipole to get coil direction estimate
       c7=(*fielddata)(0,0)/norm;
       c8=(*fielddata)(0,1)/norm;
       c9=(*fielddata)(0,2)/norm;
       valid_normal=true;
     }

     if(c7==0 && c8==0 && c9==0)
     {
      std::ostringstream ostr3;
      ostr3 << " The TMS coil defined in table row " << i+1 << " could not get any valid normal information, found normal contained only zeros (0,0,0). " << std::endl;
      remark(ostr3.str());
      valid_normal=false;
     }

     if (!valid_position && fieldnodes->nrows()>=1 && fieldnodes->ncols()==3)
     {
       double x=0,y=0,z=0;
       for(int j=0;j<fieldnodes->nrows();j++)
        {
	 x+=(*fieldnodes)(j,0);
	 y+=(*fieldnodes)(j,1);
	 z+=(*fieldnodes)(j,2);
	}
       x/=fieldnodes->nrows();
       y/=fieldnodes->nrows();
       z/=fieldnodes->nrows();
       c3=x;
       c4=y;
       c5=z;
       valid_position=true;
     }

    if (c2==tDCS_stim_type) /// tDCS?
    {
     if(valid_position)
     {
      elc_prototyp_map.push_back(c1);
      elc_angle_rotation.push_back(c6);
      elc_x.push_back(c3);
      elc_y.push_back(c4);
      elc_z.push_back(c5);
      elc_thickness.push_back(c10);;
     }

    } else

    if (c2==TMS_stim_type) /// TMS?
    {
     if(valid_normal && valid_position)
     {
      coil_prototyp_map.push_back(c1);
      coil_angle_rotation.push_back(c6);
      coil_x.push_back(c3);
      coil_y.push_back(c4);
      coil_z.push_back(c5);
      coil_nx.push_back(c7);
      coil_ny.push_back(c8);
      coil_nz.push_back(c9);
     } else
        {
	  std::ostringstream ostr3;
	  ostr3 << " The TMS coil defined in table row " << i+1 << " has no normal defined (NX,NY,NZ). Further, no normal could be taken from the linked prototyp field data. " << std::endl;
	  remark(ostr3.str());
	}
    }

  }
 }

  bool valid_tdcs=false, valid_tms=false;
  int t1=elc_angle_rotation.size();
  int t2=elc_thickness.size();
  int t3=elc_x.size();
  int t4=elc_y.size();
  int t5=elc_z.size();
  int t14=elc_coil_proto.size();

  if (t1==t2 && t1==t3 && t1==t4 && t1==t5 && t14>0 && t1>0)
  {
    boost::tie(elc_sponge_locations, electrodes_field, final_electrodes_field, table_output) = make_tdcs_electrodes(scalp, elc_coil_proto, elc_prototyp_map, elc_x, elc_y, elc_z, elc_angle_rotation, elc_thickness, table_output);
    valid_tdcs=true;
  }

  int t6=coil_prototyp_map.size();
  int t7=coil_angle_rotation.size();
  int t8=coil_x.size();
  int t9=coil_y.size();
  int t10=coil_z.size();
  int t11=coil_nx.size();
  int t12=coil_ny.size();
  int t13=coil_nz.size();

  if (t6==t7 && t6==t8 && t6==t9 && t6==t10 && t6==t11 && t6==t12 && t6==t13 && t14>0 && t6>0)
  {
   coils_field = make_tms(scalp, elc_coil_proto, coil_prototyp_map, coil_x, coil_y, coil_z, coil_angle_rotation, coil_nx, coil_ny, coil_nz);
   valid_tms =true;
  }

  if (!valid_tdcs && !valid_tms)
  {
   std::ostringstream ostr0;
   ostr0 << " Not a single table row contains valid information. " << std::endl;
   remark(ostr0.str());
  }

 return boost::make_tuple(table_output, elc_sponge_locations, electrodes_field, final_electrodes_field, coils_field);
}

AlgorithmOutput ElectrodeCoilSetupAlgorithm::run_generic(const AlgorithmInput& input) const
{
  auto scalp = input.get<Field>(SCALP_SURF);
  auto locations = input.get<DenseMatrix>(LOCATIONS);
  auto elc_coil_proto = input.getList<Field>(ELECTRODECOILPROTOTYPES);

  if (!scalp)
  {
    THROW_ALGORITHM_PROCESSING_ERROR(" SCALP_SURF (first input) field empty. ");
  }

  if (!locations)
  {
    THROW_ALGORITHM_PROCESSING_ERROR(" LOCATIONS (second input) matrix empty. ");
  }

  if (!(elc_coil_proto.size()>=1))
  {
    THROW_ALGORITHM_PROCESSING_ERROR(" At least one prototypical coil (POINTMESH) or electrode (TRISURFMESH) definition as a field input must be provided.");
  }

  if(locations->ncols()!=3)
  {
    THROW_ALGORITHM_PROCESSING_ERROR(" Locations (second module input) needs to be a dense matrix input with dimensions lx3 (l being > 0). ");
  }

  VariableHandle table;
  DenseMatrixHandle elc_sponge_loc_avr;
  FieldHandle coils_field,electrodes_field, final_electrode_field;
  boost::tie(table, elc_sponge_loc_avr, electrodes_field, final_electrode_field, coils_field) = run(scalp, locations, elc_coil_proto);

  AlgorithmOutput output;
  output[ELECTRODE_SPONGE_LOCATION_AVR] = elc_sponge_loc_avr;
  output[MOVED_ELECTRODES_FIELD] = electrodes_field;
  output[FINAL_ELECTRODES_FIELD] = final_electrode_field;
  output[COILS_FIELD] = coils_field;
  output.setAdditionalAlgoOutput(table);

  return output;
}


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     interaction.cpp
 * \author   Collin Johnson
 *
 * Definition of find_interactions.
 */

#include "mpepc/evaluation/interaction.h"
#include "hssh/local_topological/area.h"
#include "hssh/local_topological/local_topo_map.h"
#include "math/interpolation.h"
#include "mpepc/evaluation/mpepc_log.h"
#include "mpepc/social/social_norm_utils.h"
#include <boost/range/iterator_range.hpp>
#include <tuple>

namespace vulcan
{
namespace mpepc
{

std::tuple<pose_t, velocity_t>
  motion_state_at_time(int64_t time, MPEPCLog::motion_state_iterator begin, MPEPCLog::motion_state_iterator end);
interaction_t create_interaction(const pose_t& pose,
                                 const velocity_t& velocity,
                                 const tracker::DynamicObjectCollection& objects,
                                 const hssh::LocalTopoMap& topoMap,
                                 int numLateralBins,
                                 double maxDistance,
                                 double ignoreConeAngle);
bool is_interacting_with_object(const pose_t& pose,
                                const tracker::DynamicObject& object,
                                double maxDistance,
                                double ignoreConeAngle);

std::vector<interaction_t> find_interactions(MPEPCLog& log,
                                             const hssh::LocalTopoMap& topoMap,
                                             int numLateralBins,
                                             double maxDistance,
                                             double ignoreConeAngle)
{
    const int64_t kChunkDurationUs = 5000000;

    std::vector<interaction_t> interactions;

    // Process the log in chunks to ensure we don't run out of memory trying to load the big planning logs
    int64_t startTimeUs = 0;

    pose_t pose;
    velocity_t velocity;

    while (startTimeUs < log.durationUs()) {
        log.loadTimeRange(startTimeUs, startTimeUs + (2 * kChunkDurationUs));

        // The objects are detected at a lower rate than the poses, so for each object collection, create an interaction
        // using the interpolated pose
        for (auto& objs : boost::make_iterator_range(log.beginObjects(startTimeUs),
                                                     log.endObjects(startTimeUs + kChunkDurationUs))) {
            std::tie(pose, velocity) =
              motion_state_at_time(objs.timestamp(), log.beginMotionState(), log.endMotionState());
            interactions.push_back(
              create_interaction(pose, velocity, objs, topoMap, numLateralBins, maxDistance, ignoreConeAngle));
        }

        startTimeUs += kChunkDurationUs;
    }

    return interactions;
}

std::tuple<pose_t, velocity_t>
  motion_state_at_time(int64_t time, MPEPCLog::motion_state_iterator begin, MPEPCLog::motion_state_iterator end)
{
    // If there's nothing to interpolate, then return one of the ends
    assert(begin != end);

    if (time <= begin->timestamp) {
        return std::make_tuple(begin->pose, begin->velocity);
    } else if (time >= (end - 1)->timestamp) {
        return std::make_tuple((end - 1)->pose, (end - 1)->velocity);
    }

    pose_t pose;
    velocity_t velocity;

    pose.timestamp = time;
    velocity.timestamp = time;

    for (auto next = begin + 1; next < end; ++begin, ++next) {
        // Find the range to interpolate between.
        if ((begin->timestamp <= time) && (time <= next->timestamp)) {
            // Ensure the timestamps are converted, since MPEPCLog doesn't deep-change the time
            pose_t startPose = begin->pose;
            startPose.timestamp = begin->timestamp;

            pose_t nextPose = next->pose;
            nextPose.timestamp = next->timestamp;

            pose = interpolate_pose(startPose, nextPose, time);

            double scale = static_cast<double>(time - begin->timestamp) / (next->timestamp - begin->timestamp);
            velocity.linear = math::linear_interpolation(begin->velocity.linear, next->velocity.linear, scale);
            velocity.angular = math::linear_interpolation(begin->velocity.angular, next->velocity.angular, scale);
        }
    }

    return std::make_tuple(pose, velocity);
}

interaction_t create_interaction(const pose_t& pose,
                                 const velocity_t& velocity,
                                 const tracker::DynamicObjectCollection& objects,
                                 const hssh::LocalTopoMap& topoMap,
                                 int numLateralBins,
                                 double maxDistance,
                                 double ignoreConeAngle)
{
    interaction_t interaction;
    interaction.timestamp = objects.timestamp();
    interaction.velocity = velocity;
    interaction.pose = pose;

    for (auto obj : objects) {
        if (is_interacting_with_object(pose, *obj, maxDistance, ignoreConeAngle)) {
            if (auto agent = convert_to_topo_agent(obj, topoMap)) {
                interaction.agents.push_back(*agent);
            }
        }
    }

    auto robotAgent = create_agent_for_robot(motion_state_t(pose, velocity), topoMap);

    if (topoMap.pathSegmentWithId(robotAgent.areaId)) {
        interaction.pathSituation = PathSituation(robotAgent, interaction.agents, numLateralBins, topoMap);
    } else if (is_about_to_transition(robotAgent, topoMap)) {
        interaction.placeSituation = PlaceSituation(robotAgent, interaction.agents, topoMap);
    }

    interaction.robotAgent = robotAgent;
    interaction.areaId = robotAgent.areaId;

    return interaction;
}

bool is_interacting_with_object(const pose_t& pose,
                                const tracker::DynamicObject& object,
                                double maxDistance,
                                double ignoreConeAngle)
{
    bool is_close_enough = distance_between_points(pose.toPoint(), object.position()) < maxDistance;
    bool is_in_cone = angle_diff_abs(angle_to_point(pose.toPoint(), object.position()), pose.theta) < ignoreConeAngle;

    return is_close_enough && is_in_cone;
}

}   // namespace mpepc
}   // namespace vulcan


// Copyright (c) 2009-2013 The Dacrs developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "crypter.h"

//#include "script.h"
#include "util.h"
#include <string>
#include <vector>
#include <boost/foreach.hpp>
#include <openssl/aes.h>
#include <openssl/evp.h>

bool CCrypter::SetKeyFromPassphrase(const SecureString& strKeyData, const vector<unsigned char>& vchSalt,
		const unsigned int unRounds, const unsigned int unDerivationMethod) {
	if (unRounds < 1 || vchSalt.size() != g_kWalletCryptoSaltSize) {
		return false;
	}
	int i = 0;
	if (unDerivationMethod == 0) {
		i = EVP_BytesToKey(EVP_aes_256_cbc(), EVP_sha512(), &vchSalt[0], (unsigned char *) &strKeyData[0],
						strKeyData.size(), unRounds, m_chKey, m_chIV);
	}
	if (i != (int) g_kWalletCryptoKeySize) {
		OPENSSL_cleanse(m_chKey, sizeof(m_chKey));
		OPENSSL_cleanse(m_chIV, sizeof(m_chIV));
		return false;
	}
	m_bKeySet = true;

	return true;
}

bool CCrypter::SetKey(const CKeyingMaterial& vchNewKey, const vector<unsigned char>& vchNewIV) {
	if (vchNewKey.size() != g_kWalletCryptoKeySize || vchNewIV.size() != g_kWalletCryptoKeySize) {
		return false;
	}
	memcpy(&m_chKey[0], &vchNewKey[0], sizeof m_chKey);
	memcpy(&m_chIV[0], &vchNewIV[0], sizeof m_chIV);
	m_bKeySet = true;

	return true;
}

bool CCrypter::Encrypt(const CKeyingMaterial& vchPlaintext, vector<unsigned char> &vchCiphertext) {
	if (!m_bKeySet) {
		return false;
	}
	// max ciphertext len for a n bytes of plaintext is
	// n + AES_BLOCK_SIZE - 1 bytes
	int nLen = vchPlaintext.size();
	int nCLen = nLen + AES_BLOCK_SIZE, nFLen = 0;
	vchCiphertext = vector<unsigned char>(nCLen);

	EVP_CIPHER_CTX sEvpCipherCtx;

	bool bOk = true;

	EVP_CIPHER_CTX_init(&sEvpCipherCtx);
	if (bOk) {
		bOk = EVP_EncryptInit_ex(&sEvpCipherCtx, EVP_aes_256_cbc(), NULL, m_chKey, m_chIV);
	}
	if (bOk) {
		bOk = EVP_EncryptUpdate(&sEvpCipherCtx, &vchCiphertext[0], &nCLen, &vchPlaintext[0], nLen);
	}
	if (bOk) {
		bOk = EVP_EncryptFinal_ex(&sEvpCipherCtx, (&vchCiphertext[0]) + nCLen, &nFLen);
	}

	EVP_CIPHER_CTX_cleanup(&sEvpCipherCtx);

	if (!bOk) {
		return false;
	}
	vchCiphertext.resize(nCLen + nFLen);

	return true;
}

bool CCrypter::Decrypt(const vector<unsigned char>& vchCiphertext, CKeyingMaterial& vchPlaintext) {
	if (!m_bKeySet) {
		return false;
	}
	// plaintext will always be equal to or lesser than length of ciphertext
	int nLen = vchCiphertext.size();
	int nPLen = nLen, nFLen = 0;

	vchPlaintext = CKeyingMaterial(nPLen);
	EVP_CIPHER_CTX sEvpCipherCtx;
	bool bOk = true;

	EVP_CIPHER_CTX_init(&sEvpCipherCtx);
	if (bOk) {
		bOk = EVP_DecryptInit_ex(&sEvpCipherCtx, EVP_aes_256_cbc(), NULL, m_chKey, m_chIV);
	}
	if (bOk) {
		bOk = EVP_DecryptUpdate(&sEvpCipherCtx, &vchPlaintext[0], &nPLen, &vchCiphertext[0], nLen);
	}
	if (bOk) {
		bOk = EVP_DecryptFinal_ex(&sEvpCipherCtx, (&vchPlaintext[0]) + nPLen, &nFLen);
	}

	EVP_CIPHER_CTX_cleanup(&sEvpCipherCtx);

	if (!bOk) {
		return false;
	}
	vchPlaintext.resize(nPLen + nFLen);

	return true;
}

bool EncryptSecret(const CKeyingMaterial& vMasterKey, const CKeyingMaterial &vPlaintext, const uint256& cIV,
		vector<unsigned char> &vchCiphertext) {
	CCrypter cKeyCrypter;
	vector<unsigned char> vchIV(g_kWalletCryptoKeySize);
	memcpy(&vchIV[0], &cIV, g_kWalletCryptoKeySize);
	if (!cKeyCrypter.SetKey(vMasterKey, vchIV)) {
		return false;
	}

	return cKeyCrypter.Encrypt(*((const CKeyingMaterial*) &vPlaintext), vchCiphertext);
}

bool DecryptSecret(const CKeyingMaterial& vMasterKey, const vector<unsigned char>& vchCiphertext, const uint256& cIV,
		CKeyingMaterial& vPlaintext) {
	CCrypter cKeyCrypter;
	vector<unsigned char> vchIV(g_kWalletCryptoKeySize);
	memcpy(&vchIV[0], &cIV, g_kWalletCryptoKeySize);
	if (!cKeyCrypter.SetKey(vMasterKey, vchIV)) {
		return false;
	}

	return cKeyCrypter.Decrypt(vchCiphertext, *((CKeyingMaterial*) &vPlaintext));
}

bool CCryptoKeyStore::SetCrypted() {
	LOCK(cs_KeyStore);
	if (m_bUseCrypto) {
		return true;
	}
	if (IsContainMainKey()) {
		return false;
	}
	m_bUseCrypto = true;

	return true;
}

bool CCryptoKeyStore::Lock() {
	if (!SetCrypted()) {
		return false;
	}
	{
		LOCK(cs_KeyStore);
		m_vMasterKey.clear();
	}
	g_pUIInterface->NotifyMessage("Lock");
	NotifyStatusChanged(this);

	return true;
}

bool CCryptoKeyStore::Unlock(const CKeyingMaterial& vMasterKeyIn) {
	{
		LOCK(cs_KeyStore);
		if (!SetCrypted()) {
			return false;
		}
		CryptedKeyMap::const_iterator mi = m_mapCryptedKeys.begin();
		for (; mi != m_mapCryptedKeys.end(); ++mi) {
			const CPubKey &cPubKey = (*mi).second.first;
			const vector<unsigned char> &vchCryptedSecret = (*mi).second.second;
			CKeyingMaterial vSecret;
			if (!DecryptSecret(vMasterKeyIn, vchCryptedSecret, cPubKey.GetHash(), vSecret)) {
				return false;
			}
			if (vSecret.size() != 32) {
				return false;
			}
			CKey cKey;
			cKey.Set(vSecret.begin(), vSecret.end(), cPubKey.IsCompressed());
			if (cKey.GetPubKey() == cPubKey) {
				break;
			}

			return false;
		}
		m_vMasterKey = vMasterKeyIn;
	}
	g_pUIInterface->NotifyMessage("UnLock");
	NotifyStatusChanged(this);

	return true;
}

bool CCryptoKeyStore::AddKeyCombi(const CKeyID & cKeyId, const CKeyCombi &cKeyCombi) {
	{
		LOCK(cs_KeyStore);

		if (!IsCrypted()) {
			return CBasicKeyStore::AddKeyCombi(cKeyId, cKeyCombi);
		}
		if (IsLocked()) {
			return false;
		}
		CKey cMainKey;
		cKeyCombi.GetCKey(cMainKey, false);
		CKeyCombi newkeyCombi = cKeyCombi;
		newkeyCombi.CleanMainKey();
		CBasicKeyStore::AddKeyCombi(cKeyId, cKeyCombi);

		vector<unsigned char> vchCryptedSecret;
		CKeyingMaterial vchSecret(cMainKey.begin(), cMainKey.end());
		CPubKey cPubKey;
		cPubKey = cMainKey.GetPubKey();
		if (!EncryptSecret(m_vMasterKey, vchSecret, cPubKey.GetHash(), vchCryptedSecret)) {
			return false;
		}
		if (!AddCryptedKey(cPubKey, vchCryptedSecret)) {
			return false;
		}
	}

	return true;
}

bool CCryptoKeyStore::AddCryptedKey(const CPubKey &cPubKey, const vector<unsigned char> &vchCryptedSecret) {
	{
		LOCK(cs_KeyStore);
		if (!SetCrypted()) {
			return false;
		}
		m_mapCryptedKeys[cPubKey.GetKeyID()] = make_pair(cPubKey, vchCryptedSecret);
	}

	return true;
}

bool CCryptoKeyStore::GetKey(const CKeyID &cAddress, CKey& ckeyOut, bool bIsMine) const {
	{
		LOCK(cs_KeyStore);
		if (bIsMine) {
			return CBasicKeyStore::GetKey(cAddress, ckeyOut, bIsMine);
		} else {
			if (!IsCrypted()) {
				return CBasicKeyStore::GetKey(cAddress, ckeyOut);
			}
			CryptedKeyMap::const_iterator mi = m_mapCryptedKeys.find(cAddress);
			if (mi != m_mapCryptedKeys.end()) {
				const CPubKey &cPubKey = (*mi).second.first;
				const vector<unsigned char> &vchCryptedSecret = (*mi).second.second;
				CKeyingMaterial vSecret;
				if (!DecryptSecret(m_vMasterKey, vchCryptedSecret, cPubKey.GetHash(), vSecret)) {
					return false;
				}
				if (vSecret.size() != 32) {
					return false;
				}
				ckeyOut.Set(vSecret.begin(), vSecret.end(), cPubKey.IsCompressed());
				return true;
			}
		}
	}

	return false;
}

bool CCryptoKeyStore::GetPubKey(const CKeyID &cAddress, CPubKey& cPubKeyOut, bool bIsMine) const {
	{
		LOCK(cs_KeyStore);
		if (bIsMine) {
			return CKeyStore::GetPubKey(cAddress, cPubKeyOut, bIsMine);
		} else {
			if (!IsCrypted()) {
				return CKeyStore::GetPubKey(cAddress, cPubKeyOut, bIsMine);
			}
			CryptedKeyMap::const_iterator mi = m_mapCryptedKeys.find(cAddress);
			if (mi != m_mapCryptedKeys.end()) {
				cPubKeyOut = (*mi).second.first;
				return true;
			}
		}
	}

	return false;
}

bool CCryptoKeyStore::GetKeyCombi(const CKeyID & cAddress, CKeyCombi & cKeyCombiOut) const {
	CBasicKeyStore::GetKeyCombi(cAddress, cKeyCombiOut);
	if (!IsCrypted()) {
		return true;
	}
	CKey cKeyOut;
	if (!IsLocked()) {
		if (!GetKey(cAddress, cKeyOut)) {
			return false;
		}
		cKeyCombiOut.SetMainKey(cKeyOut);
	}

	return true;
}

bool CCryptoKeyStore::EncryptKeys(CKeyingMaterial& vMasterKeyIn) {
	{
		LOCK(cs_KeyStore);
		if (!m_mapCryptedKeys.empty() || IsCrypted()) {
			return false;
		}
		m_bUseCrypto = true;
		for (auto& mKey : mapKeys) {
			CKey cMainKey;
			mKey.second.GetCKey(cMainKey, false);
			CPubKey cPubKey = cMainKey.GetPubKey();
			CKeyingMaterial vSecret(cMainKey.begin(), cMainKey.end());
			vector<unsigned char> vchCryptedSecret;
			if (!EncryptSecret(vMasterKeyIn, vSecret, cPubKey.GetHash(), vchCryptedSecret)) {
				return false;
			}
			if (!AddCryptedKey(cPubKey, vchCryptedSecret)) {
				return false;
			}

			mKey.second.CleanMainKey();
		}
	}

	return true;
}