#include "RobotModel.h"

#include "utils.h"

#include "r3d.h"
#include "raylib.h"
#include "raymath.h"
#include "rlgl.h"

#include <SpaceVecAlg/Conversions.h>

#include <mc_rtc/config.h>

#include <boost/filesystem.hpp>
namespace bfs = boost::filesystem;

#ifdef MC_RTC_HAS_ROS_SUPPORT
#  include <ros/package.h>
#endif

std::string GOOSH_VERTEX_SHADER_ES = R"(
#version 100

// Input vertex attributes
attribute vec3 vertexPosition;
attribute vec2 vertexTexCoord;
attribute vec3 vertexNormal;
attribute vec4 vertexColor;

// Input uniform values
uniform mat4 mvp;
uniform mat4 matModel;

// Output vertex attributes (to fragment shader)
varying vec3 fragPosition;
varying vec2 fragTexCoord;
varying vec4 fragColor;
varying vec3 fragNormal;

// https://github.com/glslify/glsl-inverse
mat3 inverse(mat3 m)
{
  float a00 = m[0][0], a01 = m[0][1], a02 = m[0][2];
  float a10 = m[1][0], a11 = m[1][1], a12 = m[1][2];
  float a20 = m[2][0], a21 = m[2][1], a22 = m[2][2];

  float b01 = a22*a11 - a12*a21;
  float b11 = -a22*a10 + a12*a20;
  float b21 = a21*a10 - a11*a20;

  float det = a00*b01 + a01*b11 + a02*b21;

  return mat3(b01, (-a22*a01 + a02*a21), (a12*a01 - a02*a11),
              b11, (a22*a00 - a02*a20), (-a12*a00 + a02*a10),
              b21, (-a21*a00 + a01*a20), (a11*a00 - a01*a10))/det;
}

// https://github.com/glslify/glsl-transpose
mat3 transpose(mat3 m)
{
  return mat3(m[0][0], m[1][0], m[2][0],
              m[0][1], m[1][1], m[2][1],
              m[0][2], m[1][2], m[2][2]);
}

void main()
{
    // Send vertex attributes to fragment shader
    fragPosition = vec3(matModel*vec4(vertexPosition, 1.0));
    fragTexCoord = vertexTexCoord;
    fragColor = vertexColor;

    mat3 normalMatrix = transpose(inverse(mat3(matModel)));
    fragNormal = normalize(normalMatrix*vertexNormal);

    // Calculate final vertex position
    gl_Position = mvp*vec4(vertexPosition, 1.0);
}
)";

std::string GOOSH_VERTEX_SHADER = R"(
#version 330

// Input vertex attributes
in vec3 vertexPosition;
in vec2 vertexTexCoord;
in vec3 vertexNormal;
in vec4 vertexColor;

// Input uniform values
uniform mat4 mvp;
uniform mat4 matModel;

// Output vertex attributes (to fragment shader)
out vec3 fragPosition;
out vec2 fragTexCoord;
out vec4 fragColor;
out vec3 fragNormal;

void main()
{
    // Send vertex attributes to fragment shader
    fragPosition = vec3(matModel*vec4(vertexPosition, 1.0));
    fragTexCoord = vertexTexCoord;
    fragColor = vertexColor;

    mat3 normalMatrix = transpose(inverse(mat3(matModel)));
    fragNormal = normalize(normalMatrix*vertexNormal);

    // Calculate final vertex position
    gl_Position = mvp*vec4(vertexPosition, 1.0);
}
)";

// Fragment shader from https://rendermeapangolin.wordpress.com/2015/05/07/gooch-shading/

std::string GOOSH_FRAGMENT_SHADER_ES = R"(
#version 100

precision mediump float;

// Vertex attributes
varying vec3 fragPosition;
varying vec2 fragTexCoord;
varying vec4 fragColor;
varying vec3 fragNormal;

// Input uniform values
uniform sampler2D texture0;
uniform vec4 colDiffuse;
uniform vec3 viewPos;
uniform vec3 targetPos;

vec4 gooch_shading(vec4 m_color, //color of the mesh
                   float m_shine, //shininess of the surface
                   vec3 l_direction, //light direction
                   vec3 v_normal, //normal
                   vec3 c_direction) //camera direction
{
    //diffuse
    float kd = 1.0;
    float a = 0.1;
    float b = 0.9;

    float NL = dot(normalize(v_normal), normalize(l_direction));

    float it = ((1.0 + NL) / 2.0);
    vec3 color = (1.0-it) * (vec3(0.0, 0.0, 0.2) + a*m_color.xyz)
               +  it * (vec3(0.2, 0.2, 0.0) + b*m_color.xyz);

    //Highlights
    vec3 R = reflect( -normalize(l_direction),
                      normalize(v_normal) );
    float ER = clamp( dot( normalize(c_direction),
                           normalize(R)),
                     0.0, 1.0);

    vec4 spec = vec4(1.0) * pow(ER, m_shine);

    return vec4(color+spec.xyz, m_color.a);
}

void main()
{
  gl_FragColor = texture2D(texture0, fragTexCoord) * colDiffuse * fragColor;
  gl_FragColor = gooch_shading(gl_FragColor,
                               10.0,
                               vec3(0, 0, 1),
                               fragNormal,
                               normalize(viewPos - targetPos));
}
)";

std::string GOOSH_FRAGMENT_SHADER = R"(
#version 330

// Vertex attributes
in vec3 fragPosition;
in vec2 fragTexCoord;
in vec4 fragColor;
in vec3 fragNormal;

// Input uniform values
uniform sampler2D texture0;
uniform vec4 colDiffuse;
uniform vec3 viewPos;
uniform vec3 targetPos;

// Output color
out vec4 finalColor;

vec4 gooch_shading(vec4 m_color, //color of the mesh
                   float m_shine, //shininess of the surface
                   vec3 l_direction, //light direction
                   vec3 v_normal, //normal
                   vec3 c_direction) //camera direction
{
    //diffuse
    float kd = 1;
    float a = 0.1;
    float b = 0.9;

    float NL = dot(normalize(v_normal), normalize(l_direction));

    float it = ((1 + NL) / 2);
    vec3 color = (1-it) * (vec3(0, 0, 0.2) + a*m_color.xyz)
               +  it * (vec3(0.2, 0.2, 0) + b*m_color.xyz);

    //Highlights
    vec3 R = reflect( -normalize(l_direction),
                      normalize(v_normal) );
    float ER = clamp( dot( normalize(c_direction),
                           normalize(R)),
                     0, 1);

    vec4 spec = vec4(1) * pow(ER, m_shine);

    return vec4(color+spec.xyz, m_color.a);
}

void main()
{
  finalColor = texture(texture0, fragTexCoord) * colDiffuse * fragColor;
  finalColor = gooch_shading(finalColor,
                             10.0,
                             vec3(0, 0, 1),
                             fragNormal,
                             normalize(viewPos - targetPos));
}
)";

bfs::path convertURI(const std::string & uri)
{
  const std::string package = "package://";
  if(uri.size() >= package.size() && uri.find(package) == 0)
  {
    size_t split = uri.find('/', package.size());
    std::string pkg = uri.substr(package.size(), split - package.size());
    auto leaf = bfs::path(uri.substr(split + 1));
    bfs::path MC_ENV_DESCRIPTION_PATH(mc_rtc::MC_ENV_DESCRIPTION_PATH);
#ifndef __EMSCRIPTEN__
#  ifndef MC_RTC_HAS_ROS_SUPPORT
    // FIXME Prompt the user for unknown packages
    if(pkg == "jvrc_description")
    {
      pkg = (MC_ENV_DESCRIPTION_PATH / ".." / "jvrc_description").string();
    }
    else if(pkg == "mc_env_description")
    {
      pkg = MC_ENV_DESCRIPTION_PATH.string();
    }
    else if(pkg == "mc_int_obj_description")
    {
      pkg = (MC_ENV_DESCRIPTION_PATH / ".." / "mc_int_obj_description").string();
    }
    else
    {
      mc_rtc::log::critical("Cannot resolve package: {}", pkg);
    }
#  else
    pkg = ros::package::getPath(pkg);
#  endif
#else
    pkg = "/assets/" + pkg;
#endif
    return pkg / leaf;
  }
  const std::string file = "file://";
  if(uri.size() >= file.size() && uri.find(file) == 0)
  {
    return bfs::path(uri.substr(file.size()));
  }
  return uri;
}

BodyDrawer::ModelData::~ModelData()
{
  UnloadModel(model);
}

