//------------------------------------------------------------------------------
// @file   : tms_rp_voronoi_map.cpp
// @brief  : voronoi map for robot's path planning
// @author : Yoonseok Pyo, Kouhei Nakashima
// @version: Ver0.0.3 (since 2015.08.10)
// @date   : 2015.09.03
//------------------------------------------------------------------------------
// include for ROS
#include <ros/ros.h>
#include <ros/package.h>
#include <visualization_msgs/Marker.h>

#include <tms_msg_db/TmsdbGetData.h>
#include <tms_msg_db/TmsdbStamped.h>
#include <tms_msg_db/Tmsdb.h>

#include <tms_msg_ss/tracking_points.h>

#include <tms_msg_rp/rps_map_full.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <boost/date_time/posix_time/posix_time.hpp>

// include for std
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <sstream>
#include <iostream>
#include <vector>

namespace tms_rp
{
using namespace std;
using Eigen::Vector3d;
using Eigen::Matrix3d;
typedef Eigen::Vector3d Vector3;
typedef Eigen::Matrix3d Matrix3;

class CollisionMapData
{
public:
  bool object_;
  double dist_from_obj_;
  bool collision_;
  bool voronoi_;
  int thinning_flg_;
  double dist_from_voronoi_;
  bool path_;
  double dist_from_path_;
  double dist_from_goal_;
  bool person_view_;
  double table_height_;
};

class TmsRpVoronoiMap
{
private:
  // ROS NodeHandle
  ros::NodeHandle nh;
  ros::NodeHandle nh_priv;
  // ROS Timer
  ros::Timer static_map_update_timer;
  ros::Timer dynamic_map_update_timer;
  // ROS Topic Subscriber
  ros::Publisher static_map_pub_;
  ros::Publisher dynamic_map_pub_;
  vector< vector< CollisionMapData > > collision_map_;
  tms_msg_rp::rps_map_full static_map_;
  tms_msg_rp::rps_map_full dynamic_map_;
  string result_msg_;
  ros::ServiceClient get_data_client_;
  ros::Publisher nonvoronoi_map_marker_pub, static_map_marker_pub, dynamic_marker_pub;
  // ROS Parameters:
  double update_time;
  bool is_debug;

public:
  TmsRpVoronoiMap()
    : nh_priv("~")
    , update_time(1)
    ,  // sec
    is_debug(false)
  {
    // Init parameter
    nh_priv.param("update_time", update_time, update_time);
    nh_priv.param("is_debug", is_debug, is_debug);
    ROS_ASSERT(initTmsRpVoronoiMap());
    // Subscriber for tms_db_data topic
    get_data_client_ = nh.serviceClient< tms_msg_db::TmsdbGetData >("/tms_db_reader");
    static_map_pub_ = nh.advertise< tms_msg_rp::rps_map_full >("rps_map_data", 1);
    dynamic_map_pub_ = nh.advertise< tms_msg_rp::rps_map_full >("rps_dynamic_map", 1);
    nonvoronoi_map_marker_pub = nh.advertise< visualization_msgs::Marker >("nonvoronoi_map_marker", 1);
    static_map_marker_pub = nh.advertise< visualization_msgs::Marker >("voronoi_map_marker", 1);
    dynamic_marker_pub = nh.advertise< visualization_msgs::Marker >("dynamic_map_marker", 1);
    // TimerEvent
    static_map_update_timer = nh.createTimer(ros::Duration(update_time), &TmsRpVoronoiMap::staticMapPublish, this);
    dynamic_map_update_timer = nh.createTimer(ros::Duration(update_time), &TmsRpVoronoiMap::dynamicMapPublish, this);

    initCollisionMap(collision_map_);
    setVoronoiLine(collision_map_, result_msg_);
    calcDistFromObj(collision_map_, result_msg_);
    convertMap(collision_map_, static_map_);
  }

  ~TmsRpVoronoiMap()
  {
    ROS_ASSERT(shutdownTmsRpVoronoiMap());
  }

private:
  double x_llimit_, x_ulimit_, y_llimit_, y_ulimit_, cell_size_;  // Meter
  bool initTmsRpVoronoiMap()
  {
    ROS_INFO("tms_rp_voronoi_map : Init OK!\n");
    return true;
  }

  bool shutdownTmsRpVoronoiMap()
  {
    return true;
  }

  bool initCollisionMap(vector< vector< CollisionMapData > >& map)
  {
    FILE* fp;
    string file_name;
    char home_dir[255];
    const char* fname;

    //      strcpy(home_dir, getenv("HOME"));
    //      file_name = home_dir;
    //      file_name += "/catkin_ws/src/ros_tms/tms_rp/tms_rp_action/map/use_collision_map.csv";
    file_name = ros::package::getPath("tms_rp_voronoi_map") + "/map/use_collision_map.csv";
    ROS_INFO(file_name.c_str());
    fname = file_name.c_str();

    while (1)
    {
      if ((fp = fopen(fname, "r")) == NULL)
      {
        cout << "Error: collision map cannot open" << endl;
        return false;
      }
      else
        break;
    }

    map.clear();
    vector< CollisionMapData > tempMapLine;
    CollisionMapData tempMapData;

    char* tp, buff[4096];

    if (fgets(buff, 4096, fp) == NULL)
      return false;

    tp = strtok(buff, ",");
    x_llimit_ = atof(tp);
    tp = strtok(NULL, ",");
    x_ulimit_ = atof(tp);
    tp = strtok(NULL, ",");
    y_llimit_ = atof(tp);
    tp = strtok(NULL, ",");
    y_ulimit_ = atof(tp);
    tp = strtok(NULL, ",");
    cell_size_ = atof(tp);

    int CommaCount = 0;

    while (fgets(buff, 4096, fp) != NULL)  // collision
    {
      CommaCount = 0;
      tp = strtok(buff, ",");
      tempMapData.object_ = atof(tp);

      if (tempMapData.object_)
      {
        tempMapData.dist_from_obj_ = 0.0;
        tempMapData.collision_ = true;
      }
      else
      {
        tempMapData.dist_from_obj_ = (x_ulimit_ - x_llimit_) * (y_ulimit_ - y_llimit_);
        tempMapData.collision_ = false;
      }

      tempMapData.voronoi_ = false;
      tempMapData.path_ = false;
      tempMapData.thinning_flg_ = 0;
      tempMapData.dist_from_voronoi_ = (x_ulimit_ - x_llimit_) * (y_ulimit_ - y_llimit_);
      tempMapData.dist_from_goal_ = (x_ulimit_ - x_llimit_) * (y_ulimit_ - y_llimit_);
      tempMapData.dist_from_path_ = (x_ulimit_ - x_llimit_) * (y_ulimit_ - y_llimit_);
      tempMapLine.push_back(tempMapData);

      CommaCount++;

      while (tp != NULL)
      {
        if (CommaCount == int(round((y_ulimit_ - y_llimit_) / cell_size_)) + 1)
          break;

        tp = strtok(NULL, ",");
        CommaCount++;

        if (tp != NULL)
        {
          tempMapData.object_ = atof(tp);
          if (tempMapData.object_)
          {
            tempMapData.dist_from_obj_ = 0.0;
            tempMapData.collision_ = true;
          }
          else
          {
            tempMapData.dist_from_obj_ = (x_ulimit_ - x_llimit_) * (y_ulimit_ - y_llimit_);
            tempMapData.collision_ = false;
          }

          tempMapData.voronoi_ = false;
          tempMapData.path_ = false;
          tempMapData.thinning_flg_ = 0;
          tempMapData.dist_from_voronoi_ = (x_ulimit_ - x_llimit_) * (y_ulimit_ - y_llimit_);
          tempMapData.dist_from_goal_ = (x_ulimit_ - x_llimit_) * (y_ulimit_ - y_llimit_);
          tempMapData.dist_from_path_ = (x_ulimit_ - x_llimit_) * (y_ulimit_ - y_llimit_);
          tempMapLine.push_back(tempMapData);
        }
      }

      map.push_back(tempMapLine);
      tempMapLine.clear();
    }

    fclose(fp);

    return true;
  }

  bool setVoronoiLine(vector< vector< CollisionMapData > >& map, string& message)
  {
    if (map.empty())
    {
      message = "Error : Map is empty";
      cout << message << endl;
      return false;
    }

    for (unsigned int x = 0; x < map.size(); x++)
    {
      for (unsigned int y = 0; y < map[x].size(); y++)
      {
        map[x][y].voronoi_ = false;
        if (!map[x][y].collision_)
          map[x][y].thinning_flg_ = 1;
        else
          map[x][y].thinning_flg_ = 0;
      }
    }

    bool flg = true;

    while (flg)
    {
      flg = false;
      for (int k = 0; k < 4; k++)
      {
        for (int x = 1; x < map.size() - 1; x++)
        {
          for (int y = 1; y < map[x].size() - 1; y++)
          {
            int i, j;

            switch (k)
            {
              case 0:
                i = x + 1;
                j = y;
                break;
              case 1:
                i = x;
                j = y - 1;
                break;
              case 2:
                i = x - 1;
                j = y;
                break;
              case 3:
                i = x;
                j = y + 1;
                break;
            }

            if ((map[x][y].thinning_flg_ == 1) && (map[i][j].thinning_flg_ == 0))
            {
              if (((map[x][y - 1].thinning_flg_ == 0) && (map[x + 1][y].thinning_flg_ == 1) &&
                   (map[x][y + 1].thinning_flg_ == 1) && (map[x + 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x][y - 1].thinning_flg_ == 1) &&
                   (map[x - 1][y].thinning_flg_ == 1) && (map[x + 1][y].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x][y - 1].thinning_flg_ == 1) &&
                   (map[x - 1][y].thinning_flg_ == 1) && (map[x][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y].thinning_flg_ == 0) && (map[x + 1][y].thinning_flg_ == 1) &&
                   (map[x][y + 1].thinning_flg_ == 1) && (map[x + 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y].thinning_flg_ == 0) && (map[x + 1][y].thinning_flg_ == 0) &&
                   (map[x][y + 1].thinning_flg_ == 1)) ||
                  ((map[x][y - 1].thinning_flg_ == 0) && (map[x - 1][y].thinning_flg_ == 1) &&
                   (map[x][y + 1].thinning_flg_ == 0)) ||
                  ((map[x][y - 1].thinning_flg_ == 1) && (map[x - 1][y].thinning_flg_ == 0) &&
                   (map[x + 1][y].thinning_flg_ == 0)) ||
                  ((map[x][y - 1].thinning_flg_ == 0) && (map[x + 1][y].thinning_flg_ == 1) &&
                   (map[x][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y].thinning_flg_ == 0) && (map[x - 1][y + 1].thinning_flg_ == 1) &&
                   (map[x][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 1) && (map[x][y - 1].thinning_flg_ == 0) &&
                   (map[x - 1][y].thinning_flg_ == 0)) ||
                  ((map[x][y - 1].thinning_flg_ == 0) && (map[x + 1][y - 1].thinning_flg_ == 1) &&
                   (map[x + 1][y].thinning_flg_ == 0)) ||
                  ((map[x + 1][y].thinning_flg_ == 0) && (map[x][y + 1].thinning_flg_ == 0) &&
                   (map[x + 1][y + 1].thinning_flg_ == 1)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x][y - 1].thinning_flg_ == 1) &&
                   (map[x + 1][y - 1].thinning_flg_ == 0) && (map[x - 1][y].thinning_flg_ == 1) &&
                   (map[x + 1][y].thinning_flg_ == 1) && (map[x - 1][y + 1].thinning_flg_ == 0) &&
                   (map[x + 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x][y - 1].thinning_flg_ == 1) &&
                   (map[x + 1][y - 1].thinning_flg_ == 0) && (map[x + 1][y].thinning_flg_ == 1) &&
                   (map[x - 1][y + 1].thinning_flg_ == 0) && (map[x][y + 1].thinning_flg_ == 1) &&
                   (map[x + 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x + 1][y - 1].thinning_flg_ == 0) &&
                   (map[x - 1][y].thinning_flg_ == 1) && (map[x + 1][y].thinning_flg_ == 1) &&
                   (map[x - 1][y + 1].thinning_flg_ == 0) && (map[x][y + 1].thinning_flg_ == 1) &&
                   (map[x + 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x][y - 1].thinning_flg_ == 1) &&
                   (map[x + 1][y - 1].thinning_flg_ == 0) && (map[x - 1][y].thinning_flg_ == 1) &&
                   (map[x - 1][y + 1].thinning_flg_ == 0) && (map[x][y + 1].thinning_flg_ == 1) &&
                   (map[x + 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x][y - 1].thinning_flg_ == 0) &&
                   (map[x + 1][y - 1].thinning_flg_ == 0) && (map[x - 1][y].thinning_flg_ == 0) &&
                   (map[x - 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x][y - 1].thinning_flg_ == 0) &&
                   (map[x + 1][y - 1].thinning_flg_ == 0) && (map[x + 1][y].thinning_flg_ == 0) &&
                   (map[x + 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x - 1][y].thinning_flg_ == 0) &&
                   (map[x - 1][y + 1].thinning_flg_ == 0) && (map[x][y + 1].thinning_flg_ == 0) &&
                   (map[x + 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x + 1][y - 1].thinning_flg_ == 0) && (map[x + 1][y].thinning_flg_ == 0) &&
                   (map[x - 1][y + 1].thinning_flg_ == 0) && (map[x][y + 1].thinning_flg_ == 0) &&
                   (map[x + 1][y + 1].thinning_flg_ == 0)))
                map[x][y].thinning_flg_ = 1;
              else
              {
                flg = true;
                map[x][y].thinning_flg_ = 3;
              }
            }
          }
        }
        for (int x = 1; x < map.size() - 1; x++)
        {
          for (int y = 1; y < map[x].size() - 1; y++)
          {
            if (map[x][y].thinning_flg_ == 3)
              map[x][y].thinning_flg_ = 0;
          }
        }
      }
    }

