// Copyright (c) 2017 Graphcore Ltd. All rights reserved.
#include "poplibs_support/logging.hpp"
#include <boost/multi_array.hpp>
#include <cassert>
#include <poplibs_test/GeneralMatrixAdd.hpp>
#include <poplibs_test/GeneralMatrixMultiply.hpp>
#include <poplibs_test/Lstm.hpp>
#include <poplibs_test/NonLinearity.hpp>
#include <unordered_map>

// Fwd state array indices
#define LSTM_FWD_STATE_FORGET_GATE 2
#define LSTM_FWD_STATE_CAND_TANH 3
#define LSTM_FWD_STATE_INPUT_GATE 4
#define LSTM_FWD_STATE_OUTPUT_GATE 5
#define LSTM_FWD_STATE_OUTPUT_TANH 6

using IndexRange = boost::multi_array_types::index_range;
using Array1dRef = boost::multi_array_ref<double, 1>;
using Array1dRefUNSIGNED = boost::multi_array_ref<unsigned, 1>;
using Array2dRef = boost::multi_array_ref<double, 2>;
using Array2d = boost::multi_array<double, 2>;
using Array3dRef = boost::multi_array_ref<double, 3>;
using Array4dRef = boost::multi_array_ref<double, 4>;
using Array3d = boost::multi_array<double, 3>;

using namespace poplibs_support;
using namespace poplibs_test;

static void matrixZero(boost::multi_array_ref<double, 2> matA) {
  std::fill(matA.data(), matA.data() + matA.num_elements(), 0.0);
}

/**
 * Process a given unit type within an LSTM given its weights and biases.
 * The non-linearity is also specified although it may be derived from the unit
 */
static void processBasicLstmUnit(const Array2dRef prevOutput,
                                 const Array2dRef input,
                                 const Array3dRef weightsInput,
                                 const Array3dRef weightsOutput,
                                 const Array2dRef biases, Array2dRef output,
                                 unsigned lstmUnitOffset,
                                 popnn::NonLinearityType nonLinearityType) {
  const auto batchSize = prevOutput.shape()[0];
  const auto outputSize = prevOutput.shape()[1];

  /* split weight into two parts:
   * 1) part which weighs only the previous output
   * 2) part which weighs only the input
   */
  Array2d weightsOutputUnit = weightsOutput[lstmUnitOffset];
  Array2d weightsInputUnit = weightsInput[lstmUnitOffset];

  gemm::generalMatrixMultiply(prevOutput, weightsOutputUnit, output, output,
                              1.0, 0, false, false);
  gemm::generalMatrixMultiply(input, weightsInputUnit, output, output, 1.0, 1.0,
                              false, false);
  /* add bias */
  for (auto b = 0U; b != batchSize; ++b) {
    for (auto i = 0U; i != outputSize; ++i) {
      output[b][i] += biases[lstmUnitOffset][i];
    }
  }

  /* apply non-linearity */
  nonLinearity(nonLinearityType, output);
}

/**
 * Apply mask to gates.
 */
static void applySeqMask(const boost::optional<Array1dRefUNSIGNED> &timeSteps,
                         const unsigned step, Array2dRef ionput) {
  if (timeSteps) {
    const auto batchSize = ionput.shape()[0];
    const auto outputSize = ionput.shape()[1];
    for (auto b = 0U; b != batchSize; ++b) {
      for (auto i = 0U; i != outputSize; ++i) {
        auto limit = (*timeSteps)[timeSteps->size() > 1 ? b : 0];
        if (step >= limit) {
          ionput[b][i] = 0;
        }
      }
    }
  }
}

/**
 * Update output for batches that have not reached iteration limit
 */
static void
copyIfStepWithinRange(const boost::optional<Array1dRefUNSIGNED> &timeSteps,
                      const unsigned step,
                      const boost::optional<Array2dRef> &current,
                      const Array2dRef update, Array2dRef dst) {
  const auto batchSize = dst.shape()[0];
  const auto outputSize = dst.shape()[1];
  for (auto b = 0U; b != batchSize; ++b) {
    for (auto i = 0U; i != outputSize; ++i) {
      auto limit = (*timeSteps)[(timeSteps->size() > 1) ? b : 0];
      if (step < limit) {
        dst[b][i] = update[b][i];
      } else if (current) {
        dst[b][i] = (*current)[b][i];
      }
    }
  }
}

static void
copyIfStepWithinRange(const boost::optional<Array1dRefUNSIGNED> &timeSteps,
                      const unsigned step, const Array2dRef update,
                      Array2dRef dst) {
  copyIfStepWithinRange(timeSteps, step, {}, update, dst);
}

static std::unordered_map<BasicLstmCellUnit, unsigned>
getCellMapping(const std::vector<BasicLstmCellUnit> &cellOrder) {
  // build a mapping of the order that the gates are stored in.
  std::unordered_map<BasicLstmCellUnit, unsigned> cellMapping;
  for (unsigned i = 0; i < cellOrder.size(); ++i) {
    auto gate = cellOrder.at(i);
    cellMapping.insert(std::make_pair(gate, i));
  }

  return cellMapping;
}

void poplibs_test::lstm::basicLstmCellForwardPass(
    const Array3dRef input, const Array2dRef biases,
    const Array2dRef prevOutput, const Array3dRef weightsInput,
    const Array3dRef weightsOutput,
    const boost::optional<Array1dRefUNSIGNED> &timeSteps,
    Array2dRef prevCellState, Array4dRef state, Array2dRef lastOutput,
    Array2dRef lastCellState, const std::vector<BasicLstmCellUnit> &cellOrder,
    const popnn::NonLinearityType activation,
    const popnn::NonLinearityType recurrentActivation) {
  const auto sequenceSize = state.shape()[1];
  const auto batchSize = state.shape()[2];
  const auto outputSize = state.shape()[3];
#ifndef NDEBUG
  const auto inputSize = input.shape()[2];
#endif
  assert(state.shape()[0] == LSTM_NUM_FWD_STATES);
  assert(weightsInput.shape()[0] == BASIC_LSTM_CELL_NUM_UNITS);
  assert(weightsInput.shape()[1] == inputSize);
  assert(weightsInput.shape()[2] == outputSize);
  assert(weightsOutput.shape()[0] == BASIC_LSTM_CELL_NUM_UNITS);
  assert(weightsOutput.shape()[1] == outputSize);
  assert(weightsOutput.shape()[2] == outputSize);
  assert(prevCellState.shape()[0] == batchSize);
  assert(prevCellState.shape()[1] == outputSize);
  assert(biases.shape()[0] == BASIC_LSTM_CELL_NUM_UNITS);
  assert(biases.shape()[1] == outputSize);
  assert(prevOutput.shape()[0] == batchSize);
  assert(prevOutput.shape()[1] == outputSize);

  auto cellMapping = getCellMapping(cellOrder);

  Array2d nextOutput = prevOutput;
  Array2d nextCellState = prevCellState;
  for (auto s = 0U; s != sequenceSize; ++s) {
    Array2d inputThisStep = input[s];

    auto cellState = nextCellState;

    /* forget gate */
    Array2d forgetGate(boost::extents[batchSize][outputSize]);
    processBasicLstmUnit(nextOutput, inputThisStep, weightsInput, weightsOutput,
                         biases, forgetGate,
                         cellMapping.at(BASIC_LSTM_CELL_FORGET_GATE),
                         recurrentActivation);
    applySeqMask(timeSteps, s, forgetGate);
    state[LSTM_FWD_STATE_FORGET_GATE][s] = forgetGate;

    /* input gate */
    Array2d inputGate(boost::extents[batchSize][outputSize]);
    processBasicLstmUnit(nextOutput, inputThisStep, weightsInput, weightsOutput,
                         biases, inputGate,
                         cellMapping.at(BASIC_LSTM_CELL_INPUT_GATE),
                         recurrentActivation);
    applySeqMask(timeSteps, s, inputGate);
    state[LSTM_FWD_STATE_INPUT_GATE][s] = inputGate;

    /* new candidate contribution to this cell */
    Array2d candidate(boost::extents[batchSize][outputSize]);
    processBasicLstmUnit(nextOutput, inputThisStep, weightsInput, weightsOutput,
                         biases, candidate,
                         cellMapping.at(BASIC_LSTM_CELL_CANDIDATE), activation);
    applySeqMask(timeSteps, s, candidate);
    state[LSTM_FWD_STATE_CAND_TANH][s] = candidate;

    /* output gate */
    Array2d outputGate(boost::extents[batchSize][outputSize]);
    processBasicLstmUnit(nextOutput, inputThisStep, weightsInput, weightsOutput,
                         biases, outputGate,
                         cellMapping.at(BASIC_LSTM_CELL_OUTPUT_GATE),
                         recurrentActivation);
    applySeqMask(timeSteps, s, outputGate);
    state[LSTM_FWD_STATE_OUTPUT_GATE][s] = outputGate;

    poplibs_test::gemm::hadamardProduct(forgetGate, cellState, cellState);
    poplibs_test::gemm::hadamardProduct(inputGate, candidate, candidate);
    poplibs_test::axpby::add(cellState, candidate, cellState);

    /* need to maintain the cell state for next step */
    Array2d outputThisStep = cellState;
    nonLinearity(activation, outputThisStep);
    state[LSTM_FWD_STATE_OUTPUT_TANH][s] = outputThisStep;
    gemm::hadamardProduct(outputThisStep, outputGate, outputThisStep);

    if (timeSteps) {
      copyIfStepWithinRange(timeSteps, s, outputThisStep, nextOutput);
      copyIfStepWithinRange(timeSteps, s, cellState, nextCellState);
    } else {
      nextOutput = outputThisStep;
      nextCellState = cellState;
    }
    state[LSTM_FWD_STATE_ACTS_IDX][s] = outputThisStep;
    state[LSTM_FWD_STATE_CELL_STATE_IDX][s] = cellState;
  }

  // Save final state
  lastOutput = nextOutput;
  lastCellState = nextCellState;
}

static void computeGradients(const Array2dRef weightIn,
                             const Array2dRef weightPrev, const Array2dRef grad,
                             Array2dRef gradIn, Array2dRef gradPrev, bool acc) {
  double k = acc ? 1.0 : 0.0;
  gemm::generalMatrixMultiply(grad, weightIn, gradIn, gradIn, 1.0, k, false,
                              true);
  gemm::generalMatrixMultiply(grad, weightPrev, gradPrev, gradPrev, 1.0, k,
                              false, true);
}