BodyDrawer::BodyDrawer(const std::vector<rbd::parsers::Visual> & v, Shader * shader)
{
  auto applyMaterial = [shader](const rbd::parsers::Visual & v, ModelData & data) {
    if(v.material.type == rbd::parsers::Material::Type::COLOR)
    {
      const auto & c = boost::get<rbd::parsers::Material::Color>(v.material.data);
      data.color = ColorFromNormalized(
          {static_cast<float>(c.r), static_cast<float>(c.g), static_cast<float>(c.b), static_cast<float>(c.a)});
    }
    else if(v.material.type == rbd::parsers::Material::Type::TEXTURE)
    {
      const auto & textureIn = boost::get<rbd::parsers::Material::Texture>(v.material.data);
      auto filename = convertURI(textureIn.filename).string();
      auto texture = LoadTexture(filename.c_str());
      data.model.materials[0].maps[MAP_ALBEDO].texture = texture;
    }
    if(shader)
    {
      for(int i = 0; i < data.model.materialCount; ++i)
      {
        data.model.materials[i].shader = *shader;
      }
    }
  };
  auto fromMesh = [&](const rbd::parsers::Visual & v) {
    const auto & mesh = boost::get<rbd::parsers::Geometry::Mesh>(v.geometry.data);
    auto path = convertURI(mesh.filename);
#ifndef __EMSCRIPTEN__
    auto cwd = bfs::current_path();
    bfs::current_path(path.parent_path());
#else
    auto cwd = get_current_dir_name();
    chdir(path.parent_path().string().c_str());
#endif
    if(path.extension() == ".obj" || path.extension() == ".gltf" || path.extension() == ".glb")
    {
      models_.emplace_back(
          new ModelData{LoadModel(path.string().c_str()), static_cast<float>(mesh.scale), WHITE, v.origin});
    }
    else
    {
      models_.emplace_back(
          new ModelData{LoadModelAdvanced(path.string().c_str()), static_cast<float>(mesh.scale), WHITE, v.origin});
    }
    applyMaterial(v, *models_.back());
#ifndef __EMSCRIPTEN__
    bfs::current_path(cwd);
#else
    chdir(cwd);
    free(cwd);
#endif
  };
  auto fromBox = [&](const rbd::parsers::Visual & v) {
    const auto & box = boost::get<rbd::parsers::Geometry::Box>(v.geometry.data);
    Eigen::Vector3f s = box.size.cast<float>();
    models_.emplace_back(new ModelData{LoadModelFromMesh(GenMeshCube(s.x(), s.y(), s.z())), 1.0, WHITE, v.origin});
    applyMaterial(v, *models_.back());
  };
  auto fromCylinder = [&](const rbd::parsers::Visual & v) {
    const auto & cylinder = boost::get<rbd::parsers::Geometry::Cylinder>(v.geometry.data);
    models_.emplace_back(new ModelData{LoadModelFromMesh(GenMeshCylinderROS(cylinder.radius, cylinder.length, 32)), 1.0,
                                       WHITE, v.origin});
    applyMaterial(v, *models_.back());
  };
  auto fromSphere = [&](const rbd::parsers::Visual & v) {
    const auto & sphere = boost::get<rbd::parsers::Geometry::Sphere>(v.geometry.data);
    models_.emplace_back(new ModelData{LoadModelFromMesh(GenMeshSphere(sphere.radius, 32, 32)), 1.0, WHITE, v.origin});
    applyMaterial(v, *models_.back());
  };
  for(const auto & visual : v)
  {
    switch(visual.geometry.type)
    {
      case rbd::parsers::Geometry::MESH:
        fromMesh(visual);
        break;
      case rbd::parsers::Geometry::BOX:
        fromBox(visual);
        break;
      case rbd::parsers::Geometry::CYLINDER:
        fromCylinder(visual);
        break;
      case rbd::parsers::Geometry::SPHERE:
        fromSphere(visual);
        break;
      default:
        break;
    }
  }
}

void BodyDrawer::update(const sva::PTransformd & pose)
{
  for(auto & m : models_)
  {
    m->model.transform = MatrixMultiply(MatrixScale(m->scale, m->scale, m->scale), convert(m->X_b_model * pose));
  }
}

void BodyDrawer::draw()
{
  for(const auto & m : models_)
  {
    DrawModel(m->model, {0, 0, 0}, 1.0, m->color);
  }
}

RobotModel::RobotModel(const mc_rbdyn::Robot & robot, bool useCollisionModel)
{
  const auto & visual = useCollisionModel ? robot.module()._collision : robot.module()._visual;
#ifndef __EMSCRIPTEN__
  shader_ = LoadShaderCode(GOOSH_VERTEX_SHADER.c_str(), GOOSH_FRAGMENT_SHADER.c_str());
#else
  shader_ = LoadShaderCode(GOOSH_VERTEX_SHADER_ES.c_str(), GOOSH_FRAGMENT_SHADER_ES.c_str());
#endif
  shader_.locs[LOC_MATRIX_MODEL] = GetShaderLocation(shader_, "matModel");
  shader_.locs[LOC_VECTOR_VIEW] = GetShaderLocation(shader_, "viewPos");
  for(const auto & b : robot.mb().bodies())
  {
    if(!visual.count(b.name()))
    {
      bodies_.push_back({{}, nullptr});
    }
    else
    {
      bodies_.emplace_back(visual.at(b.name()), &shader_);
    }
  }
}

void RobotModel::update(const mc_rbdyn::Robot & robot)
{
  const auto & poses = robot.mbc().bodyPosW;
  for(size_t i = 0; i < poses.size(); ++i)
  {
    bodies_[i].update(poses[i]);
  }
}

void RobotModel::draw(Camera camera)
{
  float cameraPos[3] = {camera.position.x, camera.position.y, camera.position.z};
  SetShaderValue(shader_, shader_.locs[LOC_VECTOR_VIEW], cameraPos, UNIFORM_VEC3);
  float targetPos[3] = {camera.target.x, camera.target.y, camera.target.z};
  SetShaderValue(shader_, GetShaderLocation(shader_, "targetPos"), targetPos, UNIFORM_VEC3);
  for(auto & b : bodies_)
  {
    b.draw();
  }
}

RobotModel::~RobotModel()
{
  UnloadShader(shader_);
}


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ConvTest
#include "ConvUtilInternal.hpp"
#include "poputil/TileMapping.hpp"
#include <boost/random.hpp>
#include <boost/test/unit_test.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <poplibs_support/VectorUtils.hpp>
#include <poplibs_test/Convolution.hpp>
#include <poplibs_test/Util.hpp>
#include <poplin/Convolution.hpp>
#include <poplin/codelets.hpp>
#include <random>

using namespace poplar;
using namespace poplin;
using namespace poplibs_test::util;
using namespace poplibs_support;

BOOST_AUTO_TEST_CASE(MappingSplitOutChansSerially) {
  constexpr std::size_t numTiles = 16;
  constexpr std::size_t split = 4;
  constexpr std::size_t numOutChans = 16;
  auto device = createTestDevice(TEST_TARGET, 1, numTiles);
  Graph graph(device.getTarget());

  static_assert(numOutChans % split == 0, "output channels must be evenly "
                                          "divisible by the serial split");
  const auto params =
      ConvParams(HALF, 4 /* batchSize */, {8, 8} /* inputFieldShape */,
                 {3, 3} /* kernelShape */, 16 /* inputChannels */,
                 numOutChans /* outputChannels */, 1 /* numConvGroups */);

  const OptionFlags options{
      {
          "planConstraints",
          R"({"0":{"partition":{"outChanSplit":{"serial":)" +
              std::to_string(split) + R"(}}}})",
      },
  };

  PlanningCache cache;
  const auto weights = createWeights(graph, params, "weights", options, &cache);

  std::stringstream ss;
  reportPlanInfo(ss, graph, params, options, &cache);
  BOOST_TEST_MESSAGE(ss.str());

  // Check each of the splits in the serial partition has the same mapping
  // for efficient dynamic slicing.
  const auto outChansPerSplit = numOutChans / split;
  const auto referenceMapping =
      graph.getTileMapping(weights.slice(0, outChansPerSplit, 1));
  for (std::size_t s = 1; s < split; ++s) {
    const auto slice =
        weights.slice(s * outChansPerSplit, (s + 1) * outChansPerSplit, 1);
    BOOST_CHECK(graph.getTileMapping(slice) == referenceMapping);
  }
}

void convolve(const boost::const_multi_array_ref<double, 3> &in,
              const boost::const_multi_array_ref<double, 4> &weights,
              const boost::const_multi_array_ref<double, 1> &biases,
              const boost::multi_array_ref<double, 3> &out,
              const ConvParams &params) {
  poplibs_test::conv::convolution(
      vectorConvert<unsigned>(params.inputFieldShape),
      params.inputTransform.truncationLower,
      params.inputTransform.truncationUpper, params.inputTransform.dilation,
      params.inputTransform.paddingLower, params.inputTransform.paddingUpper,
      params.inputTransform.flip, vectorConvert<unsigned>(params.kernelShape),
      params.kernelTransform.truncationLower,
      params.kernelTransform.truncationUpper, params.kernelTransform.dilation,
      params.kernelTransform.paddingLower, params.kernelTransform.paddingUpper,
      params.kernelTransform.flip, params.outputTransform.truncationLower,
      params.outputTransform.truncationUpper, params.outputTransform.stride,
      params.outputTransform.paddingLower, params.outputTransform.paddingUpper,
      in, weights, biases, out);
}

boost::multi_array<double, 3> createOut(const ConvParams &params) {
  boost::multi_array<double, 3> out(
      boost::extents[params.batchSize]
                    [params.numConvGroups * params.outputChannelsPerConvGroup]
                    [product(params.getOutputFieldShape())]);
  return out;
}

boost::multi_array<double, 1> createDummyBiases(const ConvParams &params) {
  boost::multi_array<double, 1> biases(
      boost::extents[params.numConvGroups * params.outputChannelsPerConvGroup]);
  std::fill(biases.data(), biases.data() + biases.num_elements(), 0.0);
  return biases;
}

ConvParams createParams() {
  ConvParams p;

  // Random sensible convolution parameters
  p.numConvGroups = 1;
  p.batchSize = 2;
  p.inputChannelsPerConvGroup = 2;
  p.outputChannelsPerConvGroup = 3;
  p.inputType = HALF;
  p.outputType = FLOAT;

  p.inputFieldShape = {4, 4};
  p.inputTransform.truncationLower = {0, 0};
  p.inputTransform.truncationUpper = {0, 0};
  p.inputTransform.dilation = {1, 1};
  p.inputTransform.paddingLower = {0, 0};
  p.inputTransform.paddingUpper = {0, 0};
  p.inputTransform.flip = {false, false};

  p.kernelShape = {1, 1};
  p.kernelTransform.truncationLower = {0, 0};
  p.kernelTransform.truncationUpper = {0, 0};
  p.kernelTransform.dilation = {1, 1};
  p.kernelTransform.paddingLower = {0, 0};
  p.kernelTransform.paddingUpper = {0, 0};
  p.kernelTransform.flip = {false, false};

  p.outputTransform.truncationLower = {0, 0};
  p.outputTransform.truncationUpper = {0, 0};
  p.outputTransform.stride = {1, 1};
  p.outputTransform.paddingLower = {0, 0};
  p.outputTransform.paddingUpper = {0, 0};

  return p;
}