    for (int x = 0; x < map.size(); x++)
    {
      for (int y = 0; y < map[x].size(); y++)
      {
        if (map[x][y].thinning_flg_ == 1)
        {
          map[x][y].voronoi_ = 1;
          map[x][y].dist_from_voronoi_ = 0.0;
        }
        else
        {
          map[x][y].voronoi_ = 0;
        }
      }
    }
    return true;
  }

  bool calcDistFromObj(vector< vector< CollisionMapData > >& map, string& message)
  {
    if (map.empty())
    {
      message = "Error : Map is empty";
      cout << message << endl;
      return false;
    }

    for (int x = 0; x < map.size(); x++)
    {
      for (int y = 0; y < map[x].size(); y++)
      {
        if (map[x][y].object_)
          map[x][y].dist_from_obj_ = 0.0;
        else
          map[x][y].dist_from_obj_ = (x_ulimit_ - x_llimit_) * (y_ulimit_ - y_llimit_);
      }
    }

    for (int x = 1; x < map.size() - 1; x++)
    {
      for (int y = 1; y < map[x].size() - 1; y++)
      {
        if (!map[x][y].object_)
        {
          map[x][y].dist_from_obj_ = min(min(min(map[x - 1][y - 1].dist_from_obj_ + sqrt(2.0) * cell_size_,
                                                 map[x][y - 1].dist_from_obj_ + 1.0 * cell_size_),
                                             min(map[x - 1][y + 1].dist_from_obj_ + sqrt(2.0) * cell_size_,
                                                 map[x - 1][y].dist_from_obj_ + 1.0 * cell_size_)),
                                         map[x][y].dist_from_obj_);
        }
      }
    }

    for (int x = map.size() - 2; x > 0; x--)
    {
      for (int y = map[x].size() - 2; y > 0; y--)
      {
        if (!map[x][y].object_)
        {
          map[x][y].dist_from_obj_ = min(min(min(map[x + 1][y].dist_from_obj_ + 1.0 * cell_size_,
                                                 map[x + 1][y - 1].dist_from_obj_ + sqrt(2.0) * cell_size_),
                                             min(map[x][y + 1].dist_from_obj_ + 1.0 * cell_size_,
                                                 map[x + 1][y + 1].dist_from_obj_ + sqrt(2.0) * cell_size_)),
                                         map[x][y].dist_from_obj_);
        }
      }
    }

    return true;
  }

  void convertMap(vector< vector< CollisionMapData > > map, tms_msg_rp::rps_map_full& pp_map)
  {
    pp_map.rps_map_x.clear();

    pp_map.x_llimit = x_llimit_;
    pp_map.x_ulimit = x_ulimit_;
    pp_map.y_llimit = y_llimit_;
    pp_map.y_ulimit = y_ulimit_;
    pp_map.cell_size = cell_size_;

    tms_msg_rp::rps_map_data temp_map_d;
    tms_msg_rp::rps_map_y temp_map_y;

    for (unsigned int x = 0; x < map.size(); x++)
    {
      temp_map_y.rps_map_y.clear();
      for (unsigned int y = 0; y < map[x].size(); y++)
      {
        temp_map_d.object = map[x][y].object_;
        temp_map_d.voronoi = map[x][y].voronoi_;
        temp_map_d.dist_from_obj_f = map[x][y].dist_from_obj_;

        temp_map_y.rps_map_y.push_back(temp_map_d);
      }
      pp_map.rps_map_x.push_back(temp_map_y);
    }
  }

  void staticMapPublish(const ros::TimerEvent& e)
  {
    static_map_pub_.publish(static_map_);

    uint32_t shape = visualization_msgs::Marker::POINTS;

    visualization_msgs::Marker voronoi_marker;
    voronoi_marker.header.frame_id = "world_link";
    voronoi_marker.header.stamp = ros::Time::now();
    voronoi_marker.ns = "voronoi_map";
    voronoi_marker.id = 0;
    voronoi_marker.type = shape;
    voronoi_marker.action = visualization_msgs::Marker::ADD;

    voronoi_marker.pose.orientation.x = 0.0;
    voronoi_marker.pose.orientation.y = 0.0;
    voronoi_marker.pose.orientation.z = 0.0;
    voronoi_marker.pose.orientation.w = 1.0;

    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    voronoi_marker.scale.x = 0.1;
    voronoi_marker.scale.y = 0.1;
    voronoi_marker.scale.z = 0.1;

    // Set the color -- be sure to set alpha to something non-zero!
    voronoi_marker.color.r = 0.0f;
    voronoi_marker.color.g = 1.0f;
    voronoi_marker.color.b = 0.0f;
    voronoi_marker.color.a = 1.0;

    visualization_msgs::Marker nonvoronoi_marker;
    nonvoronoi_marker.header.frame_id = "world_link";
    nonvoronoi_marker.header.stamp = ros::Time::now();
    nonvoronoi_marker.ns = "nonvoronoi_map";
    nonvoronoi_marker.id = 0;
    nonvoronoi_marker.type = shape;
    nonvoronoi_marker.action = visualization_msgs::Marker::ADD;

    nonvoronoi_marker.pose.orientation.x = 0.0;
    nonvoronoi_marker.pose.orientation.y = 0.0;
    nonvoronoi_marker.pose.orientation.z = 0.0;
    nonvoronoi_marker.pose.orientation.w = 1.0;

    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    nonvoronoi_marker.scale.x = 0.1;
    nonvoronoi_marker.scale.y = 0.1;
    nonvoronoi_marker.scale.z = 0.1;

    // Set the color -- be sure to set alpha to something non-zero!
    nonvoronoi_marker.color.r = 0.65;
    nonvoronoi_marker.color.g = 0.65;
    nonvoronoi_marker.color.b = 0.65;
    nonvoronoi_marker.color.a = 1.0;

    geometry_msgs::Point p;

    for (unsigned int x = 0; x < static_map_.rps_map_x.size(); x++)
    {
      for (unsigned int y = 0; y < static_map_.rps_map_x[x].rps_map_y.size(); y++)
      {
        if (static_map_.rps_map_x[x].rps_map_y[y].voronoi)
        {
          // p.x = x*0.1 - 0.3;
          // p.y = y*0.1 - 0.5;
          p.x = x * 0.1;
          p.y = y * 0.1;
          p.z = 0.01;
          voronoi_marker.points.push_back(p);
        }

        if (!static_map_.rps_map_x[x].rps_map_y[y].object)
        {
          p.x = x * 0.1;
          p.y = y * 0.1;
          p.z = 0.005;
          nonvoronoi_marker.points.push_back(p);
        }
      }
    }
    static_map_marker_pub.publish(voronoi_marker);
    nonvoronoi_map_marker_pub.publish(nonvoronoi_marker);
  }

  void dynamicMapPublish(const ros::TimerEvent& e)
  {
    int map_x = 0, map_y = 0;
    vector< vector< CollisionMapData > > temp_Map;
    string result_msg;

    temp_Map.clear();
    temp_Map = collision_map_;

    Vector3 object_pos = Vector3(0, 0, 0);
    Matrix3 object_ori;
    Vector3 object_rpy;
    //    BodyItemPtr item;
    //    TmsRpController trc;

    //    // add current position of obstacle (wagon)
    //    item = trc.objTag2Item()["wagon"];
    //    if(!item){
    //      ROS_INFO("Error: The tagId is not recorded.");
    //    }
    //    object_pos = item->body()->link(0)->p();
    //    object_ori = item->body()->link(0)->R();
    //    item->calcForwardKinematics();
    //    item->notifyKinematicStateChange();

    //    object_rpy = grasp::rpyFromRot(object_ori);

    //    map_x = (int)round( ( object_pos(0) - x_llimit_ ) / cell_size_ );
    //    map_y = (int)round( ( object_pos(1) - y_llimit_ ) / cell_size_ );

    //    for(int check_x_size=map_x-2;check_x_size<=map_x+2;check_x_size++)
    //    {
    //      for(int check_y_size=map_y-2;check_y_size<=map_y+2;check_y_size++)
    //      {
    //        if (check_x_size < 0 || check_y_size < 0 || check_x_size > 80 || check_y_size > 45) continue;
    //        temp_Map[check_x_size][check_y_size].object_        = 1;
    //        temp_Map[check_x_size][check_y_size].dist_from_obj_ = 0.0;
    //        temp_Map[check_x_size][check_y_size].collision_     = true;
    //      }
    //    }

    setVoronoiLine(temp_Map, result_msg);
    calcDistFromObj(temp_Map, result_msg);
    convertMap(temp_Map, dynamic_map_);

    dynamic_map_pub_.publish(dynamic_map_);

    uint32_t shape = visualization_msgs::Marker::POINTS;
    visualization_msgs::Marker marker;
    marker.header.frame_id = "world_link";
    marker.header.stamp = ros::Time::now();
    marker.ns = "dynamic_map";
    marker.id = 0;
    marker.type = shape;
    marker.action = visualization_msgs::Marker::ADD;

    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    marker.scale.x = 0.1;
    marker.scale.y = 0.1;
    marker.scale.z = 0.1;

    // Set the color -- be sure to set alpha to something non-zero!
    marker.color.r = 1.0f;
    marker.color.g = 0.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;

    geometry_msgs::Point p;

    for (unsigned int x = 0; x < dynamic_map_.rps_map_x.size(); x++)
    {
      for (unsigned int y = 0; y < dynamic_map_.rps_map_x[x].rps_map_y.size(); y++)
      {
        if (dynamic_map_.rps_map_x[x].rps_map_y[y].voronoi)
        {
          marker.id = 0;
          // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
          p.x = x * 0.1;
          p.y = y * 0.1;
          p.z = 0.01;
          marker.points.push_back(p);
        }
      }
    }

    dynamic_marker_pub.publish(marker);
  }

  void dynamicMapPublish(tms_msg_ss::tracking_points unknown_moving_object_pos)
  {
    int map_x = 0, map_y = 0;
    int obstacle_pos_x = 0, obstacle_pos_y = 0;
    vector< vector< CollisionMapData > > temp_Map;
    string result_msg;

    temp_Map.clear();
    temp_Map = collision_map_;

    Vector3 object_pos = Vector3(0, 0, 0);
    Matrix3 object_ori;
    Vector3 object_rpy;
    //    BodyItemPtr item;
    //    TmsRpController trc;

    //    // add current position of obstacle (wagon)
    //    item = trc.objTag2Item()["wagon"];
    //    if(!item){
    //      ROS_INFO("Error: The tagId is not recorded.");
    //    }
    //    object_pos = item->body()->link(0)->p();
    //    object_ori = item->body()->link(0)->R();
    //    item->calcForwardKinematics();
    //    item->notifyKinematicStateChange();

    //    object_rpy = grasp::rpyFromRot(object_ori);

    //    map_x = (int)round( ( object_pos(0) - x_llimit_ ) / cell_size_ );
    //    map_y = (int)round( ( object_pos(1) - y_llimit_ ) / cell_size_ );

    //    for(int check_x_size=map_x-2;check_x_size<=map_x+2;check_x_size++)
    //    {
    //      for(int check_y_size=map_y-2;check_y_size<=map_y+2;check_y_size++)
    //      {
    //        if (check_x_size < 0 || check_y_size < 0 || check_x_size > 80 || check_y_size > 45) continue;
    //        temp_Map[check_x_size][check_y_size].object_        = 1;
    //        temp_Map[check_x_size][check_y_size].dist_from_obj_ = 0.0;
    //        temp_Map[check_x_size][check_y_size].collision_     = true;
    //      }
    //    }

    //    // add current position of unknown moving object (umo)
    //    for(unsigned int i=0;i<unknown_moving_object_pos.tracking_grid.size();i++)
    //    {

    //      obstacle_pos_x = unknown_moving_object_pos.tracking_grid[i].x/1000;
    //      obstacle_pos_y = unknown_moving_object_pos.tracking_grid[i].y/1000;

    //      map_x = (int)round( ( obstacle_pos_x - x_llimit_ ) / cell_size_ );
    //      map_y = (int)round( ( obstacle_pos_y - y_llimit_ ) / cell_size_ );

    //      for(int check_x_size=map_x-2;check_x_size<=map_x+2;check_x_size++)
    //      {
    //        for(int check_y_size=map_y-2;check_y_size<=map_y+2;check_y_size++)
    //        {
    //        if (check_x_size < 0 || check_y_size < 0 || check_x_size > 80 || check_y_size > 45) continue;
    //          temp_Map[check_x_size][check_y_size].object_        = 1;
    //          temp_Map[check_x_size][check_y_size].dist_from_obj_ = 0.0;
    //          temp_Map[check_x_size][check_y_size].collision_     = true;
    //        }
    //      }
    //    }

    setVoronoiLine(temp_Map, result_msg);
    calcDistFromObj(temp_Map, result_msg);
    convertMap(temp_Map, dynamic_map_);

    dynamic_map_pub_.publish(dynamic_map_);
  }
};
}  // namespace tms_rp

