// Copyright (c) 2020, OUXT-Polaris
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <polaris/types/type_base.hpp>
#include <polaris/built_in_functions/functions.hpp>
#include <polaris/exception.hpp>

#include <quaternion_operation/quaternion_operation.h>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/point.hpp>

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

#include <functional>
#include <unordered_map>
#include <memory>
#include <string>
#include <vector>

namespace polaris
{
namespace built_in_functions
{
boost::any Functions::fetchVariable(std::shared_ptr<peg::Ast> ast)
{
  if (variables_.count(ast->token) == 0) {
    POLARIS_THROW_EVALUATION_ERROR(ast->nodes[0],
      "variable " + ast->token + " did not defined yet.");
  }
  return variables_[ast->token];
}

boost::any Functions::constructBoolean(std::shared_ptr<peg::Ast> ast)
{
  if (ast->name == "BOOLEAN") {
    types::TypeBase<bool> bool_value;
    if (ast->token == "true") {
      bool_value.setValue(true);
    } else if (ast->token == "false") {
      bool_value.setValue(false);
    } else {
      POLARIS_THROW_EVALUATION_ERROR(ast, "bool value should be true or false");
    }
    return bool_value;
  }
  return boost::none;
}

boost::any Functions::constructEntity(std::shared_ptr<peg::Ast> ast)
{
  auto pose_value = evaluate(ast->nodes[0]->name, ast->nodes[0]);
  geometry_msgs::msg::Pose pose;
  if (pose_value.type() == typeid(types::TypeBase<geometry_msgs::msg::Pose>)) {
    pose = boost::any_cast<types::TypeBase<geometry_msgs::msg::Pose>>(pose_value).getValue();
  } else {
    POLARIS_THROW_EVALUATION_ERROR(ast->nodes[0], "failed to parse pose");
  }
  auto type_value = evaluate(ast->nodes[1]->name, ast->nodes[1]);
  std::string type;
  std::vector<std::string> types;
  if (type_value.type() == typeid(types::TypeBase<std::string>)) {
    type = boost::any_cast<types::TypeBase<std::string>>(type_value).getValue();
  } else if (type_value.type() == typeid(types::TypeBase<std::vector<std::string>>)) {
    types = boost::any_cast<types::TypeBase<std::vector<std::string>>>(type_value).getValue();
  } else {
    POLARIS_THROW_EVALUATION_ERROR(ast->nodes[1], "failed to parse types");
  }
  auto polygon_value = evaluate(ast->nodes[2]->name, ast->nodes[2]);
  std::vector<geometry_msgs::msg::Point> polygon;
  if (polygon_value.type() == typeid(types::TypeBase<std::vector<geometry_msgs::msg::Point>>)) {
    polygon =
      boost::any_cast<types::TypeBase<std::vector<geometry_msgs::msg::Point>>>(polygon_value).
      getValue();
  } else {
    POLARIS_THROW_EVALUATION_ERROR(ast->nodes[2], "failed to parse polygon");
  }
  if (types.size() == 0) {
    return types::TypeBase<polaris::types::Entity>(polaris::types::Entity(pose, type, polygon));
  } else {
    return types::TypeBase<polaris::types::Entity>(polaris::types::Entity(pose, types, polygon));
  }
  return boost::none;
}

boost::any Functions::constructArray(std::shared_ptr<peg::Ast> ast)
{
  if (ast->nodes.size() == 0) {
    POLARIS_THROW_EVALUATION_ERROR(ast,
      "array is empty");
  }
  const auto first_value = evaluate(ast->nodes[0]->name, ast->nodes[0]);
  const auto & value_type = first_value.type();
  if (value_type == typeid(types::TypeBase<int>)) {
    types::TypeBase<std::vector<int>> array;
    std::vector<int> array_value;
    for (const auto & node : ast->nodes) {
      auto value = evaluate(node->name, node);
      if (value.type() == typeid(types::TypeBase<int>)) {
        array_value.emplace_back(boost::any_cast<types::TypeBase<int>>(value).getValue());
      } else {
        POLARIS_THROW_EVALUATION_ERROR(ast, "array value is not int");
      }
    }
    array.setValue(array_value);
    return array;
  }
  if (value_type == typeid(types::TypeBase<double>)) {
    types::TypeBase<std::vector<double>> array;
    std::vector<double> array_value;
    for (const auto & node : ast->nodes) {
      auto value = evaluate(node->name, node);
      if (value.type() == typeid(types::TypeBase<double>)) {
        array_value.emplace_back(boost::any_cast<types::TypeBase<double>>(value).getValue());
      } else {
        POLARIS_THROW_EVALUATION_ERROR(ast, "array value is not double");
      }
    }
    array.setValue(array_value);
    return array;
  }
  if (value_type == typeid(types::TypeBase<std::string>)) {
    types::TypeBase<std::vector<std::string>> array;
    std::vector<std::string> array_value;
    for (const auto & node : ast->nodes) {
      auto value = evaluate(node->name, node);
      if (value.type() == typeid(types::TypeBase<std::string>)) {
        array_value.emplace_back(boost::any_cast<types::TypeBase<std::string>>(value).getValue());
      } else {
        POLARIS_THROW_EVALUATION_ERROR(ast, "array value is not string");
      }
    }
    array.setValue(array_value);
    return array;
  }
  if (value_type == typeid(types::TypeBase<bool>)) {
    types::TypeBase<std::vector<bool>> array;
    std::vector<bool> array_value;
    for (const auto & node : ast->nodes) {
      auto value = evaluate(node->name, node);
      if (value.type() == typeid(types::TypeBase<bool>)) {
        array_value.emplace_back(boost::any_cast<types::TypeBase<bool>>(value).getValue());
      } else {
        POLARIS_THROW_EVALUATION_ERROR(ast, "array value is not bool");
      }
    }
    array.setValue(array_value);
    return array;
  }
  if (value_type == typeid(types::TypeBase<geometry_msgs::msg::Quaternion>)) {
    types::TypeBase<std::vector<geometry_msgs::msg::Quaternion>> array;
    std::vector<geometry_msgs::msg::Quaternion> array_value;
    for (const auto & node : ast->nodes) {
      auto value = evaluate(node->name, node);
      if (value.type() == typeid(types::TypeBase<geometry_msgs::msg::Quaternion>)) {
        array_value.emplace_back(boost::any_cast<types::TypeBase<geometry_msgs::msg::Quaternion>>(
            value).getValue());
      } else {
        POLARIS_THROW_EVALUATION_ERROR(ast, "array value is not quaternion");
      }
    }
    array.setValue(array_value);
    return array;
  }
  if (value_type == typeid(types::TypeBase<geometry_msgs::msg::Point>)) {
    types::TypeBase<std::vector<geometry_msgs::msg::Point>> array;
    std::vector<geometry_msgs::msg::Point> array_value;
    for (const auto & node : ast->nodes) {
      auto value = evaluate(node->name, node);
      if (value.type() == typeid(types::TypeBase<geometry_msgs::msg::Point>)) {
        array_value.emplace_back(boost::any_cast<types::TypeBase<geometry_msgs::msg::Point>>(
            value).getValue());
      } else {
        POLARIS_THROW_EVALUATION_ERROR(ast, "array value is not point");
      }
    }
    array.setValue(array_value);
    return array;
  }
  if (value_type == typeid(types::TypeBase<geometry_msgs::msg::Pose>)) {
    types::TypeBase<std::vector<geometry_msgs::msg::Pose>> array;
    std::vector<geometry_msgs::msg::Pose> array_value;
    for (const auto & node : ast->nodes) {
      auto value = evaluate(node->name, node);
      if (value.type() == typeid(types::TypeBase<geometry_msgs::msg::Pose>)) {
        array_value.emplace_back(boost::any_cast<types::TypeBase<geometry_msgs::msg::Pose>>(
            value).getValue());
      } else {
        POLARIS_THROW_EVALUATION_ERROR(ast, "array value is not pose");
      }
    }
    array.setValue(array_value);
    return array;
  }
  if (value_type == typeid(types::TypeBase<types::Entity>)) {
    types::TypeBase<std::vector<types::Entity>> array;
    std::vector<types::Entity> array_value;
    for (const auto & node : ast->nodes) {
      auto value = evaluate(node->name, node);
      if (value.type() == typeid(types::TypeBase<types::Entity>)) {
        array_value.emplace_back(boost::any_cast<types::TypeBase<types::Entity>>(
            value).getValue());
      } else {
        POLARIS_THROW_EVALUATION_ERROR(ast, "array value is not pose");
      }
    }
    array.setValue(array_value);
    return array;
  }
  return boost::none;
}

boost::any Functions::constructString(std::shared_ptr<peg::Ast> ast)
{
  if (ast->name == "STRING") {
    types::TypeBase<std::string> str_value;
    str_value.setValue(ast->token);
    return str_value;
  }
  return boost::none;
}

boost::any Functions::constructInteger(std::shared_ptr<peg::Ast> ast)
{
  if (ast->name == "INTEGER") {
    try {
      types::TypeBase<int> int_value;
      int_value.setValue(std::stoi(ast->token));
      return int_value;
    } catch (std::invalid_argument) {
      POLARIS_THROW_EVALUATION_ERROR(ast,
        "failed to parse token into int value, std::invalid_argument");
    } catch (std::out_of_range) {
      POLARIS_THROW_EVALUATION_ERROR(ast,
        "failed to parse token into int value, std::out_of_range");
    }
  }
  return boost::none;
}

boost::any Functions::constructDouble(std::shared_ptr<peg::Ast> ast)
{
  if (ast->name == "DOUBLE" || ast->name == "INTEGER") {
    try {
      types::TypeBase<double> double_value;
      double_value.setValue(std::stod(ast->token));
      return double_value;
    } catch (std::invalid_argument) {
      POLARIS_THROW_EVALUATION_ERROR(ast,
        "failed to parse token into double value, std::invalid_argument");
    } catch (std::out_of_range) {
      POLARIS_THROW_EVALUATION_ERROR(ast,
        "failed to parse token into double value, std::out_of_range");
    }
  }
  return boost::none;
}

boost::any Functions::constructPose(std::shared_ptr<peg::Ast> ast)
{
  geometry_msgs::msg::Pose pose;
  if (ast->name == "ARGUMENTS") {
    if (ast->nodes[0]->name == "CALL") {
      if (ast->nodes[0]->nodes[0]->name == "IDENTIFIER") {
        boost::any val = evaluate(ast->nodes[0]->nodes[0]->token, ast->nodes[0]);
        if (ast->nodes[0]->nodes[0]->token == "point") {
          if (val.type() == typeid(types::TypeBase<geometry_msgs::msg::Point>)) {
            auto p = boost::any_cast<types::TypeBase<geometry_msgs::msg::Point>>(val).getValue();
            pose.position = p;
          } else {
            POLARIS_THROW_EVALUATION_ERROR(ast,
              "constracting point type failed");
          }
        } else {
          POLARIS_THROW_EVALUATION_ERROR(ast,
            "first argument shold be point type");
        }
      }
    } else if (ast->nodes[0]->name == "IDENTIFIER") {
      if (variables_.count(ast->nodes[0]->token) == 0) {
        POLARIS_THROW_EVALUATION_ERROR(ast,
          "variable " + ast->nodes[0]->token + " did not difined.");
      }
      if (variables_[ast->nodes[0]->token].type() ==
        typeid(types::TypeBase<geometry_msgs::msg::Point>))
      {
        pose.position =
          boost::any_cast<types::TypeBase<geometry_msgs::msg::Point>>(variables_[ast->nodes[0]->
            token]).getValue();
      }
    } else {
      POLARIS_THROW_EVALUATION_ERROR(ast, "name of the node is invalid");
    }
    if (ast->nodes[1]->name == "CALL") {
      if (ast->nodes[1]->nodes[0]->name == "IDENTIFIER") {
        boost::any val = evaluate(ast->nodes[1]->nodes[0]->token, ast->nodes[1]);
        if (ast->nodes[1]->nodes[0]->token == "quaternion") {
          if (val.type() == typeid(types::TypeBase<geometry_msgs::msg::Quaternion>)) {
            auto q =
              boost::any_cast<types::TypeBase<geometry_msgs::msg::Quaternion>>(val).getValue();
            pose.orientation = q;
          } else {
            POLARIS_THROW_EVALUATION_ERROR(ast,
              "constracting quaternion type failed");
          }
        } else {
          POLARIS_THROW_EVALUATION_ERROR(ast,
            "second argument shold be quaternion type");
        }
      }
    } else if (ast->nodes[1]->name == "IDENTIFIER") {
      if (variables_.count(ast->nodes[1]->token) == 0) {
        POLARIS_THROW_EVALUATION_ERROR(ast,
          "variable " + ast->nodes[1]->token + " did not difined.");
      }
      if (variables_[ast->nodes[1]->token].type() ==
        typeid(types::TypeBase<geometry_msgs::msg::Quaternion>))
      {
        pose.orientation =
          boost::any_cast<types::TypeBase<geometry_msgs::msg::Quaternion>>(
          variables_[ast->nodes[1]->token]).getValue();
      }
    } else {
      POLARIS_THROW_EVALUATION_ERROR(ast, "name of the node is invalid");
    }
  }
  types::TypeBase<geometry_msgs::msg::Pose> pose_value;
  pose_value.setValue(pose);
  return pose_value;
}

boost::any Functions::constructPoint(std::shared_ptr<peg::Ast> ast)
{
  geometry_msgs::msg::Point point;
  if (ast->name == "ARGUMENTS") {
    try {
      if (ast->nodes[0]->name == "CALL") {
        auto val = evaluate(ast->nodes[0]->nodes[0]->token, ast->nodes[0]->nodes[1]);
        if (val.type() != typeid(types::TypeBase<double>)) {
          POLARIS_THROW_EVALUATION_ERROR(ast, "failed to interprit as double value");
        }
        point.x = boost::any_cast<types::TypeBase<double>>(val).getValue();
      } else if (ast->nodes[0]->name == "IDENTIFIER") {
        point.x =
          boost::any_cast<types::TypeBase<double>>(variables_[ast->nodes[0]->token]).getValue();
      } else {
        point.x =
          boost::any_cast<types::TypeBase<double>>(constructDouble(ast->nodes[0])).getValue();
      }
      if (ast->nodes[1]->name == "CALL") {
        auto val = evaluate(ast->nodes[1]->nodes[0]->token, ast->nodes[1]->nodes[1]);
        if (val.type() != typeid(types::TypeBase<double>)) {
          POLARIS_THROW_EVALUATION_ERROR(ast, "failed to interprit as double value");
        }
        point.y = boost::any_cast<types::TypeBase<double>>(val).getValue();
      } else if (ast->nodes[1]->name == "IDENTIFIER") {
        point.y =
          boost::any_cast<types::TypeBase<double>>(variables_[ast->nodes[1]->token]).getValue();
      } else {
        point.y =
          boost::any_cast<types::TypeBase<double>>(constructDouble(ast->nodes[1])).getValue();
      }
      if (ast->nodes[2]->name == "CALL") {
        auto val = evaluate(ast->nodes[2]->nodes[0]->token, ast->nodes[2]->nodes[1]);
        if (val.type() != typeid(types::TypeBase<double>)) {
          POLARIS_THROW_EVALUATION_ERROR(ast, "failed to interprit as double value");
        }
        point.z = boost::any_cast<types::TypeBase<double>>(val).getValue();
      } else if (ast->nodes[2]->name == "IDENTIFIER") {
        point.z =
          boost::any_cast<types::TypeBase<double>>(variables_[ast->nodes[2]->token]).getValue();
      } else {
        point.z =
          boost::any_cast<types::TypeBase<double>>(constructDouble(ast->nodes[2])).getValue();
      }
      types::TypeBase<geometry_msgs::msg::Point> point_value;
      point_value.setValue(point);
      return point_value;
    } catch (boost::bad_any_cast) {
      POLARIS_THROW_EVALUATION_ERROR(ast,
        "failed to cast as double value in constructing point, boost::bad_any_cast");
    }
  }
  return boost::none;
}

boost::any Functions::constructQuaternionFromRpy(std::shared_ptr<peg::Ast> ast)
{
  geometry_msgs::msg::Vector3 rpy;
  if (ast->name == "ARGUMENTS") {
    try {
      if (ast->nodes[0]->name == "CALL") {
        auto val = evaluate(ast->nodes[0]->nodes[0]->token, ast->nodes[0]->nodes[1]);
        if (val.type() != typeid(types::TypeBase<double>)) {
          POLARIS_THROW_EVALUATION_ERROR(ast, "failed to interprit as double value");
        }
        rpy.x = boost::any_cast<types::TypeBase<double>>(val).getValue();
      } else if (ast->nodes[0]->name == "IDENTIFIER") {
        rpy.x =
          boost::any_cast<types::TypeBase<double>>(variables_[ast->nodes[0]->token]).getValue();
      } else {
        rpy.x =
          boost::any_cast<types::TypeBase<double>>(constructDouble(ast->nodes[0])).getValue();
      }
      if (ast->nodes[1]->name == "CALL") {
        auto val = evaluate(ast->nodes[1]->nodes[0]->token, ast->nodes[1]->nodes[1]);
        if (val.type() != typeid(types::TypeBase<double>)) {
          POLARIS_THROW_EVALUATION_ERROR(ast, "failed to interprit as double value");
        }
        rpy.y = boost::any_cast<types::TypeBase<double>>(val).getValue();
      } else if (ast->nodes[1]->name == "IDENTIFIER") {
        rpy.y =
          boost::any_cast<types::TypeBase<double>>(variables_[ast->nodes[1]->token]).getValue();
      } else {
        rpy.y =
          boost::any_cast<types::TypeBase<double>>(constructDouble(ast->nodes[1])).getValue();
      }
      if (ast->nodes[2]->name == "CALL") {
        auto val = evaluate(ast->nodes[2]->nodes[0]->token, ast->nodes[2]->nodes[1]);
        if (val.type() != typeid(types::TypeBase<double>)) {
          POLARIS_THROW_EVALUATION_ERROR(ast, "failed to interprit as double value");
        }
        rpy.z = boost::any_cast<types::TypeBase<double>>(val).getValue();
      } else if (ast->nodes[2]->name == "IDENTIFIER") {
        rpy.z =
          boost::any_cast<types::TypeBase<double>>(variables_[ast->nodes[2]->token]).getValue();
      } else {
        rpy.z =
          boost::any_cast<types::TypeBase<double>>(constructDouble(ast->nodes[2])).getValue();
      }
      types::TypeBase<geometry_msgs::msg::Quaternion> quat_value;
      geometry_msgs::msg::Quaternion quat =
        quaternion_operation::convertEulerAngleToQuaternion(rpy);
      quat_value.setValue(quat);
      return quat_value;
    } catch (boost::bad_any_cast) {
      POLARIS_THROW_EVALUATION_ERROR(ast,
        "failed to cast as double value in constructing quaternion, boost::bad_any_cast");
    }
  }
  return boost::none;
}

boost::any Functions::constructQuaternion(std::shared_ptr<peg::Ast> ast)
{
  geometry_msgs::msg::Quaternion quat;
  if (ast->name == "ARGUMENTS") {
    try {
      if (ast->nodes[0]->name == "CALL") {
        auto val = evaluate(ast->nodes[0]->nodes[0]->token, ast->nodes[0]->nodes[1]);
        if (val.type() != typeid(types::TypeBase<double>)) {
          POLARIS_THROW_EVALUATION_ERROR(ast, "failed to interprit as double value");
        }
        quat.x = boost::any_cast<types::TypeBase<double>>(val).getValue();
      } else if (ast->nodes[0]->name == "IDENTIFIER") {
        quat.x =
          boost::any_cast<types::TypeBase<double>>(variables_[ast->nodes[0]->token]).getValue();
      } else {
        quat.x =
          boost::any_cast<types::TypeBase<double>>(constructDouble(ast->nodes[0])).getValue();
      }
      if (ast->nodes[1]->name == "CALL") {
        auto val = evaluate(ast->nodes[1]->nodes[0]->token, ast->nodes[1]->nodes[1]);
        if (val.type() != typeid(types::TypeBase<double>)) {
          POLARIS_THROW_EVALUATION_ERROR(ast, "failed to interprit as double value");
        }
        quat.y = boost::any_cast<types::TypeBase<double>>(val).getValue();
      } else if (ast->nodes[1]->name == "IDENTIFIER") {
        quat.y =
          boost::any_cast<types::TypeBase<double>>(variables_[ast->nodes[1]->token]).getValue();
      } else {
        quat.y =
          boost::any_cast<types::TypeBase<double>>(constructDouble(ast->nodes[1])).getValue();
      }
      if (ast->nodes[2]->name == "CALL") {
        auto val = evaluate(ast->nodes[2]->nodes[0]->token, ast->nodes[2]->nodes[1]);
        if (val.type() != typeid(types::TypeBase<double>)) {
          POLARIS_THROW_EVALUATION_ERROR(ast, "failed to interprit as double value");
        }
        quat.z = boost::any_cast<types::TypeBase<double>>(val).getValue();
      } else if (ast->nodes[2]->name == "IDENTIFIER") {
        quat.z =
          boost::any_cast<types::TypeBase<double>>(variables_[ast->nodes[2]->token]).getValue();
      } else {
        quat.z =
          boost::any_cast<types::TypeBase<double>>(constructDouble(ast->nodes[2])).getValue();
      }
      if (ast->nodes[3]->name == "CALL") {
        auto val = evaluate(ast->nodes[3]->nodes[0]->token, ast->nodes[3]->nodes[1]);
        if (val.type() != typeid(types::TypeBase<double>)) {
          POLARIS_THROW_EVALUATION_ERROR(ast, "failed to interprit as double value");
        }
        quat.w = boost::any_cast<types::TypeBase<double>>(val).getValue();
      } else if (ast->nodes[3]->name == "IDENTIFIER") {
        quat.w =
          boost::any_cast<types::TypeBase<double>>(variables_[ast->nodes[3]->token]).getValue();
      } else {
        quat.w =
          boost::any_cast<types::TypeBase<double>>(constructDouble(ast->nodes[3])).getValue();
      }
      types::TypeBase<geometry_msgs::msg::Quaternion> quat_value;
      quat_value.setValue(quat);
      return quat_value;
    } catch (boost::bad_any_cast) {
      POLARIS_THROW_EVALUATION_ERROR(ast,
        "failed to cast as double value in constructing quaternion, boost::bad_any_cast");
    }
  }
  return boost::none;
}

boost::any Functions::multiplication(std::shared_ptr<peg::Ast> ast)
{
  auto v0 = evaluate(ast->nodes[0]->name, ast->nodes[0]);
  auto v1 = evaluate(ast->nodes[2]->name, ast->nodes[2]);
  // (quaternion value) * (quaternion value)
  if (v0.type() == typeid(types::TypeBase<geometry_msgs::msg::Quaternion>) &&
    v1.type() == typeid(types::TypeBase<geometry_msgs::msg::Quaternion>))
  {
    types::TypeBase<geometry_msgs::msg::Quaternion> ret;
    ret.setValue(boost::any_cast<types::TypeBase<geometry_msgs::msg::Quaternion>>(v0).getValue() *
      boost::any_cast<types::TypeBase<geometry_msgs::msg::Quaternion>>(v1).getValue());
    return ret;
  }
  // (double value) * (double value)
  if (v0.type() == typeid(types::TypeBase<double>) &&
    v1.type() == typeid(types::TypeBase<double>))
  {
    types::TypeBase<double> ret;
    ret.setValue(boost::any_cast<types::TypeBase<double>>(v0).getValue() *
      boost::any_cast<types::TypeBase<double>>(v1).getValue());
    return ret;
  }
  // (int value) * (double value)
  if (v0.type() == typeid(types::TypeBase<int>) &&
    v1.type() == typeid(types::TypeBase<double>))
  {
    types::TypeBase<double> ret;
    ret.setValue(boost::any_cast<types::TypeBase<int>>(v0).getValue() *
      boost::any_cast<types::TypeBase<double>>(v1).getValue());
    return ret;
  }
  // (double value) * (int value)
  if (v0.type() == typeid(types::TypeBase<double>) &&
    v1.type() == typeid(types::TypeBase<int>))
  {
    types::TypeBase<double> ret;
    ret.setValue(boost::any_cast<types::TypeBase<double>>(v0).getValue() *
      boost::any_cast<types::TypeBase<int>>(v1).getValue());
    return ret;
  }
  // (int value) * (int value)
  if (v0.type() == typeid(types::TypeBase<int>) &&
    v1.type() == typeid(types::TypeBase<int>))
  {
    types::TypeBase<int> ret;
    ret.setValue(boost::any_cast<types::TypeBase<int>>(v0).getValue() *
      boost::any_cast<types::TypeBase<int>>(v1).getValue());
    return ret;
  }
  POLARIS_THROW_EVALUATION_ERROR(ast, "multiplication operators did not defined yet.");
}

boost::any Functions::division(std::shared_ptr<peg::Ast> ast)
{
  auto v0 = evaluate(ast->nodes[0]->name, ast->nodes[0]);
  auto v1 = evaluate(ast->nodes[2]->name, ast->nodes[2]);
  // (double value) * (double value)
  if (v0.type() == typeid(types::TypeBase<double>) &&
    v1.type() == typeid(types::TypeBase<double>))
  {
    types::TypeBase<double> ret;
    ret.setValue(boost::any_cast<types::TypeBase<double>>(v0).getValue() /
      boost::any_cast<types::TypeBase<double>>(v1).getValue());
    return ret;
  }
  // (int value) * (double value)
  if (v0.type() == typeid(types::TypeBase<int>) &&
    v1.type() == typeid(types::TypeBase<double>))
  {
    types::TypeBase<double> ret;
    ret.setValue(boost::any_cast<types::TypeBase<int>>(v0).getValue() /
      boost::any_cast<types::TypeBase<double>>(v1).getValue());
    return ret;
  }
  // (double value) * (int value)
  if (v0.type() == typeid(types::TypeBase<double>) &&
    v1.type() == typeid(types::TypeBase<int>))
  {
    types::TypeBase<double> ret;
    ret.setValue(boost::any_cast<types::TypeBase<double>>(v0).getValue() /
      boost::any_cast<types::TypeBase<int>>(v1).getValue());
    return ret;
  }
  // (int value) * (int value)
  if (v0.type() == typeid(types::TypeBase<int>) &&
    v1.type() == typeid(types::TypeBase<int>))
  {
    types::TypeBase<int> ret;
    ret.setValue(boost::any_cast<types::TypeBase<int>>(v0).getValue() /
      boost::any_cast<types::TypeBase<int>>(v1).getValue());
    return ret;
  }
  POLARIS_THROW_EVALUATION_ERROR(ast, "division operators did not defined yet.");
}

boost::any Functions::subtraction(std::shared_ptr<peg::Ast> ast)
{
  auto v0 = evaluate(ast->nodes[0]->name, ast->nodes[0]);
  auto v1 = evaluate(ast->nodes[2]->name, ast->nodes[2]);
  // (double value) - (double value)
  if (v0.type() == typeid(types::TypeBase<double>) &&
    v1.type() == typeid(types::TypeBase<double>))
  {
    types::TypeBase<double> ret;
    ret.setValue(boost::any_cast<types::TypeBase<double>>(v0).getValue() -
      boost::any_cast<types::TypeBase<double>>(v1).getValue());
    return ret;
  }
  // (int value) - (double value)
  if (v0.type() == typeid(types::TypeBase<int>) &&
    v1.type() == typeid(types::TypeBase<double>))
  {
    types::TypeBase<double> ret;
    ret.setValue(static_cast<double>(boost::any_cast<types::TypeBase<int>>(v0).getValue()) -
      boost::any_cast<types::TypeBase<double>>(v1).getValue());
    return ret;
  }
  // (double value) - (int value)
  if (v0.type() == typeid(types::TypeBase<double>) &&
    v1.type() == typeid(types::TypeBase<int>))
  {
    types::TypeBase<double> ret;
    ret.setValue(boost::any_cast<types::TypeBase<double>>(v0).getValue() -
      static_cast<double>(boost::any_cast<types::TypeBase<int>>(v1).getValue()));
    return ret;
  }
  // (int value) - (int value)
  if (v0.type() == typeid(types::TypeBase<double>) &&
    v1.type() == typeid(types::TypeBase<int>))
  {
    types::TypeBase<int> ret;
    ret.setValue(boost::any_cast<types::TypeBase<int>>(v0).getValue() -
      boost::any_cast<types::TypeBase<int>>(v1).getValue());
    return ret;
  }
  POLARIS_THROW_EVALUATION_ERROR(ast, "subcraction operators did not defined yet.");
}

boost::any Functions::addition(std::shared_ptr<peg::Ast> ast)
{
  auto v0 = evaluate(ast->nodes[0]->name, ast->nodes[0]);
  auto v1 = evaluate(ast->nodes[2]->name, ast->nodes[2]);
  // (double value) + (double value)
  if (v0.type() == typeid(types::TypeBase<double>) &&
    v1.type() == typeid(types::TypeBase<double>))
  {
    types::TypeBase<double> ret;
    ret.setValue(boost::any_cast<types::TypeBase<double>>(v0).getValue() +
      boost::any_cast<types::TypeBase<double>>(v1).getValue());
    return ret;
  }
  // (int value) + (double value)
  if (v0.type() == typeid(types::TypeBase<int>) &&
    v1.type() == typeid(types::TypeBase<double>))
  {
    types::TypeBase<double> ret;
    ret.setValue(static_cast<double>(boost::any_cast<types::TypeBase<int>>(v0).getValue()) +
      boost::any_cast<types::TypeBase<double>>(v1).getValue());
    return ret;
  }
  // (double value) + (int value)
  if (v0.type() == typeid(types::TypeBase<double>) &&
    v1.type() == typeid(types::TypeBase<int>))
  {
    types::TypeBase<double> ret;
    ret.setValue(boost::any_cast<types::TypeBase<double>>(v0).getValue() +
      static_cast<double>(boost::any_cast<types::TypeBase<int>>(v1).getValue()));
    return ret;
  }
  // (int value) + (int value)
  if (v0.type() == typeid(types::TypeBase<double>) &&
    v1.type() == typeid(types::TypeBase<int>))
  {
    types::TypeBase<int> ret;
    ret.setValue(boost::any_cast<types::TypeBase<int>>(v0).getValue() +
      boost::any_cast<types::TypeBase<int>>(v1).getValue());
    return ret;
  }
  POLARIS_THROW_EVALUATION_ERROR(ast, "addition operators did not defined yet.");
}
}  // namespace built_in_functions
}  // namespace polaris


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2016 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#define BOOST_PYTHON_MAX_ARITY 46
#include <boost/python.hpp>
#include <cmath>
#include <iostream>
#include <functional>