multiconv::ConvolutionArgs
createConvolutionArgs(Graph &graph, const CanonicalConvParams &cp) {
  multiconv::ConvolutionArgs ca;
  ca.params = *cp;
  poplin::PlanningCache cache;
  OptionFlags convOptions;
  ca.inputs =
      poplin::createInput(graph, ca.params, "inputs", convOptions, &cache);
  ca.weights =
      poplin::createWeights(graph, ca.params, "weights", convOptions, &cache);
  return ca;
}

std::vector<boost::multi_array<double, 3>> combineAndRunConvolution(
    TestDevice &device, Graph &graph,
    const std::vector<CanonicalConvParams> &convParams,
    const std::vector<boost::multi_array<double, 3>> &hostIns,
    const std::vector<boost::multi_array<double, 4>> &hostWeights) {
  assert(convParams.size() == hostIns.size());
  assert(convParams.size() == hostWeights.size());

  // Allocate host memory for input and weight tensors
  std::vector<multiconv::ConvolutionArgs> convolutionArgs;
  poplar::program::Sequence uploadProg, downloadProg;
  static std::vector<std::pair<std::string, char *>> tmap;
  std::vector<std::unique_ptr<char[]>> rawHostIns, rawHostWeights;
  auto target = device.getTarget();
  for (unsigned i(0); i < convParams.size(); ++i) {
    const auto ca = createConvolutionArgs(graph, convParams[i]);
    convolutionArgs.push_back(ca);
    rawHostIns.push_back(
        allocateHostMemoryForTensor(ca.inputs, "inputs" + std::to_string(i),
                                    graph, uploadProg, downloadProg, tmap));
    rawHostWeights.push_back(
        allocateHostMemoryForTensor(ca.weights, "weights" + std::to_string(i),
                                    graph, uploadProg, downloadProg, tmap));
    copy(target, hostIns[i], convParams[i]->inputType, rawHostIns.back().get());
    copy(target, hostWeights[i], convParams[i]->inputType,
         rawHostWeights.back().get());
  }

  // Combine convolution parameters and tensors
  auto argsWithConvOptions = convertToConvOptions(graph, convolutionArgs);
  BOOST_CHECK(poplin::canBeCombined(argsWithConvOptions));
  auto ca = poplin::combine(argsWithConvOptions);

  // Create a single convolution
  poplar::program::Sequence prog;
  poplin::PlanningCache cache;
  auto out = poplin::convolution(graph, ca.inputs, ca.weights, *ca.params,
                                 false, prog, "conv", {}, &cache);
  // Split the result tensor
  auto outs = poplin::splitOutput(convParams, out);
  assert(outs.size() == convParams.size());
  // Allocate host memory for split output tensors
  std::vector<std::unique_ptr<char[]>> rawHostOuts;
  for (unsigned i(0); i < outs.size(); ++i) {
    rawHostOuts.push_back(
        allocateHostMemoryForTensor(outs[i], "out" + std::to_string(i), graph,
                                    uploadProg, downloadProg, tmap));
  }

  Engine e(graph, poplar::program::Sequence{uploadProg, prog, downloadProg});
  attachStreams(e, tmap);

  // Run convolution and extract result
  device.bind([&](const Device &d) { e.loadAndRun(d); });
  std::vector<boost::multi_array<double, 3>> hostOuts;
  for (unsigned i(0); i < convParams.size(); ++i) {
    const auto cp = convParams[i];
    hostOuts.emplace_back(
        boost::extents[cp->batchSize]
                      [cp->numConvGroups * cp->outputChannelsPerConvGroup]
                      [product(cp->getOutputFieldShape())]);
    copy(target, cp->outputType, rawHostOuts[i].get(), hostOuts.back());
  }
  return hostOuts;
}

void runAndCheckConvolution(
    TestDevice &device, Graph &graph,
    const std::vector<CanonicalConvParams> &convParams) {
  // Write random values to input tensors
  std::vector<boost::multi_array<double, 3>> hostIns;
  std::vector<boost::multi_array<double, 4>> hostWeights;
  std::mt19937 randomEngine;
  auto target = device.getTarget();
  for (const auto &ccp : convParams) {
    const auto &p = *ccp;
    hostIns.emplace_back(
        boost::extents[p.batchSize]
                      [p.numConvGroups * p.inputChannelsPerConvGroup]
                      [product(p.inputFieldShape)]);
    hostWeights.emplace_back(
        boost::extents[p.numConvGroups][p.outputChannelsPerConvGroup]
                      [p.inputChannelsPerConvGroup][product(p.kernelShape)]);
    writeRandomValues(target, p.inputType, hostIns.back(), -1.0, +5.0,
                      randomEngine);
    writeRandomValues(target, p.inputType, hostWeights.back(), -1.0, +7.0,
                      randomEngine);
  }

  auto hostOuts =
      combineAndRunConvolution(device, graph, convParams, hostIns, hostWeights);
  assert(hostOuts.size() == convParams.size());

  // Compare results to model
  for (unsigned i(0); i < convParams.size(); ++i) {
    auto params = convParams[i];
    auto hostBiases = createDummyBiases(*params);
    auto modelOut = createOut(*params);
    convolve(hostIns[i], hostWeights[i], hostBiases, modelOut, *params);
    double absoluteTolerance = params->outputType == FLOAT ? 1e-6 : 1e-5;
    double relativeTolerance = params->outputType == FLOAT ? 0.01 : 0.1;
    bool isClose = checkIsClose("combined_conv", hostOuts[i], modelOut,
                                relativeTolerance, absoluteTolerance);
    BOOST_CHECK(isClose);
  }
}

BOOST_AUTO_TEST_CASE(MultiConvCanBeCombined) {
  auto createArgsPair = [](Graph &graph, const auto &modify) {
    auto cp1 = createParams();
    auto cp2 = createParams();
    modify(cp1, cp2);

    auto ca1 = createConvolutionArgs(graph, cp1);
    auto ca2 = createConvolutionArgs(graph, cp2);
    return convertToConvOptions(graph, {ca1, ca2});
  };
  auto device = createTestDevice(TEST_TARGET, 1, 16);
  Graph graph(device.getTarget());
  {
    auto args =
        createArgsPair(graph, [](ConvParams &first, ConvParams &second) {
          first.batchSize = 1;
          second.batchSize = 2;
        });
    BOOST_CHECK(!poplin::canBeCombined(args));
  }
  {
    auto args =
        createArgsPair(graph, [](ConvParams &first, ConvParams &second) {
          first.inputFieldShape = {1, 1};
          second.inputFieldShape = {2, 2};
        });
    BOOST_CHECK(!poplin::canBeCombined(args));
  }
  {
    auto args =
        createArgsPair(graph, [](ConvParams &first, ConvParams &second) {
          first.kernelTransform.paddingLower = {0, 0};
          second.kernelTransform.paddingLower = {1, 1};
        });
    BOOST_CHECK(!poplin::canBeCombined(args));
  }
  {
    auto args = createArgsPair(graph, [](const auto &, const auto &) {});
    args[0].options.interIpuPartialsType = poplar::FLOAT;
    args[1].options.interIpuPartialsType = poplar::HALF;
    BOOST_CHECK(!poplin::canBeCombined(args));
  }
}

BOOST_AUTO_TEST_CASE(MultiConvCombination) {
  auto device = createTestDevice(TEST_TARGET, 1, 16);
  Graph graph(device.getTarget());
  poplin::addCodelets(graph);

  auto cp1 = createParams();
  auto cp2 = createParams();
  auto cp3 = createParams();
  cp2.numConvGroups = 2;
  cp3.numConvGroups = 3;

  runAndCheckConvolution(device, graph, {cp1, cp2, cp3});
}

const auto printMapping = [](const auto &mapping) {
  for (unsigned tile = 0; tile < mapping.size(); ++tile) {
    std::stringstream ss;
    ss << "tile = " << tile << ", intervals=[ ";
    for (const auto &i : mapping[tile])
      ss << i << " ";
    ss << "]";

    BOOST_TEST_MESSAGE(ss.str());
  }
};

const auto checkMappingEntirelyOneTile = [](const Graph &graph, const Tensor &t,
                                            const unsigned i) {
  auto mapping = graph.getTileMapping(t);

  for (unsigned tile = 0; tile < mapping.size(); ++tile) {
    if (tile == i) {
      BOOST_TEST(!mapping[tile].empty());
    } else {
      BOOST_TEST(mapping[tile].empty());
    }
  }
};

const auto checkSplitTensorsMappedToSeparateTiles =
    [](const Graph &graph, const std::vector<Tensor> &result) {
      BOOST_TEST(result.size() == 3);

      for (unsigned i = 0; i < result.size(); ++i) {
        std::stringstream ss0;
        ss0 << "result << " << i << " ";
        result[i].outputRegions(ss0);
        BOOST_TEST_MESSAGE(ss0.str());

        // the first conv params should be entirely mapped on tile 0, etc.
        checkMappingEntirelyOneTile(graph, result[i], i);
      }
    };

BOOST_AUTO_TEST_CASE(SplitOutput) {
  constexpr static auto batch = 2;
  constexpr static auto outChans = 3;

  const auto makeConvParams = [](unsigned convGroups) {
    return ConvParams(poplar::HALF, batch, {1}, {1}, 1, outChans, convGroups);
  };

  auto device = createTestDevice(TEST_TARGET, 1, 4);
  Graph graph(device.getTarget());

  // out shape should [B][G * Co]...
  //   where G*Co = (4+5+6)*3 = 45
  const std::vector<CanonicalConvParams> params{
      makeConvParams(4), makeConvParams(5), makeConvParams(6)};

  const auto uut =
      graph.addVariable(poplar::HALF, {batch, (4 + 5 + 6) * outChans, 1});

  // map each ConvParam to a separate tile.
  const unsigned gDim = 1;
  graph.setTileMapping(uut.slice(0, 4 * outChans, gDim), 0);
  graph.setTileMapping(uut.slice(4 * outChans, (4 + 5) * outChans, gDim), 1);
  graph.setTileMapping(
      uut.slice((4 + 5) * outChans, (4 + 5 + 6) * outChans, gDim), 2);
  printMapping(graph.getTileMapping(uut));

  // split the output and verify that each one is entirely mapped to the
  // expected tile.
  auto result = splitOutput(params, uut);
  checkSplitTensorsMappedToSeparateTiles(graph, result);
}