int main(int argc, char** argv)
{
  // Init ROS node
  ros::init(argc, argv, "tms_rp_voronoi_map");
  tms_rp::TmsRpVoronoiMap vm;
  ros::spin();
  return 0;
}
// EOF


// Copyright 2015-2022 The ALMA Project Developers
//
// 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.

/// @file
/// Computes effective conductivities versus temperature for
/// nanosystems (nanowires and nanoribbons)
/// Material information and sweep settings provided via XML input.

#include <iostream>
#include <functional>
#include <boost/filesystem.hpp>
#include <boost/mpi.hpp>
#include <utilities.hpp>
#include <vasp_io.hpp>
#include <qpoint_grid.hpp>
#include <processes.hpp>
#include <isotopic_scattering.hpp>
#include <bulk_hdf5.hpp>
#include <analytic1d.hpp>
#include <io_utils.hpp>
#include <bulk_properties.hpp>
#include <vector>
#include <string>
#include <Eigen/Dense>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <nanos.hpp>

#define TBB_PREVIEW_GLOBAL_CONTROL true
#include <tbb/global_control.h>

int main(int argc, char** argv) {
    // set up MPI environment
    boost::mpi::environment env;
    boost::mpi::communicator world;

    if (world.size() > 1) {
        std::cout
            << "*** ERROR: kappa_Tsweep does not run in parallel mode. ***"
            << std::endl;
        world.abort(1);
    }

    if (argc < 2) {
        std::cout << "USAGE: kappa_Tsweep <inputfile.xml>" << std::endl;
        return 1;
    }

    else {
        // Set parallelism
        std::size_t nthreadsTBB = 1;
        if (argc > 2)
            nthreadsTBB = std::atoi(argv[2]);

        tbb::global_control control(
            tbb::global_control::max_allowed_parallelism, nthreadsTBB);

        // define variables
        std::string target_directory = "AUTO";
        std::string target_filename = "AUTO";
        std::string h5_repository = ".";
        std::string mat_directory;
        std::string mat_base;
        int gridDensityA = -1;
        int gridDensityB = -1;
        int gridDensityC = -1;
        bool logsweep = false;
        double Tmin = -1.0;
        double Tmax = -1.0;
        int NT = -1;
        // bool Gductivity = false;
        bool fullBTE = false;
        bool fullBTE_iterative = true;
        // bool outputCapacity = false;
        bool superlattice = false;
        std::string superlattice_UID = "NULL";

        /// Variables about nanoribbons and nanowires
        std::string system_name = "NULL";
        Eigen::Vector3d uvector(0.0, 0.0, 0.0);
        Eigen::Vector3d u_norm(0.0, 0.0, 0.0);
        /// Limiting length (nanoribbon width or nanowire radii in nm)
        double limiting_length = -1.0;
        /// Name for anhIFCs
        std::string AIFCfile = "NULL";
        double scalebroad_three = -1;

        std::cout << "*************************************" << std::endl;
        std::cout << "This is ALMA/kappa_Tsweep_nanos version "
                  << ALMA_VERSION_MAJOR << "." << ALMA_VERSION_MINOR
                  << std::endl;
        std::cout << "*************************************" << std::endl;
        std::cout << std::endl
                  << " # TBB threads : " << nthreadsTBB << std::endl;

        // verify that input file exists.
        if (!boost::filesystem::exists(boost::filesystem::path{argv[1]})) {
            std::cout << "ERROR: input file " << argv[1] << " does not exist."
                      << std::endl;
            exit(1);
        }

        /////////////////////////
        /// PARSE INPUT FILE  ///
        /////////////////////////

        std::string xmlfile(argv[1]);
        std::cout << "PARSING " << xmlfile << " ..." << std::endl;

        // Create empty property tree object
        boost::property_tree::ptree tree;

        // Parse XML input file into the tree
        boost::property_tree::read_xml(xmlfile, tree);

        for (const auto& v : tree.get_child("Tsweep")) {
            if (v.first == "H5repository") {
                h5_repository =
                    alma::parseXMLfield<std::string>(v, "root_directory");
            }

            if (v.first == "target") {
                target_directory =
                    alma::parseXMLfield<std::string>(v, "directory");
                target_filename = alma::parseXMLfield<std::string>(v, "file");
            }

            if (v.first == "compound") {
                mat_directory =
                    alma::parseXMLfield<std::string>(v, "directory");
                mat_base = alma::parseXMLfield<std::string>(v, "base");

                gridDensityA = alma::parseXMLfield<int>(v, "gridA");
                gridDensityB = alma::parseXMLfield<int>(v, "gridB");
                gridDensityC = alma::parseXMLfield<int>(v, "gridC");
            }

            if (v.first == "system") {
                system_name = alma::parseXMLfield<std::string>(v, "name");
                limiting_length = alma::parseXMLfield<double>(v, "L");
            }

            if (v.first == "superlattice") {
                superlattice_UID = alma::parseXMLfield<std::string>(v, "UID");
            }

            if (v.first == "sweep") {
                std::string sweepID =
                    alma::parseXMLfield<std::string>(v, "type");

                if (sweepID.compare("log") == 0) {
                    logsweep = true;
                }

                Tmin = alma::parseXMLfield<double>(v, "start");
                Tmax = alma::parseXMLfield<double>(v, "stop");
                NT = alma::parseXMLfield<int>(v, "points");
            }

            if (v.first == "fullBTE") {
                fullBTE = true;
                fullBTE_iterative = alma::parseXMLfield<bool>(v, "iterative");
            }

            if (v.first == "transportAxis") {
                double ux = alma::parseXMLfield<double>(v, "x");
                double uy = alma::parseXMLfield<double>(v, "y");
                double uz = alma::parseXMLfield<double>(v, "z");

                uvector << ux, uy, uz;
            }
            if (v.first == "AnharmonicIFC") {
                AIFCfile = alma::parseXMLfield<std::string>(v, "name");
                scalebroad_three = alma::parseXMLfield<double>(v, "scalebroad");
            }

        } // end XML parsing

        // Ensure that provided information is within expected bounds

        bool badinput = false;

        if (gridDensityA < 1) {
            std::cout << "ERROR: provided gridA is " << gridDensityA
                      << std::endl;
            std::cout << "Value must be at least 1." << std::endl;
            badinput = true;
        }

        if (gridDensityB < 1) {
            std::cout << "ERROR: provided gridB is " << gridDensityB
                      << std::endl;
            std::cout << "Value must be at least 1." << std::endl;
            badinput = true;
        }

        if (gridDensityC < 1) {
            std::cout << "ERROR: provided gridC is " << gridDensityC
                      << std::endl;
            std::cout << "Value must be at least 1." << std::endl;
            badinput = true;
        }

        if (Tmin <= 0.0) {
            std::cout << "ERROR: provided start temperature is " << Tmin
                      << std::endl;
            std::cout << "Value must be positive." << std::endl;
            badinput = true;
        }

        if ((Tmax < Tmin) || (Tmax <= 0.0)) {
            std::cout << "ERROR: provided end temperature is " << Tmax
                      << std::endl;
            std::cout << "Value must be positive and >= start temperature."
                      << std::endl;
            badinput = true;
        }

        if (NT <= 0) {
            std::cout << "ERROR: provided number of temperature values is "
                      << NT << std::endl;
            std::cout << "Value must be at least 1." << std::endl;
            badinput = true;
        }

        if (uvector.norm() < 1e-12) {
            std::cout << "ERROR: provided transport axis vector has zero norm."
                      << std::endl;
            badinput = true;
        }

        if (system_name != "nanowire" and system_name != "nanoribbon") {
            std::cout << "ERROR: only supported nanosystems options are:"
                      << std::endl
                      << "nanoribbon and nanowire" << std::endl;

            badinput = true;
        }

        if (system_name == "nanoribbon" and gridDensityC > 1) {
            std::cout << "ERROR: nanoribbons are expected to exist in xy plane"
                      << std::endl;

            badinput = true;
        }

        if (limiting_length <= 0.) {
            std::cout
                << "ERROR: limiting length of nanosystems cannot be negative"
                << std::endl;

            badinput = true;
        }

        if (AIFCfile == "NULL" and fullBTE) {
            std::cout << "ERROR: Need to provide anharmonic IFC file"
                      << std::endl;

            badinput = true;
        }

        if (scalebroad_three < 0. and fullBTE) {
            std::cout << "ERROR: scalebroad need to be supplied" << std::endl;

            badinput = true;
        }

        if (badinput) {
            world.abort(1);
        }

        // Initialise file system and verify that directories actually exist
        auto launch_path = boost::filesystem::current_path();
        auto basedir = boost::filesystem::path(h5_repository);

        if (!(boost::filesystem::exists(boost::filesystem::path(basedir)))) {
            std::cout << "ERROR:" << std::endl;
            std::cout << "Repository directory " << basedir
                      << " does not exist." << std::endl;
            world.abort(1);
        }

        if (!(boost::filesystem::exists(
                boost::filesystem::path(basedir / mat_directory)))) {
            std::cout << "ERROR:" << std::endl;
            std::cout << "Material directory " << mat_directory
                      << " does not exist within the HDF5 repository."
                      << std::endl;
            world.abort(1);
        }

        // Resolve name of HDF5 file
        std::stringstream h5namebuilder;
        h5namebuilder << mat_base << "_" << gridDensityA << "_" << gridDensityB
                      << "_" << gridDensityC << ".h5";
        std::string h5filename = h5namebuilder.str();

        // obtain phonon data from HDF5 file
        auto hdf5_path = basedir / boost::filesystem::path(mat_directory) /
                         boost::filesystem::path(h5filename);

        if (!(boost::filesystem::exists(hdf5_path))) {
            std::cout << "ERROR:" << std::endl;
            std::cout << "H5 file " << h5filename
                      << " does not exist within the material directory."
                      << std::endl;
            world.abort(1);
        }

        std::cout << "Opening HDF5 file " << hdf5_path << std::endl;

        auto hdf5_data =
            alma::load_bulk_hdf5(hdf5_path.string().c_str(), world);
        auto description = std::get<0>(hdf5_data);
        auto poscar = std::move(std::get<1>(hdf5_data));
        auto syms = std::move(std::get<2>(hdf5_data));
        auto grid = std::move(std::get<3>(hdf5_data));
        auto processes = std::move(std::get<4>(hdf5_data));

        if (processes->size() == 0) {
            std::cout << "ERROR:" << std::endl;
            std::cout << "List of 3-phonon processes is missing in H5 file."
                      << std::endl;
            world.abort(1);
        }

        // Check if we are dealing with a superlattice.
        // If so, load the applicable scattering data.

        auto subgroups =
            alma::list_scattering_subgroups(hdf5_path.string().c_str(), world);

        int superlattice_count = 0;

        for (std::size_t ngroup = 0; ngroup < subgroups.size(); ngroup++) {
            if (subgroups.at(ngroup).find("superlattice") !=
                std::string::npos) {
                superlattice_count++;
            }
        }

        superlattice_count /= 2;

        if (superlattice_count > 0) {
            superlattice = true;
        }

        Eigen::ArrayXXd w0_SLdisorder;
        Eigen::ArrayXXd w0_SLbarriers;

        if (superlattice) {
            // complain if there are multiple possibilities

            if ((superlattice_count > 1) && (superlattice_UID == "NULL")) {
                std::cout << "ERROR:" << std::endl;
                std::cout << "H5 file contains scattering information for "
                             "multiple superlattices."
                          << std::endl;
                std::cout << "Must provide the superlattice UID via the "
                             "<superlattice> XML tag."
                          << std::endl;
                world.abort(1);
            }

            // if the user provided a UID, verify that corresponding data exists

            if (superlattice_UID != "NULL") {
                int UIDcount = 0;

                for (std::size_t ngroup = 0; ngroup < subgroups.size();
                     ngroup++) {
                    if (subgroups.at(ngroup).find(superlattice_UID) !=
                        std::string::npos) {
                        UIDcount++;
                    }
                }

                if (UIDcount != 2) {
                    std::cout << "ERROR:" << std::endl;
                    std::cout << "H5 file does not contain any superlattice "
                                 "data with provided UID "
                              << superlattice_UID << "." << std::endl;
                    world.abort(1);
                }
            }

            // load the scattering rates from the H5 file

            bool UIDmatch = true;

            for (std::size_t ngroup = 0; ngroup < subgroups.size(); ngroup++) {
                bool contains_SLdisorder =
                    (subgroups.at(ngroup).find("superlattice") !=
                     std::string::npos) &&
                    (subgroups.at(ngroup).find("disorder") !=
                     std::string::npos);

                bool contains_SLbarriers =
                    (subgroups.at(ngroup).find("superlattice") !=
                     std::string::npos) &&
                    (subgroups.at(ngroup).find("barriers") !=
                     std::string::npos);

                if (superlattice_count > 1) {
                    UIDmatch = (subgroups.at(ngroup).find(superlattice_UID) !=
                                std::string::npos);
                }

                if (contains_SLdisorder && UIDmatch) {
                    auto mysubgroup = alma::load_scattering_subgroup(
                        hdf5_path.string().c_str(),
                        subgroups.at(ngroup),
                        world);
                    w0_SLdisorder = mysubgroup.w0;
                }

                if (contains_SLbarriers && UIDmatch) {
                    auto mysubgroup = alma::load_scattering_subgroup(
                        hdf5_path.string().c_str(),
                        subgroups.at(ngroup),
                        world);
                    w0_SLbarriers = mysubgroup.w0;
                }
            }
        }

        // Build list of temperature values
        Eigen::VectorXd Tlist;

        if (logsweep) {
            Tlist = alma::logSpace(Tmin, Tmax, NT);
        }

        else {
            Tlist.setLinSpaced(NT, Tmin, Tmax);
        }

        // Create output writer
        std::stringstream outputbuffer;

        // Write file header

        outputbuffer << "Temp[K],kappaRTA<" << uvector(0) << "," << uvector(1)
                     << "," << uvector(2) << ">[W/m-K]";

        if (fullBTE) {
            outputbuffer << ",kappaBTE<" << uvector(0) << "," << uvector(1)
                         << "," << uvector(2) << ">[W/m-K]";
        }

        outputbuffer << std::endl;

        // RUN CALCULATIONS

        std::cout << "Running Tsweep for " << mat_base << std::endl;

        auto twoph_processes = alma::find_allowed_twoph(*grid, world);

        // temperature-independent scattering rates
        Eigen::ArrayXXd w_elastic;

        if (superlattice) {
            w_elastic = w0_SLbarriers.array() + w0_SLdisorder.array();
        }
        else {
            w_elastic =
                alma::calc_w0_twoph(*poscar, *grid, twoph_processes, world);
        }

        u_norm = uvector.array() / uvector.norm();

        /// processes list
        std::unordered_map<std::array<std::size_t, 3>, alma::Threeph_process>
            emission_processes;
        std::unordered_map<std::array<std::size_t, 3>, alma::Threeph_process>
            absorption_processes;
        std::unordered_map<std::pair<std::size_t, std::size_t>, double>
            isotopic_processes;

        if (fullBTE) {
            std::cout << "# Recalculating processes list in full BZ\n";
            std::cout << "# for symmetry reasons\n";

            alma::nanos::get_fullBZ_processes(*grid,
                                              *poscar,
                                              AIFCfile,
                                              emission_processes,
                                              absorption_processes,
                                              isotopic_processes,
                                              world,
                                              scalebroad_three);
            std::cout << "# Recalculation => DONE" << std::endl;
            std::cout.flush();
        }

        for (int nT = 0; nT < NT; nT++) {
            double T = Tlist(nT);

            std::cout << " Processing " << T << " K (temperature " << nT + 1
                      << " of " << NT << ")" << std::endl;

            Eigen::ArrayXXd w3(
                alma::calc_w0_threeph(*grid, *processes, T, world));
            Eigen::ArrayXXd w(w3 + w_elastic);

            double kappa_RTA = alma::nanos::calc_kappa_RTA(
                *poscar, *grid, w, u_norm, system_name, limiting_length, T);

            double kappa_BTE;

            if (fullBTE) {
                kappa_BTE = alma::nanos::calc_kappa_nanos(*poscar,
                                                          *grid,
                                                          *syms,
                                                          emission_processes,
                                                          absorption_processes,
                                                          isotopic_processes,
                                                          w,
                                                          u_norm,
                                                          system_name,
                                                          limiting_length,
                                                          T,
                                                          fullBTE_iterative,
                                                          world);
            }

            outputbuffer << T << "," << kappa_RTA;

            if (fullBTE) {
                outputbuffer << "," << kappa_BTE;
            }

            outputbuffer << std::endl;
        } // END CALCULATIONS

        // WRITE FILE

        // go to the launch directory
        boost::filesystem::current_path(launch_path);

        // resolve output directory if AUTO is selected
        if (target_directory.compare("AUTO") == 0) {
            target_directory = "output/kappa_Tsweep";
        }

        // create output directory if it doesn't exist yet
        auto outputfolder = boost::filesystem::path(target_directory);

        if (!(boost::filesystem::exists(outputfolder))) {
            boost::filesystem::create_directories(outputfolder);
        }

        boost::filesystem::current_path(launch_path);

        // resolve file name if AUTO selected

        if (target_filename.compare("AUTO") == 0) {
            std::stringstream filenamebuilder;
            filenamebuilder << mat_base << "_" << gridDensityA << "_"
                            << gridDensityB << "_" << gridDensityC;

            std::string lstring = std::to_string(limiting_length);
            lstring.resize(7);

            filenamebuilder << "_" << system_name << "_L_";
            filenamebuilder << lstring;
            filenamebuilder << "_" << uvector(0) << "," << uvector(1) << ","
                            << uvector(2);

            filenamebuilder << "_" << Tmin << "_" << Tmax;

            filenamebuilder << ".Tsweep";
            target_filename = filenamebuilder.str();
        }

        // save file

        std::cout << std::endl;
        std::cout << "Writing to file " << target_filename << std::endl;
        std::cout << "in subdirectory " << target_directory << std::endl;
        std::cout << "under root directory " << launch_path << std::endl;

        std::ofstream outputwriter;
        outputwriter.open("./" + target_directory + "/" + target_filename);
        outputwriter << outputbuffer.str();
        outputwriter.close();

        std::cout << std::endl << "[DONE.]" << std::endl;

        return 0;
    }
}