#include "numpy_bind.hh"

#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

#include "graph_filtering.hh"

#include "graph.hh"
#include "graph_selectors.hh"
#include "graph_properties.hh"
#include "graph_util.hh"
#include "graph_python_interface.hh"

#include "random.hh"

#include "config.h"

#include "graph_blockmodel_covariates.hh"
#include "graph_blockmodel.hh"
#include "graph_blockmodel_overlap.hh"

using namespace graph_tool;
using namespace std;

template <class Eprop, class Vprop, class VVprop, class VEprop, class BMap>
struct cov_move_sweep_dispatch
{
    cov_move_sweep_dispatch(Eprop ce, VVprop cv, VVprop vmap,
                            vector<std::reference_wrapper<Eprop>>& eweight,
                            vector<std::reference_wrapper<Vprop>>& vweight,
                            vector<std::reference_wrapper<boost::any>>& egroups,
                            vector<std::reference_wrapper<VEprop>>& esrcpos,
                            vector<std::reference_wrapper<VEprop>>& etgtpos,
                            Vprop& label, vector<int>& vlist, bool deg_corr,
                            bool dense, bool multigraph, double beta,
                            bool sequential, bool parallel, bool random_move,
                            bool node_coherent, bool confine_layers, double c,
                            bool verbose, size_t meindex,
                            vector<size_t> max_edge_index, size_t nmerges,
                            size_t niter, Vprop merge_map,
                            vector<std::reference_wrapper<partition_stats_t>>& partition_stats,
                            vector<std::reference_wrapper<overlap_partition_stats_t>>& overlap_partition_stats,
                            vector<std::reference_wrapper<overlap_stats_t>>& overlap_stats,
                            vector<bool>& master, vector<bool>& slave,
                            rng_t& rng, double& S, size_t& nmoves,
                            vector<std::reference_wrapper<GraphInterface>>& bgi,
                            BMap& block_map,
                            vector<std::reference_wrapper<Vprop>>& block_rmap,
                            vector<std::reference_wrapper<vector<size_t>>>& free_blocks,
                            size_t B)