void poplibs_test::lstm::basicLstmCellBackwardPass(
    bool outputFullSequence, const Array3dRef weightsInput,
    const Array3dRef weightsOutput, const Array3dRef gradsNextLayer,
    const Array2dRef prevCellState, const Array4dRef fwdState,
    const boost::optional<Array2dRef> initOutputGrad,
    const boost::optional<Array2dRef> initCellStateGrad,
    const boost::optional<Array1dRefUNSIGNED> &timeSteps, Array4dRef bwdState,
    Array3dRef gradsPrevLayer, Array2dRef lastGradLayerOut,
    Array2dRef lastGradCellState,
    const std::vector<BasicLstmCellUnit> &cellOrder,
    const popnn::NonLinearityType activation,
    const popnn::NonLinearityType recurrentActivation) {
  const auto sequenceSize = fwdState.shape()[1];
  const auto batchSize = fwdState.shape()[2];
  const auto outputSize = fwdState.shape()[3];
  const auto inputSize = gradsPrevLayer.shape()[2];

  assert(fwdState.shape()[0] == LSTM_NUM_FWD_STATES);
  assert(bwdState.shape()[0] == LSTM_NUM_BWD_STATES);
  assert(weightsInput.shape()[0] == BASIC_LSTM_CELL_NUM_UNITS);
  assert(weightsInput.shape()[1] == inputSize);
  assert(weightsInput.shape()[2] == outputSize);
  assert(weightsOutput.shape()[0] == BASIC_LSTM_CELL_NUM_UNITS);
  assert(weightsOutput.shape()[1] == outputSize);
  assert(weightsOutput.shape()[2] == outputSize);
  assert(prevCellState.shape()[0] == batchSize);
  assert(prevCellState.shape()[1] == outputSize);
  assert(fwdState.shape()[1] == sequenceSize);
  assert(fwdState.shape()[2] == batchSize);
  assert(fwdState.shape()[3] == outputSize);
  assert(bwdState.shape()[1] == sequenceSize);
  assert(bwdState.shape()[2] == batchSize);
  assert(bwdState.shape()[3] == outputSize);
  assert(gradsNextLayer.shape()[0] == sequenceSize);
  assert(gradsNextLayer.shape()[1] == batchSize);
  assert(gradsNextLayer.shape()[2] == outputSize);
  assert(gradsPrevLayer.shape()[0] == sequenceSize);
  assert(gradsPrevLayer.shape()[1] == batchSize);

  auto cellMapping = getCellMapping(cellOrder);

  // gradient of cell state for this step
  Array2d prevGradCellState(boost::extents[batchSize][outputSize]);
  if (initCellStateGrad) {
    prevGradCellState = *initCellStateGrad;
  } else {
    matrixZero(prevGradCellState);
  }

  // gradient of output of this step
  Array2d gradOutput(boost::extents[batchSize][outputSize]);
  if (initOutputGrad) {
    gradOutput = *initOutputGrad;
  } else {
    matrixZero(gradOutput);
  }

  for (auto i = sequenceSize; i != 0; --i) {
    const auto s = i - 1;
    Array2d gradOut(boost::extents[batchSize][outputSize]);
    Array2d sumGradOut(boost::extents[batchSize][outputSize]);

    if (outputFullSequence) {
      gradOut = gradsNextLayer[s];
    } else {
      // Only the last layer receive the gradient
      if (s == sequenceSize - 1)
        gradOut = gradsNextLayer[0];
      else {
        matrixZero(gradOut);
      }
    }
    auto gradCellState = prevGradCellState;
    axpby::add(gradOut, gradOutput, sumGradOut);

    Array2d actOutGate = fwdState[LSTM_FWD_STATE_OUTPUT_GATE][s];
    Array2d gradAtOTanhInp(boost::extents[batchSize][outputSize]);
    gemm::hadamardProduct(actOutGate, sumGradOut, gradAtOTanhInp);

    Array2d actTanhOutGate = fwdState[LSTM_FWD_STATE_OUTPUT_TANH][s];
    Array2d gradAtOutGate(boost::extents[batchSize][outputSize]);
    ;

    gemm::hadamardProduct(actTanhOutGate, sumGradOut, gradAtOutGate);

    bwdNonLinearity(activation, actTanhOutGate, gradAtOTanhInp);

    bwdNonLinearity(recurrentActivation, actOutGate, gradAtOutGate);

    Array2dRef gradAtCellStateSum = gradAtOTanhInp;
    axpby::add(gradAtOTanhInp, gradCellState, gradAtCellStateSum);

    Array2d actInpGate = fwdState[LSTM_FWD_STATE_INPUT_GATE][s];
    Array2d gradAtCand(boost::extents[batchSize][outputSize]);
    ;
    gemm::hadamardProduct(actInpGate, gradAtCellStateSum, gradAtCand);
    Array2d actCand = fwdState[LSTM_FWD_STATE_CAND_TANH][s];
    Array2d gradAtInpGate(boost::extents[batchSize][outputSize]);
    ;
    gemm::hadamardProduct(actCand, gradAtCellStateSum, gradAtInpGate);
    bwdNonLinearity(activation, actCand, gradAtCand);
    bwdNonLinearity(recurrentActivation, actInpGate, gradAtInpGate);

    Array2d actForgetGate = fwdState[LSTM_FWD_STATE_FORGET_GATE][s];
    gemm::hadamardProduct(actForgetGate, gradAtCellStateSum, gradCellState);

    Array2d pCellAct(boost::extents[batchSize][outputSize]);

    if (s == 0) {
      pCellAct = prevCellState;
    } else {
      pCellAct = fwdState[LSTM_FWD_STATE_CELL_STATE_IDX][s - 1];
    }
    Array2d gradAtForgetGate(boost::extents[batchSize][outputSize]);
    ;

    gemm::hadamardProduct(pCellAct, gradAtCellStateSum, gradAtForgetGate);
    bwdNonLinearity(recurrentActivation, actForgetGate, gradAtForgetGate);

    Array2d gradIn(boost::extents[batchSize][inputSize]);
    ;
    Array2d weightsInUnit =
        weightsInput[cellMapping.at(BASIC_LSTM_CELL_FORGET_GATE)];
    Array2d weightsOutUnit =
        weightsOutput[cellMapping.at(BASIC_LSTM_CELL_FORGET_GATE)];
    Array2d nextGradOut(boost::extents[batchSize][outputSize]);
    computeGradients(weightsInUnit, weightsOutUnit, gradAtForgetGate, gradIn,
                     nextGradOut, false);
    weightsInUnit = weightsInput[cellMapping.at(BASIC_LSTM_CELL_INPUT_GATE)];
    weightsOutUnit = weightsOutput[cellMapping.at(BASIC_LSTM_CELL_INPUT_GATE)];
    computeGradients(weightsInUnit, weightsOutUnit, gradAtInpGate, gradIn,
                     nextGradOut, true);
    weightsInUnit = weightsInput[cellMapping.at(BASIC_LSTM_CELL_OUTPUT_GATE)];
    weightsOutUnit = weightsOutput[cellMapping.at(BASIC_LSTM_CELL_OUTPUT_GATE)];
    computeGradients(weightsInUnit, weightsOutUnit, gradAtOutGate, gradIn,
                     nextGradOut, true);
    weightsInUnit = weightsInput[cellMapping.at(BASIC_LSTM_CELL_CANDIDATE)];
    weightsOutUnit = weightsOutput[cellMapping.at(BASIC_LSTM_CELL_CANDIDATE)];
    computeGradients(weightsInUnit, weightsOutUnit, gradAtCand, gradIn,
                     nextGradOut, true);

    if (timeSteps) {
      if (outputFullSequence) {
        copyIfStepWithinRange(timeSteps, s, nextGradOut, gradOutput);
      } else {
        copyIfStepWithinRange(timeSteps, s, sumGradOut, nextGradOut,
                              gradOutput);
      }
      copyIfStepWithinRange(timeSteps, s, gradCellState, prevGradCellState);
    } else {
      gradOutput = nextGradOut;
      prevGradCellState = gradCellState;
    }
    gradsPrevLayer[s] = gradIn;

    // save bwd state for weight update
    bwdState[BASIC_LSTM_CELL_FORGET_GATE][s] = gradAtForgetGate;
    bwdState[BASIC_LSTM_CELL_INPUT_GATE][s] = gradAtInpGate;
    bwdState[BASIC_LSTM_CELL_OUTPUT_GATE][s] = gradAtOutGate;
    bwdState[BASIC_LSTM_CELL_CANDIDATE][s] = gradAtCand;
  }
  lastGradLayerOut = gradOutput;
  lastGradCellState = prevGradCellState;
}

void poplibs_test::lstm::basicLstmCellParamUpdate(
    const Array3dRef prevLayerActs, const Array4dRef fwdState,
    const Array2dRef outputActsInit, const Array4dRef bwdState,
    Array3dRef weightsInputDeltas, Array3dRef weightsOutputDeltas,
    Array2dRef biasDeltas, const std::vector<BasicLstmCellUnit> &cellOrder) {
  const auto sequenceSize = prevLayerActs.shape()[0];
  const auto batchSize = prevLayerActs.shape()[1];
  const auto inputSize = prevLayerActs.shape()[2];
  const auto outputSize = fwdState.shape()[3];

  assert(fwdState.shape()[0] == LSTM_NUM_FWD_STATES);
  assert(fwdState.shape()[1] == sequenceSize);
  assert(fwdState.shape()[2] == batchSize);
  assert(outputActsInit.shape()[0] == batchSize);
  assert(outputActsInit.shape()[1] == outputSize);
  assert(bwdState.shape()[0] == LSTM_NUM_BWD_STATES);
  assert(bwdState.shape()[1] == sequenceSize);
  assert(bwdState.shape()[2] == batchSize);
  assert(bwdState.shape()[3] == outputSize);
  assert(weightsInputDeltas.shape()[0] == BASIC_LSTM_CELL_NUM_UNITS);
  assert(weightsInputDeltas.shape()[1] == inputSize);
  assert(weightsInputDeltas.shape()[2] == outputSize);
  assert(weightsOutputDeltas.shape()[0] == BASIC_LSTM_CELL_NUM_UNITS);
  assert(weightsOutputDeltas.shape()[1] == outputSize);
  assert(weightsOutputDeltas.shape()[2] == outputSize);
  assert(biasDeltas.shape()[0] == BASIC_LSTM_CELL_NUM_UNITS);
  assert(biasDeltas.shape()[1] == outputSize);

  auto cellMapping = getCellMapping(cellOrder);

  for (auto it = weightsInputDeltas.data(),
            end = weightsInputDeltas.data() + weightsInputDeltas.num_elements();
       it != end; ++it) {
    *it = 0;
  }
  for (auto it = weightsOutputDeltas.data(),
            end =
                weightsOutputDeltas.data() + weightsOutputDeltas.num_elements();
       it != end; ++it) {
    *it = 0;
  }
  for (auto it = biasDeltas.data(),
            end = biasDeltas.data() + biasDeltas.num_elements();
       it != end; ++it) {
    *it = 0;
  }

  for (auto i = sequenceSize; i != 0; --i) {
    const auto s = i - 1;
    Array2d outActs(boost::extents[batchSize][outputSize]);
    if (s == 0) {
      outActs = outputActsInit;
    } else {
      outActs = fwdState[LSTM_FWD_STATE_ACTS_IDX][s - 1];
    }
    Array2d inActs = prevLayerActs[s];
    for (auto i = 0; i != BASIC_LSTM_CELL_NUM_UNITS; ++i) {
      const auto unit = static_cast<BasicLstmCellUnit>(i);

      Array2d grad = bwdState[i][s];
      Array2d wInputDeltasUnit(boost::extents[inputSize][outputSize]);

      gemm::generalMatrixMultiply(inActs, grad, wInputDeltasUnit,
                                  wInputDeltasUnit, 1.0, 0, true, false);
      for (auto ic = 0u; ic != inputSize; ++ic) {
        for (auto oc = 0u; oc != outputSize; ++oc) {
          weightsInputDeltas[cellMapping.at(unit)][ic][oc] +=
              wInputDeltasUnit[ic][oc];
        }
      }
      Array2d wOutputDeltasUnit(boost::extents[outputSize][outputSize]);

      gemm::generalMatrixMultiply(outActs, grad, wOutputDeltasUnit,
                                  wOutputDeltasUnit, 1.0, 0, true, false);
      for (auto oc1 = 0u; oc1 != outputSize; ++oc1) {
        for (auto oc2 = 0u; oc2 != outputSize; ++oc2) {
          weightsOutputDeltas[cellMapping.at(unit)][oc1][oc2] +=
              wOutputDeltasUnit[oc1][oc2];
        }
      }

      for (auto oc = 0u; oc != outputSize; ++oc) {
        for (auto b = 0u; b != batchSize; ++b) {
          biasDeltas[cellMapping.at(unit)][oc] += grad[b][oc];
        }
      }
    }
  }
}


#include <queue>
#include <unordered_set>
#include <boost/math/constants/constants.hpp>
#include <CGAL/boost/graph/Euler_operations.h>
#include <Euclid/Geometry/TriMeshGeometry.h>
#include <Euclid/Math/Numeric.h>