/*
 * File:   auto_win_q.cpp
 * Author: root
 *
 * Created on February 10, 2010, 4:36 PM
 */

#include <limits>
#include <iostream>
#include <map>
#include <string>
#include <cmath>

//for random number generator
#include <boost/random/uniform_int.hpp>
#include <boost/random/variate_generator.hpp>

#include "auto_win_q.h"
#include "q_func_calc.h"
#include "distance_measures.h"
#include "BipartNj.h"

using namespace std;
using namespace uqam_doc;

using uqam_doc::AutoWinQ;
using uqam_doc::qFuncCalc;
using uqam_doc::BipartNj;

//give context msa and position to extract data himself
//AutoWinQ::AutoWinQ(qFuncCalc& ctx, int win_pos) : ctx_(ctx) {

AutoWinQ::AutoWinQ(qFuncCalc& ctx, int win_pos) {

    ctx_ = &ctx;

    win_pos_ = win_pos;

    //

    //a window has exactly the same types that its context
    //not all sequences, could be filtered with ids_work
    //using copy constructor
    ident_all_ = ctx_->all_types_;

    NB_GROUPS_ = 2;
    //empty before.
    //initialize group containers
    //we are in constructor
    v_1g_cumul_.resize(NB_GROUPS_, 0.0);
    v_1g_nb_.resize(NB_GROUPS_, 0.0);
    v_1g_inv_nb_.resize(NB_GROUPS_, 0.0);
    v_1g_inv_prop_.resize(NB_GROUPS_, 0.0);
    v_1g_.resize(NB_GROUPS_, 0.0);

    //initialize group containers
    vector<float> vd(NB_GROUPS_, 0.0);
    d_2g_cumul_.resize(NB_GROUPS_, vd);
    d_2g_nb_.resize(NB_GROUPS_, vd);
    d_2g_inv_nb_.resize(NB_GROUPS_, vd);
    d_2g_inv_prop_.resize(NB_GROUPS_, vd);
    d_2g_.resize(NB_GROUPS_, vd);

    //differences
    new_v_1g_cumul_.resize(NB_GROUPS_, 0.0);
    new_v_1g_nb_.resize(NB_GROUPS_, 0.0);
    new_v_1g_inv_nb_.resize(NB_GROUPS_, 0.0);
    new_v_1g_inv_prop_.resize(NB_GROUPS_, 0.0);
    new_v_1g_.resize(NB_GROUPS_, 0.0);

    new_d_2g_cumul_.resize(NB_GROUPS_, vd);
    new_d_2g_nb_.resize(NB_GROUPS_, vd);
    new_d_2g_inv_nb_.resize(NB_GROUPS_, vd);
    new_d_2g_inv_prop_.resize(NB_GROUPS_, vd);
    new_d_2g_.resize(NB_GROUPS_, vd);

    //and by functions
    NB_FUNC_ = 8;
    //initialize functions
    q_func_.resize(NB_FUNC_, 0.0);
    q_p_val_inf_.resize(NB_FUNC_, 0.0);
    q_p_val_sup_.resize(NB_FUNC_, 0.0);
    //differences
    new_q_func_.resize(NB_FUNC_, 0.0);
    //
    NB_RANDOM_SEEDS_ = 100;

    //optimize context function
    q_func_opt_max_idx_ = ctx_->appl_data_->q_func_opt_max_idx_;
    q_func_opt_max_val_ = -numeric_limits<float>::max();
    //
    simple_window_ = NULL;
    best_window_ = NULL;
    //results
    uncalculable_ = false;

    //calculate gap proportion
    MAX_GAP_PROPORTION_ = 1;
    //gap_proportion_ = 0;
    gap_proportion_ = calculateGapProportion();
    //cout << "gap proporiton: " << gap_proportion_ << endl;

    if (gap_proportion_ >= MAX_GAP_PROPORTION_) {
        uncalculable_ = true;
    }

}

//copy constructor
//AutoWinQ::AutoWinQ(qFuncCalc& ctx, AutoWinQ& orig): ctx_(ctx) {

AutoWinQ::AutoWinQ(qFuncCalc& ctx, AutoWinQ& orig) {
    //cout << "=====================: copy constructor " << endl;

    //cout << "ident_grouped : size " << orig.ident_grouped_.size() << endl;

    ctx_ = orig.ctx_;

    win_pos_ = orig.win_pos_;

    ident_all_ = orig.ident_all_;
    ident_grouped_ = orig.ident_grouped_;

    NB_GROUPS_ = orig.NB_GROUPS_;

    //nb positions valides
    v_1g_nb_ = orig.v_1g_nb_;
    d_2g_nb_ = orig.d_2g_nb_;
    v_all_nb_ = orig.v_all_nb_;

    //nb positions invalides
    v_1g_inv_nb_ = orig.v_1g_inv_nb_;
    d_2g_inv_nb_ = orig.d_2g_inv_nb_;
    v_all_inv_nb_ = orig.v_all_nb_;

    //proportion invalides
    v_1g_inv_prop_ = orig.v_1g_inv_prop_;
    d_2g_inv_prop_ = orig.d_2g_inv_prop_;
    v_all_inv_prop_ = orig.v_all_inv_prop_;

    //valeurs finales à transformer en fonctions qui scalent
    v_1g_ = orig.v_1g_;
    d_2g_ = orig.d_2g_;
    v_all_ = orig.v_all_;

    NB_FUNC_ = orig.NB_FUNC_;

    //
    q_func_ = orig.q_func_;
    //cout << "q_func4: " << q_func_[4] << endl;

    q_p_val_inf_ = orig.q_p_val_inf_;
    q_p_val_sup_ = orig.q_p_val_sup_;

    //
    //q_func_opt_max_val_;
    //q_func_opt_max_idx_;

    //copy(orig.ident_grouped_.begin(), orig.ident_grouped_.end(), this->ident_grouped_.begin());
    //
    simple_window_ = NULL;
    best_window_ = NULL;

    MAX_GAP_PROPORTION_ = orig.MAX_GAP_PROPORTION_;
    gap_proportion_ = orig.gap_proportion_;

    //cout << "ident_grouped : size " << ident_grouped_.size() << endl;

}