        : ce(ce), cv(cv), vmap(vmap), eweight(eweight), vweight(vweight),
          oegroups(egroups), esrcpos(esrcpos), etgtpos(etgtpos), label(label), vlist(vlist),
          deg_corr(deg_corr), dense(dense), multigraph(multigraph), beta(beta),
          sequential(sequential), parallel(parallel), random_move(random_move),
          node_coherent(node_coherent), confine_layers(confine_layers),
          c(c), verbose(verbose), meindex(meindex),
          max_edge_index(max_edge_index),
          nmerges(nmerges), niter(niter), merge_map(merge_map),
          partition_stats(partition_stats),
          overlap_partition_stats(overlap_partition_stats),
          overlap_stats(overlap_stats),
          master(master), slave(slave),
          rng(rng), S(S), nmoves(nmoves), bgi(bgi), block_map(block_map),
          block_rmap(block_rmap), free_blocks(free_blocks), B(B)
    {}

    Eprop ce;
    VVprop cv;
    VVprop vmap;
    vector<std::reference_wrapper<Eprop>>& eweight;
    vector<std::reference_wrapper<Vprop>>& vweight;
    vector<std::reference_wrapper<boost::any>> oegroups;
    vector<std::reference_wrapper<VEprop>>& esrcpos;
    vector<std::reference_wrapper<VEprop>>& etgtpos;
    Vprop& label;
    size_t n;
    vector<int>& vlist;
    bool deg_corr;
    bool dense;
    bool multigraph;
    double beta;
    bool sequential;
    bool parallel;
    bool random_move;
    bool node_coherent;
    bool confine_layers;
    double c;
    bool verbose;
    size_t meindex;
    vector<size_t> max_edge_index;
    size_t nmerges;
    size_t niter;
    Vprop merge_map;
    vector<std::reference_wrapper<partition_stats_t>>& partition_stats;
    vector<std::reference_wrapper<overlap_partition_stats_t>>& overlap_partition_stats;
    vector<std::reference_wrapper<overlap_stats_t>>& overlap_stats;
    vector<bool> master;
    vector<bool> slave;
    rng_t& rng;
    double& S;
    size_t& nmoves;
    vector<std::reference_wrapper<GraphInterface>>& bgi;
    BMap& block_map;
    vector<std::reference_wrapper<Vprop>>& block_rmap;
    vector<std::reference_wrapper<vector<size_t>>>& free_blocks;
    size_t B;