namespace Euclid
{

namespace _impl
{

template<typename Mesh>
void init_queue(const Mesh& mesh,
                std::queue<edge_t<Mesh>>& queue,
                std::vector<bool>& inqueue)
{
    auto eimap = get(boost::edge_index, mesh);
    inqueue.resize(num_edges(mesh), false);
    for (auto e : edges(mesh)) {
        if (!is_delaunay(mesh, e)) {
            queue.push(e);
            inqueue[get(eimap, e)] = true;
        }
    }
}

template<typename Mesh>
void init_physical_edges(const Mesh& mesh, std::vector<bool>& physical)
{
    auto eimap = get(boost::edge_index, mesh);
    physical.resize(num_edges(mesh), true);
    for (auto e : edges(mesh)) {
        if (eq_almost(dihedral_angle(e, mesh),
                      boost::math::constants::pi<FT_t<Mesh>>())) {
            physical[get(eimap, e)] = false;
        }
    }
}

template<typename Mesh, typename Visitor>
void flip(Mesh& mesh,
          Visitor& visitor,
          edge_t<Mesh> e,
          std::queue<edge_t<Mesh>>& queue,
          std::vector<bool>& inqueue)
{
    auto eimap = get(boost::edge_index, mesh);
    auto h = halfedge(e, mesh);
    auto ho = opposite(h, mesh);
    auto ha = next(h, mesh);
    auto hb = next(ha, mesh);
    auto hc = next(ho, mesh);
    auto hd = next(hc, mesh);
    auto ea = edge(ha, mesh);
    auto eb = edge(hb, mesh);
    auto ec = edge(hc, mesh);
    auto ed = edge(hd, mesh);
    visitor.on_flipping(mesh, e);
    CGAL::Euler::flip_edge(h, mesh);
    visitor.on_flipped(mesh, e);
    if (!inqueue[get(eimap, ea)]) {
        queue.push(ea);
        inqueue[get(eimap, ea)] = true;
    }
    if (!inqueue[get(eimap, eb)]) {
        queue.push(eb);
        inqueue[get(eimap, eb)] = true;
    }
    if (!inqueue[get(eimap, ec)]) {
        queue.push(ec);
        inqueue[get(eimap, ec)] = true;
    }
    if (!inqueue[get(eimap, ed)]) {
        queue.push(ed);
        inqueue[get(eimap, ed)] = true;
    }
}

template<typename Mesh>
bool is_flip_topologically_ok(const Mesh& mesh, edge_t<Mesh>& e)
{
    auto h = halfedge(e, mesh);
    return !halfedge(target(next(h, mesh), mesh),
                     target(next(opposite(h, mesh), mesh), mesh),
                     mesh)
                .second;
}

template<typename Mesh, typename Visitor>
void split(Mesh& mesh,
           Visitor& visitor,
           edge_t<Mesh> e,
           std::queue<edge_t<Mesh>>& queue,
           std::vector<bool>& inqueue,
           std::vector<bool>& physical_edges,
           std::unordered_set<vertex_t<Mesh>>& split_vertices)
{
    auto hpq = halfedge(e, mesh);
    auto hqp = opposite(hpq, mesh);
    auto hqv = next(hpq, mesh);
    auto hvp = prev(hpq, mesh);
    auto hpu = next(hqp, mesh);
    auto huq = prev(hqp, mesh);

    // do edge split
    visitor.on_splitting(mesh, e);
    auto hps = CGAL::Euler::split_edge(hpq, mesh);
    auto hsq = next(hps, mesh);
    auto hqs = opposite(hsq, mesh);
    auto hsv = CGAL::Euler::split_face(hps, hqv, mesh);
    auto hsu = CGAL::Euler::split_face(hqs, hpu, mesh);
    auto vs = target(hps, mesh);

    SplitSite<Mesh> site;
    site.epq = e;
    site.vs = vs;
    site.hps = hps;
    site.hqs = opposite(hsq, mesh);
    site.hvs = opposite(hsv, mesh);
    site.hus = opposite(hsu, mesh);
    visitor.on_split(mesh, site);

    // update realted buffers
    auto eimap = get(boost::edge_index, mesh);
    auto eqv = edge(hqv, mesh);
    if (!inqueue[get(eimap, eqv)]) {
        queue.push(eqv);
        inqueue[get(eimap, eqv)] = true;
    }
    auto evp = edge(hvp, mesh);
    if (!inqueue[get(eimap, evp)]) {
        queue.push(evp);
        inqueue[get(eimap, evp)] = true;
    }
    auto epu = edge(hpu, mesh);
    if (!inqueue[get(eimap, epu)]) {
        queue.push(epu);
        inqueue[get(eimap, epu)] = true;
    }
    auto euq = edge(huq, mesh);
    if (!inqueue[get(eimap, euq)]) {
        queue.push(euq);
        inqueue[get(eimap, euq)] = true;
    }
    auto esp = edge(hps, mesh);
    auto esq = edge(hsq, mesh);
    auto esu = edge(hsu, mesh);
    auto esv = edge(hsv, mesh);
    // auto isp = get(eimap, esp);
    // auto isq = get(eimap, esq);
    // auto isu = get(eimap, esu);
    // auto isv = get(eimap, esv);
    queue.push(esp);
    queue.push(esq);
    queue.push(esu);
    queue.push(esv);
    inqueue[get(eimap, esq)] = true;
    inqueue.push_back(true);         // esp
    inqueue.push_back(true);         // esv
    inqueue.push_back(true);         // esu
    physical_edges.push_back(true);  // esp
    physical_edges.push_back(false); // esv
    physical_edges.push_back(false); // esu
    split_vertices.insert(vs);

    // determine new vertex position
    auto vpmap = get(boost::vertex_point, mesh);
    auto vp = target(hvp, mesh);
    auto vq = target(huq, mesh);
    auto pp = get(vpmap, vp);
    auto pq = get(vpmap, vq);
    auto pm = CGAL::midpoint(pp, pq);
    auto lm = length(pp - pm);
    auto l = 0.0;
    if (lm > 1.5) {
        l = 2.0;
        while (l < lm) {
            l *= 2.0;
        }
        if (std::abs(l - lm) > std::abs(l * 0.5 - lm)) {
            l *= 0.5;
        }
    }
    else if (lm < 0.75) {
        l = 0.5;
        while (l > lm) {
            l *= 0.5;
        }
        if (std::abs(l - lm) > std::abs(l * 2.0 - lm)) {
            l *= 2.0;
        }
    }
    else {
        l = 1.0;
    }
    if (split_vertices.find(vp) == split_vertices.end()) {
        auto ps = pp + normalized(pm - pp) * l;
        put(vpmap, vs, ps);
    }
    else {
        auto ps = pq + normalized(pm - pq) * l;
        put(vpmap, vs, ps);
    }
}

template<typename Mesh, typename Visitor>
void remesh_delaunay_simple_flip(Mesh& mesh, Visitor& visitor)
{
    visitor.on_started(mesh);
    auto eimap = get(boost::edge_index, mesh);
    std::queue<edge_t<Mesh>> queue;
    std::vector<bool> inqueue;
    init_queue(mesh, queue, inqueue);
    while (!queue.empty()) {
        auto e = queue.front();
        queue.pop();
        inqueue[get(eimap, e)] = false;
        if (!is_delaunay(mesh, e) && is_flip_topologically_ok(mesh, e)) {
            flip(mesh, visitor, e, queue, inqueue);
        }
        else {
            visitor.on_nonflippable(mesh, e);
        }
    }
    visitor.on_finished(mesh);
}

template<typename Mesh, typename Visitor>
void remesh_delaunay_geometry_preserving(Mesh& mesh, Visitor& visitor)
{
    visitor.on_started(mesh);
    auto eimap = get(boost::edge_index, mesh);
    std::queue<edge_t<Mesh>> queue;
    std::vector<bool> inqueue;
    init_queue(mesh, queue, inqueue);
    std::vector<bool> physical_edges;
    init_physical_edges(mesh, physical_edges);
    std::unordered_set<vertex_t<Mesh>> split_vertices;
    while (!queue.empty()) {
        auto e = queue.front();
        queue.pop();
        inqueue[get(eimap, e)] = false;
        if (!is_delaunay(mesh, e)) {
            if (!physical_edges[get(eimap, e)]) {
                flip(mesh, visitor, e, queue, inqueue);
            }
            else {
                visitor.on_nonflippable(mesh, e);
                split(mesh,
                      visitor,
                      e,
                      queue,
                      inqueue,
                      physical_edges,
                      split_vertices);
            }
        }
    }
    visitor.on_finished(mesh);
}

template<typename Mesh, typename Visitor>
void remesh_delaunay_feature_preserving(Mesh& mesh,
                                        Visitor& visitor,
                                        double threshold)
{
    visitor.on_started(mesh);
    auto eimap = get(boost::edge_index, mesh);
    std::queue<edge_t<Mesh>> queue;
    std::vector<bool> inqueue;
    init_queue(mesh, queue, inqueue);
    std::vector<bool> physical_edges;
    init_physical_edges(mesh, physical_edges);
    std::unordered_set<vertex_t<Mesh>> split_vertices;
    while (!queue.empty()) {
        auto e = queue.front();
        queue.pop();
        inqueue[get(eimap, e)] = false;
        if (!is_delaunay(mesh, e)) {
            if (!physical_edges[get(eimap, e)] ||
                dihedral_angle(e, mesh) > threshold) {
                flip(mesh, visitor, e, queue, inqueue);
            }
            else {
                visitor.on_nonflippable(mesh, e);
                split(mesh,
                      visitor,
                      e,
                      queue,
                      inqueue,
                      physical_edges,
                      split_vertices);
            }
        }
    }
    visitor.on_finished(mesh);
}

} // namespace _impl

template<typename Mesh>
bool is_delaunay(const Mesh& mesh, double eps)
{
    for (auto e : edges(mesh)) {
        if (!is_delaunay(mesh, e, eps)) {
            return false;
        }
    }
    return true;
}

template<typename Mesh>
bool is_delaunay(const Mesh& mesh, edge_t<Mesh> e, double eps)
{
    auto cot = static_cast<double>(cotangent_weight(e, mesh));
    return cot > eps;
}

template<typename Mesh>
void remesh_delaunay(Mesh& mesh,
                     RemeshDelaunayScheme scheme,
                     double dihedral_angle)
{
    RemeshDelaunayVisitor<Mesh> visitor;
    remesh_delaunay(mesh, visitor, scheme, dihedral_angle);
}

template<typename Mesh, typename Visitor>
void remesh_delaunay(Mesh& mesh,
                     Visitor& visitor,
                     RemeshDelaunayScheme scheme,
                     double dihedral_angle)
{
    switch (scheme) {
    case RemeshDelaunayScheme::FeaturePreserving:
        _impl::remesh_delaunay_feature_preserving(
            mesh,
            visitor,
            boost::math::constants::pi<double>() -
                dihedral_angle * boost::math::constants::degree<double>());
        break;
    case RemeshDelaunayScheme::GeometryPreserving:
        _impl::remesh_delaunay_geometry_preserving(mesh, visitor);
        break;
    case RemeshDelaunayScheme::SimpleFlip:
    default:
        _impl::remesh_delaunay_simple_flip(mesh, visitor);
        break;
    }
}

} // namespace Euclid


#define _USE_MATH_DEFINES

#include <cstdlib>
#include <iostream>
#include <regex>
#include <fstream>
#include <iterator>
#include <random>
#include <experimental/filesystem>
#include <cmath>
#include <string>

#include <boost/thread/mutex.hpp>
#include <boost/thread/lock_guard.hpp>

#include "Macros.h"
#include "StandardLibrary.h"
#include "DataTypes/Ops/Ops.h"
#include "DataTypes/Ops/StringOps.h"
#include "DataTypes/Ops/ArrayOps.h"
#include "DataTypes/Ops/IntegerOps.h"
#include "Utils/FileStreamHelper.h"

namespace FPTL
{
	namespace Runtime
	{
		namespace {
			void id(SExecutionContext & aCtx)
			{
				// Копируем данные аргументы от начала фрейма.
				for (size_t i = 0; i < aCtx.argNum; ++i)
				{
					aCtx.push(aCtx.getArg(i));
				}
			}

			void tupleLength(SExecutionContext & aCtx)
			{
				aCtx.push(DataBuilders::createInt(static_cast<long long>(aCtx.argNum)));
			}

			void Not(SExecutionContext & aCtx)
			{
				const auto & arg = aCtx.getArg(0);

				aCtx.push(DataBuilders::createBoolean(!arg.getOps()->toInt(arg)));
			}

			void And(SExecutionContext & aCtx)
			{
				const auto & lhs = aCtx.getArg(0);
				const auto & rhs = aCtx.getArg(1);

				aCtx.push(DataBuilders::createBoolean((lhs.getOps()->toInt(lhs) * rhs.getOps()->toInt(rhs))));
			}

			void Or(SExecutionContext & aCtx)
			{
				const auto & lhs = aCtx.getArg(0);
				const auto & rhs = aCtx.getArg(1);

				aCtx.push(DataBuilders::createBoolean((lhs.getOps()->toInt(lhs) + rhs.getOps()->toInt(rhs))));
			}

			void Xor(SExecutionContext & aCtx)
			{
				const auto & lhs = aCtx.getArg(0);
				const auto & rhs = aCtx.getArg(1);

				aCtx.push(DataBuilders::createBoolean((lhs.getOps()->toInt(lhs) ^ rhs.getOps()->toInt(rhs))));
			}

			void equal(SExecutionContext & aCtx)
			{
				const auto & lhs = aCtx.getArg(0);
				const auto & rhs = aCtx.getArg(1);

				aCtx.push(lhs.getOps()->combine(rhs.getOps())->equal(lhs, rhs));
			}

			void notEqual(SExecutionContext & aCtx)
			{
				const auto & lhs = aCtx.getArg(0);
				const auto & rhs = aCtx.getArg(1);

				aCtx.push(
					DataBuilders::createBoolean(
						!lhs.getOps()->combine(rhs.getOps())->equal(lhs, rhs).mIntVal
					)
				);
			}

			void greater(SExecutionContext & aCtx)
			{
				const auto & lhs = aCtx.getArg(0);
				const auto & rhs = aCtx.getArg(1);

				aCtx.push(lhs.getOps()->combine(rhs.getOps())->greater(lhs, rhs));
			}

			void greaterOrEqual(SExecutionContext & aCtx)
			{
				const auto & lhs = aCtx.getArg(0);
				const auto & rhs = aCtx.getArg(1);

				aCtx.push(
					DataBuilders::createBoolean(
						!lhs.getOps()->combine(rhs.getOps())->less(lhs, rhs).mIntVal
					)
				);
			}

			void less(SExecutionContext & aCtx)
			{
				const auto & lhs = aCtx.getArg(0);
				const auto & rhs = aCtx.getArg(1);

				aCtx.push(lhs.getOps()->combine(rhs.getOps())->less(lhs, rhs));
			}

			void lessOrEqual(SExecutionContext & aCtx)
			{
				const auto & lhs = aCtx.getArg(0);
				const auto & rhs = aCtx.getArg(1);

				aCtx.push(
					DataBuilders::createBoolean(
						!lhs.getOps()->combine(rhs.getOps())->greater(lhs, rhs).mIntVal
					)
				);
			}

			void add(SExecutionContext & aCtx)
			{
				const auto & lhs = aCtx.getArg(0);
				const auto & rhs = aCtx.getArg(1);

				aCtx.push(lhs.getOps()->combine(rhs.getOps())->add(lhs, rhs));
			}

			void sub(SExecutionContext & aCtx)
			{
				const auto & lhs = aCtx.getArg(0);
				const auto & rhs = aCtx.getArg(1);

				aCtx.push(lhs.getOps()->combine(rhs.getOps())->sub(lhs, rhs));
			}

			void mul(SExecutionContext & aCtx)
			{
				const auto & lhs = aCtx.getArg(0);
				const auto & rhs = aCtx.getArg(1);

				aCtx.push(lhs.getOps()->combine(rhs.getOps())->mul(lhs, rhs));
			}