BOOST_AUTO_TEST_CASE(SplitInput) {
  constexpr static auto batch = 2;
  constexpr static auto inChans = 3;

  const auto makeConvParams = [](unsigned convGroups) {
    return ConvParams(poplar::HALF, batch, {1}, {1}, inChans, 1, convGroups);
  };

  auto device = createTestDevice(TEST_TARGET, 1, 4);
  Graph graph(device.getTarget());

  // out shape should [B][G * Ci]...
  //   where G*Co = (4+5+6)*3 = 45
  const std::vector<CanonicalConvParams> params{
      makeConvParams(4), makeConvParams(5), makeConvParams(6)};

  const auto uut =
      graph.addVariable(poplar::HALF, {batch, (4 + 5 + 6) * inChans, 1});

  // map each ConvParam to a separate tile.
  const unsigned gDim = 1;
  graph.setTileMapping(uut.slice(0, 4 * inChans, gDim), 0);
  graph.setTileMapping(uut.slice(4 * inChans, (4 + 5) * inChans, gDim), 1);
  graph.setTileMapping(
      uut.slice((4 + 5) * inChans, (4 + 5 + 6) * inChans, gDim), 2);
  printMapping(graph.getTileMapping(uut));

  // split the input and verify that each one is entirely mapped to the
  // expected tile.
  auto result = splitInput(params, uut);
  checkSplitTensorsMappedToSeparateTiles(graph, result);
}

BOOST_AUTO_TEST_CASE(SplitWeights) {
  constexpr static auto inChans = 3;
  constexpr static auto outChans = 4;

  const auto makeConvParams = [](unsigned convGroups) {
    return ConvParams(poplar::HALF, 1, {1}, {1}, inChans, outChans, convGroups);
  };

  auto device = createTestDevice(TEST_TARGET, 1, 4);
  Graph graph(device.getTarget());

  // weights shape should [G][Ci][Co]...
  const std::vector<CanonicalConvParams> params{
      makeConvParams(4), makeConvParams(5), makeConvParams(6)};

  const auto uut =
      graph.addVariable(poplar::HALF, {4 + 5 + 6, inChans, outChans, 1});

  // map each ConvParam to a separate tile.
  const unsigned gDim = 0;
  graph.setTileMapping(uut.slice(0, 4, gDim), 0);
  graph.setTileMapping(uut.slice(4, 4 + 5, gDim), 1);
  graph.setTileMapping(uut.slice(4 + 5, 4 + 5 + 6, gDim), 2);
  printMapping(graph.getTileMapping(uut));

  // split the input and verify that each one is entirely mapped to the
  // expected tile.
  auto result = splitWeights(params, uut);
  checkSplitTensorsMappedToSeparateTiles(graph, result);
}

BOOST_AUTO_TEST_CASE(CombineCreateTensorArgs) {
  constexpr static auto batch = 2;
  constexpr static auto inChans = 3;

  const auto makeConvParams = [](unsigned convGroups) {
    return ConvParams(poplar::HALF, batch, {1}, {1}, inChans, 1, convGroups);
  };

  auto device = createTestDevice(TEST_TARGET);
  ConvOptions options{};
  options.pass = Pass::FC_TRAINING_WU;

  std::vector<multiconv::internal::CreateTensorArgs> args{
      {makeConvParams(4), options, "four"},
      {makeConvParams(5), options, "five"},
      {makeConvParams(6), options, "six"}};
  auto result = combine(args);

  BOOST_TEST(*result.params == makeConvParams(4 + 5 + 6));
  BOOST_TEST(result.options == options);
  // for now only the first name is preserved.
  BOOST_TEST(result.name == "four");
}

BOOST_AUTO_TEST_CASE(CombineConvolutionArgs) {
  constexpr static auto batch = 2;
  constexpr static auto inChans = 3;
  constexpr static auto outChans = 4;

  const auto makeConvParams = [](unsigned convGroups) {
    return ConvParams(poplar::HALF, batch, {1}, {1}, inChans, outChans,
                      convGroups);
  };

  auto device = createTestDevice(TEST_TARGET, 1, 8);
  Graph graph(device.getTarget());

  const auto makeInput = [&graph](unsigned convGroups) {
    // acts external shape: [N][G * C]...
    auto t = graph.addVariable(poplar::HALF, {batch, convGroups * inChans, 1});
    graph.setTileMapping(t, convGroups);
    return t;
  };

  const auto makeWeights = [&graph](unsigned convGroups) {
    // weights external shape: [G][Co][Ci]...
    auto t =
        graph.addVariable(poplar::HALF, {convGroups, outChans, inChans, 1});
    graph.setTileMapping(t, convGroups);
    return t;
  };

  ConvOptions options{};
  options.pass = Pass::FC_TRAINING_WU;

  std::vector<multiconv::internal::ConvolutionArgs> args{
      {makeInput(5), makeWeights(5), makeConvParams(5), options},
      {makeInput(6), makeWeights(6), makeConvParams(6), options},
      {makeInput(7), makeWeights(7), makeConvParams(7), options},
  };

  auto result = combine(args);
  BOOST_TEST(*result.params == makeConvParams(5 + 6 + 7));
  BOOST_TEST(result.options == options);

  const unsigned iDim = 1;
  checkMappingEntirelyOneTile(graph, result.inputs.slice(0, 5 * inChans, iDim),
                              5);
  checkMappingEntirelyOneTile(
      graph, result.inputs.slice(5 * inChans, (5 + 6) * inChans, iDim), 6);
  checkMappingEntirelyOneTile(
      graph,
      result.inputs.slice((5 + 6) * inChans, (5 + 6 + 7) * inChans, iDim), 7);

  const unsigned wDim = 0;
  checkMappingEntirelyOneTile(graph, result.weights.slice(0, 5, wDim), 5);
  checkMappingEntirelyOneTile(graph, result.weights.slice(5, 5 + 6, wDim), 6);
  checkMappingEntirelyOneTile(graph,
                              result.weights.slice(5 + 6, 5 + 6 + 7, wDim), 7);
}

BOOST_AUTO_TEST_CASE(CombineCalculateWeightDeltasArgs) {
  constexpr static auto batch = 2;
  constexpr static auto inChans = 3;

  const auto makeConvParams = [](unsigned convGroups) {
    return ConvParams(poplar::HALF, batch, {1}, {1}, inChans, 1, convGroups);
  };

  auto device = createTestDevice(TEST_TARGET, 1, 8);
  Graph graph(device.getTarget());

  const auto makeInput = [&graph](unsigned convGroups) {
    // acts external shape: [N][G * C]...
    auto t = graph.addVariable(poplar::HALF, {batch, convGroups * inChans, 1});
    graph.setTileMapping(t, convGroups);
    return t;
  };

  ConvOptions options{};
  options.pass = Pass::FC_TRAINING_WU;

  std::vector<multiconv::internal::CalculateWeightDeltasArgs> args{
      {makeInput(5), makeInput(5), makeConvParams(5), options},
      {makeInput(6), makeInput(6), makeConvParams(6), options},
      {makeInput(7), makeInput(7), makeConvParams(7), options},
  };

  auto result = combine(args);
  BOOST_TEST(*result.params == makeConvParams(5 + 6 + 7));
  BOOST_TEST(result.options == options);

  const unsigned d = 1;
  checkMappingEntirelyOneTile(graph, result.zDeltas.slice(0, 5 * inChans, d),
                              5);
  checkMappingEntirelyOneTile(
      graph, result.zDeltas.slice(5 * inChans, (5 + 6) * inChans, d), 6);
  checkMappingEntirelyOneTile(
      graph, result.zDeltas.slice((5 + 6) * inChans, (5 + 6 + 7) * inChans, d),
      7);

  checkMappingEntirelyOneTile(graph,
                              result.activations.slice(0, 5 * inChans, d), 5);
  checkMappingEntirelyOneTile(
      graph, result.activations.slice(5 * inChans, (5 + 6) * inChans, d), 6);
  checkMappingEntirelyOneTile(
      graph,
      result.activations.slice((5 + 6) * inChans, (5 + 6 + 7) * inChans, d), 7);
}