    template <class Graph>
    void operator()(vector<std::reference_wrapper<Eprop>>& mrs,
                    vector<std::reference_wrapper<Vprop>>& mrp,
                    vector<std::reference_wrapper<Vprop>>& mrm,
                    vector<std::reference_wrapper<Vprop>>& wr,
                    Vprop& b, vector<std::reference_wrapper<Vprop>>& bs,
                    vector<std::reference_wrapper<GraphInterface>>& bgis, Graph& g,
                    vector<std::reference_wrapper<GraphInterface>>& ags,
                    vector<std::reference_wrapper<boost::any>>& emat,
                    vector<std::reference_wrapper<boost::any>>& sampler,
                    vector<std::reference_wrapper<boost::any>>& cavity_sampler,
                    bool weighted) const
    {
        vector<std::reference_wrapper<Graph>> gs;
        for (GraphInterface& ag : ags)
            gs.push_back(*any_cast<Graph*>(ag.GetGraphView()));

        if (is_directed::apply<Graph>::type::value)
        {
            vector<std::reference_wrapper<GraphInterface::multigraph_t>> bgs;
            for (GraphInterface& bgi : bgis)
                bgs.push_back(bgi.GetGraph());
            dispatch(mrs, mrp, mrm, wr, b, bs, g, gs, emat, sampler,
                     cavity_sampler, bgs, weighted);
        }
        else
        {
            vector<UndirectedAdaptor<GraphInterface::multigraph_t>> ubgs;
            for (GraphInterface& bgi : bgis)
                ubgs.push_back(UndirectedAdaptor<GraphInterface::multigraph_t>(bgi.GetGraph()));
            vector<std::reference_wrapper<UndirectedAdaptor<GraphInterface::multigraph_t>>> rubgs;
            for (auto& bg : ubgs)
                rubgs.push_back(bg);
            dispatch(mrs, mrp, mrm, wr, b, bs, g, gs, emat, sampler,
                     cavity_sampler, rubgs, weighted);
        }
    }