			void div(SExecutionContext & aCtx)
			{
				const auto & lhs = aCtx.getArg(0);
				const auto & rhs = aCtx.getArg(1);

				aCtx.push(lhs.getOps()->combine(rhs.getOps())->div(lhs, rhs));
			}

			void mod(SExecutionContext & aCtx)
			{
				const auto & lhs = aCtx.getArg(0);
				const auto & rhs = aCtx.getArg(1);

				aCtx.push(lhs.getOps()->combine(rhs.getOps())->mod(lhs, rhs));
			}

			// Генерирует случайное вещественное число в диапазоне от 0 до 1.
			void rand(SExecutionContext & aCtx)
			{
				static thread_local std::random_device rd;
				static thread_local std::mt19937_64 gen(rd());
				static std::uniform_real_distribution realDistrib;
				aCtx.push(DataBuilders::createDouble(realDistrib(gen)));
			}

			void sqrt(SExecutionContext & aCtx)
			{
				const auto & arg = aCtx.getArg(0);
				aCtx.push(DataBuilders::createDouble(std::sqrt(arg.getOps()->toDouble(arg))));
			}

			void sin(SExecutionContext & aCtx)
			{
				const auto & arg = aCtx.getArg(0);
				aCtx.push(DataBuilders::createDouble(std::sin(arg.getOps()->toDouble(arg))));
			}

			void cos(SExecutionContext & aCtx)
			{
				const auto & arg = aCtx.getArg(0);
				aCtx.push(DataBuilders::createDouble(std::cos(arg.getOps()->toDouble(arg))));
			}

			void tan(SExecutionContext & aCtx)
			{
				const auto & arg = aCtx.getArg(0);
				aCtx.push(DataBuilders::createDouble(std::tan(arg.getOps()->toDouble(arg))));
			}

			void asin(SExecutionContext & aCtx)
			{
				const auto & arg = aCtx.getArg(0);
				aCtx.push(DataBuilders::createDouble(std::asin(arg.getOps()->toDouble(arg))));
			}

			void atan(SExecutionContext & aCtx)
			{
				const auto & arg = aCtx.getArg(0);
				aCtx.push(DataBuilders::createDouble(std::atan(arg.getOps()->toDouble(arg))));
			}

			void round(SExecutionContext & aCtx)
			{
				const auto & arg = aCtx.getArg(0);
				aCtx.push(DataBuilders::createDouble(std::floor(arg.getOps()->toDouble(arg) + 0.5)));
			}

			void log(SExecutionContext & aCtx)
			{
				const auto & arg = aCtx.getArg(0);
				aCtx.push(DataBuilders::createDouble(std::log(arg.getOps()->toDouble(arg))));
			}

			void exp(SExecutionContext & aCtx)
			{
				const auto & arg = aCtx.getArg(0);
				aCtx.push(DataBuilders::createDouble(std::exp(arg.getOps()->toDouble(arg))));
			}

			void loadPi(SExecutionContext & aCtx)
			{
				aCtx.push(DataBuilders::createDouble(M_PI));
			}

			void loadE(SExecutionContext & aCtx)
			{
				aCtx.push(DataBuilders::createDouble(M_E));
			}

			void abs(SExecutionContext & aCtx)
			{
				const auto & arg = aCtx.getArg(0);
				aCtx.push(arg.getOps()->abs(arg));
			}

			void print(const SExecutionContext & aCtx)
			{
				static boost::mutex outputMutex;
				boost::lock_guard<boost::mutex> guard(outputMutex);

				aCtx.print(std::cout);
			}

			void printType(const SExecutionContext & aCtx)
			{
				static boost::mutex outputMutex;
				boost::lock_guard<boost::mutex> guard(outputMutex);

				aCtx.printTypes(std::cout);
			}

			// Преобразование в строку.
			void toString(SExecutionContext & aCtx)
			{
				std::stringstream strStream;
				const auto & arg = aCtx.getArg(0);

				arg.getOps()->print(arg, strStream);

				aCtx.push(StringBuilder::create(aCtx, strStream.str()));
			}

			// Преобразование в вещественное число.
			void toInteger(SExecutionContext & aCtx)
			{
				const auto & arg = aCtx.getArg(0);

				aCtx.push(DataBuilders::createInt(arg.getOps()->toInt(arg)));
			}

			// Преобразование в целое число.
			void toDouble(SExecutionContext & aCtx)
			{
				const auto & arg = aCtx.getArg(0);

				aCtx.push(DataBuilders::createDouble(arg.getOps()->toDouble(arg)));
			}

			// Конкатенация строк.
			void concat(SExecutionContext & aCtx)
			{
				size_t len = 0;
				for (size_t i = 0; i < aCtx.argNum; ++i)
				{
					const auto & arg = aCtx.getArg(i);
					const auto inStr = arg.getOps()->toString(arg);
					len += inStr->length();
				}

				const auto val = StringBuilder::create(aCtx, len);
				const auto str = val.mString->getChars();
				size_t curPos = 0;

				for (size_t i = 0; i < aCtx.argNum; ++i)
				{
					const auto & arg = aCtx.getArg(i);
					const auto inStr = arg.getOps()->toString(arg);
					std::memcpy(str + curPos, inStr->getChars(), inStr->length());
					curPos += inStr->length();
				}

				aCtx.push(val);
			}

			// Длина строки.
			void length(SExecutionContext & aCtx)
			{
				const auto & arg = aCtx.getArg(0);
				const auto str = arg.getOps()->toString(arg);

				aCtx.push(DataBuilders::createInt(static_cast<long long>(str->length())));
			}

			// Поиск по регулярному выражению.
			void search(SExecutionContext & aCtx)
			{
				const auto & arg0 = aCtx.getArg(0);
				const auto & arg1 = aCtx.getArg(1);

				const auto src = arg0.getOps()->toString(arg0);
				const auto regEx = arg1.getOps()->toString(arg1);

				std::regex rx(regEx->str());
				std::cmatch matchResults;

				if (std::regex_search(static_cast<const char *>(src->getChars()), static_cast<const char *>(src->getChars() + src->length()), matchResults, rx))
				{
					for (size_t i = 0; i < rx.mark_count(); i++)
					{
						auto & m = matchResults[i + 1];
						auto val = StringBuilder::create(aCtx, src, m.first - src->contents(), m.second - src->contents());
						aCtx.push(val);
					}
				}
				else
				{
					aCtx.push(DataBuilders::createUndefinedValue());
				}
			}

			// Проверка соответствия по регулярному выражению.
			void match(SExecutionContext & aCtx)
			{
				const auto & arg0 = aCtx.getArg(0);
				const auto & arg1 = aCtx.getArg(1);

				const auto src = arg0.getOps()->toString(arg0);
				const auto regEx = arg1.getOps()->toString(arg1);

				std::regex rx(regEx->str());
				std::cmatch match;

				if (std::regex_match(static_cast<const char *>(src->getChars()), static_cast<const char *>(src->getChars() + src->length()), match, rx))
				{
					for (size_t i = 0; i < rx.mark_count(); ++i)
					{
						const auto & m = match[i + 1];
						const auto val = StringBuilder::create(aCtx, src, m.first - src->contents(), m.second - src->contents());
						aCtx.push(val);
					}
				}
				else
				{
					aCtx.push(DataBuilders::createUndefinedValue());
				}
			}

			// Замена по регулярному выражению.
			void replace(SExecutionContext & aCtx)
			{
				const auto & arg0 = aCtx.getArg(0);
				const auto & arg1 = aCtx.getArg(1);
				const auto & arg2 = aCtx.getArg(2);

				const auto src = arg0.getOps()->toString(arg0);
				const auto pattern = arg1.getOps()->toString(arg1);
				const auto format = arg2.getOps()->toString(arg2);

				const std::regex rx(pattern->str());
				const auto result = std::regex_replace(src->str(), rx, format->str());
				aCtx.push(StringBuilder::create(aCtx, result));
			}

			// Выделение лексемы с начала строки.
			void getToken(SExecutionContext & aCtx)
			{
				const auto & arg0 = aCtx.getArg(0);
				const auto & arg1 = aCtx.getArg(1);

				const auto src = arg0.getOps()->toString(arg0);
				const auto pattern = arg1.getOps()->toString(arg1);

				const std::regex rx("^(?:\\s*)(" + pattern->str() + ")");

				std::cmatch matchResults;

				const auto first = static_cast<const char *>(src->getChars());
				const auto last = static_cast<const char *>(src->getChars() + src->length());

				if (std::regex_search(first, last, matchResults, rx))
				{
					const auto prefix = StringBuilder::create(aCtx, src, matchResults[1].first - src->contents(), matchResults[1].second - src->contents());
					aCtx.push(prefix);

					const auto suffix = StringBuilder::create(aCtx, src, matchResults.suffix().first - src->contents(), matchResults.suffix().second - src->contents());
					aCtx.push(suffix);
				}
				else
				{
					aCtx.push(DataBuilders::createUndefinedValue());
				}
			}

			// Чтение содержимого файла.
			void readFile(SExecutionContext & aCtx)
			{
				// Проверяем имя файла.
				const auto & arg = aCtx.getArg(0);

				const auto fileName = arg.getOps()->toString(arg);

				DataValue val;
				std::string errMsg = "";
				std::fstream input;
				input.exceptions(std::ifstream::failbit | std::ifstream::badbit);
				try
				{
					// Вычисляем размер файла.
					const auto begin = input.tellg();
					input.seekg(0, std::ios::end);
					const auto size = input.tellg() - begin;
					input.seekg(0, std::ios::beg);

					// Резервируем память под файл.
					val = StringBuilder::create(aCtx, size);
					// Читаем данные.
					std::copy(std::istreambuf_iterator<char>(input), std::istreambuf_iterator<char>(), val.mString->getChars());
					input.close();
				}
				catch (const std::ios_base::failure& exc)
				{
					val = DataBuilders::createUndefinedValue();
					errMsg = Utils::getfStreamError(input);
					if (errMsg == "") errMsg = exc.what();
				}
				aCtx.push(val);
				aCtx.push(StringBuilder::create(aCtx, errMsg));
			}

			void writeToFile(SExecutionContext & aCtx, std::ios::openmode mode)
			{
				// Проверяем имя файла.
				const auto & val = aCtx.getArg(0);
				const auto & file = aCtx.getArg(1);

				const auto fileName = file.getOps()->toString(file);

				Utils::setPermissions(fileName->str());

				DataValue res;
				std::string errMsg = "";
				std::fstream output;
				output.exceptions(std::ifstream::failbit | std::ifstream::badbit);
				try
				{
					output.open(fileName->str(), mode);
					output.precision(std::numeric_limits<double>::max_digits10);
					val.getOps()->write(val, output);
					res = DataBuilders::createBoolean(true);
					output.close();
				}
				catch (const std::ios_base::failure& exc)
				{
					res = DataBuilders::createBoolean(false);
					errMsg = Utils::getfStreamError(output);
					if (errMsg == "") errMsg = exc.what();
				}
				aCtx.push(res);
				aCtx.push(StringBuilder::create(aCtx, errMsg));
			}

			// Создание или перезапись файла.
			void createFile(SExecutionContext & aCtx)
			{
				writeToFile(aCtx, std::ios::out);
			}

			// Запись в конец файла.
			void appendFile(SExecutionContext & aCtx)
			{
				writeToFile(aCtx, std::ios::app);
			}

			// Создание массива.
			void createArray(SExecutionContext & aCtx)
			{
				const auto sizeVal = aCtx.getArg(0);
				const auto& initialVal = aCtx.getArg(1);

				const auto intSize = sizeVal.getOps()->toInt(sizeVal);

#if fptlDebugBuild
				if (sizeVal.getOps() != IntegerOps::get())
					throw BaseOps::invalidOperation(sizeVal.getOps()->getType(sizeVal), "toInt");
				if (intSize <= 0) throw std::invalid_argument(ArrayValue::negativeSizeMsg(intSize));
#endif

				const auto size = static_cast<size_t>(intSize);
				aCtx.push(ArrayValue::create(aCtx, size, initialVal));
			}

			// Чтение элемента из массива.
			void getArrayElement(SExecutionContext & aCtx)
			{
				const auto & arrVal = aCtx.getArg(0);
				const auto & posVal = aCtx.getArg(1);

#if fptlDebugBuild
				ArrayValue::arrayValueCheck(arrVal);
				if (posVal.getOps() != IntegerOps::get())
					throw BaseOps::invalidOperation(posVal.getOps()->getType(posVal), "toInt");
#endif

				const size_t pos = posVal.getOps()->toInt(posVal);
				aCtx.push(ArrayValue::get(arrVal, pos));
			}

			// Запись элемента в массив.
			void setArrayElement(SExecutionContext & aCtx)
			{
				const auto & arrVal = aCtx.getArg(0);
				const auto & posVal = aCtx.getArg(1);
				const auto & val = aCtx.getArg(2);

#if fptlDebugBuild
				ArrayValue::arrayValueCheck(arrVal);
				if (posVal.getOps() != IntegerOps::get())
					throw BaseOps::invalidOperation(posVal.getOps()->getType(posVal), "toInt");
#endif

				const size_t pos = posVal.getOps()->toInt(posVal);

				ArrayValue::set(const_cast<DataValue &>(arrVal), pos, val);

				aCtx.push(arrVal);
			}