BOOST_AUTO_TEST_CASE(CombineConvWeightUpdateArgs) {
  constexpr static auto batch = 2;
  constexpr static auto inChans = 3;
  constexpr static auto outChans = 4;

  const auto makeConvParams = [](unsigned convGroups) {
    return ConvParams(poplar::HALF, batch, {1}, {1}, inChans, outChans,
                      convGroups);
  };

  auto device = createTestDevice(TEST_TARGET, 1, 8);
  Graph graph(device.getTarget());

  const auto makeInput = [&graph](unsigned convGroups) {
    // acts external shape: [N][G * C]...
    auto t = graph.addVariable(poplar::HALF, {batch, convGroups * inChans, 1});
    graph.setTileMapping(t, convGroups);
    return t;
  };

  const auto makeWeights = [&graph](unsigned convGroups) {
    // weights external shape: [G][Co][Ci]...
    auto t =
        graph.addVariable(poplar::HALF, {convGroups, outChans, inChans, 1});
    graph.setTileMapping(t, convGroups);
    return t;
  };

  ConvOptions options{};
  options.pass = Pass::FC_TRAINING_WU;

  std::vector<multiconv::internal::ConvWeightUpdateArgs<float>> args{
      {makeInput(5), makeWeights(5), makeInput(5), 15, makeConvParams(5),
       options},
      {makeInput(6), makeWeights(6), makeInput(6), 16, makeConvParams(6),
       options},
      {makeInput(7), makeWeights(7), makeInput(7), 17, makeConvParams(7),
       options},
  };

  auto result = combine(args);
  // for now only the first scale is preserved... obviously needs fixing before
  // multi-convs are complete.
  BOOST_TEST(result.scale == 15);
  BOOST_TEST(*result.params == makeConvParams(5 + 6 + 7));
  BOOST_TEST(result.options == options);

  const unsigned iDim = 1;
  checkMappingEntirelyOneTile(graph, result.zDeltas.slice(0, 5 * inChans, iDim),
                              5);
  checkMappingEntirelyOneTile(
      graph, result.zDeltas.slice(5 * inChans, (5 + 6) * inChans, iDim), 6);
  checkMappingEntirelyOneTile(
      graph,
      result.zDeltas.slice((5 + 6) * inChans, (5 + 6 + 7) * inChans, iDim), 7);

  checkMappingEntirelyOneTile(
      graph, result.activations.slice(0, 5 * inChans, iDim), 5);
  checkMappingEntirelyOneTile(
      graph, result.activations.slice(5 * inChans, (5 + 6) * inChans, iDim), 6);
  checkMappingEntirelyOneTile(
      graph,
      result.activations.slice((5 + 6) * inChans, (5 + 6 + 7) * inChans, iDim),
      7);

  const unsigned wDim = 0;
  checkMappingEntirelyOneTile(graph, result.weights.slice(0, 5, wDim), 5);
  checkMappingEntirelyOneTile(graph, result.weights.slice(5, 5 + 6, wDim), 6);
  checkMappingEntirelyOneTile(graph,
                              result.weights.slice(5 + 6, 5 + 6 + 7, wDim), 7);
}

BOOST_AUTO_TEST_CASE(ReadConvParamsFromJSON) {
  std::stringstream is;
  is << R"(
    {
      "dataType": "float",
      "batchSize": 1,
      "numConvGroups": 2,
      "inputChannelsPerConvGroup": 3,
      "outputChannelsPerConvGroup": 4,
      "kernelShape": [50, 60],
      "inputFieldShape": [70, 80],
      "inputTransform": {
        "truncationLower": [1, 2],
        "paddingUpper": [3, 4],
        "flip": [true, false]
      },
      "outputTransform": {
        "stride": [5, 6]
      }
    }
  )";

  ConvParams uut{};
  is >> uut;

  ConvParams expected(FLOAT, 1, {70, 80}, {50, 60}, 3, 4, 2);
  expected.inputTransform.truncationLower = {1, 2};
  expected.inputTransform.paddingUpper = {3, 4};
  expected.inputTransform.flip = {true, false};
  expected.outputTransform.stride = {5, 6};
  BOOST_TEST(uut == expected);
}


#include <boost/filesystem.hpp>

#include "bvsma/include/functions.hpp"
#include "bvsma/include/ZKProofs.hpp"
#include "bvsma/include/SchnorrSignature.hpp"
#include "bvsma/include/fileFunctions.hpp"
#include "bvsma/include/functionsParallel.hpp"

using namespace boost::filesystem;

using namespace std;
typedef unsigned char byte;

/* Gen exp */

//  ./client 0 192 501 1 /home/obre/bvsma/data/BMDB.csv BMDB 36 16
//  ./client 0 224 501 1 /home/obre/bvsma/data/BMDB.csv BMDB 36 16
//  ./client 0 256 501 1 /home/obre/bvsma/data/BMDB.csv BMDB 36 16

//  ./client 0 192 501 1 /home/obre/bvsma/data/PUT.csv PUT 49 64
//  ./client 0 224 501 1 /home/obre/bvsma/data/PUT.csv PUT 49 64
//  ./client 0 256 501 1 /home/obre/bvsma/data/PUT.csv PUT 49 64

//  ./client 0 192 501 1 /home/obre/bvsma/data/FRGC.csv FRGC 94 64
//  ./client 0 224 501 1 /home/obre/bvsma/data/FRGC.csv FRGC 94 64
//  ./client 0 256 501 1 /home/obre/bvsma/data/FRGC.csv FRGC 94 64

/* Imp exp */