    template <class Graph, class BGraph>
    void dispatch(vector<std::reference_wrapper<Eprop>>& mrs,
                  vector<std::reference_wrapper<Vprop>>& mrp,
                  vector<std::reference_wrapper<Vprop>>& mrm,
                  vector<std::reference_wrapper<Vprop>>& wr,
                  Vprop& b,
                  vector<std::reference_wrapper<Vprop>>& bs, Graph& g,
                  vector<std::reference_wrapper<Graph>>& gs,
                  vector<std::reference_wrapper<boost::any>>& aemat,
                  vector<std::reference_wrapper<boost::any>>& asampler,
                  vector<std::reference_wrapper<boost::any>>& acavity_sampler,
                  vector<std::reference_wrapper<BGraph>>& bg,
                  bool weighted) const
    {
        if (weighted)
        {
            typedef typename property_map_type::apply<DynamicSampler<std::tuple<typename graph_traits<Graph>::edge_descriptor, bool> >,
                                                      GraphInterface::vertex_index_map_t>::type vemap_t;
            vector<std::reference_wrapper<vemap_t>> egroups;
            for (auto& eg : oegroups)
                egroups.push_back(any_cast<vemap_t&>(eg));

            try
            {
                typedef typename get_emat_t::apply<BGraph>::type emat_t;
                vector<std::reference_wrapper<emat_t>> emat;
                for (auto& m : aemat)
                    emat.push_back(any_cast<emat_t&>(m));
                size_t B = num_vertices(bg[0].get());
                size_t max_BE = is_directed::apply<Graph>::type::value ?
                    B * B : (B * (B + 1)) / 2;
                vector<typename Eprop::unchecked_t> umrs;
                for (auto& m : mrs)
                    umrs.push_back(m.get().get_unchecked(max_BE));
                vector<std::reference_wrapper<typename Eprop::unchecked_t>> rumrs;
                for (auto& m : umrs)
                    rumrs.push_back(m);
                dispatch(rumrs, mrp, mrm, wr, b, bs, g, gs, asampler,
                         acavity_sampler, bg, egroups, emat);
            }
            catch (bad_any_cast)
            {
                typedef typename get_ehash_t::apply<BGraph>::type emat_t;
                vector<std::reference_wrapper<emat_t>> emat;
                for (auto& m : aemat)
                    emat.push_back(any_cast<emat_t&>(m));
                dispatch(mrs, mrp, mrm, wr, b, bs, g, gs, asampler,
                         acavity_sampler, bg, egroups, emat);
            }
        }
        else
        {
            typedef typename property_map_type::apply<vector<std::tuple<typename graph_traits<Graph>::edge_descriptor, bool> >,
                                                      GraphInterface::vertex_index_map_t>::type vemap_t;
            vector<std::reference_wrapper<vemap_t>> egroups;
            for (auto& eg : oegroups)
                egroups.push_back(any_cast<vemap_t&>(eg));

            try
            {
                typedef typename get_emat_t::apply<BGraph>::type emat_t;
                vector<std::reference_wrapper<emat_t>> emat;
                for (auto& m : aemat)
                    emat.push_back(any_cast<emat_t&>(m));
                dispatch(mrs, mrp, mrm, wr, b, bs, g, gs, asampler,
                         acavity_sampler, bg, egroups, emat);
            }
            catch (bad_any_cast)
            {
                typedef typename get_ehash_t::apply<BGraph>::type emat_t;
                vector<std::reference_wrapper<emat_t>> emat;
                for (auto& m : aemat)
                    emat.push_back(any_cast<emat_t&>(m));
                dispatch(mrs, mrp, mrm, wr, b, bs, g, gs, asampler,
                         acavity_sampler, bg, egroups, emat);
            }
        }
    }

    template <class Graph, class BGraph, class Egroups, class Emat, class EMprop>
    void dispatch(vector<std::reference_wrapper<EMprop>>& mrs,
                  vector<std::reference_wrapper<Vprop>>& mrp,
                  vector<std::reference_wrapper<Vprop>>& mrm,
                  vector<std::reference_wrapper<Vprop>>& wr,
                  Vprop& b,
                  vector<std::reference_wrapper<Vprop>>& bs, Graph& g,
                  vector<std::reference_wrapper<Graph>>& gs,
                  vector<std::reference_wrapper<boost::any>>& asampler,
                  vector<std::reference_wrapper<boost::any>>& acavity_sampler,
                  vector<std::reference_wrapper<BGraph>>& bgs,
                  vector<std::reference_wrapper<Egroups>>& egroups,
                  vector<std::reference_wrapper<Emat>>& emat) const
    {
        typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;

        typedef typename property_map<Graph, vertex_index_t>::type vindex_map_t;
        typedef typename property_map_type::apply<Sampler<vertex_t, boost::mpl::false_>,
                                                  vindex_map_t>::type::unchecked_t
            sampler_map_t;

        vector<std::reference_wrapper<sampler_map_t>> sampler;
        for (auto& s : asampler)
            sampler.push_back(any_cast<sampler_map_t&>(s.get()));
        vector<std::reference_wrapper<sampler_map_t>> cavity_sampler;
        for (auto& s : acavity_sampler)
            cavity_sampler.push_back(any_cast<sampler_map_t&>(s.get()));

        bool overlap = overlap_stats[0].get().is_enabled();
        if (!overlap)
        {
            typedef BlockState<Graph, BGraph, EMprop,
                               typename Eprop::unchecked_t,
                               typename Vprop::unchecked_t, Emat,
                               typename Egroups::unchecked_t,
                               typename VEprop::unchecked_t, sampler_map_t,
                               partition_stats_t,
                               overlap_stats_t,
                               typename BMap::value_type,
                               typename Vprop::unchecked_t> state_t;

            vector<state_t> states;
            vector<EntrySet<Graph>> m_entries;
            overlap_stats_t ostats;

            for (size_t i = 0; i < mrs.size(); ++i)
            {
                size_t eidx = random_move ? 1 : max_edge_index[i];

                state_t state = make_block_state(gs[i].get(),
                                                 eweight[i].get().get_unchecked(max_edge_index[i]),
                                                 vweight[i].get().get_unchecked(num_vertices(gs[i].get())),
                                                 bs[i].get().get_unchecked(num_vertices(gs[i].get())),
                                                 bgs[i].get(),
                                                 emat[i].get(),
                                                 mrs[i].get(),
                                                 mrp[i].get().get_unchecked(num_vertices(bgs[i].get())),
                                                 mrm[i].get().get_unchecked(num_vertices(bgs[i].get())),
                                                 wr[i].get().get_unchecked(num_vertices(bgs[i].get())),
                                                 egroups[i].get().get_unchecked(num_vertices(bgs[i].get())),
                                                 esrcpos[i].get().get_unchecked(eidx),
                                                 etgtpos[i].get().get_unchecked(eidx),
                                                 sampler[i].get(), cavity_sampler[i].get(),
                                                 partition_stats[i].get(), ostats,
                                                 block_map[i],
                                                 block_rmap[i].get().get_unchecked(num_vertices(bgs[i].get())),
                                                 free_blocks[i].get(),
                                                 master[i],
                                                 slave[i],
                                                 false);
                states.push_back(state);
                m_entries.emplace_back(num_vertices(bgs[i].get()));
            }

            move_sweep(states, m_entries,
                       wr[0].get().get_unchecked(B),
                       b.get_unchecked(num_vertices(g)),
                       cv.get_unchecked(num_vertices(g)),
                       vmap.get_unchecked(num_vertices(g)),
                       label.get_unchecked(B),
                       vlist, deg_corr,
                       dense, multigraph, beta,
                       eweight[0].get().get_unchecked(max_edge_index[0]),
                       vweight[0].get().get_unchecked(num_vertices(g)),
                       g, sequential, parallel, random_move, c,
                       nmerges,
                       merge_map.get_unchecked(num_vertices(g)),
                       niter, B,
                       verbose, rng, S, nmoves);
        }
        else
        {
            typedef BlockState<Graph, BGraph, EMprop,
                               typename Eprop::unchecked_t,
                               typename Vprop::unchecked_t, Emat,
                               typename Egroups::unchecked_t,
                               typename VEprop::unchecked_t, sampler_map_t,
                               overlap_partition_stats_t,
                               overlap_stats_t,
                               typename BMap::value_type,
                               typename Vprop::unchecked_t> state_t;

            vector<state_t> states;
            for (size_t i = 0; i < mrs.size(); ++i)
            {
                size_t eidx = random_move ? 1 : max_edge_index[i];

                state_t state = make_block_state(gs[i].get(),
                                                 eweight[i].get().get_unchecked(max_edge_index[i]),
                                                 vweight[i].get().get_unchecked(num_vertices(gs[i].get())),
                                                 bs[i].get().get_unchecked(num_vertices(gs[i].get())),
                                                 bgs[i].get(),
                                                 emat[i].get(),
                                                 mrs[i].get(),
                                                 mrp[i].get().get_unchecked(num_vertices(bgs[i].get())),
                                                 mrm[i].get().get_unchecked(num_vertices(bgs[i].get())),
                                                 wr[i].get().get_unchecked(num_vertices(bgs[i].get())),
                                                 egroups[i].get().get_unchecked(num_vertices(bgs[i].get())),
                                                 esrcpos[i].get().get_unchecked(eidx),
                                                 etgtpos[i].get().get_unchecked(eidx),
                                                 sampler[i].get(), cavity_sampler[i].get(),
                                                 overlap_partition_stats[i].get(),
                                                 overlap_stats[i].get(),
                                                 block_map[i],
                                                 block_rmap[i].get().get_unchecked(num_vertices(bgs[i].get())),
                                                 free_blocks[i].get(),
                                                 master[i],
                                                 slave[i],
                                                 false);
                states.push_back(state);
            }
            vector<SingleEntrySet<Graph>> m_entries(states.size());

            if (nmerges == 0)
            {
                if (!node_coherent)
                {
                    move_sweep_overlap(states, m_entries, overlap_stats[0].get(),
                                       wr[0].get().get_unchecked(B),
                                       b.get_unchecked(num_vertices(g)), cv,
                                       vmap, label.get_unchecked(B),
                                       vlist, deg_corr, dense, multigraph, beta,
                                       vweight[0].get().get_unchecked(num_vertices(g)), g,
                                       sequential, parallel, random_move, c, niter,
                                       B, verbose, rng, S, nmoves);
                }
                else
                {
                    vector<EntrySet<Graph>> m_entries;
                    for (auto& state : states)
                        m_entries.emplace_back(num_vertices(state.bg));
                    coherent_move_sweep_overlap(states, m_entries, overlap_stats[0].get(),
                                                wr[0].get().get_unchecked(B),
                                                b.get_unchecked(num_vertices(g)), cv,
                                                vmap, label.get_unchecked(B),
                                                vlist, deg_corr, dense, multigraph, beta,
                                                vweight[0].get().get_unchecked(num_vertices(g)), g,
                                                sequential, random_move, c,
                                                confine_layers, niter,
                                                B, rng, S, nmoves);
                }
            }
            else
            {
                merge_sweep_overlap(states, m_entries, overlap_stats[0].get(),
                                    wr[0].get().get_unchecked(B),
                                    b.get_unchecked(num_vertices(g)), ce, cv,
                                    vmap, label.get_unchecked(B), vlist,
                                    deg_corr, dense, multigraph, g,
                                    random_move, confine_layers, nmerges, niter,
                                    B, rng, S, nmoves);
            }
        }
    }
};