			void getArrayLength(SExecutionContext & aCtx)
			{
				const auto & arrVal = aCtx.getArg(0);
				ArrayValue::arrayValueCheck(arrVal);
				aCtx.push(DataBuilders::createInt(ArrayValue::getLen(arrVal)));
			}

			void ArrayConcat(SExecutionContext & aCtx)
			{
				aCtx.push(ArrayValue::concat(aCtx));
			}

			void ArrayCopy(SExecutionContext & aCtx)
			{
				const auto & arrVal = aCtx.getArg(0);
				ArrayValue::arrayValueCheck(arrVal);
				aCtx.push(ArrayValue::copy(aCtx, arrVal));
			}

			void ArrayDot(SExecutionContext & aCtx)
			{
				const auto & arrVal1 = aCtx.getArg(0);
				const auto & arrVal2 = aCtx.getArg(1);
				ArrayValue::arrayValueCheck(arrVal1);
				ArrayValue::arrayValueCheck(arrVal2);
				aCtx.push(ArrayValue::dot(aCtx, arrVal1, arrVal2));
			}

			// Запись элемента в массив.
			void arrayFromFile(SExecutionContext & aCtx)
			{
				auto arrVal = aCtx.getArg(0);
				const auto fileVal = aCtx.getArg(1);

				ArrayValue::arrayValueCheck(arrVal);
				const auto fileName = fileVal.getOps()->toString(fileVal);

				std::string errMsg = "";
				std::fstream input;
				input.exceptions(std::ifstream::failbit | std::ifstream::badbit);
				try
				{
					input.open(fileName->str());
					ArrayValue::fromString(arrVal, static_cast<std::istream&>(input));
					input.close();
				}
				catch (const std::ios_base::failure& exc)
				{
					arrVal = DataBuilders::createUndefinedValue();
					errMsg = Utils::getfStreamError(input);
					if (errMsg == "") errMsg = exc.what();
				}
				aCtx.push(arrVal);
				aCtx.push(StringBuilder::create(aCtx, errMsg));
			}
		} // anonymous namespace

		const std::map<std::string, TFunction> StandardLibrary::mFunctions =
		{
			// Работа с кортежем.
			{"id", &id},
			{"tupleLen", &tupleLength},

			// Арифметические.
			{"add",&add},
			{"sub", &sub},
			{"mul", &mul},
			{"div", &div},
			{"mod", &mod},
			{"abs", &abs},
			{"sqrt", &sqrt},
			{"exp", &exp},
			{"ln", &log},
			{"round", &round},
			{"sin", &sin},
			{"cos", &cos},
			{"tan", &tan},
			{"asin", &asin},
			{"atan", &atan},
			{"Pi", &loadPi},
			{"E", &loadE},
			{"rand", &rand},

			//Логические.
			{"not", &Not},
			{"and", &And},
			{"or", &Or },
			{"xor", &Xor },
			{"equal", &equal},
			{"nequal", &notEqual},
			{"greater", &greater},
			{"gequal", &greaterOrEqual},
			{"less", &less},
			{"lequal", &lessOrEqual},

			// Работа со строками.
			{"length", &length},
			{"cat", &concat},
			{"search", &search},
			{"replace", &replace},
			{"match", &match},
			{"getToken", &getToken},

			// Преобразования типов.
			{"toInt", &toInteger},
			{"toReal", &toDouble},
			{"toString", &toString},

			// Ввод / вывод.
			{"print", &print},
			{"printType", &printType},
			{"readFile", &readFile},
			{"createFile", &createFile},
			{"appendFile", &appendFile},

			// Работа с массивами.
			{"arrayCreate", &createArray},
			{"arrayGet", &getArrayElement},
			{"arraySet", &setArrayElement},
			{"arrayLen", &getArrayLength},
			{"arrayCat", &ArrayConcat},
			{"arrayCopy", &ArrayCopy},
			{"ArrayDot", &ArrayDot},
			{"arrayFromFile", &arrayFromFile}
		};

		StandardLibrary::StandardLibrary() : FunctionLibrary("StdLib")
		{
			addFunctions(mFunctions);
		}
	}
}


/***********************************************************************************************************************
 *  OpenStudio(R), Copyright (c) 2008-2018, Alliance for Sustainable Energy, LLC. All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 *  following conditions are met:
 *
 *  (1) Redistributions of source code must retain the above copyright notice, this list of conditions and the following
 *  disclaimer.
 *
 *  (2) Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
 *  following disclaimer in the documentation and/or other materials provided with the distribution.
 *
 *  (3) Neither the name of the copyright holder nor the names of any contributors may be used to endorse or promote
 *  products derived from this software without specific prior written permission from the respective party.
 *
 *  (4) Other than as required in clauses (1) and (2), distributions in any form of modifications or other derivative
 *  works may not use the "OpenStudio" trademark, "OS", "os", or any other confusingly similar designation without
 *  specific prior written permission from Alliance for Sustainable Energy, LLC.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER, THE UNITED STATES GOVERNMENT, OR ANY CONTRIBUTORS BE LIABLE FOR
 *  ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 *  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *  AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 **********************************************************************************************************************/

#ifndef UTILITIES_GEOMETRY_GEOMETRY_HPP
#define UTILITIES_GEOMETRY_GEOMETRY_HPP

#include "../UtilitiesAPI.hpp"

#include <vector>
#include <boost/optional.hpp>

namespace openstudio{

  class Point3d;
  class PointLatLon;
  class Vector3d;

  /// convert degrees to radians
  UTILITIES_API double degToRad(double degrees);

  /// convert radians to degrees
  UTILITIES_API double radToDeg(double radians);

  /// compute area from surface as Point3dVector
  UTILITIES_API boost::optional<double> getArea(const std::vector<Point3d>& points);

  /// compute Newall vector from surface as Point3dVector, direction is same as outward normal
  /// magnitude is twice the area
  UTILITIES_API boost::optional<Vector3d> getNewallVector(const std::vector<Point3d>& points);

  /// compute outward normal from surface as Point3dVector
  UTILITIES_API boost::optional<Vector3d> getOutwardNormal(const std::vector<Point3d>& points);

  /// compute centroid from surface as Point3dVector
  UTILITIES_API boost::optional<Point3d> getCentroid(const std::vector<Point3d>& points);

  /// reorder points to upper-left-corner convention
  UTILITIES_API std::vector<Point3d> reorderULC(const std::vector<Point3d>& points);

  /// removes collinear points, tolerance is for length of cross product after normalizing each line segment
  UTILITIES_API std::vector<Point3d> removeCollinear(const std::vector<Point3d>& points, double tol = 0.001);

  /// return distance between two points
  UTILITIES_API double getDistance(const Point3d& point1, const Point3d& point2);

  /// return distance between a point and a line segment
  /// returns 0 if lineSegment does not have length 2
  UTILITIES_API double getDistancePointToLineSegment(const Point3d& point, const std::vector<Point3d>& lineSegment);

  /// return distance between a point and a triangle
  /// returns 0 if triangle does not have length 3
  UTILITIES_API double getDistancePointToTriangle(const Point3d& point, const std::vector<Point3d>& triangle);

  /// return angle (in radians) between two vectors
  UTILITIES_API double getAngle(const Vector3d& vector1, const Vector3d& vector2);

  /// check if two vectors of points are equal (within tolerance) irregardless of initial ordering.
  UTILITIES_API bool circularEqual(const std::vector<Point3d>& points1, const std::vector<Point3d>& points2, double tol = 0.001);

  /// if point3d is within tol of any existing points then returns existing point
  /// otherwise adds point3d to allPoints and returns point3d
  UTILITIES_API Point3d getCombinedPoint(const Point3d& point3d, std::vector<Point3d>& allPoints, double tol = 0.001);

  /// compute triangulation of vertices, holes are removed in the triangulation
  /// requires that vertices and holes are in clockwise order on the z = 0 plane (i.e. in face coordinates but reversed)
  UTILITIES_API std::vector<std::vector<Point3d> > computeTriangulation(const std::vector<Point3d>& vertices, const std::vector<std::vector<Point3d> >& holes, double tol = 0.001);

  /// move all vertices towards point by distance, pass negative distance to move away from point
  /// no guarantee that resulting polygon will be valid
  UTILITIES_API std::vector<Point3d> moveVerticesTowardsPoint(const std::vector<Point3d>& vertices, const Point3d& point, double distance);

  /// reverse order of vertices
  UTILITIES_API std::vector<Point3d> reverse(const std::vector<Point3d>& vertices);

  /// Sets view and daylighting window, overhang and light shelf vertices by reference.  Returns true if successful, false otherwise.
  UTILITIES_API bool applyViewAndDaylightingGlassRatios(double viewGlassToWallRatio, double daylightingGlassToWallRatio,
                                                        double desiredViewGlassSillHeight, double desiredDaylightingGlassHeaderHeight,
                                                        double exteriorShadingProjectionFactor, double interiorShelfProjectionFactor,
                                                        const std::vector<Point3d>& surfaceVertices, std::vector<Point3d>& viewVertices,
                                                        std::vector<Point3d>& daylightingVertices, std::vector<Point3d>& exteriorShadingVertices,
                                                        std::vector<Point3d>& interiorShelfVertices);

} // openstudio

#endif //UTILITIES_GEOMETRY_GEOMETRY_HPP


// MIT License
//
// Copyright (c) 2019 worldwide-asset-exchange
//
// 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.

#pragma once

#include <cstddef>
#include <iterator>
#include <stdexcept>
#include <string>
#include <vector>

#include <boost/multiprecision/cpp_int.hpp>
#include <fc/crypto/sha256.hpp>

namespace wax {
    //// Simple RSA signer class
    class rsa_signer {
    public:
        /// @param private_exp Non empty hex string
        /// @param modulus Non empty hex string, the 1st char cannot be 0 (zero)
        /// @exception std::invalid_argument if strings are empty, non hex or
        ///            1st char in modulus is 0
        rsa_signer(const std::string& private_exp, const std::string& modulus)
            : private_exp_{ "0x" + check_emptiness(private_exp) }
            , modulus_{ "0x" + check_modulus(check_emptiness(modulus)) }
            , modulus_size_{ modulus.size() } {
        }

        /// @param message Message/Data to sign
        /// @return Signed message as hex string
        /// @exception std::invalid_argument if message is empty
        std::string sign(const std::string& message) const {
            using namespace boost::multiprecision;

            check_emptiness(message);
            fc::sha256 message_hash(fc::sha256::hash(message.data(), message.size()));

            std::string pkcs1_encoding =
                pkcs1_encode(modulus_size_, bytes_to_string(message_hash.data(), message_hash.data_size()));

            cpp_int pkcs1_encoding_big{ "0x" + pkcs1_encoding };
            cpp_int modexp = powm(pkcs1_encoding_big, private_exp_, modulus_);

            std::vector<char> result;
            export_bits(modexp, std::back_inserter(result), 8);

            return bytes_to_string(result.data(), result.size());
        }

        /// @todo Add getters/setter for private_exp and modulus

    // Implementation
    private:
        boost::multiprecision::cpp_int private_exp_;
        boost::multiprecision::cpp_int modulus_;
        std::size_t                    modulus_size_;

        static std::string bytes_to_string(const char* in, std::size_t size) {
            std::string out;
            const char* hex = "0123456789abcdef";
            for (std::size_t i = 0; i < size; i++) {
                out += hex[(in[i]>>4) & 0xF];
                out += hex[in[i] & 0xF];
            }
            return out;
        }

        static const std::string& check_emptiness(const std::string& str) {
            if (str.empty()) throw std::invalid_argument("String cannot be empty");
            return str;
        }

        static const std::string& check_modulus(const std::string& modulus) {
            if (modulus[0] == '0') throw std::invalid_argument("No leading zeroes allowed in modulus");
            return modulus;
        }

        /**
         * @dev Generates the pkcs1 encoding for an already hashed (via sha256) message.
         * @param modulus_length [size_t] the length in hex digits of the modulus, not including any leading zeroes
         * @param message_hash [string] the sha256 hash of the message intended for signing over
         * @returns [string] the pkcs1 encoding for the message hash given the modulus length
         *
         * For more on RSA signatures, see:
         * https://www.emc.com/collateral/white-papers/h11300-pkcs-1v2-2-rsa-cryptography-standard-wp.pdf
         * Page 39 is extra useful for understanding the padding scheme
         */
        static std::string pkcs1_encode(std::size_t modulus_length, const std::string& message_hash) {
            // pkcs1 padding constant indicating sha256 was used as the message digest for signing
            const char* PKCS1_SHA256 = "003031300d060960864801650304020105000420";

            // Prepend the hash type identifier for sha256
            std::string pkcs1_encoding = PKCS1_SHA256 + message_hash;

            // Prepend all f's and a starting 1 so that the final result is as long as the modulus, minus 3
            pkcs1_encoding = std::string(modulus_length - pkcs1_encoding.size() - 3, 'f').append(pkcs1_encoding);

            // First byte must be 1
            pkcs1_encoding[0] = '1';
            return pkcs1_encoding;
        }

    }; // class rsa_signer

} // namespace wax