int main(int argc, char* argv[]){
    cout << "Run Malicious Client ..." << endl;

    boost::asio::io_service io_service;

    auto clientIp = IpAddress::from_string("127.0.0.1");
    auto serverIp = IpAddress::from_string("127.0.0.1");
    auto enrollServerIp = IpAddress::from_string("127.0.0.1");

    SocketPartyData clientParty(clientIp, 1222);
    SocketPartyData clientParty2(clientIp, 8002);

    SocketPartyData serverParty(serverIp, 1223);
    SocketPartyData enrollServerParty(enrollServerIp, 1224);

    shared_ptr<CommParty> withEnrollServer, client;

    if (stoi(argv[1]) == 0){
        withEnrollServer = make_shared<CommPartyTCPSynced>(io_service, clientParty2, enrollServerParty);
    }
    else if(stoi(argv[1]) == 1){
        client = make_shared<CommPartyTCPSynced>(io_service, clientParty, serverParty);
    }

    boost::thread t(boost::bind(&boost::asio::io_service::run, &io_service));

    string dirClient = "../../../data/client/"+ string(argv[6]) +"/P"+ string(argv[2]);
    string enrLogTimer = dirClient +"/MALenrClientLogTimer.csv";
    string verLogTimer = dirClient +"/MALverClientLogTimer.csv";
    string resultMatch = dirClient +"/MALresultMatch.csv";
    string keyStorage;
    string dirUser;

    string qBinsFile = "../../../data/lookupTables/"+ string(argv[6]) +"/"+ string(argv[6]) +"_qbins.csv";
    string sensorFile = string(argv[5]);
    string dirLLR = "../../../data/lookupTables/"+ string(argv[6]) +"/";

    vector<int> maxNFQ(stoi(argv[7]), stoi(argv[8]));

    string configFile = "../../../../libscapi/include/configFiles/NISTEC.txt";
    string curve = "P-"+string(argv[2]);

	auto dlog = make_shared<OpenSSLDlogECFp>(configFile, curve);
    auto g = dlog->getGenerator();
    auto q = dlog->getOrder();

    shared_ptr<ElGamalOnGroupElementEnc> elgamalTH = make_shared<ElGamalOnGroupElementEnc>(dlog);
    shared_ptr<ElGamalOnGroupElementEnc> elgamal = make_shared<ElGamalOnGroupElementEnc>(dlog);

    unsigned int seedClt = 149423;

    int resultInt;
    bool resultBool;

    string str;

    std::chrono::time_point<std::chrono::system_clock> globalStart;
    std::chrono::time_point<std::chrono::system_clock> localStart;
    std::chrono::time_point<std::chrono::system_clock> proofStart;

    std::ofstream out_file;

    try
    {

        if (stoi(argv[1]) == 0)
        {
            int count = stoi(argv[3]);
            int step = stoi(argv[4]);

            withEnrollServer->join(500, 5000);

            for (size_t i = 1; i < count; i=i+step)
            {
                out_file.open(enrLogTimer, ios::app);
                if (!out_file)
                {
                    throw "Error creating file"s;
                }
                cout << "======= Enrollment =======" << endl;
                localStart = std::chrono::system_clock::now();
                globalStart = localStart;

                auto pairSample = readSampleWithIDInLineFPD(sensorFile,  i  );
                auto userId = biginteger(pairSample.first);
                auto sample = pairSample.second;
                dirUser = dirClient +"/MAL/USERS/user"+userId.str();

                path pathUser(dirUser);
                create_directory(pathUser);
                dirUser = dirUser+"/";

                keyStorage = dirUser+"keystorage.txt";

                auto pair = elgamal->generateKey();
                elgamal->setKey(pair.first, pair.second);
                auto x = dynamic_cast<ElGamalPrivateKey*>(pair.second.get())->getX();
                writeBigintinFile(keyStorage, "client", "PrivKS", x);
                writePubKeyinFile(keyStorage, "client", "PubKS", pair.first);
                cout << "-> Client PrivKS and PubKS Generated and Stored  ..." << endl;

                sendBigint(withEnrollServer, userId);
                cout << "-> User " << userId << " Enrollment Request ..." << endl;

                sendElGamalPubKey(withEnrollServer, pair.first);
                cout << "-> Client PubKS Sent ..." << endl;

                auto pairTH = elgamalTH->generateKey();
                auto s1 = dynamic_cast<ElGamalPrivateKey*>(pairTH.second.get())->getX();
                writeBigintinFile(keyStorage, "client", "PrivTH", s1);

                auto clientPKeyTH = pairTH.first;
                writePubKeyinFile(keyStorage, "client", "PubTH", clientPKeyTH);
                cout << "-> Client PrivTH and PubTH Generated and Stored ..." << endl;

                sendElGamalPubKey(withEnrollServer, clientPKeyTH);
                cout << "-> Client PubTH Sent ..." << endl;

                auto verKey = receiveGpElt(withEnrollServer, dlog);
                writeGPELTinFile(keyStorage, "enrollServer", "VerKey", verKey);
                cout << "-> EnrollmentServer VerKey Received and stored ..." << endl;

                auto serverPKeyTH = receiveElGamalPubKey(withEnrollServer, dlog, elgamalTH);
                writePubKeyinFile(keyStorage, "server", "PubTH", serverPKeyTH);
                cout << "-> Server PubTH Received and Stored ..." << endl;

                auto jointPubKey = receiveElGamalPubKey(withEnrollServer, dlog, elgamalTH);
                elgamalTH->setKey(jointPubKey, pairTH.second);
                writePubKeyinFile(keyStorage, "joint", "jointTH", jointPubKey);
                writeCSVNew(out_file, localStart, false);
                cout << "-> Joint jointTH Received and stored ..." << endl;
                cout << "----> Keys Establishment DONE ..." << endl;

                cout << "-------------------------------------------" << endl;

                localStart = std::chrono::system_clock::now();
                auto thresholdEncSet = receiveVectElGamalCipher(withEnrollServer, dlog, elgamalTH);
                writeVectCipherinFile(dirUser+"thresholdEncSet.txt", thresholdEncSet);
                writeCSVNew(out_file, localStart, false);
                cout << "----> Threshold Set Encrypted and Permuted Received and Stored ..." << endl;

                cout << "-------------------------------------------" << endl;

                localStart = std::chrono::system_clock::now();

                auto colEncMap = encryptColIndices(dlog, elgamal, maxNFQ);
                auto quantizedSample = quantizefeaturesFPD(qBinsFile, sample);
                auto scoreEncMap = encryptRowsFromHELR(dirLLR, dlog, elgamalTH, quantizedSample);

                vector<int> vectBegin(maxNFQ.size(),0);
                map<int, vector<int>> permutation = generateMapIntPermutation(seedClt, maxNFQ.size(), vectBegin, maxNFQ);
                writePermutation(dirUser+"permutation.txt", permutation);
                cout << "-> Permutation Generated and Stored ..." << endl;

                auto partialTemplate = generatePartialTemplate(userId, permutation, colEncMap, scoreEncMap);
                sendMapOfVectComponent(withEnrollServer, partialTemplate);
                writeCSVNew(out_file, localStart, false);
                writeCSVNew(out_file, globalStart, true);
                cout << "-> Partial Template Generated and Sent ..." << endl;
                cout << "======= Enrollment END =======" << endl;

            }

        }
        else if(stoi(argv[1]) == 1){

            int count = stoi(argv[3]);
            int step = stoi(argv[4]);

            client->join(500, 5000);

            for (size_t i = 2; i < count; i=i+step)
            {
                out_file.open(verLogTimer, ios::app);
                if (!out_file)
                {
                    throw "Error creating file"s;
                }
                cout << "======= Verification =======" << endl;
                localStart = std::chrono::system_clock::now();
                globalStart = localStart;

                // cout << "--> Step 1: User's existence verification ..." << endl;

                auto pairSample = readSampleWithIDInLineFPD(sensorFile, i);
                auto userId = biginteger(pairSample.first);
                userId -= 1;
                sendBigint(client, userId);
                str = receiveStr(client);
                if( "Unregistered" == str){
                    throw "Unregistered User"s;
                }
                cout << "User " << userId << " is " << str << endl;
                writeCSVNew(out_file, localStart, false);

                // load
                localStart = std::chrono::system_clock::now();

                dirUser = dirClient +"/MAL/USERS/user"+userId.str()+"/";
                keyStorage = dirUser+"keystorage.txt";

                auto s1 = readBigintFromfile(keyStorage, "client", "PrivTH");
                ElGamalPrivateKey cltPrivKeyTH(s1);
                shared_ptr<PrivateKey> clientPrivKeyTH = make_shared<ElGamalPrivateKey>(cltPrivKeyTH);
                auto serverPKeyTH = readPubKeyFromfile(keyStorage, dlog, "server", "PubTH");
                auto jointPubKey = readPubKeyFromfile(keyStorage, dlog, "joint", "jointTH");
                elgamalTH->setKey(jointPubKey, clientPrivKeyTH);

                SchnorrSignature schnorr(dlog);
                auto verKey = readGPEltFromfile(keyStorage, dlog, "enrollServer", "VerKey");
                schnorr.setVerifKey(verKey);

                auto x = readBigintFromfile(keyStorage, "client", "PrivKS");
                ElGamalPrivateKey cltPrivKey(x);
                shared_ptr<PrivateKey> clientPrivKey = make_shared<ElGamalPrivateKey>(cltPrivKey);
                auto clientPKey = readPubKeyFromfile(keyStorage, dlog, "client", "PubKS");
                elgamal->setKey(clientPKey, clientPrivKey);

                auto thresholdEncSet = readVectCipherFromFile(dirUser+"thresholdEncSet.txt", dlog);

                writeCSVNew(out_file, localStart, false);
                cout << "----> Keys and Threshold Set loaded ..." << endl;
                cout << "--> Step 1: DONE ..." << endl;
                cout << endl;

                cout << "-------------------------------------------" << endl;
                cout << "--> Step 2: Probe verification ..." << endl;
                localStart = std::chrono::system_clock::now();

                auto sample = pairSample.second;
                vector<int> rawProbe = quantizefeaturesFPD(qBinsFile, sample);
                vector<biginteger> quantizedProbe(rawProbe.begin(), rawProbe.end());
                auto vectRforProbe = generateVectBigintRand_parallel( q, quantizedProbe.size());
                // auto vectRforProbe = generateVectBigintRand( q, quantizedProbe.size());

                auto probeEnc = generateVectElGamalCiphersFromVects_parallel(configFile, curve, clientPKey, quantizedProbe, vectRforProbe);
                // auto probeEnc = generateVectElGamalCiphersFromVects(dlog, elgamal, quantizedProbe, vectRforProbe);
                sendVectElGamalCipher(client, probeEnc);
                auto indexCompVect = readSpecificPermutation(dirUser+"permutation.txt", rawProbe);
                sendIntVect(client, indexCompVect);

                proofStart = std::chrono::system_clock::now();
                resultInt = zkPoKMultiANDPlainProver_parallel(client, configFile, curve, clientPKey, quantizedProbe, vectRforProbe);
                if( resultInt == 0){
                    client->writeWithSize("Incorrect Commitment");
                    throw "Incorrect Commitment"s;
                }
                client->writeWithSize("Correct Commitment");

                str = receiveStr(client);
                if( "Proof Failure" == str){
                    throw "Proof Failure"s;
                }
                cout << str << endl;
                writeCSVNew(out_file, proofStart, false);

                writeCSVNew(out_file, localStart, false);
                cout << "--> Step 2: DONE ..." << endl;
                cout << endl;

                cout << "-------------------------------------------" << endl;
                cout << "--> Step 3: Protected individual scores exchange ..." << endl;
                localStart = std::chrono::system_clock::now();

                auto requestedComp = receiveVectComponent(client, dlog, elgamal, elgamalTH);
                auto colEncFromTemp = extractColEncFromTemp_parallel(requestedComp);
                auto scoreEncFromTemp = extractScoreEncFromTemp_parallel(requestedComp);
                // auto colEncFromTemp = extractColEncFromTemp(requestedComp);
                // auto scoreEncFromTemp = extractScoreEncFromTemp(requestedComp);

                proofStart = std::chrono::system_clock::now();
                auto verSigResult = verifyVectCompSignatures_parallel(configFile, curve, verKey, requestedComp);
                if( verSigResult == false){
                    client->writeWithSize("Incorrect Signature");
                    throw "Incorrect Signature"s;
                }
                client->writeWithSize("Correct Signature");
                cout << "Correct Signature" << endl;
                writeCSVNew(out_file, proofStart, false);

                auto cipherDecZeroVect = subtractTwoCipherVect_parallel(configFile, curve, clientPKey, colEncFromTemp, probeEnc);
                // auto cipherDecZeroVect = subtractTwoCipherVect(dlog, elgamal, colEncFromTemp, probeEnc);

                proofStart = std::chrono::system_clock::now();
                resultInt = niZKMultiANDDecZeroProver_parallel(client, configFile, curve, clientPKey, cipherDecZeroVect, x);
                if( resultInt == 0){
                    throw "Proof Failure"s;
                }
                cout << "Successful Proof" << endl;
                writeCSVNew(out_file, proofStart, false);

                writeCSVNew(out_file, localStart, false);
                cout << "--> Step 3: DONE ..." << endl;
                cout << endl;

                cout << "-------------------------------------------" << endl;
                cout << "--> Step 4: Comparison of the similarity score with the threshold set ..." << endl;
                localStart = std::chrono::system_clock::now();

                // auto finalScore = multiplyCiphersOfVect_parallel(configFile, curve, jointPubKey, scoreEncFromTemp);
                auto finalScore = multiplyCiphersOfVect(elgamalTH, scoreEncFromTemp);

                auto comparisonVect = subtractOneCipherFromCipherVect_parallel(configFile, curve, jointPubKey, thresholdEncSet, finalScore);
                // auto comparisonVect = subtractOneCipherFromCipherVect(dlog, elgamalTH, thresholdEncSet, finalScore);

                auto comparisonVectBlinded = receiveVectElGamalCipher(client, dlog, elgamalTH);
                auto partialDecBlindedCompVect = receiveVectGpElt(client, dlog);

                proofStart = std::chrono::system_clock::now();
                // resultBool = niZKMultiANDBlindedVerifier_parallel(client, configFile, curve, jointPubKey, comparisonVect, comparisonVectBlinded);
                resultBool = niZKMultiANDBlindedVerifier(client, dlog, elgamalTH, comparisonVect, comparisonVectBlinded);
                if(resultBool == false){
                    throw "Proof Failure"s;
                }
                cout << "Successful Proof" << endl;
                writeCSVNew(out_file, proofStart, false);

                proofStart = std::chrono::system_clock::now();
                // resultBool = niZKMultiANDPartialDecVerifier_parallel(client, configFile, curve, serverPKeyTH, comparisonVectBlinded, partialDecBlindedCompVect);
                resultBool = niZKMultiANDPartialDecVerifier(client, dlog, serverPKeyTH, comparisonVectBlinded, partialDecBlindedCompVect);
                if(resultBool == false){
                    throw "Proof Failure"s;
                }
                cout << "Successful Proof" << endl;
                writeCSVNew(out_file, proofStart, false);

                writeCSVNew(out_file, localStart, false);
                cout << "--> Step 4: DONE ..." << endl;
                cout << endl;

                cout << "-------------------------------------------" << endl;
                cout << "--> Step 5: Verification of the similarity score with the threshold ..." << endl;
                localStart = std::chrono::system_clock::now();

                resultBool = finalDecryptionANDMatch(elgamalTH, comparisonVectBlinded, partialDecBlindedCompVect);
                if( resultBool == false){
                    client->writeWithSize("No Match");
                    writeResult(resultMatch, 0);
                    cout << "No Match" << endl;
                }else{
                    client->writeWithSize("Match");
                    writeResult(resultMatch, 1);
                    cout << "Match" << endl;
                }
                writeCSVNew(out_file, localStart, false);
                writeCSVNew(out_file, globalStart, true);
                cout << "--> Step 5: DONE ..." << endl;
                cout << endl;
                cout << "======= Verification END =======" << endl;

            }

        }

    }
    catch (const logic_error& e) {
        out_file.close();
        io_service.stop();
        t.join();
		cerr << e.what();
	}
    catch (string &e){
        out_file.close();
        io_service.stop();
        t.join();
        cerr << e << endl;
    }

    io_service.stop();
    t.join();

    cout << "END Malicious Client ..." << endl;
    return 0;

}