//virtual destructor

AutoWinQ::~AutoWinQ() {
    ident_all_.clear();

    if (simple_window_ != NULL) {
        delete simple_window_;
    }

    if (best_window_ != NULL) {
       delete best_window_;

    }

}

float AutoWinQ::calculateGapProportion() {

    float gap_proportion = 0;

    set<string>::iterator it1 = ident_all_.begin();
    while (it1 != ident_all_.end()) {

        //cout << "*it1: " << *it1 << endl;
        //cout << "ctx_->win_l_: " << ctx_->win_l_ << endl;

        string seq = ctx_->align_mult_types_[*it1].substr(win_pos_, ctx_->win_l_);
        for (int i = 0; i < seq.length(); i++) {
            if (seq[i] == '-') {
                gap_proportion++;
            }
        }

        ++it1;
    }

    gap_proportion /= (ctx_->win_l_ * ctx_->align_mult_types_.size());

    return gap_proportion;
}

//calculate distance matrix

void AutoWinQ::calculateDistMatrix() {

    //map< string, map <string, DistAtom > >  dmap;
    //DistAtom da = DistAtom(0,0,0,0.0);
    //dmap["a"]["b"]=da;

    set<string>::iterator it1 = ident_all_.begin();
    while (it1 != ident_all_.end()) {
        set<string>::iterator it2 = ident_all_.begin();
        while (it2 != ident_all_.end()) {

            if (it2 != it1) {
              //verify dist matrix
            //string seq_a_test = ctx_.align_mult_types_[*it1].substr(win_pos_, ctx_.win_l_);
            //string seq_b_test = ctx_.align_mult_types_[*it2].substr(win_pos_, ctx_.win_l_);
            //cout << "seq_a_test: " << seq_a_test << endl;
            DistAtom dist = ctx_->dist_calc_->dist_2seq(ctx_->align_mult_types_[*it1].substr(win_pos_, ctx_->win_l_), ctx_->align_mult_types_[*it2].substr(win_pos_, ctx_->win_l_));
            //symetric matrix
            dist_matrix_[*it1][*it2] = dist;
            dist_matrix_[*it2][*it1] = dist_matrix_[*it1][*it2];
            } else {
               dist_matrix_[*it1][*it2] = DistAtom(0,0,0,0);
            }

            ++it2;
        }
        ++it1;
    }

}

//Alix bipartitions of the distance matrix
void AutoWinQ::calculateNjBipartitions() {

    BipartNj *rb = new BipartNj(dist_matrix_);
    cout << "dist_matrix_.size(): " << dist_matrix_.size() << endl;

    //rb.lectureFichier();
    //outputDistanceMatrix();

    rb->recherche_bipartition();
    rb->calculateBipartVector();

    //rb->outputBipartVector();

     cout << "----------------calculateDistMatrix size 1: " << bipart_matrix_.size() << endl;
     bipart_matrix_.clear();

     rb->getBipartVector(&bipart_matrix_);

     cout << "---------------calculateDistMatrix size 2: " << bipart_matrix_.size() << endl;

     //cout << "--bla: " << endl;

    delete(rb);
    //cout << "--blaxxx: " << endl;

}

//update distance matrix one step

void AutoWinQ::updateDistMatrixOneStep() {

    //map< string, map <string, DistAtom > >  dmap;
    //DistAtom da = DistAtom(0,0,0,0.0);
    //dmap["a"]["b"]=da;

    //advance position
    win_pos_ += ctx_->appl_data_->win_step_;
    calculateDistMatrix();
}

void AutoWinQ::outputDistanceMatrix() {

    //output distance matrix
    cout << "outputDistanceMatrix(): " << endl;
    set<string>::iterator it1 = ident_all_.begin();
    while (it1 != ident_all_.end()) {
        set<string>::iterator it2 = ident_all_.begin();
        while (it2 != ident_all_.end()) {
            DistAtom da = dist_matrix_[*it1][*it2];
            cout << *it1 << "-" << *it2 << "= " << da.real_dist_
                    << " seq_a: " << ctx_->align_mult_types_[*it1].substr(win_pos_, ctx_->win_l_)
                    << " seq_b: " << ctx_->align_mult_types_[*it2].substr(win_pos_, ctx_->win_l_)
                    << endl;
            ++it2;
        }
        ++it1;
    }

}

void AutoWinQ::outputBipartMatrix() {

    //vector< vector <set<string> > >

    cout << "outputBipartMatrix(): " << endl;
    for (int i = 0; i < bipart_matrix_.size(); i++) {
        cout << "outputBipartMatrix size: " << bipart_matrix_.size() << endl;
        cout << "bipartition: " << i << endl;
        vector <set<string> > one_bipart = bipart_matrix_[i];
        for (int j = 0; j < one_bipart.size(); j++) {
            cout << "  group: " << j << endl;

            set<string> one_group = one_bipart[j];
            set<string>::iterator it1 = one_group.begin();
            while (it1 != one_group.end()) {
                cout << "    ident: " << *it1 << endl;
                ++it1;
            }

        }

    }

}

void AutoWinQ::outputGroupings(ostream & os) {

    //output groupings
    string type_ident = "*";

    os << "outputGroupings():" << endl;
    for (int i = 0; i < NB_GROUPS_; i++) {
        os << "group: " << i << endl;
        for (set<string>::iterator it = ident_grouped_[i].begin();
                it != ident_grouped_[i].end();
                ++it) {

            if (ctx_->x_ident_.find(*it) != ctx_->x_ident_.end()) {
                type_ident = "*";
            } else {
                type_ident = " ";
            }

            os << type_ident << " " << *it
                    << " " << ctx_->align_mult_types_[*it].substr(win_pos_, ctx_->win_l_)
                    << endl;

        }

    }

}

void AutoWinQ::outputGroupingsToFiles(ostream& os_a, ostream & os_b) {

    //output groupings
    string type_ident = "*";

    for (int i = 0; i < NB_GROUPS_; i++) {

        if (i == 1) {

        }

        for (set<string>::iterator it = ident_grouped_[i].begin();
                it != ident_grouped_[i].end();
                ++it) {

            if (ctx_->x_ident_.find(*it) != ctx_->x_ident_.end()) {
                type_ident = "*";
            } else {
                type_ident = " ";
            }

            if (i == 0) {
                os_a << type_ident << " " << *it
                        //<< " " << ctx_->align_mult_types_[*it].substr(win_pos_, ctx_->win_l_)
                        << endl;

            } else if (i == 1) {
                os_b << type_ident << " " << *it
                        //<< " " << ctx_->align_mult_types_[*it].substr(win_pos_, ctx_->win_l_)
                        << endl;

            }

        }

    }

}

float AutoWinQ::randIndex() {

    float rand_index;
    //calculated each time, no need to reinitialize
    int calc_set[2];
    int file_set[2];

    int a, b, c, d;
    //swap one group
    int g0;

    //swap groups
    //for (g0 = 0; g0 < 2; g0++) {
    g0 = 1;
    rand_index = 0;
    a = 0;
    b = 0;
    c = 0;
    d = 0;

    //for every pair of identifiers
    set<string>::iterator it1, it2;
    it1 = ident_all_.begin();
    while (it1 != --(ident_all_.end())) {
        it2 = it1;
        it2++;

        while (it2 != ident_all_.end()) {

            // cout << " " << *it1
            //         << " " << *it2;

            //find calculated set
            // *it1 in group 0
            if (ident_grouped_[g0].find(*it1) != ident_grouped_[g0].end()) {
                calc_set[0] = 0;
            } else { // *it1 in group 1
                calc_set[0] = 1;
            }

            // *it2 in group 0
            if (ident_grouped_[g0].find(*it2) != ident_grouped_[g0].end()) {
                calc_set[1] = 0;
            } else { // *it1 in group 1
                calc_set[1] = 1;
            }

            //find file set
            // *it1 in group 0 (X)
            if (ctx_-> x_ident_.find(*it1) != ctx_->x_ident_.end()) {
                file_set[0] = 0;
            } else { // *it1 in group 1
                file_set[0] = 1;
            }

            // *it2 in group 0 (X)
            if (ctx_->x_ident_.find(*it2) != ctx_->x_ident_.end()) {
                file_set[1] = 0;
            } else { // *it2 in group 1
                file_set[1] = 1;
            }

            if (calc_set[0] == calc_set[1]) {
                //same set in X (Rand wikipedia)
                if (file_set[0] == file_set[1]) {
                    //same set in Y (Rand wikipedia)
                    //cout << " a: same X, same Y";
                    a++;
                } else {
                    //different set in Y (Rand wikipedia)
                    //cout << " c: same X, different Y";
                    c++;
                }
            } else {
                //different sets in X (Rand wikipedia)
                if (file_set[0] == file_set[1]) {
                    //same set in Y (Rand wikipedia)
                    //cout << " d: different X, same Y";
                    d++;
                } else {
                    //different set in Y (Rand wikipedia)
                    //cout << " b: different X, different Y";
                    b++;
                }

            }
            //cout << endl;

            ++it2;
        }
        ++it1;
    }

    rand_index = max(rand_index, (float(a + b) / float(a + b + c + d)));

    /*
    cout << "a: " << a
            << ",b: " << b
            << ",c: " << c
            << ",d: " << d
            << ",float(a + b): " << float(a + b)
            << ",float(a + b + c + d): " << float(a + b + c + d)
            << ",rand_index: " << rand_index
            << endl;
  */

    //}

    return rand_index;

}

float AutoWinQ::adjustedRandIndex() {

    float adjusted_rand_index;
    //calculated each time, no need to reinitialize
    int calc_set[2];
    int file_set[2];

    int a, b, c, d;
    //swap one group
    int g0;

    //swap groups
    //for (g0 = 0; g0 < 2; g0++) {
    g0 = 1;
    adjusted_rand_index = 0;

    a = 0;
    b = 0;
    c = 0;
    d = 0;

    //for every pair of identifiers
    set<string>::iterator it1, it2;
    it1 = ident_all_.begin();
    while (it1 != --(ident_all_.end())) {
        it2 = it1;
        it2++;

        while (it2 != ident_all_.end()) {

            //cout << " " << *it1
            //        << " " << *it2;

            //find calculated set
            // *it1 in group 0
            if (ident_grouped_[g0].find(*it1) != ident_grouped_[g0].end()) {
                calc_set[0] = 0;
            } else { // *it1 in group 1
                calc_set[0] = 1;
            }

            // *it2 in group 0
            if (ident_grouped_[g0].find(*it2) != ident_grouped_[g0].end()) {
                calc_set[1] = 0;
            } else { // *it1 in group 1
                calc_set[1] = 1;
            }

            //find file set
            // *it1 in group 0 (X)
            if (ctx_-> x_ident_.find(*it1) != ctx_->x_ident_.end()) {
                file_set[0] = 0;
            } else { // *it1 in group 1
                file_set[0] = 1;
            }

            // *it2 in group 0 (X)
            if (ctx_->x_ident_.find(*it2) != ctx_->x_ident_.end()) {
                file_set[1] = 0;
            } else { // *it2 in group 1
                file_set[1] = 1;
            }

            if (calc_set[0] == calc_set[1]) {
                //same group in U
                if (file_set[0] == file_set[1]) {
                    //same group in V
                    //cout << " a: same U, same V";
                    a++;
                } else {
                    //different set in V
                    //cout << " b: same U, different V";
                    b++;
                }
            } else {
                //different sets in U
                if (file_set[0] == file_set[1]) {
                    //same set in V
                    //cout << " c: different U, same V";
                    c++;
                } else {
                    //different set in V
                    //cout << " d: different U, different V";
                    d++;
                }

            }
            //cout << endl;

            ++it2;
        }
        ++it1;
    }

    //n_2 = binom(n,2)
    float n_2 = a + b + c + d;
    float cor = ((a + b) * (a + c) + (c + d) * (b + d));

    float num = n_2 * (a + d) - cor;
    float den = (n_2 * n_2) - cor;
    float ari_val = num / den;

    adjusted_rand_index = ari_val;

    /*
    cout << "a: " << a
            << ",b: " << b
            << ",c: " << c
            << ",d: " << d
            << ",cor: " << cor
            << ",num: " << num
            << ",den: " << den
            << ",adjusted_rand_index: " << adjusted_rand_index
            << endl;
    */

    //}

    return adjusted_rand_index;

}