/* Copyright (C) 2020 IBM Corp.
 * This program 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. See accompanying LICENSE file.
 */

// This is a sample program for education purposes only.
// It implements a very simple homomorphic encryption based
// db search algorithm for demonstration purposes.

// This country lookup example is derived from the BGV database demo
// code originally written by Jack Crawford for a lunch and learn
// session at IBM Research (Hursley) in 2019.
// The original example code ships with HElib and can be found at
// https://github.com/IBM-HElib/HElib/tree/master/examples/BGV_database_lookup

#include <iostream>

#include <helib/helib.h>
#include <helib/EncryptedArray.h>
#include <helib/ArgMap.h>
#include <NTL/BasicThreadPool.h>

// Utility function to print polynomials
void printPoly(NTL::ZZX& poly)
{
  for (int i = NTL::deg(poly); i >= 0; i--) {
    std::cout << poly[i] << "x^" << i;
    if (i > 0)
      std::cout << " + ";
    else
      std::cout << "\n";
  }
}

// Utility function to read <K,V> CSV data from file
std::vector<std::pair<std::string, std::string>> read_csv(std::string filename)
{
  std::vector<std::pair<std::string, std::string>> dataset;
  std::ifstream data_file(filename);

  if (!data_file.is_open())
    throw std::runtime_error(
        "Error: This example failed trying to open the data file: " + filename +
        "\n           Please check this file exists and try again.");

  std::vector<std::string> row;
  std::string line, entry, temp;

  if (data_file.good()) {
    // Read each line of file
    while (std::getline(data_file, line)) {
      row.clear();
      std::stringstream ss(line);
      while (getline(ss, entry, ',')) {
        row.push_back(entry);
      }
      // Add key value pairs to dataset
      dataset.push_back(std::make_pair(row[0], row[1]));
    }
  }

  data_file.close();
  return dataset;
}

int main(int argc, char* argv[])
{
  /************ HElib boiler plate ************/

  // Note: The parameters have been chosen to provide a somewhat
  // faster running time with a non-realistic security level.
  // Do Not use these parameters in real applications.

  // Plaintext prime modulus
  unsigned long p = 131;
  // Cyclotomic polynomial - defines phi(m)
  unsigned long m = 130; // this will give 48 slots
  // Hensel lifting (default = 1)
  unsigned long r = 1;
  // Number of bits of the modulus chain
  unsigned long bits = 1000;
  // Number of columns of Key-Switching matrix (default = 2 or 3)
  unsigned long c = 2;
  // Size of NTL thread pool (default =1)
  unsigned long nthreads = 1;
  // input database file name
  std::string db_filename = "./countries_dataset.csv";
  // debug output (default no debug output)
  bool debug = false;

  helib::ArgMap amap;
  amap.arg("m", m, "Cyclotomic polynomial ring");
  amap.arg("p", p, "Plaintext prime modulus");
  amap.arg("r", r, "Hensel lifting");
  amap.arg("bits", bits, "# of bits in the modulus chain");
  amap.arg("c", c, "# fo columns of Key-Switching matrix");
  amap.arg("nthreads", nthreads, "Size of NTL thread pool");
  amap.arg("db_filename",
           db_filename,
           "Qualified name for the database filename");
  amap.toggle().arg("-debug", debug, "Toggle debug output", "");
  amap.parse(argc, argv);

  // set NTL Thread pool size
  if (nthreads > 1)
    NTL::SetNumThreads(nthreads);

  std::cout << "\n*********************************************************";
  std::cout << "\n*           Privacy Preserving Search Example           *";
  std::cout << "\n*           =================================           *";
  std::cout << "\n*                                                       *";
  std::cout << "\n* This is a sample program for education purposes only. *";
  std::cout << "\n* It implements a very simple homomorphic encryption    *";
  std::cout << "\n* based db search algorithm for demonstration purposes. *";
  std::cout << "\n*                                                       *";
  std::cout << "\n*********************************************************";
  std::cout << "\n" << std::endl;

  std::cout << "---Initialising HE Environment ... ";
  // Initialize context
  // This object will hold information about the algebra used for this scheme.
  std::cout << "\nInitializing the Context ... ";
  HELIB_NTIMER_START(timer_Context);
  helib::Context context = helib::ContextBuilder<helib::BGV>()
                               .m(m)
                               .p(p)
                               .r(r)
                               .bits(bits)
                               .c(c)
                               .build();
  HELIB_NTIMER_STOP(timer_Context);

  // Secret key management
  std::cout << "\nCreating Secret Key ...";
  HELIB_NTIMER_START(timer_SecKey);
  // Create a secret key associated with the context
  helib::SecKey secret_key = helib::SecKey(context);
  // Generate the secret key
  secret_key.GenSecKey();
  HELIB_NTIMER_STOP(timer_SecKey);

  // Compute key-switching matrices that we need
  HELIB_NTIMER_START(timer_SKM);
  helib::addSome1DMatrices(secret_key);
  HELIB_NTIMER_STOP(timer_SKM);

  // Public key management
  // Set the secret key (upcast: FHESecKey is a subclass of FHEPubKey)
  std::cout << "\nCreating Public Key ...";
  HELIB_NTIMER_START(timer_PubKey);
  const helib::PubKey& public_key = secret_key;
  HELIB_NTIMER_STOP(timer_PubKey);

  // Get the EncryptedArray of the context
  const helib::EncryptedArray& ea = context.getEA();

  // Print the context
  std::cout << std::endl;
  if (debug)
    context.printout();

  // Print the security level
  // Note: This will be negligible to improve performance time.
  std::cout << "\n***Security Level: " << context.securityLevel()
            << " *** Negligible for this example ***" << std::endl;

  // Get the number of slot (phi(m))
  long nslots = ea.size();
  std::cout << "\nNumber of slots: " << nslots << std::endl;

  /************ Read in the database ************/
  std::vector<std::pair<std::string, std::string>> country_db;
  try {
    country_db = read_csv(db_filename);
  } catch (std::runtime_error& e) {
    std::cerr << "\n" << e.what() << std::endl;
    exit(1);
  }

  // Convert strings into numerical vectors
  std::cout << "\n---Initializing the encrypted key,value pair database ("
            << country_db.size() << " entries)...";
  std::cout
      << "\nConverting strings to numeric representation into Ptxt objects ..."
      << std::endl;

  // Generating the Plain text representation of Country DB
  HELIB_NTIMER_START(timer_PtxtCountryDB);
  std::vector<std::pair<helib::Ptxt<helib::BGV>, helib::Ptxt<helib::BGV>>>
      country_db_ptxt;
  for (const auto& country_capital_pair : country_db) {
    if (debug) {
      std::cout << "\t\tname_addr_pair.first size = "
                << country_capital_pair.first.size() << " ("
                << country_capital_pair.first << ")"
                << "\tname_addr_pair.second size = "
                << country_capital_pair.second.size() << " ("
                << country_capital_pair.second << ")" << std::endl;
    }

    helib::Ptxt<helib::BGV> country(context);
    // std::cout << "\tname size = " << country.size() << std::endl;
    for (long i = 0; i < country_capital_pair.first.size(); ++i)
      country.at(i) = country_capital_pair.first[i];

    helib::Ptxt<helib::BGV> capital(context);
    for (long i = 0; i < country_capital_pair.second.size(); ++i)
      capital.at(i) = country_capital_pair.second[i];
    country_db_ptxt.emplace_back(std::move(country), std::move(capital));
  }
  HELIB_NTIMER_STOP(timer_PtxtCountryDB);

  // Encrypt the Country DB
  std::cout << "Encrypting the database..." << std::endl;
  HELIB_NTIMER_START(timer_CtxtCountryDB);
  std::vector<std::pair<helib::Ctxt, helib::Ctxt>> encrypted_country_db;
  for (const auto& country_capital_pair : country_db_ptxt) {
    helib::Ctxt encrypted_country(public_key);
    helib::Ctxt encrypted_capital(public_key);
    public_key.Encrypt(encrypted_country, country_capital_pair.first);
    public_key.Encrypt(encrypted_capital, country_capital_pair.second);
    encrypted_country_db.emplace_back(std::move(encrypted_country),
                                      std::move(encrypted_capital));
  }

  HELIB_NTIMER_STOP(timer_CtxtCountryDB);

  // Print DB Creation Timers
  if (debug) {
    helib::printNamedTimer(std::cout << std::endl, "timer_Context");
    helib::printNamedTimer(std::cout, "timer_Chain");
    helib::printNamedTimer(std::cout, "timer_SecKey");
    helib::printNamedTimer(std::cout, "timer_SKM");
    helib::printNamedTimer(std::cout, "timer_PubKey");
    helib::printNamedTimer(std::cout, "timer_PtxtCountryDB");
    helib::printNamedTimer(std::cout, "timer_CtxtCountryDB");
  }

  std::cout << "\nInitialization Completed - Ready for Queries" << std::endl;
  std::cout << "--------------------------------------------" << std::endl;

  /** Create the query **/

  // Read in query from the command line
  std::string query_string;
  std::cout << "\nPlease enter the name of an European Country: ";
  // std::cin >> query_string;
  std::getline(std::cin, query_string);
  std::cout << "Looking for the Capital of " << query_string << std::endl;
  std::cout << "This may take few minutes ... " << std::endl;

  HELIB_NTIMER_START(timer_TotalQuery);

  HELIB_NTIMER_START(timer_EncryptQuery);
  // Convert query to a numerical vector
  helib::Ptxt<helib::BGV> query_ptxt(context);
  for (long i = 0; i < query_string.size(); ++i)
    query_ptxt[i] = query_string[i];

  // Encrypt the query
  helib::Ctxt query(public_key);
  public_key.Encrypt(query, query_ptxt);
  HELIB_NTIMER_STOP(timer_EncryptQuery);

  /************ Perform the database search ************/

  HELIB_NTIMER_START(timer_QuerySearch);
  std::vector<helib::Ctxt> mask;
  mask.reserve(country_db.size());
  for (const auto& encrypted_pair : encrypted_country_db) {
    helib::Ctxt mask_entry = encrypted_pair.first; // Copy of database key
    mask_entry -= query;                           // Calculate the difference
    mask_entry.power(p - 1);                       // Fermat's little theorem
    mask_entry.negate();                           // Negate the ciphertext
    mask_entry.addConstant(NTL::ZZX(1));           // 1 - mask = 0 or 1
    // Create a vector of copies of the mask
    std::vector<helib::Ctxt> rotated_masks(ea.size(), mask_entry);
    for (int i = 1; i < rotated_masks.size(); i++)
      ea.rotate(rotated_masks[i], i);             // Rotate each of the masks
    totalProduct(mask_entry, rotated_masks);      // Multiply each of the masks
    mask_entry.multiplyBy(encrypted_pair.second); // multiply mask with values
    mask.push_back(mask_entry);
  }

  // Aggregate the results into a single ciphertext
  // Note: This code is for educational purposes and thus we try to refrain
  // from using the STL and do not use std::accumulate
  helib::Ctxt value = mask[0];
  for (int i = 1; i < mask.size(); i++)
    value += mask[i];

  HELIB_NTIMER_STOP(timer_QuerySearch);

  /************ Decrypt and print result ************/

  HELIB_NTIMER_START(timer_DecryptQueryResult);
  helib::Ptxt<helib::BGV> plaintext_result(context);
  secret_key.Decrypt(plaintext_result, value);
  HELIB_NTIMER_STOP(timer_DecryptQueryResult);

  // Convert from ASCII to a string
  std::string string_result;
  for (long i = 0; i < plaintext_result.size(); ++i)
    string_result.push_back(static_cast<long>(plaintext_result[i]));

  HELIB_NTIMER_STOP(timer_TotalQuery);

  // Print DB Query Timers
  if (debug) {
    helib::printNamedTimer(std::cout << std::endl, "timer_EncryptQuery");
    helib::printNamedTimer(std::cout, "timer_QuerySearch");
    helib::printNamedTimer(std::cout, "timer_DecryptQueryResult");
    std::cout << std::endl;
  }

  if (string_result.at(0) == 0x00) {
    string_result =
        "Country name not in the database."
        "\n*** Please make sure to enter the name of a European Country"
        "\n*** with the first letter in upper case.";
  }
  std::cout << "\nQuery result: " << string_result << std::endl;
  helib::printNamedTimer(std::cout, "timer_TotalQuery");

  return 0;
}


/*******************************************************************************
 * Copyright (c) 2012, Dougal J. Sutherland (dsutherl@cs.cmu.edu).             *
 * All rights reserved.                                                        *
 *                                                                             *
 * Redistribution and use in source and binary forms, with or without          *
 * modification, are permitted provided that the following conditions are met: *
 *                                                                             *
 *     * Redistributions of source code must retain the above copyright        *
 *       notice, this list of conditions and the following disclaimer.         *
 *                                                                             *
 *     * Redistributions in binary form must reproduce the above copyright     *
 *       notice, this list of conditions and the following disclaimer in the   *
 *       documentation and/or other materials provided with the distribution.  *
 *                                                                             *
 *     * Neither the name of Carnegie Mellon University nor the                *
 *       names of the contributors may be used to endorse or promote products  *
 *       derived from this software without specific prior written permission. *
 *                                                                             *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" *
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE   *
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE  *
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE   *
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR         *
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF        *
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS    *
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN     *
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)     *
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE  *
 * POSSIBILITY OF SUCH DAMAGE.                                                 *
 ******************************************************************************/