template <class Type>
vector<Type> from_list(boost::python::object list)
{
    vector<Type> v;
    for (int i = 0; i < boost::python::len(list); ++i)
        v.push_back(boost::python::extract<Type>(list[i])());
    return v;
};

template <class Type>
vector<std::reference_wrapper<Type>> from_rlist(boost::python::object list)
{
    vector<std::reference_wrapper<Type>> v;
    for (int i = 0; i < boost::python::len(list); ++i)
        v.emplace_back(boost::python::extract<Type&>(list[i])());
    return v;
};

template <class Type>
vector<std::reference_wrapper<Type>> from_any_list(boost::python::object list)
{
    vector<std::reference_wrapper<Type>> v;
    for (int i = 0; i < boost::python::len(list); ++i)
        v.emplace_back(any_cast<Type&>(boost::python::extract<boost::any&>(list[i])()));
    return v;
};

boost::python::object do_cov_move_sweep(GraphInterface& gi,
                                        boost::any& oce,
                                        boost::any& ocv,
                                        boost::any& ovmap,
                                        boost::python::object& ogis,
                                        boost::python::object& obgi,
                                        boost::python::object& oemat,
                                        boost::python::object& osampler,
                                        boost::python::object& ocavity_sampler,
                                        boost::python::object& omrs,
                                        boost::python::object& omrp,
                                        boost::python::object& omrm,
                                        boost::python::object& owr,
                                        boost::any& ob,
                                        boost::python::object& obs,
                                        bmap_t& bmap,
                                        boost::python::object& obrmap,
                                        boost::python::object& ofree_blocks,
                                        boost::python::object& omaster,
                                        boost::python::object& oslave,
                                        boost::any& olabel, vector<int>& vlist,
                                        bool deg_corr, bool dense,
                                        bool multigraph,
                                        boost::python::object& oeweight,
                                        boost::python::object& ovweight,
                                        boost::python::object& oegroups,
                                        boost::python::object& oesrcpos,
                                        boost::python::object& oetgtpos,
                                        double beta, bool sequential,
                                        bool parallel, bool random_move,
                                        boost::python::object onode_coherent,
                                        double c, bool weighted, size_t nmerges,
                                        boost::any omerge_map,
                                        size_t niter,
                                        size_t B,
                                        boost::python::object& opartition_stats,
                                        boost::python::object& ooverlap_partition_stats,
                                        boost::python::object& ooverlap_stats,
                                        bool verbose, rng_t& rng)
{
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::vertex_index_map_t>::type
        vmap_t;
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::edge_index_map_t>::type
        emap_t;
    typedef property_map_type::apply<vector<int32_t>,
                                     GraphInterface::vertex_index_map_t>::type
        vvmap_t;
    auto mrs = from_any_list<emap_t>(omrs);
    auto mrp = from_any_list<vmap_t>(omrp);
    auto mrm = from_any_list<vmap_t>(omrm);
    auto wr = from_any_list<vmap_t>(owr);
    vmap_t b = any_cast<vmap_t>(ob);
    auto bs = from_any_list<vmap_t>(obs);

    auto brmap = from_any_list<vmap_t>(obrmap);
    auto free_blocks = from_rlist<vector<size_t>>(ofree_blocks);

    auto eweight = from_any_list<emap_t>(oeweight);
    auto vweight = from_any_list<vmap_t>(ovweight);
    auto egroups = from_rlist<boost::any>(oegroups);
    auto esrcpos = from_any_list<emap_t>(oesrcpos);
    auto etgtpos = from_any_list<emap_t>(oetgtpos);

    vmap_t label = any_cast<vmap_t>(olabel);
    emap_t ce = any_cast<emap_t>(oce);
    vvmap_t cv = any_cast<vvmap_t>(ocv);
    vvmap_t vmap = any_cast<vvmap_t>(ovmap);

    double S = 0;
    size_t nmoves = 0;

    vmap_t merge_map = any_cast<vmap_t>(omerge_map);

    auto gis = from_rlist<GraphInterface>(ogis);

    vector<size_t> eidx;
    for (GraphInterface& g : gis)
        eidx.push_back(g.GetMaxEdgeIndex());

    auto bgi = from_rlist<GraphInterface>(obgi);

    auto partition_stats = from_rlist<partition_stats_t>(opartition_stats);
    auto overlap_partition_stats = from_rlist<overlap_partition_stats_t>(ooverlap_partition_stats);
    auto overlap_stats = from_rlist<overlap_stats_t>(ooverlap_stats);

    auto emat = from_rlist<boost::any>(oemat);

    auto sampler = from_rlist<boost::any>(osampler);
    auto cavity_sampler = from_rlist<boost::any>(ocavity_sampler);

    auto master = from_list<bool>(omaster);
    auto slave = from_list<bool>(oslave);

    bool node_coherent = python::extract<bool>(onode_coherent[0]);
    bool confine_layers = python::extract<bool>(onode_coherent[1]);

    run_action<graph_tool::detail::all_graph_views, boost::mpl::true_>()
        (gi, std::bind(cov_move_sweep_dispatch<emap_t, vmap_t, vvmap_t, emap_t, bmap_t>
                       (ce, cv, vmap, eweight, vweight, egroups, esrcpos, etgtpos,
                        label, vlist, deg_corr, dense, multigraph, beta,
                        sequential, parallel, random_move, node_coherent,
                        confine_layers, c, verbose,
                        gi.GetMaxEdgeIndex(), eidx, nmerges, niter, merge_map,
                        partition_stats, overlap_partition_stats, overlap_stats,
                        master, slave, rng, S, nmoves, bgi, bmap, brmap, free_blocks, B),
                       std::ref(mrs), std::ref(mrp), std::ref(mrm), std::ref(wr),
                       std::ref(b), std::ref(bs), std::ref(bgi), placeholders::_1, std::ref(gis),
                       std::ref(emat), std::ref(sampler), std::ref(cavity_sampler), weighted))();

    return boost::python::make_tuple(S, nmoves);
}

struct covariate_entropy
{
    template <class Graph, class Emap>
    void operator()(Graph& bg, Emap mrs, double& S) const
    {
        for (auto e : edges_range(bg))
            S -= lgamma_fast(mrs[e] + 1);
    }
};

double do_covariate_entropy(GraphInterface& gi, boost::any omrs)
{
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::edge_index_map_t>::type
        emap_t;
    emap_t mrs = any_cast<emap_t>(omrs);

    double S = 0;
    run_action<>()
        (gi, std::bind(covariate_entropy(),
                       placeholders::_1, mrs, std::ref(S)))();
    return S;
}

// void do_create_echash(GraphInterface& gi, size_t l, size_t L,
//                       boost::any& emat_orig, boost::any& emat)
// {
//     run_action<>()(gi, std::bind<void>(create_echash(), placeholders::_1, l, L,
//                                        std::ref(emat_orig), std::ref(emat)))();
// }

void do_ec_hist(GraphInterface& gi, boost::any& aevc, boost::any& aec)
{
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::edge_index_map_t>::type
        emap_t;
    typename emap_t::unchecked_t ec =
        any_cast<emap_t&>(aec).get_unchecked(gi.GetMaxEdgeIndex());
    run_action<>()(gi, std::bind<void>(ec_hist(), placeholders::_1,
                                       placeholders::_2, std::ref(ec)),
                   edge_properties())(aevc);
}

void do_split_graph(GraphInterface& gi, boost::any& aec, boost::any& ab,
                    boost::any& aeweight, boost::any& avweight, boost::any& avc,
                    boost::any& avmap, boost::python::object& ous,
                    boost::python::object& oub,
                    boost::python::object& oueweight,
                    boost::python::object& ouvweight,
                    bmap_t& bmap,
                    boost::python::object& obrmap,
                    boost::python::object& ouvmap)
{
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::vertex_index_map_t>::type
        vmap_t;
    typedef property_map_type::apply<vector<int32_t>,
                                     GraphInterface::vertex_index_map_t>::type
        vvmap_t;
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::edge_index_map_t>::type
        emap_t;

    emap_t& ec = any_cast<emap_t&>(aec);
    vmap_t& b = any_cast<vmap_t&>(ab);
    vmap_t& vweight = any_cast<vmap_t&>(avweight);
    emap_t& eweight = any_cast<emap_t&>(aeweight);
    vvmap_t& vc = any_cast<vvmap_t&>(avc);
    vvmap_t& vmap = any_cast<vvmap_t&>(avmap);

    auto us = from_rlist<GraphInterface>(ous);
    auto ub = from_any_list<vmap_t>(oub);
    auto uvweight = from_any_list<vmap_t>(ouvweight);
    auto ueweight = from_any_list<emap_t>(oueweight);

    auto rbmap = from_any_list<vmap_t>(obrmap);
    auto uvmap = from_any_list<vmap_t>(ouvmap);

    run_action<>()(gi, std::bind<void>(split_graph(), placeholders::_1,
                                       std::ref(ec), std::ref(b), std::ref(eweight),
                                       std::ref(vweight), std::ref(vc),
                                       std::ref(vmap), std::ref(us),
                                       std::ref(ub),
                                       std::ref(uvweight), std::ref(ueweight),
                                       std::ref(bmap), std::ref(rbmap),
                                       std::ref(uvmap)))();
}

bool bmap_has(const bmap_t& bmap, size_t c, size_t r)
{
    if (c > bmap.size())
        throw GraphException("invalid covariate value:" + lexical_cast<string>(c));
    auto iter = bmap[c].find(r);
    if (iter == bmap[c].end())
        return false;
    return true;
}

size_t bmap_get(const bmap_t& bmap, size_t c, size_t r)
{
    if (c > bmap.size())
        throw GraphException("invalid covariate value:" + lexical_cast<string>(c));
    auto iter = bmap[c].find(r);
    if (iter == bmap[c].end())
        throw GraphException("no mapping for block " + lexical_cast<string>(r)
                             + " in layer " + lexical_cast<string>(c));
    return iter->second;
}