float AutoWinQ::groupHamIndex() {

    float group_ham_idx;
    string g_local;
    string g_context;

    group_ham_idx = 0;

    //for every pair of identifiers
    set<string>::iterator it, it1, it2;

    it = ident_all_.begin();
    while (it != ident_all_.end()) {

        //local in group 0
        if (ident_grouped_[0].find(*it) != ident_grouped_[0].end()) {
            g_local.push_back('0');
        } else { // local in group 1
            g_local.push_back('1');
        }

        //context group
        //local in group 0
        if (ctx_-> x_ident_.find(*it) != ctx_->x_ident_.end()) {
            g_context.push_back('0');
        } else { // local in group 1
            g_context.push_back('1');
        }

        ++it;
    }

    Hamming hm;

    group_ham_idx = hm.dist_2seq(g_local, g_context).real_dist_;

    //invert characters
    for (int i = 0; i < g_local.length(); i++) {
        if (g_local[i] == '0') {
            g_local[i] = '1';

        } else {
            g_local[i] = '0';
        }

    }

    group_ham_idx = min(group_ham_idx, hm.dist_2seq(g_local, g_context).real_dist_);

    //cout << "group_ham_idx, dist: " << group_ham_idx << "g_local: " << g_local << ", context: " << g_context << endl;

    return group_ham_idx;

}

void AutoWinQ::test() {

    outputDistanceMatrix();
    outputGroupings(cout);
    /*
    for (int i = 0; i < ident_all_.size(); i++) {
        for (int j = 0; j < ident_all_.size(); j++) {
            DistAtom da = dist_matrix_[ident_all_[i]][ident_all_[j]];
            cout << ident_all_[i] << "-" << ident_all_[j] << "= " << da.real_dist_ << endl;
        }
    }
     */

    //output groupings
    /*
    for (int i = 0; i < NB_GROUPS_; i++) {
        cout << "group: " << i << endl;
        for (int j = 0; j < ident_grouped_[i].size(); j++) {
            cout << "ident: " << ident_grouped_[i][j] << endl;
        }

    }
     */
}

//verify that each group contains more than one element

void AutoWinQ::generateRandomInitGrouping() {

    //initialize
    ident_grouped_.clear();
    ident_grouped_.reserve(NB_GROUPS_);

    set<string> ss;
    //vs.clear();
    for (int i = 0; i < NB_GROUPS_; i++) {
        ident_grouped_.push_back(ss);
    }
    //randomly distribute identifiers
    set<string>::iterator it1 = ident_all_.begin();
    while (it1 != ident_all_.end()) {
        //random_integer = (rand()%10)+1;  1-10;
        //random_integer = lowest+int(range*rand()/(RAND_MAX + 1.0));

    	///int rand_group_id = (int) ((float) rand() / ((float) RAND_MAX + 1) * 2);

    	//distribution, numbers between 0 and 1
    	boost::uniform_int<> dist(0, 1);
    	//use global generator with this distribution
  	    boost::variate_generator<boost::mt19937&, boost::uniform_int<> > die(ctx_->appl_data_->random_number_gen_, dist);

  	    int rand_group_id = die() ;

        ident_grouped_[rand_group_id].insert(*it1);
        ++it1;
    }

    //not-empty groups
    bool empty_groups = false;
    for (int i = 0; i < NB_GROUPS_; i++) {
        if (ident_grouped_[i].size() < 2) {
            empty_groups = true;
            //cout << "emptyes groups" << endl;
        }
    }

    //terminal recursion to verify non-emptyness
    //always possible
    if (empty_groups) {
        generateRandomInitGrouping();
    }
    //outputGroupings();
}

void AutoWinQ::getContextGrouping() {

    //initialize
    ident_grouped_.clear();
    ident_grouped_.reserve(NB_GROUPS_);

    set<string> ss;
    //vs.clear();
    for (int i = 0; i < NB_GROUPS_; i++) {
        ident_grouped_.push_back(ss);
    }

    //same distribution of identifiers as in context
    // X -> group 0
    set<string>::iterator it0 = ctx_->x_ident_.begin();
    while (it0 != ctx_->x_ident_.end()) {
        ident_grouped_[0].insert(*it0);
        ++it0;
    }

    // Y -> group 1
    set<string>::iterator it1 = ctx_->y_ident_.begin();
    while (it1 != ctx_->y_ident_.end()) {
        ident_grouped_[1].insert(*it1);
        ++it1;
    }

   // outputGroupings(cout);

}

void AutoWinQ::calcQRes() {

    //cout << "nx: " << ident_grouped_[0].size() << endl;
    //cout << "ny: " << ident_grouped_[1].size() << endl;

    //vX, vY
    //update running totals
    for (int g = 0; g < NB_GROUPS_; g++) {

        //    for (int i = 0; i < (ident_grouped_[g].size() - 1); i++) {
        //       for (int j = (i + 1); j < ident_grouped_[g].size(); j++) {

        //initialize group elements
        v_1g_inv_nb_[g] = 0.0; //cumulative
        v_1g_cumul_[g] = 0.0; //cumulative
        v_1g_nb_[g] = 0.0; //cumulative
        //non cumulative elements
        //initialization optional (they will be overriten)
        v_1g_[g] = 0.0;
        v_1g_inv_prop_[g] = 0.0;

        set<string>::iterator it1, it2;
        it1 = ident_grouped_[g].begin();
        while (it1 != --(ident_grouped_[g].end())) {
            it2 = it1;
            it2++;

            while (it2 != ident_grouped_[g].end()) {

                //skip same elements
                if (it2 == it1) {
                    // cout << "it2 == it1" << *it2 << "-" << *it1 << endl;
                }
                //float v_1g_elem = dist_matrix_[ident_grouped_[g][i]][ident_grouped_[g][j]].real_dist_;
                float v_1g_elem = dist_matrix_[*it1][*it2].real_dist_;

                if (v_1g_elem != numeric_limits<float>::max()) {
                    //count it
                    v_1g_nb_[g]++;
                    //add value
                    //v_1g_cumul_[g] += pow(v_1g_elem, 2.0);
                    v_1g_cumul_[g] += v_1g_elem *v_1g_elem;
                } else {
                    //count invalid
                    v_1g_inv_nb_[g]++;
                }

                ++it2;
            }
            ++it1;
        }

        //scale to valid number of elements
        v_1g_[g] = v_1g_cumul_[g] / v_1g_nb_[g];
        //proportion of invalid positions
        v_1g_inv_prop_[g] = v_1g_inv_nb_[g] / (v_1g_nb_[g] + v_1g_inv_nb_[g]);

        //cout << "v_x: " << v_1g_nb_[g] << "v_x formula: " << (ident_grouped_[g].size() * (ident_grouped_[g].size() - 1) / 2.0) << endl;
    }

    //dXY

    //update running totals
    for (int g1 = 0; g1 < (NB_GROUPS_ - 1); g1++) {
        for (int g2 = (g1 + 1); g2 < NB_GROUPS_; g2++) {
            //initialize group elements
            d_2g_inv_nb_[g1][g2] = 0.0; //cumulative
            d_2g_cumul_[g1][g2] = 0.0; //cumulative
            d_2g_nb_[g1][g2] = 0.0; //cumulative
            //non cumulative elements
            //initialization optional (they will be overriten)
            d_2g_[g1][g2] = 0.0;
            d_2g_inv_prop_[g1][g2] = 0.0;

            // for (int i = 0; i < ident_grouped_[g1].size(); i++) {
            //    for (int j = 0; j < ident_grouped_[g2].size(); j++) {

            for (set<string>::iterator it1 = ident_grouped_[g1].begin();
                    it1 != ident_grouped_[g1].end();
                    ++it1) {
                for (set<string>::iterator it2 = ident_grouped_[g2].begin();
                        it2 != ident_grouped_[g2].end();
                        ++it2) {

                    //      float d_2g_elem = dist_matrix_[ident_grouped_[g1][i]][ident_grouped_[g2][j]].real_dist_;
                    float d_2g_elem = dist_matrix_[*it1][*it2].real_dist_;

                    if (d_2g_elem != numeric_limits<float>::max()) {
                        //count it
                        d_2g_nb_[g1][g2]++;
                        //add value
                        //d_2g_cumul_[g1][g2] += pow(d_2g_elem, 2.0);
                        d_2g_cumul_[g1][g2] += d_2g_elem * d_2g_elem;
                    } else {
                        //count invalid
                        d_2g_inv_nb_[g1][g2]++;
                    }

                }
            }

            //scale to valid number of elements
            d_2g_[g1][g2] = d_2g_cumul_[g1][g2] / d_2g_nb_[g1][g2];
            //proportion of invalid positions
            d_2g_inv_prop_[g1][g2] = d_2g_inv_nb_[g1][g2] / (d_2g_nb_[g1][g2] + d_2g_inv_nb_[g1][g2]);

            // cout << "d_xy: " << d_2g_nb_[g1][g2] << "d_xy formula: " << (ident_grouped_[g1].size() * ident_grouped_[g2].size()) << endl;

            //symetric results
            d_2g_inv_nb_[g2][g1] = d_2g_inv_nb_[g1][g2]; //cumulative
            d_2g_cumul_[g2][g1] = d_2g_cumul_[g1][g2]; //cumulative
            d_2g_nb_[g2][g1] = d_2g_nb_[g1][g2]; //cumulative
            //non cumulative elements
            //initialization optional (they will be overriten)
            d_2g_[g2][g1] = d_2g_[g1][g2];
            d_2g_inv_prop_[g2][g1] = d_2g_inv_prop_[g1][g2];

        }

    }

    //debug
    //cout << "d_2d_[0][1] " << d_2g_[0][1] << endl;
    //cout << "v_1g_[0] " << v_1g_[0] << endl;
    //cout << "v_1g_[1] " << v_1g_[1] << endl;

    //vAll
    //initialize group elements
     v_all_inv_nb_ = 0.0; //cumulative
     v_all_cumul_ = 0.0; //cumulative
     v_all_nb_ = 0.0; //cumulative
     //non cumulative elements
     //initialization optional (they will be overriten)
     v_all_ = 0.0;
     v_all_inv_prop_ = 0.0;

     set<string>::iterator it1, it2;
     it1 = ident_all_.begin();
      while (it1 != --(ident_all_.end())) {
                it2 = it1;
                it2++;

                while (it2 != ident_all_.end()) {

                    //skip same elements
                    if (it2 == it1) {
                        // cout << "it2 == it1" << *it2 << "-" << *it1 << endl;
                    }

                    float v_all_elem = dist_matrix_[*it1][*it2].real_dist_;

                    if (v_all_elem != numeric_limits<float>::max()) {
                        //count it
                        v_all_nb_++;
                        //add value
                        v_all_cumul_ += v_all_elem * v_all_elem;
                    } else {
                        //count invalid
                        v_all_inv_nb_++;
                    }

                    ++it2;
                }
                ++it1;
            }
            //scale to valid number of elements
            v_all_ = v_all_cumul_ / v_all_nb_;
            //proportion of invalid positions
            v_all_inv_prop_ = v_all_inv_nb_ / (v_all_nb_ + v_all_inv_nb_);

    //recalculate all functions
    //no need to reinitialize

    //reinitialize functions
    q_func_.resize(NB_FUNC_, 0.0);

    q_func_[0] = log(1 + d_2g_[0][1] - v_1g_[0]);
    q_func_[1] = d_2g_[0][1] - v_1g_[0];
    q_func_[2] = d_2g_[0][1] - v_1g_[1];
    q_func_[3] = 2 * d_2g_[0][1] - v_1g_[0] - v_1g_[1];
    q_func_[4] = d_2g_[0][1];
    q_func_[5] = abs(v_1g_[0] - v_1g_[1]); ///// d_2g_[0][1]
    q_func_[6] = abs(v_1g_[0] / v_1g_[1]); ///// d_2g_[0][1]

    //q_func_[7] = 0; //v_all_;
    //q_func_[6] = fabs(v_1g_[0] - v_1g_[1]);
    //q_func_[6] = abs ( log(v_1g_[0]) - log(v_1g_[1]))  ; ///// d_2g_[0][1]

}