#ifndef H_trex_europa_interv_patch
# define H_trex_europa_interv_patch

#include <boost/numeric/interval.hpp>

namespace TREX {
  namespace europa {

    inline bool
    intersect(EUROPA::Domain& dom, EUROPA::edouble::basis_type lb, EUROPA::edouble::basis_type ub,
	      EUROPA::edouble::basis_type decimal_places)
    {
      EUROPA::edouble const p_inf = std::numeric_limits<EUROPA::edouble>::infinity();
      EUROPA::edouble const n_inf = std::numeric_limits<EUROPA::edouble>::minus_infinity();
      EUROPA::edouble const prec = decimal_places;

      if( lb>ub || lb>=p_inf || ub<=n_inf ) {
	dom.empty();
	return true;
      } else {
	EUROPA::edouble lo = n_inf, hi = p_inf;

	// std::cerr<<dom.toString()<<" * ["<<lb<<", "<<ub<<"]"<<std::flush;

	if( lb>lo ) {
	  lo = lb;
	  if( lo > dom.getUpperBound() )
	    lo -= prec;
	}
	if( ub<hi ) {
	  hi = ub;
	  if( hi < dom.getLowerBound() )
	    hi += prec;
	}
	bool ret = dom.intersect(lo, hi);

	// if( dom.isEmpty() )
	// 	std::cerr<<" EMPTY"<<std::endl;
	// else
	// 	std::cerr<<" = "<<dom.toString()<<std::endl;

	return ret;
      }
    }

    inline bool intersect(EUROPA::Domain &dom, EUROPA::edouble lb, EUROPA::edouble ub,
		   EUROPA::edouble::basis_type decimal_places) {

      EUROPA::edouble const prec = decimal_places;

      if( lb>dom.getUpperBound() &&
	  lb < std::numeric_limits<EUROPA::edouble>::infinity() )
	lb -= prec;
      if( ub<dom.getLowerBound() &&
	  ub > std::numeric_limits<EUROPA::edouble>::minus_infinity() )
	ub += prec;

      // std::cerr<<dom.toString()<<" * ["<<lb<<", "<<ub<<"]"<<std::endl;
      return dom.intersect(lb, ub);
    }

    /*
     * A replacement to boost::rounded_arith_opp as it has a bug under clang :
     *   - the to_int call made in the boost version needed to be replaced by
     *     Rounding::to_int to allow the compiler know that this static method
     *     is coming from the base class
     */
    template<class T, class Rounding>
    struct my_rounded_arith_opp: Rounding {
      void init() { this->upward(); }
# define BOOST_DN(EXPR)				\
      this->downward();				\
      T r = this->force_rounding(EXPR);		\
      this->upward();				\
      return r
# define BOOST_NR(EXPR)				\
      this->to_nearest();			\
      T r = this->force_rounding(EXPR);		\
      this->upward();				\
      return r
# define BOOST_UP(EXPR) return this->force_rounding(EXPR)
# define BOOST_UP_NEG(EXPR) return -this->force_rounding(EXPR)
      template<class U> T conv_down(U const &v) { BOOST_UP_NEG(-v); }
      template<class U> T conv_up  (U const &v) { BOOST_UP(v); }
      T add_down(const T& x, const T& y) { BOOST_UP_NEG((-x) - y); }
      T sub_down(const T& x, const T& y) { BOOST_UP_NEG(y - x); }
      T mul_down(const T& x, const T& y) { BOOST_UP_NEG(x * (-y)); }
      T div_down(const T& x, const T& y) { BOOST_UP_NEG(x / (-y)); }
      T add_up  (const T& x, const T& y) { BOOST_UP(x + y); }
      T sub_up  (const T& x, const T& y) { BOOST_UP(x - y); }
      T mul_up  (const T& x, const T& y) { BOOST_UP(x * y); }
      T div_up  (const T& x, const T& y) { BOOST_UP(x / y); }
      T median  (const T& x, const T& y) { BOOST_NR((x + y) / 2); }
      T sqrt_down(const T& x)
      { BOOST_NUMERIC_INTERVAL_using_math(sqrt); BOOST_DN(sqrt(x)); }
      T sqrt_up  (const T& x)
      { BOOST_NUMERIC_INTERVAL_using_math(sqrt); BOOST_UP(sqrt(x)); }
      T int_down(const T& x) { return -Rounding::to_int(-x); }
      T int_up  (const T& x) { return Rounding::to_int(x); }
# undef BOOST_DN
# undef BOOST_NR
# undef BOOST_UP
# undef BOOST_UP_NEG
    };

  }
}

#endif


#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/registration/icp.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl_conversions/pcl_conversions.h>
#include <boost/array.hpp>

// include for ROS
#include "ros/ros.h"
#include <tms_msg_ss/ods_get_robots_pos.h>
#include <tms_msg_ss/ods_pcd.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <tms_msg_rc/smartpal_control.h>
#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <stdlib.h>

ros::ServiceServer service;
ros::ServiceClient commander_to_kinect_capture;

int TABLE;

#define MAP_X 50
#define MAP_Y 50

typedef struct
{
  pcl::PointCloud< pcl::PointXYZ > cloud;
  pcl::PointCloud< pcl::PointXYZRGB > cloud_rgb;
  pcl::PointIndices inliers;
  pcl::PointXYZ g;
  int map[MAP_X][MAP_Y];
  int tf;
  double min_x;
  double max_x;
  double min_y;
  double max_y;
  double max_z;
  double min_z;
  int size_x;
  int size_y;
} object_map;

std::vector< object_map > Object_Map;

#define SMARTPAL4 0
#define SMARTPAL5 1
#define PI 3.1415926

double C_OFFSET_X;
double C_OFFSET_Y;
double C_OFFSET_Z;
double C_OFFSET_YAW;

typedef struct
{
  double min_x;
  double max_x;
  double min_y;
  double max_y;
  double min_z;
  double max_z;
} Model;

Model m_size;

typedef struct
{
  double x;
  double y;
  double theta;
} Robot;

typedef struct
{
  double x;
  double y;
  double z;
} Furniture;

Robot robot;
Furniture table;

pcl::PointCloud< pcl::PointXYZRGB > viewpoint;

void transformation(pcl::PointCloud< pcl::PointXYZRGB >& cloud, pcl::PointCloud< pcl::PointXYZ >& model)
{
  std::cout << "transformation" << std::endl;

  double camera_x = robot.x + C_OFFSET_Y * cos(robot.theta) + C_OFFSET_X * sin(robot.theta);
  double camera_y = robot.y + C_OFFSET_Y * sin(robot.theta) - C_OFFSET_X * cos(robot.theta);
  double camera_z = C_OFFSET_Z;
  double camera_yaw = C_OFFSET_YAW * (PI / 180.0);

  robot.theta *= PI / 180.0;

  Eigen::Matrix4f t1(Eigen::Matrix4f::Identity());

  t1(0, 0) = sin(robot.theta);
  t1(0, 1) = -sin(camera_yaw) * cos(robot.theta);
  t1(0, 2) = cos(camera_yaw) * cos(robot.theta);
  t1(0, 3) = camera_x;
  t1(1, 0) = -cos(robot.theta);
  t1(1, 1) = -sin(camera_yaw) * sin(robot.theta);
  t1(1, 2) = cos(camera_yaw) * sin(robot.theta);
  t1(1, 3) = camera_y;
  t1(2, 0) = 0;
  t1(2, 1) = -cos(camera_yaw);
  t1(2, 2) = -sin(camera_yaw);
  t1(2, 3) = camera_z;
  t1(3, 0) = 0;
  t1(3, 1) = 0;
  t1(3, 2) = 0;
  t1(3, 3) = 1;

  pcl::transformPointCloud(viewpoint, viewpoint, t1);
  pcl::transformPointCloud(cloud, cloud, t1);

  t1(0, 0) = 1;
  t1(0, 1) = 0;
  t1(0, 2) = 0;
  t1(0, 3) = table.x;
  t1(1, 0) = 0;
  t1(1, 1) = 1;
  t1(1, 2) = 0;
  t1(1, 3) = table.y;
  t1(2, 0) = 0;
  t1(2, 1) = 0;
  t1(2, 2) = 1;
  t1(2, 3) = table.z;
  t1(3, 0) = 0;
  t1(3, 1) = 0;
  t1(3, 2) = 0;
  t1(3, 3) = 1;

  pcl::transformPointCloud(model, model, t1);

  return;
}