void bmap_set(bmap_t& bmap, size_t c, size_t r, size_t r_u)
{
    if (c > bmap.size())
        throw GraphException("invalid covariate value:" + lexical_cast<string>(c));
    bmap[c][r] = r_u;
}

void bmap_del_c(bmap_t& bmap, size_t c)
{
    if (c > bmap.size())
        throw GraphException("invalid covariate value:" + lexical_cast<string>(c));
    bmap.erase(bmap.begin() + c);
}

bmap_t bmap_copy(const bmap_t& bmap)
{
    return bmap;
}

void export_blockmodel_covariate()
{
    boost::python::class_<bmap_t>("bmap_t")
        .def("has", bmap_has)
        .def("get", bmap_get)
        .def("set", bmap_set)
        .def("del_c", bmap_del_c)
        .def("copy", bmap_copy);

    boost::python::def("cov_move_sweep", do_cov_move_sweep);
    boost::python::def("covariate_entropy", do_covariate_entropy);
    // boost::python::def("create_echash", do_create_echash);
    boost::python::def("ec_hist", do_ec_hist);
    boost::python::def("split_graph", do_split_graph);
}


// -*- tab-width: 2; indent-tabs-mode: nil; coding: utf-8-with-signature -*-
#include <arcane/ItemPrinter.h>
#include <vector>

#include <boost/static_assert.hpp>

#include "Numerics/DiscreteOperator/AdvectionUpwind/AdvectionUpwindService.h"

#include "Utils/ItemGroupBuilder.h"

#include "Appli/IAppServiceMng.h"
#include "Numerics/BCs/IBoundaryCondition.h"

using namespace Arcane;

void AdvectionUpwindService::init() {
  if( m_initialized ) return;

  // Retrieve and initialize application service manager
  IAppServiceMng* app_service_mng = IAppServiceMng::instance(subDomain()->serviceMng());

  // Retrieve shared geometry service
  m_geometry_service = app_service_mng->find<IGeometryMng>(true);

  m_cells_group_name = IMPLICIT_UNIQ_NAME;
  m_faces_group_name = IMPLICIT_UNIQ_NAME;

  m_theta_upwind = options()->thetaUpwind();

  m_initialized = true;
}

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

void AdvectionUpwindService::prepare(const FaceGroup& internal_faces,
				     const FaceGroup& boundary_faces,
				     FaceGroup& c_internal_faces,
				     FaceGroup& cf_internal_faces,
				     CoefficientArrayT<Cell>* cell_coefficients,
				     CoefficientArrayT<Face>* face_coefficients) {

  if( !m_initialized )
    error() << " Numerical service not initialized";

  m_internal_faces = internal_faces;
  m_boundary_faces = boundary_faces;

  m_cell_coefficients = cell_coefficients;
  m_face_coefficients = face_coefficients;

  // Form face and cell groups
  ItemGroupBuilder<Face> faces_builder(m_internal_faces.mesh(), m_faces_group_name);
  faces_builder.add(m_internal_faces.enumerator());
  faces_builder.add(m_boundary_faces.enumerator());
  m_faces = faces_builder.buildGroup();

  ItemGroupBuilder<Face> c_internal_faces_builder(m_internal_faces.mesh(),
                                                  c_internal_faces.name());
  c_internal_faces_builder.add(m_internal_faces.enumerator());
  m_c_internal_faces = c_internal_faces_builder.buildGroup();

  ItemGroupBuilder<Face> cf_internal_faces_builder(m_internal_faces.mesh(),
                                                   cf_internal_faces.name());
  m_cf_internal_faces = cf_internal_faces_builder.buildGroup(); // Empty group

  ItemGroupBuilder<Cell> cells_builder(m_internal_faces.mesh(), m_cells_group_name);

  ItemGroupMapT<Face, Integer> cell_stencil_sizes(m_faces);
  ItemGroupMapT<Face, Integer> face_stencil_sizes(m_faces);

  ENUMERATE_FACE(iF, m_internal_faces) {
    const Face& F = *iF;

    cells_builder.add(F.cells());

    cell_stencil_sizes[F] = 2;
    face_stencil_sizes[F] = 0;
  }

  ENUMERATE_FACE(iF, m_boundary_faces) {
    const Face& F = *iF;

    cells_builder.add(F.cells());

    cell_stencil_sizes[F] = 1;
    face_stencil_sizes[F] = 1;
  }
  m_cells = cells_builder.buildGroup();

  // Compute stencils
  m_cell_coefficients->init(cell_stencil_sizes);
  m_face_coefficients->init(face_stencil_sizes);

  ENUMERATE_FACE(iF, m_internal_faces) {
    const Face& F = *iF;

    ArrayView<Integer> stencil_F = m_cell_coefficients->stencilLocalId(F);
    stencil_F[0] = F.backCell().localId();
    stencil_F[1] = F.frontCell().localId();
  }

  ENUMERATE_FACE(iF, m_boundary_faces) {
    const Face& F = *iF;

    info() << ItemPrinter(F);

    ArrayView<Integer> c_stencil_F = m_cell_coefficients->stencilLocalId(F);
    ArrayView<Integer> f_stencil_F = m_face_coefficients->stencilLocalId(F);

    c_stencil_F[0] = F.boundaryCell().localId();
    f_stencil_F[0] = F.localId();
  }

  m_prepared = true;
}

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

void AdvectionUpwindService::finalize() {
  m_prepared = false;
}

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

void AdvectionUpwindService::formDiscreteOperator(const VelocityFluxType& q) {
  if(!m_prepared) error() << " Numerical service not prepared";

  ENUMERATE_FACE(iF, m_internal_faces) {
    const Face& F = *iF;

    // Retrieve neighbour cells
    // const Cell& T0 = F.backCell();
    // const Cell& T1 = F.frontCell();

    // Determine face orientation
    // By convention n = n(back->front)
    const Real alpha = 1.;

    // Get face velocity flux
    const Real& qF = q[F];

    // Build theta upwind scheme
    const Real& thetaF = m_theta_upwind ;
    const Real a0 = (qF*alpha>0) ? thetaF : (1-thetaF);

    // Compute transmissivities
    StencilFluxCoeffType tau_F = m_cell_coefficients->coefficients(F);
    tau_F[0] = a0*qF;
    tau_F[1] = (1-a0)*qF;
  }

  // Boundary faces
  ENUMERATE_FACE(iF, m_boundary_faces) {
    const Face& F = *iF;

    // Retrieve neighbour cell
    const Cell& T0  = F.boundaryCell();

    // Determine output-face orientation
    const Real alpha = (F.backCell() == T0) ? 1. : -1.;

    // Get face velocity flux (with output-face orientation)
    const Real& qF = q[F]*alpha;

    // Build theta upwind scheme
    const Real& thetaF  = m_theta_upwind;
    Real a0 = (qF>0) ? thetaF : (1-thetaF);

    // Compute transmissivities
    StencilFluxCoeffType c_tau_F = m_cell_coefficients->coefficients(F);
    StencilFluxCoeffType f_tau_F = m_face_coefficients->coefficients(F);
    c_tau_F[0] = a0*qF;
    f_tau_F[0] = (1-a0)*qF;
  }
}

ARCANE_REGISTER_SERVICE_ADVECTIONUPWIND(AdvectionUpwind,
					AdvectionUpwindService);


// Copyright (C) 2010 Anders Logg
//
// This file is part of DOLFIN.
//
// DOLFIN is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// DOLFIN is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with DOLFIN. If not, see <http://www.gnu.org/licenses/>.
//
// First added:  2010-02-23
// Last changed: 2011-03-17

#include <boost/scoped_array.hpp>
#include <dolfin/common/constants.h>
#include "GenericMatrix.h"

using namespace dolfin;

//-----------------------------------------------------------------------------
void GenericMatrix::ident_zeros()
{
  std::vector<uint> columns;
  std::vector<double> values;
  std::vector<uint> zero_rows;

  // Check which rows are zero
  for (uint row = 0; row < size(0); row++)
  {
    // Get value for row
    getrow(row, columns, values);

    // Get maximum value in row
    double max = 0.0;
    for (uint k = 0; k < values.size(); k++)
      max = std::max(max, std::abs(values[k]));

    // Check if row is zero
    if (max < DOLFIN_EPS)
      zero_rows.push_back(row);
  }

  // Write a message
  log(TRACE, "Found %d zero row(s), inserting ones on the diagonal.", zero_rows.size());

  // Insert one on the diagonal for rows with only zeros. Note that we
  // are not calling ident() since that fails in PETSc if nothing
  // has been assembled into those rows.
  for (uint i = 0; i < zero_rows.size(); i++)
  {
    std::pair<uint, uint> ij(zero_rows[i], zero_rows[i]);
    setitem(ij, 1.0);
  }

  // Apply changes
  apply("insert");
}
//-----------------------------------------------------------------------------