void AutoWinQ::testOneCycleTransfers() {

    string cand_ident;
    //transfer identifiers group 0 -> 1

    float dist_cand_new_source_cumul;
    float dist_cand_new_source_cumul_inv_nb;

    float dist_cand_old_dest_cumul;
    float dist_cand_old_dest_cumul_inv_nb;

    int g0 = 1;
    int g1 = 0;
    int cycle_transfered = 0;
    int nb_null_cycles = 0;

    do {
        cycle_transfered = 0;
        int tmp = g0;
        g0 = g1;
        g1 = tmp;

        set<string>::iterator it1, it2;
        it1 = ident_grouped_[g0].begin();
        while (it1 != ident_grouped_[g0].end()) {
            cand_ident = *it1;

            //cout << "candidate: " << cand_ident << endl;

            //calculate candidate -> new_source
            dist_cand_new_source_cumul = 0;
            dist_cand_new_source_cumul_inv_nb = 0;
            it2 = ident_grouped_[g0].begin();
            while (it2 != ident_grouped_[g0].end()) {
                if (it2 != it1) {
                    //cout << "               " << "---------->" << *it2 << endl;
                    float v_1g_elem = dist_matrix_[*it1][*it2].real_dist_;

                    if (v_1g_elem != numeric_limits<float>::max()) {
                        //add value
                        dist_cand_new_source_cumul += pow(v_1g_elem, 2);
                    } else {
                        //count invalid
                        dist_cand_new_source_cumul_inv_nb++;
                    }

                }
                ++it2;
            }

            //calculate candidate -> old_dest
            dist_cand_old_dest_cumul = 0;
            dist_cand_old_dest_cumul_inv_nb = 0;
            it2 = ident_grouped_[g1].begin();
            while (it2 != ident_grouped_[g1].end()) {
                // cout << "               " << "---------->" << *it2 << endl;
                float d_2g_elem = dist_matrix_[*it1][*it2].real_dist_;

                if (d_2g_elem != numeric_limits<float>::max()) {
                    //dist_cand_old_dest_cumul += pow(d_2g_elem, 2.0);
                    dist_cand_old_dest_cumul += d_2g_elem*d_2g_elem;
                } else {
                    //count invalid
                    dist_cand_old_dest_cumul_inv_nb++;
                }

                ++it2;
            }

            //calculate differences
            new_v_1g_cumul_[g0] = v_1g_cumul_[g0] - dist_cand_new_source_cumul;
            //we do not substract invalid positions (they do not move)
            new_v_1g_nb_[g0] = v_1g_nb_[g0] - (ident_grouped_[g0].size() - 1 - dist_cand_new_source_cumul_inv_nb);
            //invalid positions move
            new_v_1g_inv_nb_[g0] = v_1g_inv_nb_[g0] - dist_cand_new_source_cumul_inv_nb;
            //update
            new_v_1g_[g0] = new_v_1g_cumul_[g0] / new_v_1g_nb_[g0];
            new_v_1g_inv_prop_[g0] = new_v_1g_inv_nb_[g0] / (new_v_1g_nb_[g0] + new_v_1g_inv_nb_[g0]);

            //
            new_v_1g_cumul_[g1] = v_1g_cumul_[g1] + dist_cand_old_dest_cumul;
            //we do not add invalid positions (they do not move)
            new_v_1g_nb_[g1] = v_1g_nb_[g1] + (ident_grouped_[g1].size() - dist_cand_old_dest_cumul_inv_nb);
            //invalid positions move
            new_v_1g_inv_nb_[g1] = v_1g_inv_nb_[g1] + dist_cand_old_dest_cumul_inv_nb;
            //update
            new_v_1g_[g1] = new_v_1g_cumul_[g1] / new_v_1g_nb_[g1];
            new_v_1g_inv_prop_[g1] = new_v_1g_inv_nb_[g1] / (new_v_1g_nb_[g1] + new_v_1g_inv_nb_[g1]);

            //
            new_d_2g_cumul_[g0][g1] = d_2g_cumul_[g0][g1] - dist_cand_old_dest_cumul + dist_cand_new_source_cumul;
            //we do not add invalid positions (they do not move)
            new_d_2g_nb_[g0][g1] = d_2g_nb_[g0][g1] -(ident_grouped_[g1].size() - dist_cand_old_dest_cumul_inv_nb)
                    + (ident_grouped_[g0].size() - 1 - dist_cand_new_source_cumul_inv_nb);
            //invalid positions move
            new_d_2g_inv_nb_[g0][g1] = d_2g_inv_nb_[g0][g1] - dist_cand_old_dest_cumul_inv_nb + dist_cand_new_source_cumul_inv_nb;
            //update
            new_d_2g_[g0][g1] = new_d_2g_cumul_[g0][g1] / new_d_2g_nb_[g0][g1];
            new_d_2g_inv_prop_[g0][g1] = new_d_2g_inv_nb_[g0][g1] / (new_d_2g_nb_[g0][g1] + new_d_2g_inv_nb_[g0][g1]);
            //symetric results
            new_d_2g_cumul_[g1][g0] = new_d_2g_cumul_[g0][g1];
            new_d_2g_nb_[g1][g0] = new_d_2g_nb_[g0][g1];
            new_d_2g_inv_nb_[g1][g0] = new_d_2g_inv_nb_[g0][g1];
            new_d_2g_[g1][g0] = new_d_2g_[g0][g1];
            new_d_2g_inv_prop_[g1][g0] = new_d_2g_inv_prop_[g0][g1];

            //
            //q_func_[0] = log(1 + d_2g_[0][1] - v_1g_[0]);

            //reinitialize functions
            new_q_func_.resize(NB_FUNC_, 0.0);

            new_q_func_[0] = log(1 + new_d_2g_[0][1] - new_v_1g_[0]);
            new_q_func_[1] = new_d_2g_[0][1] - new_v_1g_[0];
            new_q_func_[2] = new_d_2g_[0][1] - new_v_1g_[1];
            new_q_func_[3] = 2 * new_d_2g_[0][1] - new_v_1g_[0] - new_v_1g_[1];
            new_q_func_[4] = new_d_2g_[0][1];
            new_q_func_[5] =  abs(new_v_1g_[0] - new_v_1g_[1]);
            new_q_func_[6] =  abs(new_v_1g_[0] / new_v_1g_[1]);

            //q_func[5]   is invariable
            //new_q_func_[6] = abs ( log (new_v_1g_[0]) - log( new_v_1g_[1])) ;  //// new_d_2g_[0][1]
            //new_q_func_[7] = new_d_2g_[0][1] * (new_v_1g_[1] * new_v_1g_[0]);

            //cout << "q_func_[1]: " << q_func_[1] << " new_q_func_[1]: " << new_q_func_[1] << "diff: " << (new_q_func_[1] - q_func_[1]) << endl;

            //move the element if usefull
            if (new_q_func_[q_func_opt_max_idx_] > q_func_[q_func_opt_max_idx_]
                    && ident_grouped_[g0].size() > 0
                    && new_v_1g_nb_[g0] > 0
                    && new_v_1g_nb_[g1] > 0
                    && new_d_2g_nb_[g0][g1] > 0
                    ) {
                cycle_transfered++;
                //update sets
                ident_grouped_[g1].insert(*it1);
                //workaround for
                //iterator toDelete = itr2;
                //++itr2;   // increment before erasing!
                //container.erase(toDelete);
                ident_grouped_[g0].erase(it1++);

                //debug
                //calcQRes();
                //cout << "after calcQRes(): q_func_[1]: " << q_func_[4] << " new_q_func_[1]: " << new_q_func_[4] << "diff: " << (new_q_func_[4] - q_func_[4]) << endl;
                //cout << "q_func_[q_func_opt_max_idx_]: " << q_func_[q_func_opt_max_idx_] << " new_q_func_[q_func_opt_max_idx_]: " << new_q_func_[q_func_opt_max_idx_] << "diff: " << (new_q_func_[q_func_opt_max_idx_] - q_func_[q_func_opt_max_idx_]) << endl;

                //cout << " go : " << ident_grouped_[g0].size() << " g1 : " << ident_grouped_[g1].size() << endl;
                //update group information
                v_1g_cumul_[g0] = new_v_1g_cumul_[g0];
                v_1g_nb_[g0] = new_v_1g_nb_[g0];
                v_1g_inv_nb_[g0] = new_v_1g_inv_nb_[g0];
                v_1g_[g0] = new_v_1g_[g0];
                v_1g_inv_prop_[g0] = new_v_1g_inv_prop_[g0];

                v_1g_cumul_[g1] = new_v_1g_cumul_[g1];
                v_1g_nb_[g1] = new_v_1g_nb_[g1];
                v_1g_inv_nb_[g1] = new_v_1g_inv_nb_[g1];
                v_1g_[g1] = new_v_1g_[g1];
                v_1g_inv_prop_[g1] = new_v_1g_inv_prop_[g1];

                d_2g_cumul_[g0][g1] = new_d_2g_cumul_[g0][g1];
                d_2g_nb_[g0][g1] = new_d_2g_nb_[g0][g1];
                d_2g_inv_nb_[g0][g1] = new_d_2g_inv_nb_[g0][g1];
                d_2g_[g0][g1] = new_d_2g_[g0][g1];
                d_2g_inv_prop_[g0][g1] = new_d_2g_inv_prop_[g0][g1];
                //
                //symetric results
                d_2g_cumul_[g1][g0] = d_2g_cumul_[g0][g1];
                d_2g_nb_[g1][g0] = d_2g_nb_[g0][g1];
                d_2g_inv_nb_[g1][g0] = d_2g_inv_nb_[g0][g1];
                d_2g_[g1][g0] = d_2g_[g0][g1];
                d_2g_inv_prop_[g1][g0] = new_d_2g_inv_prop_[g0][g1];
                //functions 0-4 change with groupings
                q_func_[0] = new_q_func_[0];
                q_func_[1] = new_q_func_[1];
                q_func_[2] = new_q_func_[2];
                q_func_[3] = new_q_func_[3];
                q_func_[4] = new_q_func_[4];
                q_func_[5] = new_q_func_[5];
                q_func_[6] = new_q_func_[6];
                q_func_[7] = new_q_func_[7];
                //function 5 is invariable of groupings

            } else {
                ++it1;
            }

        }
        //cout << "cycle tranfered: " << cycle_transfered << " direction: " << g0 << ":" << g1 << " end q_func_[1] : " << q_func_[1]
        //       << " nx: " << ident_grouped_[0].size()
        //       << " ny: " << ident_grouped_[1].size()
        //      << endl;
        if (cycle_transfered == 0) {
            nb_null_cycles++;
        }
        //cout << "nb_null_cycles: " << nb_null_cycles << endl;
    } while (nb_null_cycles != 2);

    /*

      set<string>::iterator it = ident_all_.begin();
      while (it != ident_all_.end()) {
          if (*it == 2)
              it = ident_all_.erase(it);
          else
              ++it;
      }
     */

    //float d_2g_elem = dist_matrix_[ident_grouped_[0][i]][ident_grouped_[1][j]].real_dist_;

    //for (int j = 0; j < ident_grouped_[1].size(); j++) {

    //}

}

// with nj optimisation

void AutoWinQ::testNjBipartitions() {

	//calculate NJ bipartitions
    calculateNjBipartitions();

    float q_cycle = 0;

   cout << "q_func_opt_max_val_: " << q_func_opt_max_val_ << endl;

    //outputBipartMatrix();

    //all bipartitions are in bipart_matrix_ member variable

    //iterate over all bipartitions and CalcQRes()

    // vector < set<string> >

    cout << "outputBipartMatrix(): " << endl;
    for (int i = 0; i < bipart_matrix_.size(); i++) {

        cout << "bipartition: " << i << endl;

        //activate this bipartition
        //like generateRandomInitGrouping();
        ident_grouped_.clear();
        ident_grouped_ = bipart_matrix_[i];

        //reject trivial partitions
        if (ident_grouped_[0].size() < 2 || ident_grouped_[1].size() < 2) {
            continue;
        }

        //calculate
        calcQRes();

        q_cycle = q_func_[q_func_opt_max_idx_];
        cout << "q_cycle: " << q_cycle << endl;

        if (q_cycle > q_func_opt_max_val_) {
            //save decision position
            q_func_opt_max_val_ = q_cycle;
            //save all associated data

            if (best_window_ != NULL) {
                delete best_window_;
            }
            best_window_ = new AutoWinQ(*ctx_, *this);

        }

    }

}

//with k-means optimisation

void AutoWinQ::testManyRandomSeeds() {

    //advance position
    // win_pos_ += ctx_->win_step_;

    float q_init = 0;
    float q_cycle = 0;

    for (int i = 0; i < NB_RANDOM_SEEDS_ && !uncalculable_; i++) {
        //for (int i = 0; i < 1; i++) {

        int initial_wait = 0;
        //wait for decent values
        do {
            initial_wait++;
            //wait for decent grouping dimensions
            //at least two per group
            generateRandomInitGrouping();

            calcQRes();
            if (initial_wait >= NB_RANDOM_SEEDS_) {
                uncalculable_ = true;
                break;
            }
        } while (v_1g_nb_[0] < 1
                || v_1g_nb_[1] < 1
                || d_2g_nb_[0][1] < 1);

        /*
                q_init = q_func_[q_func_opt_max_idx_];
                if ( win_pos_ == 29 && i == 0) {
                    cout << "win_pos_: " << win_pos_ << "i: " << i << "q_init: " << q_init << endl;

                    if (q_init == numeric_limits<float>::signaling_NaN()) {
                       cout << "signaling nan: " << i << endl;
                      }

                    if (q_init == numeric_limits<float>::quiet_NaN()) {
                       cout << "QUIET nan: " << i << endl;
                      }

                }
         */

        testOneCycleTransfers();
        q_cycle = q_func_[q_func_opt_max_idx_];

        if (q_cycle > q_func_opt_max_val_) {
            //save decision position
            q_func_opt_max_val_ = q_cycle;
            //save all associated data

            if (best_window_ != NULL) {
                delete best_window_;
            }
            best_window_ = new AutoWinQ(*ctx_, *this);

        }

        if (!uncalculable_) {

            //cout << "i: " << i << " q_init: " << q_init << " q_cycle: " << q_cycle
            //       << " q_func_opt_max_val_: " << q_func_opt_max_val_
            //       << " go : " << ident_grouped_[0].size() << " g1 : " << ident_grouped_[1].size()
            //       << endl;
            /*
            cout << "best_window_: size " << best_window_->ident_grouped_.size()
                    << " go : " << best_window_->ident_grouped_[0].size() << " g1 : " << best_window_->ident_grouped_[1].size()
                    << endl;
             */

        }

    }

    //cout << "--------------------------------> " << best_window.q_func_[1] << endl;

    //outputGroupings(cout);

    //test();

}