#include "sdm/basics.hpp"
#include "sdm/kernels/polynomial.hpp"

#include <cmath>
#include <string>

#include <boost/format.hpp>
#include <boost/ptr_container/ptr_vector.hpp>

namespace sdm {

double PolynomialKernel::transformDivergence(double div) const {
    return std::pow(div + coef0, (int) degree);
}

std::string PolynomialKernel::name() const {
    return (boost::format("Polynomial(%d, %g)") % degree % coef0).str();
}
size_t PolynomialKernel::getDegree() const { return degree; }
double PolynomialKernel::getCoef0() const { return coef0; }

PolynomialKernel* PolynomialKernel::do_clone() const {
    return new PolynomialKernel(degree, coef0);
}

////////////////////////////////////////////////////////////////////////////////

const boost::ptr_vector<Kernel>* PolynomialKernelGroup::getTuningVector(
        const double* divs, size_t n)
const {
    boost::ptr_vector<Kernel>* kerns = new boost::ptr_vector<Kernel>;
    for (size_t d = 0; d < degrees.size(); d++) {
        for (size_t c = 0; c < coef0s.size(); c++) {
            kerns->push_back(new PolynomialKernel(degrees[d], coef0s[c]));
        }
    }
    return kerns;
}

PolynomialKernelGroup* PolynomialKernelGroup::do_clone() const {
    return new PolynomialKernelGroup(degrees, coef0s);
}

} // end namespace


#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <arpa/inet.h>
//#include <boost/endian/conversion.hpp>
#include <tf/transform_broadcaster.h>

#include <nav_msgs/Odometry.h>

#include <unsupported/Eigen/MatrixFunctions>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

#include <cmath>

using namespace std;
using namespace cv;
using namespace Eigen;

double dt = 1;

Eigen::VectorXd		p_k(3);
Eigen::VectorXd 		q_k(4);
Eigen::VectorXd		q_k_1(4);
Eigen::VectorXd		q_k_dot(4);
Eigen::MatrixXd 		R_k(3,3);		//passo attuale
Eigen::MatrixXd 		R_k_1(3,3);		//passo precedente
Eigen::MatrixXd 		R_k_dot(3,3);
Eigen::MatrixXd 		R_k_dot2(3,3);
Eigen::MatrixXd		S_w(3,3);
Eigen::MatrixXd		S_w2(3,3);
Eigen::MatrixXd		E_q(4,3);
Eigen::VectorXd		w(3);

void skew(const geometry_msgs::TransformStamped::ConstPtr& Transform_estimate)
{

std::cout<<"---------------FRAME_-----------------------"<< endl;
	cout <<	Transform_estimate->header.frame_id << endl;
std::cout<<"--------------------------------------"<< endl;

std::cout<<"---------------FRAME_CHILD-----------------------"<< endl;
	cout <<	Transform_estimate->child_frame_id <<endl;
std::cout<<"--------------------------------------"<< endl;

//____prendo la misura corrispondente al centro dell'oggetto

		p_k[0] = Transform_estimate->transform.translation.x;
		p_k[1] = Transform_estimate->transform.translation.y;
		p_k[2] = Transform_estimate->transform.translation.z;
		q_k[0] = Transform_estimate->transform.rotation.w;
		q_k[1] = Transform_estimate->transform.rotation.x;
		q_k[2] = Transform_estimate->transform.rotation.y;
		q_k[3] = Transform_estimate->transform.rotation.z;

//		q[0] w q[1] x  q[2] y  q[3] z

		std::cout<<"---------------QUATERNION-----------------------"<< endl;
		std::cout<<  q_k[0] << endl;
		std::cout<<  q_k[1] << endl;
		std::cout<<  q_k[2] << endl;
		std::cout<<  q_k[3] << endl;
		std::cout<<"------------------------------------------------"<< endl;

		std::cout<<"---------------QUATERNION_PREVIOUS--------------"<< endl;
		std::cout<<  q_k_1[0] << endl;
		std::cout<<  q_k_1[1] << endl;
		std::cout<<  q_k_1[2] << endl;
		std::cout<<  q_k_1[3] << endl;
		std::cout<<"------------------------------------------------"<< endl;

		std::cout<<"---------------SAMPLING_TIME--------------------"<< endl;
		std::cout<<  dt << endl;
		std::cout<<"------------------------------------------------"<< endl;

		q_k_dot(0) = ( q_k(0) - q_k_1(0) ) / dt ;
		q_k_dot(1) = ( q_k(1) - q_k_1(1) ) / dt ;
		q_k_dot(2) = ( q_k(2) - q_k_1(2) ) / dt ;
		q_k_dot(3) = ( q_k(3) - q_k_1(3) ) / dt ;

		std::cout<<"---------------QUATERNION_DOT-------------------"<< endl;
		std::cout<<  q_k_dot[0] << endl;
		std::cout<<  q_k_dot[1] << endl;
		std::cout<<  q_k_dot[2] << endl;
		std::cout<<  q_k_dot[3] << endl;
		std::cout<<"------------------------------------------------"<< endl;

		E_q(0,0) = - q_k(1) ;
		E_q(0,1) = - q_k(2) ;
		E_q(0,2) = - q_k(3) ;
		E_q(1,0) =   q_k(0) ;
		E_q(1,1) =   q_k(3) ;
		E_q(1,2) = - q_k(2) ;
		E_q(2,0) = - q_k(3) ;
		E_q(2,1) =   q_k(0) ;
		E_q(2,2) =   q_k(1) ;
		E_q(3,0) =   q_k(2) ;
		E_q(3,1) = - q_k(1) ;
		E_q(3,2) =   q_k(0) ;

		std::cout<<"---------------E_q-----------------------------"<< endl;
		std::cout<<  E_q << endl;
		std::cout<<"------------------------------------------------"<< endl;

		w = 2*E_q.transpose()*q_k_dot ;

		std::cout<<"---------------ANGULAR_VELOCITY--------------------"<< endl;
		std::cout<<  w << endl;
		std::cout<<"------------------------------------------------"<< endl;

		R_k(0,0)	= q_k[0]*q_k[0] + q_k[1]*q_k[1] - q_k[2]*q_k[2] - q_k[3]*q_k[3];
		R_k(0,1)	= 2*q_k[1]*q_k[2] - 2*q_k[0]*q_k[3];
		R_k(0,2)	= 2*q_k[1]*q_k[3] + 2*q_k[0]*q_k[2];
		R_k(1,0)	= 2*q_k[1]*q_k[2] + 2*q_k[0]*q_k[3];
		R_k(1,1)	= q_k[0]*q_k[0] + q_k[2]*q_k[2] - q_k[1]*q_k[1] - q_k[3]*q_k[3];
		R_k(1,2)	= 2*q_k[2]*q_k[3] - 2*q_k[0]*q_k[1];
		R_k(2,0)	= 2*q_k[1]*q_k[3] - 2*q_k[0]*q_k[2];
		R_k(2,1)	= 2*q_k[2]*q_k[3] + 2*q_k[0]*q_k[1];
		R_k(2,2)	= q_k[0]*q_k[0] + q_k[3]*q_k[3] - q_k[2]*q_k[2] - q_k[1]*q_k[1];

		//			R_k_dot = ( R_k - R_k_1 ) / dt ;

		R_k_dot(0,0)	= ( R_k(0,0) - R_k_1(0,0) ) / dt ;
		R_k_dot(0,1)	= ( R_k(0,1) - R_k_1(0,1) ) / dt ;
		R_k_dot(0,2)	= ( R_k(0,2) - R_k_1(0,2) ) / dt ;
		R_k_dot(1,0)	= ( R_k(1,0) - R_k_1(1,0) ) / dt ;
		R_k_dot(1,1)	= ( R_k(1,1) - R_k_1(1,1) ) / dt ;
		R_k_dot(1,2)	= ( R_k(1,2) - R_k_1(1,2) ) / dt ;
		R_k_dot(2,0)	= ( R_k(2,0) - R_k_1(2,0) ) / dt ;
		R_k_dot(2,1)	= ( R_k(2,1) - R_k_1(2,1) ) / dt ;
		R_k_dot(2,2)	= ( R_k(2,2) - R_k_1(2,2) ) / dt ;

		R_k_dot2(0,0)	= 2*(q_k(0)*q_k_dot(0) + q_k(1)*q_k_dot(1) - q_k(2)*q_k_dot(2) - q_k(3)*q_k_dot(3));
		R_k_dot2(0,1)	= 2*(q_k_dot(1)*q_k(2) + q_k(1)*q_k_dot(2) - q_k_dot(0)*q_k(3) - q_k(0)*q_k_dot(3));
		R_k_dot2(0,2)	= 2*(q_k_dot(1)*q_k(3) + q_k(1)*q_k_dot(3) + q_k_dot(0)*q_k(2) + q_k(0)*q_k_dot(2));
		R_k_dot2(1,0)	= 2*(q_k_dot(1)*q_k(2) + q_k(1)*q_k_dot(2) + q_k_dot(0)*q_k(3) + q_k(0)*q_k_dot(3));
		R_k_dot2(1,1)	= 2*(q_k(0)*q_k_dot(0) - q_k(1)*q_k_dot(1) + q_k(2)*q_k_dot(2) - q_k(3)*q_k_dot(3));
		R_k_dot2(1,2)	= 2*(q_k_dot(2)*q_k(3) + q_k(2)*q_k_dot(3) - q_k_dot(0)*q_k(1) - q_k(0)*q_k_dot(1));
		R_k_dot2(2,0)	= 2*(q_k_dot(1)*q_k(3) + q_k(1)*q_k_dot(3) - q_k_dot(0)*q_k(2) - q_k(0)*q_k_dot(2));
		R_k_dot2(2,1)	= 2*(q_k_dot(2)*q_k(3) + q_k(2)*q_k_dot(3) + q_k_dot(0)*q_k(1) + q_k(0)*q_k_dot(1));
		R_k_dot2(2,2)	= 2*(q_k(0)*q_k_dot(0) + q_k(1)*q_k_dot(1) - q_k(2)*q_k_dot(2) - q_k(3)*q_k_dot(3));

		S_w2 = R_k_dot2 * R_k.transpose() ;

		S_w = R_k_dot * R_k.transpose() ;			//body_frame

/*

		std::cout<<"---------------SAMPLING_TIME--------------------"<< endl;
		std::cout<<  dt << endl;
		std::cout<<"------------------------------------------------"<< endl;

*/

		std::cout<<"----------------ROTATION_MATRIX-----------------"<< endl;
		std::cout<<  R_k << endl;
		std::cout<<"------------------------------------------------"<< endl;

   	std::cout<<"-------------DOT_ROTATION_MATRIX----------------"<< endl;
		std::cout<<  R_k_dot << endl;
		std::cout<<"------------------------------------------------"<< endl;

		std::cout<<"-----------------SKEW_MATRIX--------------------"<< endl;
		std::cout<<  S_w << endl;
		std::cout<<"------------------------------------------------"<< endl;

/*

	   std::cout<<"----------2_____________DOT_ROTATION_MATRIX----------------"<< endl;
		std::cout<<  R_k_dot2 << endl;
		std::cout<<"------------------------------------------------"<< endl;

		std::cout<<"----------2_______SKEW_MATRIX--------------------"<< endl;
		std::cout<<  S_w2 << endl;
		std::cout<<"------------------------------------------------"<< endl;

*/

		R_k_1 = R_k ;

		q_k_1 = q_k ;

}

int main(int argc, char** argv)
{

	//inizializzazione nodo
	ros::init(argc, argv, "skew");// ROS node
   ros::NodeHandle nn;

	cout<<"1"<<endl;

	ros::Subscriber pose_sub;
	pose_sub = nn.subscribe("/transform_obj", 1, &skew);

	ros::Rate loop_rate(100);//RATE, non metterlo al massimo sennò occupo tutta la CPU ( no good )

	cout<<"2"<<endl;

//inizializzazione

	p_k     <<  0,0,0;
	q_k	  <<  1,0,0,0;
	q_k_1	  <<  1,0,0,0;
	q_k_dot <<  1,0,0,0;

	w			<< 0,0,0 ;

	R_k     <<  MatrixXd::Zero(3,3);
	R_k_1   <<  MatrixXd::Zero(3,3);
	R_k_dot <<  MatrixXd::Zero(3,3);
	R_k_dot2<<  MatrixXd::Zero(3,3);
	E_q     <<  MatrixXd::Zero(4,3);

    ros::Time current_time, last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();

	while(nn.ok())
	{

		current_time = ros::Time::now();
		dt = (current_time - last_time).toSec();

		ros::spinOnce();

		last_time = current_time;

		loop_rate.sleep();

	}

}


#include <iostream>
#include <boost/iterator/iterator_adaptor.hpp>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>

typedef CGAL::Simple_cartesian<double> K;

// The points are stored in a flat array of doubles
// The triangles are stored in a flat array of indices
// referring to an array of coordinates: three consecutive
// coordinates represent a point, and three consecutive
// indices represent a triangle.

typedef size_t* Point_index_iterator;