/*
 *  Copyright 2010-2011 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  OpenCAMlib is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef KDTREE_H
#define KDTREE_H

#include <iostream>
#include <list>

#include <boost/foreach.hpp>

#include "kdnode.hpp"
#include "bbox.hpp"
#include "millingcutter.hpp"
#include "clpoint.hpp"
#include "numeric.hpp"

namespace ocl
{

class Point;
class CLPoint;
class Triangle;
class MillingCutter;

/// \brief KDTree spread, a measure of how spread-out a list of triangles are.
///
/// simple struct-like class for storing the "spread" or maximum
/// extent of a list of triangles. Used by the kd-tree algorithm.
class Spread {
    public:
        /// constructor
        Spread(int dim, double v, double s) {
            d = dim;
            val = v;
            start = s;
        };
        /// dimension
        int d;
        /// spread-value
        double val;
        /// minimum or start value
        double start;
        /// comparison of Spread objects. Used for finding the largest spread
        /// along which the next partition/cut is made.
        static bool spread_compare(Spread *x, Spread *y) {
            if (x->val > y->val)
                return true;
            else
                return false;
        };
};

/// a kd-tree for storing triangles and fast searching for triangles
/// that overlap the cutter
template <class BBObj>
class KDTree {
    public:
        KDTree() {};
        virtual ~KDTree() {
            // std::cout << " ~KDTree()\n";
            delete root;
        }
        /// set the bucket-size
        void setBucketSize(int b){
            //std::cout << "KDTree::setBucketSize = " << b << "\n";
            bucketSize = b;
        }
        /// set the search dimension to the XY-plane
        void setXYDimensions(){
            //std::cout << "KDTree::setXYDimensions()\n";
            dimensions.clear();
            dimensions.push_back(0); // x
            dimensions.push_back(1); // x
            dimensions.push_back(2); // y
            dimensions.push_back(3); // y
        } // for drop-cutter search in XY plane
        /// set search-plane to YZ
        void setYZDimensions(){ // for X-fibers
            //std::cout << "KDTree::setYZDimensions()\n";
            dimensions.clear();
            dimensions.push_back(2); // y
            dimensions.push_back(3); // y
            dimensions.push_back(4); // z
            dimensions.push_back(5); // z
        } // for X-fibers
        /// set search plane to XZ
        void setXZDimensions(){ // for Y-fibers
            //std::cout << "KDTree::setXZDimensions()\n";
            dimensions.clear();
            dimensions.push_back(0); // x
            dimensions.push_back(1); // x
            dimensions.push_back(4); // z
            dimensions.push_back(5); // z
        } // for Y-fibers
        /// build the kd-tree based on a list of input objects
        void build(const std::list<BBObj>& list){
            //std::cout << "KDTree::build() list.size()= " << list.size() << " \n";
            root = build_node( &list, 0, NULL );
        }
        /// search for overlap with input Bbox bb, return found objects
        std::list<BBObj>* search( const Bbox& bb ){
            assert( !dimensions.empty() );
            std::list<BBObj>* tris = new std::list<BBObj>();
            this->search_node( tris, bb, root );
            return tris;
        }
        /// search for overlap with a MillingCutter c positioned at cl, return found objects
        std::list<BBObj>* search_cutter_overlap(const MillingCutter* c, CLPoint* cl ){
            double r = c->getRadius();
            // build a bounding-box at the current CL
            Bbox bb( cl->x-r, cl->x+r, cl->y-r, cl->y+r, cl->z, cl->z+c->getLength() );
            return this->search( bb );
        }
        /// string repr
        std::string str() const;

    protected:
        /// build and return a KDNode containing list *tris at depth dep.
        KDNode<BBObj>* build_node(     const std::list<BBObj> *tris,  // triangles
                                        int dep,                       // depth of node
                                        KDNode<BBObj> *par)   {       // parent node
            //std::cout << "KDNode::build_node list.size()=" << tris->size() << "\n";

            if (tris->size() == 0 ) { //this is a fatal error.
                std::cout << "ERROR: KDTree::build_node() called with tris->size()==0 ! \n";
                assert(0);
                return 0;
            }
            Spread* spr = calc_spread(tris); // calculate spread in order to know how to cut
            double cutvalue = spr->start + spr->val/2; // cut in the middle
            //std::cout << " cutvalue= " << cutvalue << "\n";
            if ( (tris->size() <= bucketSize) ||  isZero_tol( spr->val ) ) {  // then return a bucket/leaf node
                //std::cout << "KDNode::build_node BUCKET list.size()=" << tris->size() << "\n";
                KDNode<BBObj> *bucket;   //  dim   cutv   parent   hi    lo   triangles depth
                bucket = new KDNode<BBObj>(spr->d, cutvalue , par , NULL, NULL, tris, dep);
                assert( bucket->isLeaf );
                delete spr;
                return bucket; // this is the leaf/end of the recursion-tree
            }
            // build lists of triangles for hi and lo child nodes
            std::list<BBObj>* lolist = new std::list<BBObj>();
            std::list<BBObj>* hilist = new std::list<BBObj>();
            BOOST_FOREACH(BBObj t, *tris) { // loop through each triangle and put it in either lolist or hilist
                if (t.bb[spr->d] > cutvalue)
                    hilist->push_back(t);
                else
                    lolist->push_back(t);
            }

            /*
            if (hilist->empty() || lolist->empty()) {// an error ??
                std::cout << "kdtree: hilist.size()==0! or lolist.size()==0! \n";
                std::cout << "kdtree: tris->size()= " << tris->size()<< "\n";
                std::cout << "kdtree: hilist.size()= " << hilist->size()<< "\n";
                std::cout << "kdtree: lolist.size()= " << lolist->size()<< "\n";
                BOOST_FOREACH(BBObj t, *tris) {
                    std::cout << t << "\n";
                    std::cout << t.bb << "\n";
                }
                std::cout << "kdtree: spr->d= " << spr->d << "\n";
                std::cout << "kdtree: cutvalue= " << cutvalue << "\n";
                assert(0);
            }*/

            // create the current node  dim     value    parent  hi   lo   trilist  depth
            KDNode<BBObj> *node = new KDNode<BBObj>(spr->d, cutvalue, par, NULL,NULL,NULL, dep);
            // create the child-nodes through recursion
            //                    list    depth   parent
            if (!hilist->empty())
                node->hi = build_node(hilist, dep+1, node);
            //else
                //std::cout << "hilist empty!\n";

            if (!lolist->empty()) {
                node->lo = build_node(lolist, dep+1, node);
            } else {
                //std::cout << "lolist empty!\n";
            }

            lolist->clear();
            hilist->clear();
            delete spr;
            delete lolist;
            delete hilist;

            return node; // return a new node
        };

        /// calculate the spread of list *tris
        Spread* calc_spread(const std::list<BBObj> *tris) {
            std::vector<double> maxval( 6 );
            std::vector<double> minval( 6 );
            if ( tris->empty() ) {
                std::cout << " ERROR, KDTree::calc_spread() called with tris->size()==0 ! \n";
                assert( 0 );
                return NULL;
            } else {
                // find out the maximum spread
                //std::cout << "calc_spread()...\n";
                bool first=true;
                BOOST_FOREACH(BBObj t, *tris) { // check each triangle
                    for (unsigned int m=0;m<dimensions.size();++m) {
                        // dimensions[m] is the dimensions we want to update
                        // t.bb[ dimensions[m] ]   is the update value
                        if (first) {
                            maxval[ dimensions[m] ] = t.bb[ dimensions[m] ];
                            minval[ dimensions[m] ] = t.bb[ dimensions[m] ];
                            if (m==(dimensions.size()-1) )
                                first=false;
                        } else {
                            if (maxval[ dimensions[m] ] < t.bb[ dimensions[m] ] )
                                maxval[ dimensions[m] ] = t.bb[ dimensions[m] ];
                            if (minval[ dimensions[m] ] > t.bb[ dimensions[m] ])
                                minval[ dimensions[m] ] = t.bb[ dimensions[m] ];
                        }
                    }
                }
                std::vector<Spread*> spreads;// calculate the spread along each dimension
                for (unsigned int m=0;m<dimensions.size();++m) {   // dim,  spread, start
                    spreads.push_back( new Spread(dimensions[m] ,
                                       maxval[dimensions[m]]-minval[dimensions[m]],
                                       minval[dimensions[m]] ) );
                }// priority-queue could also be used ??
                assert( !spreads.empty() );
                //std::cout << " spreads.size()=" << spreads.size() << "\n";
                std::sort(spreads.begin(), spreads.end(), Spread::spread_compare); // sort the list
                Spread* s= new Spread(*spreads[0]); // this is the one we want to return
                while(!spreads.empty()) delete spreads.back(), spreads.pop_back(); // delete the others
                //std::cout << "calc_spread() done\n";
                return s; // select the biggest spread and return
            } // end tris->size != 0
        } // end spread();

        /// search kd-tree starting at *node, looking for overlap with bb, and placing
        /// found objects in *tris
        void search_node( std::list<BBObj> *tris, const Bbox& bb, KDNode<BBObj> *node) {
            if (node->isLeaf ) { // we found a bucket node, so add all triangles and return.

                BOOST_FOREACH( BBObj t, *(node->tris) ) {
                        tris->push_back(t);
                }
                //std::cout << " search_node Leaf bucket tris-size() = " << tris->size() << "\n";
                return; // end recursion
            } else if ( (node->dim % 2) == 0) { // cutting along a min-direction: 0, 2, 4
                // not a bucket node, so recursevily seach hi/lo branches of KDNode
                unsigned int maxdim = node->dim+1;
                if ( node->cutval > bb[maxdim] ) { // search only lo
                    search_node(tris, bb, node->lo );
                } else { // need to search both child nodes
                    if (node->hi)
                        search_node(tris, bb, node->hi );
                    if (node->lo)
                        search_node(tris, bb, node->lo );
                }
            } else { // cutting along a max-dimension: 1,3,5
                unsigned int mindim = node->dim-1;
                if ( node->cutval < bb[mindim] ) { // search only hi
                    search_node(tris, bb, node->hi);
                } else { // need to search both child nodes
                    if (node->hi)
                        search_node(tris, bb, node->hi);
                    if (node->lo)
                        search_node(tris, bb, node->lo);
                }
            }
            return; // Done. We get here after all the recursive calls above.
        } // end search_kdtree();
    // DATA
        /// bucket size of tree
        unsigned int bucketSize;
        /// pointer to root KDNode
        KDNode<BBObj>* root;
        /// the dimensions in this kd-tree
        std::vector<int> dimensions;
};

} // end ocl namespace
#endif
// end file kdtree.hpp