void filtering(pcl::PointCloud< pcl::PointXYZRGB >& cloud, pcl::PointCloud< pcl::PointXYZ >& model,
               pcl::PointIndices& inliers)
{
  std::cout << "filtering" << std::endl;

  pcl::PointCloud< pcl::PointXYZRGB >::Ptr tmp_cloud(new pcl::PointCloud< pcl::PointXYZRGB >);

  m_size.min_x = 100.0;
  m_size.max_x = -100.0;
  m_size.min_y = 100.0;
  m_size.max_y = -100.0;
  m_size.min_z = 100.0;
  m_size.max_z = -100.0;

  for (int i = 0; i < model.points.size(); i++)
  {
    if (model.points[i].x < m_size.min_x)
      m_size.min_x = model.points[i].x;
    if (model.points[i].x > m_size.max_x)
      m_size.max_x = model.points[i].x;
    if (model.points[i].y < m_size.min_y)
      m_size.min_y = model.points[i].y;
    if (model.points[i].y > m_size.max_y)
      m_size.max_y = model.points[i].y;
    if (model.points[i].z < m_size.min_z)
      m_size.min_z = model.points[i].z;
    if (model.points[i].z > m_size.max_z)
      m_size.max_z = model.points[i].z;
  }

  for (int i = 0; i < cloud.points.size(); i++)
  {
    if (((m_size.min_x - 0.3 <= cloud.points[i].x) && (cloud.points[i].x <= m_size.max_x + 0.3)) &&
        ((m_size.min_y - 0.3 <= cloud.points[i].y) && (cloud.points[i].y <= m_size.max_y + 0.3)) &&
        ((m_size.min_z + 0.05 <= cloud.points[i].z) && (cloud.points[i].z <= m_size.max_z + 0.3)))
    {
      tmp_cloud->push_back(cloud.points[i]);
      inliers.indices.push_back(i);
    }
  }

  pcl::copyPointCloud(*tmp_cloud, cloud);

  return;
}

//ダウンサンプリングを行う
void downsampling(pcl::PointCloud< pcl::PointXYZ >& cloud, float th)
{
  pcl::VoxelGrid< pcl::PointXYZ > sor;
  sor.setInputCloud(cloud.makeShared());
  sor.setLeafSize(th, th, th);
  sor.filter(cloud);

  return;
}

//マッチング
void registration(pcl::PointCloud< pcl::PointXYZ >& cloud, pcl::PointCloud< pcl::PointXYZ >& model,
                  pcl::PointCloud< pcl::PointXYZ >& cloud_out, pcl::PointCloud< pcl::PointXYZRGB >& tmp_rgb,
                  Eigen::Matrix4f& m)
{
  pcl::IterativeClosestPoint< pcl::PointXYZ, pcl::PointXYZ > icp;
  icp.setInputSource(cloud.makeShared());
  icp.setInputTarget(model.makeShared());
  pcl::PointCloud< pcl::PointXYZ > Final;
  icp.align(Final);

  m = icp.getFinalTransformation();
  // std::cout << m << std::endl;

  pcl::transformPointCloud(cloud, cloud, m);
  pcl::transformPointCloud(tmp_rgb, tmp_rgb, m);
  pcl::transformPointCloud(viewpoint, viewpoint, m);
  pcl::copyPointCloud(tmp_rgb, cloud_out);

  cloud_out += model;

  return;
}

///////////////////////////////////////////////////////
//*******************main function*******************//
///////////////////////////////////////////////////////

bool robot_position(tms_msg_ss::ods_get_robots_pos::Request& req, tms_msg_ss::ods_get_robots_pos::Response& res)
{
  //***************************
  // local variable declaration
  //***************************
  pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud1(new pcl::PointCloud< pcl::PointXYZRGB >);
  pcl::PointCloud< pcl::PointXYZ >::Ptr model(new pcl::PointCloud< pcl::PointXYZ >);
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

  //***************************
  // capture kinect data
  //***************************
  tms_msg_ss::ods_pcd srv;
  srv.request.id = 3;
  if (commander_to_kinect_capture.call(srv))
  {
    pcl::fromROSMsg(srv.response.cloud, *cloud1);
  }

  if (SMARTPAL4)
  {
    C_OFFSET_X = 0.02;
    C_OFFSET_Y = 0.14;
    C_OFFSET_Z = 1.38;
    C_OFFSET_YAW = 21.6;
  }

  else if (SMARTPAL5)
  {
    C_OFFSET_X = 0.0;
    C_OFFSET_Y = 0.05;  // 0.01;
    C_OFFSET_Z = 1.05;
    C_OFFSET_YAW = 10.0;
  }

  viewpoint.resize(4);
  viewpoint[0].x = 0.0;
  viewpoint[0].y = 0.0;
  viewpoint[0].z = 0.0;
  viewpoint[1].x = 1.0;
  viewpoint[1].y = 0.0;
  viewpoint[1].z = 0.0;
  viewpoint[2].x = 0.0;
  viewpoint[2].y = 1.0;
  viewpoint[2].z = 0.0;
  viewpoint[3].x = 0.0;
  viewpoint[3].y = 0.0;
  viewpoint[3].z = 1.0;

  // std::cout << "viewpoint: " << viewpoint[0].x << " " << viewpoint[0].y << " " << viewpoint[0].z << std::endl;

  // pcl::io::loadPCDFile ("src/ods_robot_position/data/robot_position/table.pcd", *cloud1);
  // pcl::io::loadPCDFile ("src/ods_robot_position/data/robot_position/model_table1.pcd", *model);
  // pcl::io::loadPCDFile ("src/ods_robot_position/data/robot_position/model_shelf.pcd", *model);

  //***************************
  // Fill in the cloud data
  //***************************
  table.x = req.furniture.position.x / 1000.0;
  table.y = req.furniture.position.y / 1000.0;
  table.z = req.furniture.position.z / 1000.0;
  robot.x = req.robot.x / 1000.0;
  robot.y = req.robot.y / 1000.0;
  robot.theta = req.robot.theta;

  /*tms_msg_rc::smartpal_control srv3;
  srv3.request.unit = 1;
  srv3.request.cmd = 8;
  if(commander_to_get_robots_info.call(srv3)){
      robots.x = srv3.response.val[0];
      robots.y = srv3.response.val[1];
      robots.theta = srv3.response.val[2];
  }*/

  std::cout << "table: " << table.x << " " << table.y << " " << table.z << std::endl;
  std::cout << "robot: " << robot.x << " " << robot.y << " " << robot.theta << std::endl;
  pcl::fromROSMsg(req.model, *model);
  // pcl::io::savePCDFile("src/ods_robot_position/data/robot_position/model.pcd", *model);

  //***************************
  // transform input cloud
  //***************************
  pcl::PointCloud< pcl::PointXYZRGB >::Ptr tfm_cloud(new pcl::PointCloud< pcl::PointXYZRGB >);

  transformation(*cloud1, *model);

  pcl::copyPointCloud(*model, *tfm_cloud);

  for (int i = 0; i < tfm_cloud->points.size(); i++)
  {
    tfm_cloud->points[i].r = 255;
    tfm_cloud->points[i].g = 255;
    tfm_cloud->points[i].b = 255;
  }

  for (int i = 0; i < cloud1->points.size(); i++)
  {
    cloud1->points[i].r = 255;
    cloud1->points[i].g = 0;
    cloud1->points[i].b = 0;
  }

  *tfm_cloud += *cloud1;

  pcl::io::savePCDFile("src/ods_robot_position/data/robot_position/tfm_cloud1.pcd", *tfm_cloud);
  pcl::io::savePCDFile("src/rods_obot_position/data/robot_position/tfm_cloud2.pcd", *cloud1);

  //***************************
  // filtering by using model
  //***************************
  filtering(*cloud1, *model, *inliers);

  pcl::io::savePCDFile("src/ods_robot_position/data/robot_position/filter.pcd", *cloud1);

  pcl::PointCloud< pcl::PointXYZ >::Ptr dsp_cloud(new pcl::PointCloud< pcl::PointXYZ >);
  pcl::copyPointCloud(*cloud1, *dsp_cloud);
  downsampling(*dsp_cloud, 0.02);

  //***************************
  // registration between two input pcd data
  //***************************
  Eigen::Matrix4f m1;
  std::cout << "registration" << std::endl;
  int n = 0;
  while (1)
  {
    // registration
    pcl::PointCloud< pcl::PointXYZ >::Ptr m_cloud(new pcl::PointCloud< pcl::PointXYZ >);
    registration(*dsp_cloud, *model, *m_cloud, *cloud1, m1);

    if ((double)(m1(0, 0) + m1(1, 1) + m1(2, 2) + m1(3, 3)) >= 4)
    {
      if (n > 2)
        break;
    }
    n++;
  }

  std::cout << viewpoint[0] << std::endl;

  res.m_robot.theta = atan2(viewpoint[3].y - viewpoint[0].y, viewpoint[3].x - viewpoint[0].x) * 180.0 / PI;
  res.m_robot.x = 1000.0 * (viewpoint[0].x + C_OFFSET_X * cos((res.m_robot.theta - 90.0) * PI / 180) -
                            C_OFFSET_Y * cos(res.m_robot.theta * PI / 180));
  res.m_robot.y = 1000.0 * (viewpoint[0].y + C_OFFSET_X * sin((res.m_robot.theta - 90.0) * PI / 180) -
                            C_OFFSET_Y * sin(res.m_robot.theta * PI / 180));

  std::cout << res.m_robot.x << " " << res.m_robot.y << " " << viewpoint[0].z - C_OFFSET_Z << " " << res.m_robot.theta
            << std::endl;

  FILE* fp;

  if ((fp = fopen("src/ods_robot_position/data/robot_position/robot_position.txt", "a")) == NULL)
  {
    std::cout << "file open error" << std::endl;
  }

  fprintf(fp, "%lf %lf %lf\n", res.m_robot.x, res.m_robot.y, res.m_robot.theta);

  fclose(fp);

  return true;
}

int main(int argc, char** argv)
{
  printf("init\n");
  ros::init(argc, argv, "ods_robot_position");
  ros::NodeHandle n;

  service = n.advertiseService("ods_robot_position", robot_position);

  commander_to_kinect_capture = n.serviceClient< tms_msg_ss::ods_pcd >("capture_cloud");

  ros::spin();

  return 0;
}