// Let us now define the iterator on triangles that the tree needs:
class Triangle_iterator
    : public boost::iterator_adaptor<
    Triangle_iterator               // Derived
    , Point_index_iterator            // Base
    , boost::use_default              // Value
    , boost::forward_traversal_tag    // CategoryOrTraversal
    >
{
public:
    Triangle_iterator()
        : Triangle_iterator::iterator_adaptor_() {}

    explicit Triangle_iterator(Point_index_iterator p)
        : Triangle_iterator::iterator_adaptor_(p) {}

private:
    friend class boost::iterator_core_access;
    void increment() { this->base_reference() += 3; }
};

// The following primitive provides the conversion facilities between
// my own triangle and point types and the CGAL ones
struct My_triangle_primitive {
public:
    typedef Triangle_iterator    Id;

    // the CGAL types returned
    typedef K::Point_3    Point;
    typedef K::Triangle_3 Datum;

    // a static pointer to the vector containing the points
    // is needed to build the triangles on the fly:
    static const double* point_container;

private:
    Id m_it; // this is what the AABB tree stores internally

public:
    My_triangle_primitive() {} // default constructor needed

    // the following constructor is the one that receives the iterators from the
    // iterator range given as input to the AABB_tree
    My_triangle_primitive(Triangle_iterator a)
        : m_it(a) {}

    Id id() const { return m_it; }

    // on the fly conversion from the internal data to the CGAL types
    Datum datum() const
    {
        Point_index_iterator p_it = m_it.base();
        Point p(*(point_container + 3 * (*p_it)),
                *(point_container + 3 * (*p_it) + 1),
                *(point_container + 3 * (*p_it) + 2) );
        ++p_it;
        Point q(*(point_container + 3 * (*p_it)),
                *(point_container + 3 * (*p_it) + 1),
                *(point_container + 3 * (*p_it) + 2));
        ++p_it;
        Point r(*(point_container + 3 * (*p_it)),
                *(point_container + 3 * (*p_it) + 1),
                *(point_container + 3 * (*p_it) + 2));

        return Datum(p, q, r); // assembles triangle from three points
    }

    // one point which must be on the primitive
    Point reference_point() const
    {
      return Point(*(point_container + 3 * (*m_it)),
                   *(point_container + 3 * (*m_it) + 1),
                   *(point_container + 3 * (*m_it) + 2));
    }
};

// types
typedef CGAL::AABB_traits<K, My_triangle_primitive> My_AABB_traits;
typedef CGAL::AABB_tree<My_AABB_traits> Tree;
const double* My_triangle_primitive::point_container = 0;

int main()
{
    // generates point set
    double points[12];
    My_triangle_primitive::point_container = points;
    points[0] = 1.0; points[1] = 0.0; points[2] = 0.0;
    points[3] = 0.0; points[4] = 1.0; points[5] = 0.0;
    points[6] = 0.0; points[7] = 0.0; points[8] = 1.0;
    points[9] = 0.0; points[10] = 0.0; points[11] = 0.0;

    // generates indexed triangle set
    size_t triangles[9];
    triangles[0] = 0; triangles[1] = 1; triangles[2] = 2;
    triangles[3] = 0; triangles[4] = 1; triangles[5] = 3;
    triangles[6] = 0; triangles[7] = 3; triangles[8] = 2;

    // constructs AABB tree
    Tree tree(Triangle_iterator(triangles),
        Triangle_iterator(triangles+9));

    // counts #intersections
    K::Ray_3 ray_query(K::Point_3(0.2, 0.2, 0.2), K::Point_3(0.0, 1.0, 0.0));
    std::cout << tree.number_of_intersected_primitives(ray_query)
        << " intersections(s) with ray query" << std::endl;

    // computes closest point
    K::Point_3 point_query(2.0, 2.0, 2.0);
    K::Point_3 closest_point = tree.closest_point(point_query);
    std::cout << "closest point to " << point_query << " is: " << closest_point.x() << " " << closest_point.y() << " " << closest_point.z() << std::endl;

    return EXIT_SUCCESS;
}


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_HYPOT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_HYPOT_HPP_INCLUDED
#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/fast.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/function/scalar/is_inf.hpp>
#include <boost/simd/function/scalar/is_nan.hpp>
#endif
#include <boost/simd/constant/maxexponentm1.hpp>
#include <boost/simd/constant/minexponent.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#include <boost/simd/function/scalar/exponent.hpp>
#include <boost/simd/function/scalar/ldexp.hpp>
#include <boost/simd/function/scalar/max.hpp>
#include <boost/simd/function/scalar/min.hpp>
#include <boost/simd/function/scalar/sqr.hpp>
#include <boost/simd/function/scalar/sqrt.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( hypot_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_<bd::floating_<A0> >
                          , bd::scalar_<bd::floating_<A0> >
                          )
  {

    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      using i_t = bd::as_integer_t<A0>;
      #ifndef BOOST_SIMD_NO_INVALIDS
      if (is_nan(a0) && is_inf(a1)) return Inf<A0>();
      if (is_inf(a0) && is_nan(a1)) return Inf<A0>();
      #endif
      A0 r =  bs::abs(a0);
      A0 i =  bs::abs(a1);
      i_t e =  exponent(bs::max(i, r));
      e = bs::min(bs::max(e,Minexponent<A0>()),Maxexponentm1<A0>());
      return bs::ldexp(sqrt(sqr(bs::ldexp(r, -e))+sqr(bs::ldexp(i, -e))), e);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( hypot_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_<bd::floating_<A0> >
                          , bd::scalar_<bd::floating_<A0> >
                          , boost::simd::std_tag
                          )
  {

    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1
                                    , std_tag const&) const BOOST_NOEXCEPT
    {
      return std::hypot(a0, a1);
    }
  };
} } }

#endif


#include <iostream>
#include <string>
#include <windows.h>
#include <time.h>
#include <Eigen/Dense>
#include "utlt.h"
#include "OneD_system_solve.h"
#include "OneD_exciton_TDDFT_Linear_resp.h"

using namespace std;
using Eigen::VectorXd;

int main()
{
	clock_t t_begin,t_end;
	t_begin=clock();
	int band_considered(3), gridnum(150), kpointnum(151);
	double amplitude1(3.5645),amplitude2(1.0);
	VectorXd w1(2), h1(w1.rows());
	// w1<<2.4, 0.6; h1<<0.0, 8.0;
	w1<<0.5, 0.5; h1<<0.0, 20.0;
	VectorXd w2(4), h2(w2.rows());
	w2<<2.4, 0.6, 2.8, 0.2;
	h2<<0.0, 8.0, 0.0, 10.0;
	// --------------------------------------------------------------------------------
	OneD_system KP_model(w1, h1, "My_kp_model", gridnum, kpointnum, band_considered);
	cout<<KP_model.name<<" is running..."<<endl;
	KP_model.Solve_wf_dens();
	KP_model.Write_Energy();
	KP_model.Write_system_spec ();
	exciton_TDDFT_Tamm_Dancoff_Soft_Coulumb KP_exciton( KP_model,1,2 );
	KP_exciton.Solve_f_kq (0.01);
	KP_exciton.Write_excitation_shift ();
	KP_exciton.Write_excitation_mix ();
	KP_exciton.Write_matrix_fkq ();
	KP_exciton.Write_eff_fkq ();
	KP_exciton.Write_sum_fkq();
	//--------------------------------------------------------------------------------
	// OneD_system Double_model(w2, h2, "dmodel", gridnum, kpointnum, band_considered);
	// cout<<Double_model.name<<" is running..."<<endl;
	// Double_model.Write_system_spec ();
	// Double_model.Solve_wf_dens ();
	// exciton_TDDFT_Tamm_Dancoff_delta Dw_exciton( Double_model,1,2 );
	// Dw_exciton.Solve_f_kq (amplitude1);
	// Dw_exciton.Write_excitation_shift ();
	// Dw_exciton.Write_excitation_mix ();
	// Dw_exciton.Write_matrix_fkq ();
	// Dw_exciton.Write_eff_fkq ();
	// Dw_exciton.Write_sum_fkq();
	//--------------------------------------------------------------------------------
	// OneD_system scmodel(w1, h1, "scmodel", gridnum, kpointnum, band_considered);
	// cout<<scmodel.name<<" is running..."<<endl;
	// scmodel.Write_system_spec ();
	// scmodel.Solve_wf_dens ();
	// exciton_TDDFT_Tamm_Dancoff_Soft_Coulumb sc_exciton( scmodel,1,2 );
	// sc_exciton.Solve_f_kq (amplitude2);
	// sc_exciton.Write_excitation_shift ();
	// sc_exciton.Write_excitation_mix ();
	// sc_exciton.Write_matrix_fkq ();
	// sc_exciton.Write_eff_fkq ();
	// sc_exciton.Write_sum_fkq();
	//---------------------------------------------------------------------------------
	// OneD_system anymodel(w1, h1, "anymodel", gridnum, kpointnum, band_considered);
	// cout<<anymodel.name<<" is running..."<<endl;
	// anymodel.Write_system_spec ();
	// anymodel.Solve_wf_dens ();
	// exciton_TDDFT_Tamm_Dancoff_any_kernel any_exciton( anymodel,1,2 );
	// any_exciton.Solve_f_kq (amplitude1);
	// any_exciton.Write_excitation_shift ();
	// any_exciton.Write_excitation_mix ();
	// any_exciton.Write_matrix_fkq ();
	// any_exciton.Write_eff_fkq ();
	// any_exciton.Write_sum_fkq();
	//---------------------------------------------------------------------------------
	Eigen::Matrix2cd a;
	a<<dcmplx(1,2),dcmplx(2,3),dcmplx(1,4),dcmplx(2,4);
	cout<<a<<endl;
	cout<<a.cwiseAbs()<<endl;
	t_end=clock();
	cout<<"program running time "<<(double(t_end)-double(t_begin))/CLOCKS_PER_SEC<<endl;
	MessageBox(NULL, L"Work finished!", L"Notice", MB_OK);
	return 0;
}


//  Copyright (c) 2011-2013 Thomas Heller
//
//  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)

#include "jacobi.hpp"
#include <boost/program_options.hpp>
#include <iostream>
#include <chrono>

using boost::program_options::variables_map;
using boost::program_options::options_description;
using boost::program_options::value;
using boost::program_options::store;
using boost::program_options::parse_command_line;

using boost::shared_ptr;
using std::min;
using std::vector;

void jacobi_kernel_wrap(size_t y_begin, size_t y_end, size_t n, vector<double> & dst, vector<double> const & src) {
    for(size_t y = y_begin; y < y_end; ++y) {
        size_t offset = y * n;
        for(size_t x = offset + 1; x < offset + n-1; ++x) {
            dst[x] = (src[x-n] + src[x+n] + src[x] + src[x-1] + src[x+1]) * 0.2;
        }
    }
}

void jacobi( size_t n , size_t iterations, size_t block_size, std::string output_filename) {
    shared_ptr< vector<double> > grid_new(new vector<double>(n * n, 1));
    shared_ptr< vector<double> > grid_old(new vector<double>(n * n, 1));

    size_t n_block = static_cast<size_t>(std::ceil(double(n)/block_size));

#pragma omp parallel
{
#pragma omp single
{
    auto start = std::chrono::high_resolution_clock::now();

    for(size_t i = 0; i < iterations; ++i) {
        for(size_t y = 1, j = 0; y < n - 1; y += block_size, ++j) {
            size_t y_end = min(y + block_size, n - 1);

            double *dest = (*grid_new).data();
            double *src = (*grid_old).data();

#pragma omp task firstprivate(y, y_end, grid_new, grid_old) depend(inout: dest[y*n], src[y*n])
        {
            jacobi_kernel_wrap(y, y_end, n, boost::ref(*grid_new), boost::cref(*grid_old));
        }
            std::swap(grid_new, grid_old);
        }
    }
#pragma omp taskwait
    auto end = std::chrono::high_resolution_clock::now();
    double elapsed = std::chrono::duration_cast< std::chrono::duration<double> >(end-start).count();
    jacobi_smp::report_timing(n, iterations, elapsed);
    jacobi_smp::output_grid(output_filename, *grid_old, n);
}}

}

int main(int argc, char **argv)
{
    options_description
        desc_cmd("usage: jacobi_omp [options]");

    desc_cmd.add_options()
        (
         "n", value<std::size_t>()->default_value(16)
         , "Will run on grid with dimensions (n x n)"
        )
        (
         "iterations", value<std::size_t>()->default_value(1000)
         , "Number of iterations"
        )
        (
         "block-size", value<std::size_t>()->default_value(256)
         , "Block size of the different chunks to calculate in parallel"
        )
        (
         "output-filename", value<std::string>()
         , "Filename of the result (if empty no result is written)"
        );

    variables_map vm;
    store(parse_command_line( argc, argv, desc_cmd), vm);
    boost::program_options::notify(vm);

    std::size_t n           = vm["n"].as<std::size_t>();
    std::size_t iterations  = vm["iterations"].as<std::size_t>();
    std::size_t block_size  = vm["block-size"].as<std::size_t>();

    std::string output_filename;
    if(vm.count("output-filename"))
    {
        output_filename = vm["output-filename"].as<std::string>();
    }

    jacobi(n, iterations, block_size, output_filename);

    return 0;
}