void AutoWinQ::calcSimple() {

    getContextGrouping();
    calcQRes();

    simple_window_ = new AutoWinQ(*ctx_, *this);

}


#include <cstdlib>
#include <iostream>
#include "MolData3Ddescriptors.h"
#include <GraphMol/RDKitBase.h>
#include <boost/math/special_functions/round.hpp>

#include "GraphMol/PartialCharges/GasteigerCharges.h"
#include "GraphMol/PartialCharges/GasteigerParams.h"

using namespace std;

MolData3Ddescriptors::MolData3Ddescriptors() {}

std::vector<double> MolData3Ddescriptors::GetUn(int numAtoms) {
  std::vector<double> u(numAtoms, 1.0);

  return u;
}

std::vector<double> MolData3Ddescriptors::GetRelativeMW(
    const RDKit::ROMol& mol) {
  double* relativeMw = data3D.getMW();
  int numAtoms = mol.getNumAtoms();

  std::vector<double> pol(numAtoms, 0.0);
  for (int i = 0; i < numAtoms; ++i) {
    pol[i] = relativeMw[mol.getAtomWithIdx(i)->getAtomicNum() - 1];
  }
  return pol;
}

std::vector<double> MolData3Ddescriptors::GetRelativePol(
    const RDKit::ROMol& mol) {
  int numAtoms = mol.getNumAtoms();
  double* relativePol = data3D.getPOL();

  std::vector<double> pol(numAtoms, 0.0);
  for (int i = 0; i < numAtoms; ++i) {
    pol[i] = relativePol[mol.getAtomWithIdx(i)->getAtomicNum() - 1];
  }
  return pol;
}

std::vector<double> MolData3Ddescriptors::GetRelativeVdW(
    const RDKit::ROMol& mol) {
  int numAtoms = mol.getNumAtoms();
  double* relativeVdW = data3D.getVDW();

  std::vector<double> vdw(numAtoms, 0.0);
  for (int i = 0; i < numAtoms; ++i) {
    vdw[i] = relativeVdW[mol.getAtomWithIdx(i)->getAtomicNum() - 1];
  }
  return vdw;
}

std::vector<double> MolData3Ddescriptors::GetRelativeRcov(
    const RDKit::ROMol& mol) {
  int numAtoms = mol.getNumAtoms();
  double* rcov = data3D.getRCOV();

  std::vector<double> wroc(numAtoms, 0.0);
  for (int i = 0; i < numAtoms; ++i) {
    wroc[i] = rcov[mol.getAtomWithIdx(i)->getAtomicNum() - 1] / rcov[5];
  }
  return wroc;
}

std::vector<double> MolData3Ddescriptors::GetRelativeENeg(
    const RDKit::ROMol& mol) {
  int numAtoms = mol.getNumAtoms();
  double* relativeNeg = data3D.getNEG();

  std::vector<double> neg(numAtoms, 0.0);
  for (int i = 0; i < numAtoms; ++i) {
    neg[i] = relativeNeg[mol.getAtomWithIdx(i)->getAtomicNum() - 1];
  }
  return neg;
}

std::vector<double> MolData3Ddescriptors::GetRelativeIonPol(
    const RDKit::ROMol& mol) {
  int numAtoms = mol.getNumAtoms();
  double* absionpol = data3D.getIonPOL();

  std::vector<double> ionpols(numAtoms, 0.0);
  for (int i = 0; i < numAtoms; ++i) {
    ionpols[i] = absionpol[mol.getAtomWithIdx(i)->getAtomicNum() - 1];
  }
  return ionpols;
}

std::vector<double> MolData3Ddescriptors::GetCustomAtomProp(
      const RDKit::ROMol& mol, const std::string &customAtomPropName) {
    int numAtoms = mol.getNumAtoms();

    std::vector<double> customAtomarray(numAtoms, 0.0);
    for (int i = 0; i < numAtoms; ++i) {

        if (mol.getAtomWithIdx(i)->hasProp(customAtomPropName)) {
            customAtomarray[i] = mol.getAtomWithIdx(i)->getProp<double>(customAtomPropName);
        }
        else {
            customAtomarray[i] =1;
        }
    }
    return customAtomarray;
}

std::vector<double> MolData3Ddescriptors::GetCharges(const RDKit::ROMol& mol) {
  std::vector<double> charges(mol.getNumAtoms(), 0);
  // use 12 iterations... can be more
  RDKit::computeGasteigerCharges(mol, charges, 12, true);
  return charges;
}

int MolData3Ddescriptors::GetPrincipalQuantumNumber(int AtomicNum) {
  if (AtomicNum <= 2)
    return 1;
  else if (AtomicNum <= 10)
    return 2;
  else if (AtomicNum <= 18)
    return 3;
  else if (AtomicNum <= 36)
    return 4;
  else if (AtomicNum <= 54)
    return 5;
  else if (AtomicNum <= 86)
    return 6;
  else
    return 7;
}

std::vector<double> MolData3Ddescriptors::GetIState(const RDKit::ROMol& mol) {
  int numAtoms = mol.getNumAtoms();
  std::vector<double> Is(numAtoms, 1.0);  // values set to 1 for Hs

  for (int i = 0; i < numAtoms; ++i) {
    const RDKit::Atom* atom = mol.getAtomWithIdx(i);
    int atNum = atom->getAtomicNum();
    int degree = atom->getDegree();  // number of substituants (heavy of not?)
    if (degree > 0 && atNum > 1) {
      int h = atom->getTotalNumHs(
          true);  // caution getTotalNumHs(true) to count h !!!!
      int dv = RDKit::PeriodicTable::getTable()->getNouterElecs(atNum) -
               h;  // number of valence (explicit with Hs)
      int N = GetPrincipalQuantumNumber(atNum);  // principal quantum number
      double d = (double)degree - h;             // degree-h
      if (d > 0) {
        Is[i] =
            boost::math::round(1000 * (4.0 / (N * N) * dv + 1.0) / d) / 1000;
      }
    }
  }

  return Is;
}

std::vector<double> MolData3Ddescriptors::GetIStateDrag(
    const RDKit::ROMol& mol) {
  int numAtoms = mol.getNumAtoms();
  std::vector<double> Is(numAtoms, 1.0);

  for (int i = 0; i < numAtoms; ++i) {
    const RDKit::Atom* atom = mol.getAtomWithIdx(i);
    int atNum = atom->getAtomicNum();
    int degree = atom->getDegree();  // number of substituants
    if (degree > 0 && atNum > 1) {
      int h = atom->getTotalNumHs(true);
      int Zv = RDKit::PeriodicTable::getTable()->getNouterElecs(
          atNum);                  // number of valence (explicit with Hs)
      double dv = (double)Zv - h;  // number of valence electron without Hs
      int N = GetPrincipalQuantumNumber(atNum);  // principal quantum number
      double d = (double)degree - h;             // degree-h
      if (d > 0) {
        Is[i] =
            boost::math::round(1000 * (4.0 / (N * N) * dv + 1.0) / d) / 1000;
      }
    }
  }

  return Is;
}

// adaptation from EState.py
// we need the Is value only there
std::vector<double> MolData3Ddescriptors::GetEState(const RDKit::ROMol& mol) {
  int numAtoms = mol.getNumAtoms();

  std::vector<double> Is = GetIState(mol);

  double tmp, p;
  double* dist = RDKit::MolOps::getDistanceMat(mol, false, false);
  std::vector<double> accum(numAtoms, 0.0);

  for (int i = 0; i < numAtoms; i++) {
    for (int j = i + 1; j < numAtoms; j++) {
      p = dist[i * numAtoms + j] + 1;
      if (p < 1e6) {
        tmp = (Is[i] - Is[j]) / (p * p);
        accum[i] += tmp;
        accum[j] -= tmp;
      }
    }
  }

  for (int i = 0; i < numAtoms; i++) {
    Is[i] += accum[i];
  }

  return Is;
}

// modification of previous code to follow documentation from Padel code
std::vector<double> MolData3Ddescriptors::GetEState2(const RDKit::ROMol& mol) {
  int numAtoms = mol.getNumAtoms();

  std::vector<double> Si = GetIState(mol);

  // in WHIM definition it's write:
  double tmp, p, d;
  double* dist = RDKit::MolOps::getDistanceMat(mol, false, false);
  std::vector<double> accum(numAtoms, 0.0);

  for (int i = 0; i < numAtoms; i++) {
    for (int j = i + 1; j < numAtoms; j++) {
      d = dist[i * numAtoms + j];
      p = dist[i * numAtoms + j] + 1;
      if (d == 1) {
        tmp = (Si[i] - Si[j]) / (p * p);
        accum[i] += tmp;
        accum[j] -= tmp;
      }
    }
  }

  // add the Accum to the Si
  // WHIM Si values
  // electrotopological indices are scaled thus: Si'=Si + 7 => Si' > 0
  // In this case, only the nonhydrogen atoms are considered,
  // and the atomic electrotopological charge of each atom depends on its atom
  // neighbor.
  // So we should not use all the terms in the sum but only Adj matrix cases!

  // Correct the Si adding the rescaling parameter for WHIM only
  for (int i = 0; i < numAtoms; i++) {
    Si[i] += accum[i] + 7.0;
  }

  return Si;
}


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

#ifndef AMA_TENSOR_IEXP_IEXP_FACTORY_HPP
#define AMA_TENSOR_IEXP_IEXP_FACTORY_HPP 1

#include <ama/tensor/iexp/iexp_mutable.hpp>
#include <ama/tensor/iexp/iexp_constant.hpp>
#include <ama/tensor/iexp/iexp_temporary.hpp>
#include <ama/tensor/iexp/tensor_reducer.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/vector/vector0_c.hpp>

namespace ama
{
  namespace tensor_
  {

    namespace mpl = ::boost::mpl;

    /* forward declaration */
    template <typename WHAT>
    struct iexp_factory
    {
      template <typename ILIST, typename TENSOR>
      static
      WHAT apply(TENSOR const & t)
      {
        BOOST_MPL_ASSERT_MSG(
              (mpl::equal<WHAT, typename TENSOR::value_type>::type::value)
            , THIS_STRUCT_CAN_BE_CALLED_ONLY_TO_REDUCE_OVER_ALL_INDICES
            , (WHAT));

        return tensor_reducer<TENSOR,ILIST>(t).template at< ::boost::mpl::vector0_c<size_t> >();
      }
    };

    /* specialization for mutable index expression */
    template <typename DERIVED, typename CTLIST, typename COLIST>
    struct iexp_factory< iexp_temporary<DERIVED, CTLIST, COLIST> >
    {
      template <typename ILIST, typename TENSOR>
      static
      iexp_temporary<DERIVED,CTLIST,COLIST>
      apply(TENSOR const & t)
      {
        return iexp_temporary<DERIVED,CTLIST,COLIST>(tensor_reducer<TENSOR, ILIST>(t));
      }
    };

    /* specialization for mutable index expression */
    template <typename DERIVED, typename CTLIST, typename COLIST>
    struct iexp_factory< iexp_mutable<DERIVED, CTLIST, COLIST> >
    {
      template <typename ULIST>
      static
      iexp_mutable<DERIVED,CTLIST,COLIST>
      apply(DERIVED & t)
      {
        return iexp_mutable<DERIVED,CTLIST,COLIST>(t);
      }
    };

    /* specialization for constant index expression */
    template <typename DERIVED, typename CTLIST, typename COLIST>
    struct iexp_factory< iexp_constant<DERIVED, CTLIST, COLIST> >
    {
      template <typename ULIST>
      static
      iexp_constant<DERIVED,CTLIST,COLIST>
      apply(DERIVED const & t)
      {
        return iexp_constant<DERIVED,CTLIST,COLIST>(t);
      }
    };

  }
}

#endif /* AMA_TENSOR_IEXP_IEXP_FACTORY_HPP */