#pragma once
#include <vector>

#include <Eigen/Dense>

#include "Utils/IO/IOUtilities.hpp"

class TVRParameter {
   public:
    double swing_time;
    Eigen::Vector2d des_loc;
    Eigen::Vector3d stance_foot_loc;
    bool b_positive_sidestep;
    double yaw_angle;
};

class TVROutput {
   public:
    double time_modification;
    double switching_state[4];
};

class TVRPlanner {
   public:
    TVRPlanner();
    virtual ~TVRPlanner();

    void PlannerInitialization(const YAML::Node& node);

    void getNextFootLocation(const Eigen::Vector3d& com_pos,
                             const Eigen::Vector3d& com_vel,
                             Eigen::Vector3d& target_loc,
                             const void* additional_input = NULL,
                             void* additional_output = NULL);

    // Set Functions
    void setOmega(double com_height) {
        b_set_omega_ = true;
        omega_ = sqrt(9.81 / com_height);
    }
    void CheckEigenValues(double swing_time);

   protected:
    // current com state: (x, y, xdot, ydot) : 4
    // switching com state: (x, y, xdot, ydot) : 4
    // target foot: (x, y) : 2
    // swing time: t : 1
    Eigen::VectorXd planner_save_data_;

    std::vector<double> t_prime_;
    std::vector<double> kappa_;
    std::vector<double> x_step_length_limit_;
    std::vector<double> y_step_length_limit_;
    std::vector<double> com_vel_limit_;
    Eigen::MatrixXd R_w_t_;

    double omega_;
    bool b_set_omega_;

    void _computeSwitchingState(double swing_time,
                                const Eigen::Vector3d& com_pos,
                                const Eigen::Vector3d& com_vel,
                                const Eigen::Vector3d& stance_foot_loc,
                                std::vector<Eigen::Vector2d>& switching_state);
    void _StepLengthCheck(Eigen::Vector3d& target_loc,
                          const std::vector<Eigen::Vector2d>& switching_state);
    void _StepLengthCheck(Eigen::Vector3d& target_loc, bool b_positive_sidestep,
                          const Eigen::Vector3d& stance_foot);
    void _StepLengthCheckConsideringRotation(
        Eigen::Vector3d& target_loc, bool b_positive_sidestep,
        const Eigen::Vector3d& stance_foot);
    void _UpdateRotation(double yaw_angle);
    int _check_switch_velocity(
        const std::vector<Eigen::Vector2d>& switch_state);
    int _check_switch_velocity_considering_rotation(
        const std::vector<Eigen::Vector2d>& switch_state);
};


/*
    MIT License

    Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com)

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/

#ifndef VOXEL_MAP_HPP
#define VOXEL_MAP_HPP

#include "voxel_dilater.hpp"
#include <memory>
#include <vector>
#include <Eigen/Eigen>

namespace voxel_map
{

    constexpr uint8_t Unoccupied = 0;
    constexpr uint8_t Occupied = 1;
    constexpr uint8_t Dilated = 2;

    class VoxelMap
    {

    public:
        VoxelMap() = default;
        VoxelMap(const Eigen::Vector3i &size,
                 const Eigen::Vector3d &origin,
                 const double &voxScale)
            : mapSize(size),
              o(origin),
              scale(voxScale),
              voxNum(mapSize.prod()),
              step(1, mapSize(0), mapSize(1) * mapSize(0)),
              oc(o + Eigen::Vector3d::Constant(0.5 * scale)),
              bounds((mapSize.array() - 1) * step.array()),
              stepScale(step.cast<double>().cwiseInverse() * scale),
              voxels(voxNum, Unoccupied) {}

    private:
        Eigen::Vector3i mapSize;
        Eigen::Vector3d o;
        double scale;
        int voxNum;
        Eigen::Vector3i step;
        Eigen::Vector3d oc;
        Eigen::Vector3i bounds;
        Eigen::Vector3d stepScale;
        std::vector<uint8_t> voxels;
        std::vector<Eigen::Vector3i> surf;

    public:
        inline Eigen::Vector3i getSize(void) const
        {
            return mapSize;
        }

        inline double getScale(void) const
        {
            return scale;
        }

        inline Eigen::Vector3d getOrigin(void) const
        {
            return o;
        }

        inline Eigen::Vector3d getCorner(void) const
        {
            return mapSize.cast<double>() * scale + o;
        }

        inline const std::vector<uint8_t> &getVoxels(void) const
        {
            return voxels;
        }

        inline void setOccupied(const Eigen::Vector3d &pos)
        {
            const Eigen::Vector3i id = ((pos - o) / scale).cast<int>();
            if (id(0) >= 0 && id(1) >= 0 && id(2) >= 0 &&
                id(0) < mapSize(0) && id(1) < mapSize(1) && id(2) < mapSize(2))
            {
                voxels[id.dot(step)] = Occupied;
            }
        }

        inline void setOccupied(const Eigen::Vector3i &id)
        {
            if (id(0) >= 0 && id(1) >= 0 && id(2) >= 0 &&
                id(0) < mapSize(0) && id(1) < mapSize(1) && id(2) < mapSize(2))
            {
                voxels[id.dot(step)] = Occupied;
            }
        }

        inline void dilate(const int &r)
        {
            if (r <= 0)
            {
                return;
            }
            else
            {
                std::vector<Eigen::Vector3i> lvec, cvec;
                lvec.reserve(voxNum);
                cvec.reserve(voxNum);
                int i, j, k, idx;
                bool check;
                for (int x = 0; x <= bounds(0); x++)
                {
                    for (int y = 0; y <= bounds(1); y += step(1))
                    {
                        for (int z = 0; z <= bounds(2); z += step(2))
                        {
                            if (voxels[x + y + z] == Occupied)
                            {
                                VOXEL_DILATER(i, j, k,
                                              x, y, z,
                                              step(1), step(2),
                                              bounds(0), bounds(1), bounds(2),
                                              check, voxels, idx, Dilated, cvec)
                            }
                        }
                    }
                }

                for (int loop = 1; loop < r; loop++)
                {
                    std::swap(cvec, lvec);
                    for (const Eigen::Vector3i &id : lvec)
                    {
                        VOXEL_DILATER(i, j, k,
                                      id(0), id(1), id(2),
                                      step(1), step(2),
                                      bounds(0), bounds(1), bounds(2),
                                      check, voxels, idx, Dilated, cvec)
                    }
                }

                surf = cvec;
            }
        }

        inline void getSurfInBox(const Eigen::Vector3i &center,
                                 const int &halfWidth,
                                 std::vector<Eigen::Vector3d> &points) const
        {
            for (const Eigen::Vector3i &id : surf)
            {
                if (std::abs(id(0) - center(0)) <= halfWidth &&
                    std::abs(id(1) / step(1) - center(1)) <= halfWidth &&
                    std::abs(id(2) / step(2) - center(2)) <= halfWidth)
                {
                    points.push_back(id.cast<double>().cwiseProduct(stepScale) + oc);
                }
            }

            return;
        }

        inline void getSurf(std::vector<Eigen::Vector3d> &points) const
        {
            points.reserve(surf.size());
            for (const Eigen::Vector3i &id : surf)
            {
                points.push_back(id.cast<double>().cwiseProduct(stepScale) + oc);
            }
            return;
        }

        inline bool query(const Eigen::Vector3d &pos) const
        {
            const Eigen::Vector3i id = ((pos - o) / scale).cast<int>();
            if (id(0) >= 0 && id(1) >= 0 && id(2) >= 0 &&
                id(0) < mapSize(0) && id(1) < mapSize(1) && id(2) < mapSize(2))
            {
                return voxels[id.dot(step)];
            }
            else
            {
                return true;
            }
        }

        inline bool query(const Eigen::Vector3i &id) const
        {
            if (id(0) >= 0 && id(1) >= 0 && id(2) >= 0 &&
                id(0) < mapSize(0) && id(1) < mapSize(1) && id(2) < mapSize(2))
            {
                return voxels[id.dot(step)];
            }
            else
            {
                return true;
            }
        }

        inline Eigen::Vector3d posI2D(const Eigen::Vector3i &id) const
        {
            return id.cast<double>() * scale + oc;
        }

        inline Eigen::Vector3i posD2I(const Eigen::Vector3d &pos) const
        {
            return ((pos - o) / scale).cast<int>();
        }
    };
}

#endif


/**
 *
 * @file process.cc
 *
 */

// Include files
#include <math.h>
#include <log/log.hh>
#include <libconfig.h++>
#include <stdio.h>
#include <stdlib.h>
#include <fstream>

#include <boost/format.hpp>
#include <boost/filesystem.hpp>

#include "process.hh"
#include "vdt4/vdt_climatology_file_reader.hh"
#include "vdt4/vdt_grid.hh"
#include "vdt4/vdt_history_check.hh"
#include "vdt4/vdt_nearest_nbr.hh"
#include "vdt4/vdt_point.hh"
#include "vdt4/vdt_probe_message_file_reader.hh"
#include "vdt4/vdt_probe_message_qc_file_reader.hh"
#include "vdt4/vdt_probe_message_qc_statistics.hh"
#include "vdt4/vdt_probe_message_qc_file_writer.hh"
#include "vdt4/vdt_probe_message_qc_statistics_file_writer.hh"
#include "vdt4/vdt_road_segment.hh"
#include "vdt4/vdt_road_segment_data.hh"
#include "vdt4/vdt_road_segment_file_reader.hh"
#include "vdt_road_segment_datasets.hh"
#include "simulate_probe.hh"

using namespace std;
using namespace libconfig;

extern Log *Logg;
extern int Debug_level;

void ymdhms(time_t time_val, string &ymdhms)
{
  struct tm *tms = gmtime(&time_val);
  const int MAX_TIME = 16;
  char time_string[MAX_TIME];
  strftime(time_string, MAX_TIME, "%Y%m%d%H%M%S", tms);
  ymdhms = string(time_string);
}

int extract_file_list(const string &file_name, vector<string> &file_list)
{
  ifstream in_file(file_name.c_str());
  if (!in_file.is_open())
    {
      Logg->write_time_error("cannot open container file: %s\n", file_name.c_str());
      return -1;
    }

  string line;
  while (getline(in_file, line))
    {
      file_list.push_back(line);
    }

  return 0;
}

int get_file_lists(const arguments &args, vector<vector<string> > &file_lists)
{
  if (args.file_list_names.size() == 0)
    {
      return 0;
    }

  size_t file_list_size = 0;

  file_lists.resize(args.file_list_names.size());

  // File lists are either equal in size or 0 in size
  // File lists can contain the name None
  vector<size_t> file_list_sizes;

  Logg->write_time_info("extracting file lists\n");

  for (size_t i=0; i<args.file_list_names.size(); i++)
    {
      if (extract_file_list(args.file_list_names[i], file_lists[i]) < 0)
	{
	  return -1;
	}
      if (i == 0)
	{
	  file_list_size = file_lists[i].size();
	}
      else
	{
	  if (file_list_size != file_lists[i].size())
	    {
	      return -1;
	    }
	}
    }

  return 0;
}

// Determine the appropriate number of probe messages for the target lat, long for a road segment
int get_probe_message_count(int density, int utc_hour, const vdt_road_segment &road_segment, const config_reader &cfg_reader)
{
  // Get road segment information include mid point and type
  vdt_point target = road_segment.get_mid_point();
  double target_lat = target.get_latitude();
  double target_lon = target.get_longitude();

  string road_type = road_segment.get_road_type();

  // Derive road fraction based on road type
  float road_fraction = 0.0;
  if (road_type == cfg_reader.interstate_highway_type_string)
    {
      road_fraction = cfg_reader.interstate_highway_fraction;
    }
  else if (road_type == cfg_reader.us_highway_type_string)
    {
      road_fraction = cfg_reader.us_highway_fraction;
    }
  else if (density != arguments::LESS_DENSE && road_type == cfg_reader.state_highway_type_string)
    {
      road_fraction = cfg_reader.state_highway_fraction;
    }
  else
    {
      road_fraction = 0;
    }

  // Determine hour fraction
  float hour_fraction = cfg_reader.hourly_fractions[utc_hour];

  // Determine overall fraction
  float fraction = road_fraction * hour_fraction;

  // Find the closest city
  int count = -1;
  double min_dist = 1000000;
  int min_index = -1;

  // Debug print count
  static int debug_print_count = 0;

  for (unsigned int i=0; i<cfg_reader.city_table.size(); i++)
    {
      double dist = vdt_point::great_circle_distance(target_lat, target_lon, cfg_reader.city_table[i].latitude, cfg_reader.city_table[i].longitude);
      if (dist < min_dist)
	{
	  min_dist = dist;
	  min_index = i;
	}
    }

  if (min_index != -1)
    {
      if (min_dist <= cfg_reader.city_table[min_index].radius_km)
	{
	  if (density == arguments::LESS_DENSE)
	    {
	      count = rintf(fraction * cfg_reader.city_table[min_index].less_dense_num_cars);
	      printf("road_type, road_fraction, hour_fraction, total_fraction, table num cars: %s %f %f %f %d\n", road_type.c_str(), road_fraction, hour_fraction, fraction, cfg_reader.city_table[min_index].less_dense_num_cars);
	      printf("city %s %d, dist %lf, count %d, target, city lat lons %lf %lf %lf %lf\n", cfg_reader.city_table[min_index].city.c_str(), min_index, min_dist, count, target_lat, target_lon, cfg_reader.city_table[min_index].latitude, cfg_reader.city_table[min_index].longitude);
	    }
	  else
	    {
	      count = rintf(fraction * cfg_reader.city_table[min_index].more_dense_num_cars);
	    }
	}
      else
	{
	  if (density == arguments::LESS_DENSE)
	    {
	      count = rintf(fraction * cfg_reader.default_less_dense_num_cars);

	      if (debug_print_count < 100)
		{
		  printf("NO CITY: road_type, road_fraction, hour_fraction, total_fraction, less dense number, count: %s %f %f %f %d %d\n", road_type.c_str(), road_fraction, hour_fraction, fraction, cfg_reader.default_less_dense_num_cars, count);
		  debug_print_count += 1;
		}

	    }
	  else
	    {
	      count = rintf(fraction * cfg_reader.default_more_dense_num_cars);
	    }
	}
    }

  return count;
}

int process::do_simulation(unsigned int ct, time_t current_time, const config_reader &cfg_reader, const vector<vector<string> > &file_lists, const vector<string> &field_names, const vector<string> &field_units, int wiper_status_field_number, const vector<float> &deviations, const vector<vdt_road_segment> &input_segments)
{

  field_info field_info_obj(field_names, field_units);

  int utc_hour = (current_time / 3600) % 24;

  // Read input files in vdt_road_segment_datasets()
  string radar_file = "";
  string metar_file = "";
  string rwis_file = "";
  string rtma_file = "";

  auto file_itr = args.file_list_position.find("RADAR");
  if (file_itr != args.file_list_position.end())
    {
      radar_file = file_lists[file_itr->second][ct];
    }

  file_itr = args.file_list_position.find("METAR");
  if (file_itr != args.file_list_position.end())
    {
      metar_file = file_lists[file_itr->second][ct];
    }

  file_itr = args.file_list_position.find("RWIS");
  if (file_itr != args.file_list_position.end())
    {
      rwis_file = file_lists[file_itr->second][ct];
    }

  file_itr = args.file_list_position.find("RTMA");
  if (file_itr != args.file_list_position.end())
    {
      rtma_file = file_lists[file_itr->second][ct];
    }

  Logg->write_time_info("constructing vdt_road_segment_datasets from input files for time %ld: %s, %s, %s, %s\n", current_time, radar_file.c_str(), metar_file.c_str(), rwis_file.c_str(), rtma_file.c_str());

  vdt_road_segment_datasets ds(cfg_reader, radar_file, metar_file, rwis_file, rtma_file, Logg);

  // Fill in known data for each road segment using road segments datasets
  Logg->write_time_info("updating road segments\n");
  vector<vdt_road_segment_data> road_segment_data_vec;
  ds.update_road_segments(current_time, cfg_reader.wiper_status, field_info_obj, input_segments, road_segment_data_vec);

  // Simulate probe messages based on the road segment data in out_segments

  Logg->write_time_info("simulating probe messages based on %ld road segments\n", road_segment_data_vec.size());
  vector<int> covered(road_segment_data_vec.size(), 0);
  vector<vdt_road_segment_data *> output_road_segment_data_vec(road_segment_data_vec.size(), 0);
  vector<vdt_generic_message> combined_probe_messages;

  for (size_t i=0; i<road_segment_data_vec.size(); i++)
    {
      //printf("radar x, y index %f %f\n", road_segment_data_vec[i].field_values[0], road_segment_data_vec[i].field_values[1]);

      // determine the number of probe messages to be used for this road segment
      int number_probe_messages = get_probe_message_count(args.car_density, utc_hour, road_segment_data_vec[i].road_segment, cfg_reader);
      if (number_probe_messages == -1)
	{
	  Logg->write_time_error("get_probe_message_count() returned -1\n");
	  return -1;
	}
      else if (number_probe_messages == 0)
	{
	  continue;
	}
      covered[i] = 1;

      // simulate probe messages for this segment and push onto probe_messages
      vector<vdt_generic_message> probe_messages;

      vdt_road_segment_data *output_road_segment_data = new vdt_road_segment_data(field_info_obj, road_segment_data_vec[i].obs_time, number_probe_messages, road_segment_data_vec[i].road_segment);

      simulate_probe_road_segment_data(number_probe_messages, field_info_obj.field_names.size(), deviations, road_segment_data_vec[i], *output_road_segment_data, probe_messages);
      std::copy (probe_messages.begin(), probe_messages.end(), combined_probe_messages.end());
      output_road_segment_data_vec[i] = output_road_segment_data;
    }

  // Output the road segment data vector in csv format if called for
  if (args.csv_out_file_base != "")
    {
      // Create output file name using date/time
      string date_string;
      ymdhms(current_time, date_string);
      string csv_out_file = args.csv_out_file_base + "." + date_string.substr(0, 8) + "." + date_string.substr(8, 4) + ".csv";

      Logg->write_time_info("writing data for %ld road segments to file: %s\n", output_road_segment_data_vec.size(), csv_out_file.c_str());

      FILE *fp = fopen(csv_out_file.c_str(), "w");
      if (fp == NULL)
	{
	  Logg->write_time_warning("cannot write csv output file %s\n", csv_out_file.c_str());
	}
      else
	{
	  bool printed_header = false;

	  for (size_t i=0; i<output_road_segment_data_vec.size(); i++)
	    {
	      if (covered[i] == 1)
		{

		  if (!printed_header)
		    {
		      output_road_segment_data_vec[i]->print_header(fp);
		      printed_header = true;
		    }

		  if (args.debug_segments)
		    {
		      road_segment_data_vec[i].print_values(fp, i);
		    }

		  output_road_segment_data_vec[i]->print_values(fp, i);
		}
	    }

	  fclose(fp);
	}
    }
  else if (args.dicast_csv_out_file_base != "")
    {
      // Create output file name using date/time
      string date_string;
      ymdhms(current_time, date_string);
      string csv_out_file = args.dicast_csv_out_file_base + "." + date_string.substr(0, 8) + "." + date_string.substr(8, 4) + ".csv";

      Logg->write_time_info("writing wiper data for %ld road segments to file: %s\n", output_road_segment_data_vec.size(), csv_out_file.c_str());

      FILE *fp = fopen(csv_out_file.c_str(), "w");
      if (fp == NULL)
	{
	  Logg->write_time_warning("cannot write csv output file %s\n", csv_out_file.c_str());
	}

      else
	{
	  for (size_t i=0; i<output_road_segment_data_vec.size(); i++)
	    {
	      if (covered[i] == 1)
		{
		  output_road_segment_data_vec[i]->print_aux_id_field(fp, wiper_status_field_number);
		}
	    }

	  fclose(fp);
	}
    }

  // Output the simulated probe messages
  Logg->write_time_info("writing %ld probe messages\n", combined_probe_messages.size());
  for (unsigned int i=0; i<output_road_segment_data_vec.size(); i++)
    {
      delete output_road_segment_data_vec[i];
      output_road_segment_data_vec[i] = 0;
    }

  Logg->write_time_info("returning from do_simulation()\n");
  return 0;
}

process::~process()
{
}

int process::run()
{
  time_t begin_time = args.begin_time;
  string begin_time_string = args.begin_time_string;
  time_t end_time = args.end_time;
  string end_time_string = args.end_time_string;

  // Read configuration file
  Logg->write_time_info("reading config file %s\n", args.cfg_file.c_str());

  config_reader cfg_reader(args.cfg_file);
  if (cfg_reader.error != string(""))
    {
      Logg->write_time_error("configuration file error %s\n", cfg_reader.error.c_str());
      error = string("configuration file error: ") + cfg_reader.error;
      return -1;
    }

  vdt_climatology_file_reader *climate_data = 0;
  vector<vdt_road_segment> input_segments;
  vector<vdt_road_segment> rwis_sites;
  vector<int> seg_ids;
  vector<int> rwis_site_ids;

  vector<string> field_names = {{"lat", "lon", "radar_x", "radar_y", "radar_ref", "wiper_status", "model_x", "model_y", "model_air_temp", "model_bar_press", "model_dewpoint", "model_geopotential_height", "model_wind_dir", "model_wind_speed", "nss_air_temp_mean", "nss_bar_press_mean", "nss_dew_temp_mean", "nss_hourly_precip_mean", "nss_prevail_vis_mean", "nss_wind_dir_mean", "nss_wind_speed_mean"}};

  int wiper_status_field_number = 5; // this value must correspond to the placement location of wiper_status in field
  vector<string> field_units = {{"degrees", "degrees", "grid", "grid", "dbZ", "none", "grid", "grid", "celsius", "hPa", "celsius", "m", "degrees", "m/s", "celsius", "hPa", "celsius", "in", "km", "degrees", "m/s"}};
  vector<float> deviations = {{0, 0, 0, 0, 0, 0, 0, 0, 0.5, 1, 0.5, 0, 10, 1, 0.5, 1, 0.5, 0, 0, 10, 1}};

  Logg->write_time_info("using the following variables, units and deviations\n");
  for (unsigned int i=0; i<field_names.size(); i++)
    {
      Logg->write_time_info("%s, %s, %f\n", field_names[i].c_str(), field_units[i].c_str(), deviations[i]);
    }

  Logg->write_time_info("calling vdt_climatology_file_reader on climatology file %s\n", cfg_reader.climatology_file.c_str());
  climate_data = new vdt_climatology_file_reader(cfg_reader.climatology_file);
  if (climate_data->error() != "")
    {
      Logg->write_time_warning("failed in vdt_climatology_file_reader constructor: %s\n", climate_data->error().c_str());
    }

  string road_seg_file;
  if (args.car_density == arguments::LESS_DENSE)
    {
      Logg->write_time_info("using less dense road segment file %s\n", cfg_reader.less_dense_file.c_str());
      road_seg_file = cfg_reader.less_dense_file;
    }
  else
    {
      Logg->write_time_info("using more dense road segment file %s\n", cfg_reader.more_dense_file.c_str());
      road_seg_file = cfg_reader.more_dense_file;
    }

  if (boost::filesystem::exists(road_seg_file))
    {
      Logg->write_time_info("calling vdt_road_segment_file_reader(%s)\n", road_seg_file.c_str());
      vdt_road_segment_file_reader seg_reader = vdt_road_segment_file_reader(road_seg_file.c_str());

      input_segments = seg_reader.get_road_segments();
      rwis_sites = seg_reader.get_rwis_sites();
    }
  else
    {
      Logg->write_time_error("cannot read road segment file %s\n", road_seg_file.c_str());
      return -1;
    }

  if (args.test)
    {
      Logg->write_time_info("initializing simulator test messages\n");
    }
  else
    {
      vector<vector<string> > file_lists;
      Logg->write_time_info("assembling input data file lists for simulation\n");
      int ret = get_file_lists(args, file_lists);
      if (ret < 0)
	{
	  Logg->write_time_error("failed in get_file_lists()\n");
	  return -1;
	}

      // Perform simulation
      Logg->write_time_info("performing simulation\n");

      unsigned int ct = 0;
      for (time_t current_time=begin_time; current_time<end_time; current_time+=args.time_delta)
	{
	  ret = do_simulation(ct, current_time, cfg_reader, file_lists, field_names, field_units, wiper_status_field_number, deviations, input_segments);
	  if (ret < 0)
	    {
	      Logg->write_time_error("failed in do_simulation()\n");
	      return -1;
	    }

	  ct += 1;
	}
    }

  return 0;
}


/*
Copyright (c) 2016 Bastien Durix

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

/**
 *  \file VoronoiSkeleton2D.cpp
 *  \brief Defines functions to compute 2d skeleton with voronoi algorithm
 *  \author Bastien Durix
 */

#include "VoronoiSkeleton2D.h"
#include <voro++/voro++.hh>
#include <Eigen/Dense>
#include <mathtools/affine/Point.h>

#include <algorithm/graphoperation/ConnectedComponents.h>

template<typename Model>
void VoronoiOrtho(typename skeleton::GraphCurveSkeleton<Model>::Ptr skel_int, const boundary::DiscreteBoundary<2>::Ptr disbnd, const mathtools::affine::Frame<2>::Ptr frame)
{
	typename skeleton::GraphCurveSkeleton<Model>::Ptr grskel(new skeleton::GraphCurveSkeleton<Model>(skel_int->getModel()));

	std::vector<mathtools::affine::Point<2> > bndpts(0);
	disbnd->getVerticesPoint(bndpts);
	std::vector<Eigen::Vector2d> bndvec(bndpts.size());

	/**
	 *  Get the points coordinates in skeleton frame
	 */
	double xinf = 0;
	double xsup = 0;
	double yinf = 0;
	double ysup = 0;
	for(unsigned int i=0; i< bndpts.size(); i++)
	{
		bndvec[i] = bndpts[i].getCoords(frame);
		if(xinf>bndvec[i](0) || i==0)
			xinf=bndvec[i](0);
		if(yinf>bndvec[i](1) || i==0)
			yinf=bndvec[i](1);
		if(xsup<bndvec[i](0) || i==0)
			xsup=bndvec[i](0);
		if(ysup<bndvec[i](1) || i==0)
			ysup=bndvec[i](1);
	}

	double xsize = xsup-xinf;
	double ysize = ysup-yinf;

	xinf -= xsize*10;
	xsup += xsize*10;
	yinf -= ysize*10;
	ysup += ysize*10;

	/*
	 *  Build Voronoi container
	 */
	voro::container vorocont(xinf,xsup,
							 yinf,ysup,
							 -1.0, 1.0,
							 6,6,6,
							 false,false,false,
							 8);

	/*
	 *  Put points in voronoi container
	 */
	for(unsigned int i=0;i<bndpts.size();i++)
	{
		vorocont.put(i,bndvec[i](0),bndvec[i](1),0.0);
	}

	/*
	 *  Loop on cells (with neighbor information)
	 */
	voro::c_loop_all voroloopall(vorocont);

	/*
	 *  Nodes that are inside the skeleton
	 */
	std::list<unsigned int> v_intsph;

	/*
	 *  Added nodes
	 */
	std::map<unsigned int,Eigen::Vector3d> v_added;

	if(voroloopall.start())
	{
		do
		{
			voro::voronoicell_neighbor voroneigh;
			if(vorocont.compute_cell(voroneigh,voroloopall))/*For each cell*/
			{
				/*Put cell corners in skeleton*/
				double x,y,z;
				voroloopall.pos(x,y,z);

				std::vector<double> vert;
				voroneigh.vertices(x,y,z,vert);

				Eigen::Vector2d center(x,y);

				std::vector<unsigned int> indices(voroneigh.p);

				for(unsigned int i=0;i<(unsigned int)voroneigh.p;i++)
				{
					if(vert[i*3+2]==1.0)
					{
						Eigen::Vector3d corner(vert[i*3],vert[i*3+1],0.0);
						corner(2) = (corner.block<2,1>(0,0)-center).norm();
						bool isin=false;
						for(std::map<unsigned int,Eigen::Vector3d>::iterator it = v_added.begin(); it != v_added.end() & !isin; it++)
						{
							if(it->second.isApprox(corner,std::numeric_limits<float>::epsilon()))
							{
								isin=true;
								indices[i] = it->first;
							}
						}
						if(!isin)
						{
							indices[i] = grskel->addNode(corner);
							v_added[indices[i]] = corner;
						}
					}
				}

				/*
				 * Then link each corner to its neighbors
				 * Except if there is a link between the two cells
				 */
				for(unsigned int i=0;i<(unsigned int)voroneigh.p;i++)
				{
					if(vert[i*3+2]==1.0)
					{
						for(int j=0;j<voroneigh.nu[i];j++) //nu : corner order (number of edge from this corner)
						{
							int ind_j = voroneigh.ed[i][j];
							if(vert[ind_j*3+2]==1.0)
							{
								bool found=false;
								unsigned int ind_neigh=0;
								/*Test if there is a link between the two cells*/
								for(int k=0;k<voroneigh.nu[i] && !found;k++)
								{
									if(voroneigh.ne[i][k] >=0 )
									{
										int face = voroneigh.ne[i][k];
										for(int l=0;l<voroneigh.nu[ind_j] && !found;l++)
										{
											if(face == voroneigh.ne[ind_j][l])
											{
												found=true;
												ind_neigh=face;
											}
										}
									}
								}

								if(!found)
								{
									grskel->addEdge(indices[i],indices[ind_j]);
								}
								else if(disbnd->getNext(voroloopall.pid())!=ind_neigh && disbnd->getNext(ind_neigh)!=(unsigned int)voroloopall.pid())
								{
									grskel->addEdge(indices[i],indices[ind_j]);
								}
								else if(disbnd->getNext(ind_neigh)==(unsigned int)voroloopall.pid() && disbnd->getFrame()->getBasis()->isDirect())
								{
									Eigen::Matrix2d mat;
									mat.block<2,1>(0,0) = bndpts[ind_neigh].getCoords() - bndpts[voroloopall.pid()].getCoords();
									mat.block<2,1>(0,1) = grskel->getNode(indices[ind_j]).template block<2,1>(0,0) - grskel->getNode(indices[i]).template block<2,1>(0,0);

									if(mat.determinant()<0)
										v_intsph.push_back(indices[ind_j]);
									else
										v_intsph.push_back(indices[i]);
								}
								else if(disbnd->getNext((unsigned int)voroloopall.pid())==ind_neigh && !disbnd->getFrame()->getBasis()->isDirect())
								{
									Eigen::Matrix2d mat;
									mat.block<2,1>(0,0) = bndpts[ind_neigh].getCoords() - bndpts[voroloopall.pid()].getCoords();
									mat.block<2,1>(0,1) = grskel->getNode(indices[ind_j]).template block<2,1>(0,0) - grskel->getNode(indices[i]).template block<2,1>(0,0);

									if(mat.determinant()>0)
										v_intsph.push_back(indices[ind_j]);
									else
										v_intsph.push_back(indices[i]);
								}
							}
						}
					}
				}
			}
		}while(voroloopall.inc());
	}

	v_intsph.sort();
	v_intsph.unique();

	/*
	 *  Separation into connected components (internal and external)
	 */
	std::list<typename skeleton::GraphCurveSkeleton<Model>::Ptr> list_comp = algorithm::graphoperation::SeparateComponents(grskel);

	for(typename std::list<typename skeleton::GraphCurveSkeleton<Model>::Ptr>::iterator it = list_comp.begin(); it!=list_comp.end(); it++)
	{
		std::list<unsigned int> list_ver;
		(*it)->getAllNodes(list_ver);

		bool intern = false;
		for(std::list<unsigned int>::iterator itl = v_intsph.begin(); itl != v_intsph.end() && !intern; itl++)
		{
			if( std::find(list_ver.begin(),list_ver.end(),*itl) != list_ver.end() )
			{
				intern=true;
			}
		}

		if(intern)
		{
			skel_int->insertExactSkel(*it);
		}
	}
}

skeleton::GraphSkel2d::Ptr algorithm::skeletonization::VoronoiSkeleton2d(const boundary::DiscreteBoundary<2>::Ptr disbnd)
{
	skeleton::model::Classic<2>::Ptr model(new skeleton::model::Classic<2>(disbnd->getFrame()));
	skeleton::GraphSkel2d::Ptr grskel(new skeleton::GraphSkel2d(model));

	VoronoiOrtho<skeleton::model::Classic<2> >(grskel,disbnd,model->getFrame());

	return grskel;
}


#ifndef MAPNIK_VECTOR_TILE_STRATEGY_HPP
#define MAPNIK_VECTOR_TILE_STRATEGY_HPP

// mapnik
#include <mapnik/config.hpp>
#include <mapnik/util/noncopyable.hpp>
#include <mapnik/proj_transform.hpp>
#include <mapnik/view_transform.hpp>
#include <mapnik/geometry.hpp>

#include <mapnik/version.hpp>
#if MAPNIK_VERSION >= 300100
#include <mapnik/geometry/envelope.hpp>
#else
#include <mapnik/geometry_envelope.hpp>
#endif

// mapbox
#include <mapbox/geometry/geometry.hpp>

#pragma GCC diagnostic push
#include <mapnik/warning_ignore.hpp>
#include "boost_geometry_adapters.hpp"
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/access.hpp>
#pragma GCC diagnostic pop

#include <memory>

namespace mapnik {

namespace vector_tile_impl {

static constexpr std::int64_t hiRange = 0x3FFFFFFFFFFFFFFFLL;
static constexpr double coord_max = static_cast<double>(hiRange);
static constexpr double coord_min = -1 * static_cast<double>(hiRange);

struct vector_tile_strategy
{
    vector_tile_strategy(view_transform const& tr)
        : tr_(tr) {}

    template <typename P1, typename P2>
    inline bool apply(P1 const& p1, P2 & p2) const
    {
        using p2_type = typename boost::geometry::coordinate_type<P2>::type;
        double x = boost::geometry::get<0>(p1);
        double y = boost::geometry::get<1>(p1);
        tr_.forward(&x,&y);
        x = std::round(x);
        y = std::round(y);
        if (x <= coord_min || x >= coord_max ||
            y <= coord_min || y >= coord_max) return false;
        boost::geometry::set<0>(p2, static_cast<p2_type>(x));
        boost::geometry::set<1>(p2, static_cast<p2_type>(y));
        return true;
    }

    template <typename P1, typename P2>
    inline P2 execute(P1 const& p1, bool & status) const
    {
        P2 p2;
        status = apply(p1, p2);
        return p2;
    }

    view_transform const& tr_;
};

struct vector_tile_strategy_proj
{
    vector_tile_strategy_proj(proj_transform const& prj_trans,
                              view_transform const& tr)
        : prj_trans_(prj_trans),
          tr_(tr) {}

    template <typename P1, typename P2>
    inline bool apply(P1 const& p1, P2 & p2) const
    {
        using p2_type = typename boost::geometry::coordinate_type<P2>::type;
        double x = boost::geometry::get<0>(p1);
        double y = boost::geometry::get<1>(p1);
        double z = 0.0;
        if (!prj_trans_.backward(x, y, z)) return false;
        tr_.forward(&x,&y);
        x = std::round(x);
        y = std::round(y);
        if (x <= coord_min || x >= coord_max ||
            y <= coord_min || y >= coord_max) return false;
        boost::geometry::set<0>(p2, static_cast<p2_type>(x));
        boost::geometry::set<1>(p2, static_cast<p2_type>(y));
        return true;
    }

    template <typename P1, typename P2>
    inline P2 execute(P1 const& p1, bool & status) const
    {
        P2 p2;
        status = apply(p1, p2);
        return p2;
    }

    proj_transform const& prj_trans_;
    view_transform const& tr_;
};

template <typename T>
struct geom_out_visitor
{
    std::shared_ptr<mapbox::geometry::geometry<T>> geom;

    template <typename T1>
    void operator() (T1 const& g)
    {
        geom = std::make_shared<mapbox::geometry::geometry<T>>(mapbox::geometry::geometry<T>(g));
    }
};

// TODO - avoid creating degenerate polygons when first/last point of ring is skipped
template <typename TransformType, typename NextProcessor>
struct transform_visitor
{
    TransformType const& tr_;
    NextProcessor & next_;
    box2d<double> const& target_clipping_extent_;

    transform_visitor(TransformType const& tr,
                      box2d<double> const& target_clipping_extent,
                      NextProcessor & next) :
      tr_(tr),
      next_(next),
      target_clipping_extent_(target_clipping_extent) {}

    inline void operator() (mapnik::geometry::point<double> const& geom)
    {
        if (!target_clipping_extent_.intersects(geom.x,geom.y))
        {
            return;
        }
        mapbox::geometry::point<std::int64_t> new_geom;
        if (!tr_.apply(geom,new_geom))
        {
            return;
        }
        return next_(new_geom);
    }

    inline void operator() (mapnik::geometry::multi_point<double> const& geom)
    {
        mapbox::geometry::multi_point<std::int64_t> new_geom;
        new_geom.reserve(geom.size());
        for (auto const& pt : geom)
        {
            mapbox::geometry::point<std::int64_t> pt2;
            if (target_clipping_extent_.intersects(pt.x, pt.y) && tr_.apply(pt,pt2))
            {
                new_geom.push_back(std::move(pt2));
            }
        }
        if (new_geom.empty())
        {
            return;
        }
        return next_(new_geom);
    }

    inline void operator() (mapnik::geometry::line_string<double> const& geom)
    {
        mapnik::box2d<double> geom_bbox = mapnik::geometry::envelope(geom);
        if (!target_clipping_extent_.intersects(geom_bbox))
        {
            return;
        }
        mapbox::geometry::line_string<std::int64_t> new_geom;
        new_geom.reserve(geom.size());
        for (auto const& pt : geom)
        {
            mapbox::geometry::point<std::int64_t> pt2;
            if (tr_.apply(pt,pt2))
            {
                new_geom.push_back(std::move(pt2));
            }
        }
        return next_(new_geom);
    }

    inline void operator() (mapnik::geometry::multi_line_string<double> const& geom)
    {
        mapbox::geometry::multi_line_string<std::int64_t> new_geom;
        new_geom.reserve(geom.size());
        for (auto const& line : geom)
        {
            mapnik::box2d<double> line_bbox = mapnik::geometry::envelope(line);
            if (!target_clipping_extent_.intersects(line_bbox)) continue;
            mapbox::geometry::line_string<std::int64_t> new_line;
            new_line.reserve(line.size());
            for (auto const& pt : line)
            {
                mapbox::geometry::point<std::int64_t> pt2;
                if (tr_.apply(pt,pt2))
                {
                    new_line.push_back(std::move(pt2));
                }
            }
            new_geom.push_back(std::move(new_line));
        }
        if (new_geom.empty())
        {
            return;
        }
        return next_(new_geom);
    }

    inline void operator() (mapnik::geometry::polygon<double> const& geom)
    {
        mapnik::box2d<double> ext_bbox = mapnik::geometry::envelope(geom);
        if (!target_clipping_extent_.intersects(ext_bbox))
        {
            return;
        }
        mapbox::geometry::polygon<std::int64_t> new_geom;
        mapbox::geometry::linear_ring<std::int64_t> exterior_ring;
        exterior_ring.reserve(geom.exterior_ring.size());
        for (auto const& pt : geom.exterior_ring)
        {
            mapbox::geometry::point<std::int64_t> pt2;
            if (tr_.apply(pt,pt2))
            {
                exterior_ring.push_back(std::move(pt2));
            }
        }
        new_geom.push_back(std::move(exterior_ring));
        for (auto const& ring : geom.interior_rings)
        {
            mapnik::box2d<double> ring_bbox = mapnik::geometry::envelope(static_cast<mapnik::geometry::line_string<double> const&>(ring));
            if (!target_clipping_extent_.intersects(ring_bbox))
            {
                continue;
            }
            mapbox::geometry::linear_ring<std::int64_t> new_ring;
            new_ring.reserve(ring.size());
            for (auto const& pt : ring)
            {
                mapbox::geometry::point<std::int64_t> pt2;
                if (tr_.apply(pt,pt2))
                {
                    new_ring.push_back(std::move(pt2));
                }
            }
            new_geom.push_back(std::move(new_ring));
        }
        return next_(new_geom);
    }

    inline void operator() (mapnik::geometry::multi_polygon<double> const& geom)
    {
        mapbox::geometry::multi_polygon<std::int64_t> new_geom;
        new_geom.reserve(geom.size());
        for (auto const& poly : geom)
        {
            mapnik::box2d<double> ext_bbox = mapnik::geometry::envelope(poly);
            if (!target_clipping_extent_.intersects(ext_bbox))
            {
                continue;
            }
            mapbox::geometry::polygon<std::int64_t> new_poly;
            mapbox::geometry::linear_ring<std::int64_t> exterior_ring;
            exterior_ring.reserve(poly.exterior_ring.size());
            for (auto const& pt : poly.exterior_ring)
            {
                mapbox::geometry::point<std::int64_t> pt2;
                if (tr_.apply(pt,pt2))
                {
                    exterior_ring.push_back(std::move(pt2));
                }
            }
            new_poly.push_back(std::move(exterior_ring));
            for (auto const& ring : poly.interior_rings)
            {
                mapnik::box2d<double> ring_bbox = mapnik::geometry::envelope(static_cast<mapnik::geometry::line_string<double> const&>(ring));
                if (!target_clipping_extent_.intersects(ring_bbox))
                {
                    continue;
                }
                mapbox::geometry::linear_ring<std::int64_t> new_ring;
                new_ring.reserve(ring.size());
                for (auto const& pt : ring)
                {
                    mapbox::geometry::point<std::int64_t> pt2;
                    if (tr_.apply(pt,pt2))
                    {
                        new_ring.push_back(std::move(pt2));
                    }
                }
                new_poly.push_back(std::move(new_ring));
            }
            new_geom.push_back(std::move(new_poly));
        }
        if (new_geom.empty())
        {
            return;
        }
        return next_(new_geom);
    }

    inline void operator() (mapnik::geometry::geometry_collection<double> const& geom)
    {
        for (auto const& g : geom)
        {
            mapnik::util::apply_visitor((*this), g);
        }
     }

    inline void operator() (mapnik::geometry::geometry_empty const&)
    {
        return;
    }
};

}
}

#endif // MAPNIK_VECTOR_TILE_STRATEGY_HPP


#include "test/testutil.h"
#include "test/fixtures.h"

#include <boost/test/unit_test.hpp>
#include <crypto/progpow/firopow_test_vectors.hpp>
#include <crypto/progpow/lib/ethash/ethash-internal.hpp>
#include <crypto/progpow/include/ethash/progpow.hpp>
#include <crypto/progpow/helpers.hpp>

BOOST_FIXTURE_TEST_SUITE(firpow_tests, BasicTestingSetup)
BOOST_AUTO_TEST_CASE(firopow_hash_and_verify) {

    ethash::epoch_context_ptr context{nullptr, nullptr};
    for(auto& t : firopow_hash_test_cases) {

        const auto epoch_number{ethash::get_epoch_number(t.block_number)};
        if (!context || context->epoch_number != epoch_number) {
            context = ethash::create_epoch_context(epoch_number);
        }

        const ethash::hash256 header{to_hash256(t.header_hash_hex)};
        const ethash::hash256 boundary{to_hash256(t.boundary_hex)};
        const uint64_t nonce{std::stoull(t.nonce_hex, nullptr, 16)};
        ethash::hash256 mix_hash{to_hash256(t.mix_hash_hex)};
        const ethash::hash256 final_hash{to_hash256(t.final_hash_hex)};

        auto result{progpow::hash(*context, t.block_number, header, nonce)};
        BOOST_CHECK(ethash::is_less_or_equal(result.final_hash, boundary)); // Must be below boundary
        BOOST_CHECK(ethash::is_equal(result.final_hash, final_hash));       // Must be equal to test case final_hash
        BOOST_CHECK(ethash::is_equal(result.mix_hash, mix_hash));           // Must be equal to test case mix_hash

        // Run verification
        BOOST_CHECK(progpow::verify(*context, t.block_number, header, mix_hash, nonce, boundary));

        // Tamper mix and rerun verification
        ++mix_hash.bytes[3];
        BOOST_CHECK_EQUAL(progpow::verify(*context, t.block_number, header, mix_hash, nonce, boundary), false);

    }
}

BOOST_AUTO_TEST_SUITE_END()


///////////////////////////////////////////////////////////////////////////////
// width_of.hpp
//
//  Copyright 2004 Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_XPRESSIVE_DETAIL_STATIC_WIDTH_OF_HPP_EAN_10_04_2005
#define BOOST_XPRESSIVE_DETAIL_STATIC_WIDTH_OF_HPP_EAN_10_04_2005

// MS compatible compilers support #pragma once
#if defined(_MSC_VER) && (_MSC_VER >= 1020)
# pragma once
#endif

#include <vector>
#include <boost/ref.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/times.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/lambda.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/logical.hpp>
#include <boost/mpl/identity.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/transform_view.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/xpressive/detail/detail_fwd.hpp>
#include <boost/xpressive/detail/static/as_xpr.hpp>

namespace boost { namespace xpressive { namespace detail
{
    ///////////////////////////////////////////////////////////////////////////////
    // add_width
    template<typename X, typename Y>
    struct add_width
      : mpl::eval_if
        <
            mpl::or_
            <
                mpl::equal_to<X, unknown_width>
              , mpl::equal_to<Y, unknown_width>
            >
          , mpl::identity<unknown_width>
          , mpl::plus<X, Y>
        >::type
    {
    };

    ///////////////////////////////////////////////////////////////////////////////
    // mult_width
    template<typename X, typename Y>
    struct mult_width
      : mpl::eval_if
        <
            mpl::or_
            <
                mpl::equal_to<X, unknown_width>
              , mpl::equal_to<Y, unknown_width>
            >
          , mpl::identity<unknown_width>
          , mpl::times<X, Y>
        >::type
    {
    };

    ///////////////////////////////////////////////////////////////////////////////
    // equal_width
    template<typename X, typename Y>
    struct equal_width
      : mpl::if_
        <
            mpl::equal_to<X, Y>
          , X
          , unknown_width
        >::type
    {
    };

    ///////////////////////////////////////////////////////////////////////////////
    // width_of
    //
    template<typename Xpr>
    struct width_of;

    template<>
    struct width_of<no_next>
      : mpl::size_t<0>
    {
    };

    template<typename Matcher>
    struct width_of<proto::unary_op<Matcher, proto::noop_tag> >
      : as_matcher_type<Matcher>::type::width
    {
    };

    template<typename Left, typename Right>
    struct width_of<proto::binary_op<Left, Right, proto::right_shift_tag> >
      : add_width<width_of<Left>, width_of<Right> >
    {
    };

    template<typename Left, typename Right>
    struct width_of<proto::binary_op<Left, Right, proto::bitor_tag> >
      : equal_width<width_of<Left>, width_of<Right> >
    {
    };

    template<typename Right>
    struct width_of<proto::binary_op<mark_tag, Right, proto::assign_tag> >
      : width_of<Right>
    {
    };

    template<typename Right>
    struct width_of<proto::binary_op<set_initializer_type, Right, proto::assign_tag> >
      : mpl::size_t<1>
    {
    };

    template<typename Modifier, typename Xpr>
    struct width_of<proto::binary_op<Modifier, Xpr, modifier_tag> >
      : width_of<Xpr>
    {
    };

    template<typename Xpr, bool Positive>
    struct width_of<proto::unary_op<Xpr, lookahead_tag<Positive> > >
      : mpl::size_t<0>
    {
    };

    template<typename Xpr, bool Positive>
    struct width_of<proto::unary_op<Xpr, lookbehind_tag<Positive> > >
      : mpl::size_t<0>
    {
    };

    template<typename Xpr>
    struct width_of<proto::unary_op<Xpr, keeper_tag> >
      : width_of<Xpr>
    {
    };

    template<typename Matcher, typename Next>
    struct width_of<static_xpression<Matcher, Next> >
      : add_width<typename Matcher::width, width_of<Next> >
    {
    };

    template<typename BidiIter>
    struct width_of<shared_ptr<matchable<BidiIter> const> >
      : unknown_width
    {
    };

    template<typename BidiIter>
    struct width_of<std::vector<shared_ptr<matchable<BidiIter> const> > >
      : unknown_width
    {
    };

    template<typename BidiIter>
    struct width_of<proto::unary_op<basic_regex<BidiIter>, proto::noop_tag> >
      : unknown_width
    {
    };

    template<typename BidiIter>
    struct width_of<proto::unary_op<reference_wrapper<basic_regex<BidiIter> const>, proto::noop_tag> >
      : unknown_width
    {
    };

    template<typename Op>
    struct width_of<proto::unary_op<Op, proto::unary_plus_tag> >
      : unknown_width
    {
    };

    template<typename Op>
    struct width_of<proto::unary_op<Op, proto::unary_star_tag> >
      : unknown_width
    {
    };

    template<typename Op>
    struct width_of<proto::unary_op<Op, proto::logical_not_tag> >
      : unknown_width
    {
    };

    template<typename Op, uint_t Min, uint_t Max>
    struct width_of<proto::unary_op<Op, generic_quant_tag<Min, Max> > >
      : mpl::if_c<Min==Max, mult_width<width_of<Op>, mpl::size_t<Min> >, unknown_width>::type
    {
    };

    template<typename Op>
    struct width_of<proto::unary_op<Op, proto::unary_minus_tag> >
      : width_of<Op>
    {
    };

    // when complementing a set or an assertion, the width is that of the set (1) or the assertion (0)
    template<typename Op>
    struct width_of<proto::unary_op<Op, proto::complement_tag> >
      : width_of<Op>
    {
    };

    // The comma is used in list-initialized sets, and the width of sets are 1
    template<typename Left, typename Right>
    struct width_of<proto::binary_op<Left, Right, proto::comma_tag> >
      : mpl::size_t<1>
    {
    };

    // The subscript operator[] is used for sets, as in set['a' | range('b','h')],
    // or for actions as in (any >> expr)[ action ]
    template<typename Left, typename Right>
    struct width_of<proto::binary_op<Left, Right, proto::subscript_tag> >
      : mpl::if_<is_same<Left, set_initializer_type>, mpl::size_t<1>, width_of<Left> >::type
    {
        // If Left is "set" then make sure that Right has a width_of 1
        BOOST_MPL_ASSERT
        ((
            mpl::or_
            <
                mpl::not_<is_same<Left, set_initializer_type> >
              , mpl::equal_to<width_of<Right>, mpl::size_t<1> >
            >
        ));
    };

    // The width of a list of alternates is N if all the alternates have width N, otherwise unknown_width
    template<typename Widths>
    struct alt_width_of
      : mpl::fold
        <
            Widths
          , typename mpl::front<Widths>::type
          , equal_width<mpl::_1, mpl::_2>
        >::type
    {
    };

    template<typename Alternates>
    struct width_of<alternates_list<Alternates> >
      : alt_width_of<mpl::transform_view<Alternates, width_of<mpl::_1> > >
    {
    };

    template<typename Op, typename Arg>
    struct width_of<proto::op_proxy<Op, Arg> >
      : width_of<Op>
    {
    };

}}} // namespace boost::xpressive::detail

#endif


#include <iostream>
#include <iomanip>
using std::cout;
using std::endl;

//--- SpatialOps includes ---//
#include <spatialops/structured/FVStaggered.h>
#include <spatialops/structured/Grid.h>
#ifdef ENABLE_THREADS
#include <spatialops/ThreadPool.h>
#endif

#include <spatialops/util/TimeLogger.h>

typedef SpatialOps::SVolField   CellField;
typedef SpatialOps::SSurfXField XSideField;
typedef SpatialOps::SSurfYField YSideField;
typedef SpatialOps::SSurfZField ZSideField;

typedef SpatialOps::BasicOpTypes<CellField>::GradX      GradX;
typedef SpatialOps::BasicOpTypes<CellField>::InterpC2FX InterpX;
typedef SpatialOps::BasicOpTypes<CellField>::DivX       DivX;

typedef SpatialOps::BasicOpTypes<CellField>::GradY      GradY;
typedef SpatialOps::BasicOpTypes<CellField>::InterpC2FY InterpY;
typedef SpatialOps::BasicOpTypes<CellField>::DivY       DivY;

typedef SpatialOps::BasicOpTypes<CellField>::GradZ      GradZ;
typedef SpatialOps::BasicOpTypes<CellField>::InterpC2FZ InterpZ;
typedef SpatialOps::BasicOpTypes<CellField>::DivZ       DivZ;

typedef SpatialOps::NeboBoundaryConditionBuilder<InterpX> DirichletXOpT;
typedef SpatialOps::NeboBoundaryConditionBuilder<InterpY> DirichletYOpT;
typedef SpatialOps::NeboBoundaryConditionBuilder<InterpZ> DirichletZOpT;

// If we are compiling with GPU CUDA support, create fields on the device.
// Otherwise, create them on the host.
#ifdef ENABLE_CUDA
# define LOCATION GPU_INDEX
#else
# define LOCATION CPU_INDEX
#endif

//-- boost includes ---//
#include <boost/program_options.hpp>

namespace po = boost::program_options;
using namespace SpatialOps;

//==============================================================================

void
initialize_mask_points( const Grid& grid,
                        std::vector<IntVec>& xminus,
                        std::vector<IntVec>& xplus,
                        std::vector<IntVec>& yminus,
                        std::vector<IntVec>& yplus,
                        std::vector<IntVec>& zminus,
                        std::vector<IntVec>& zplus )
{
  for( int k=0; k<grid.extent(2); ++k ){
    for( int  j=0; j<grid.extent(1); ++j ){
      xminus.push_back( IntVec(0, j, k) );
      xplus .push_back( IntVec(grid.extent(0), j, k) );
    }
  }
  for( int k=0; k<grid.extent(2); ++k ){
    for( int i=0; i<grid.extent(0); ++i ){
      yminus.push_back( IntVec(i,0,k) );
      yplus .push_back( IntVec(i,grid.extent(1), k) );
    }
  }
  for( int j=0; j<grid.extent(1); ++j ){
    for( int i=0; i<grid.extent(0); ++i ){
      zminus.push_back( IntVec(i,j,0) );
      zplus .push_back( IntVec(i,j,grid.extent(2)) );
    }
  }
}

//==============================================================================

int main( int iarg, char* carg[] )
{
  double dt, tend;
  int nthreads = NTHREADS;
  IntVec npts;
  DoubleVec length;
  std::string logFileName;

  // parse the command line options input describing the problem
  {
    po::options_description desc("Supported Options");
    desc.add_options()
      ( "help", "print help message" )
      ( "dt",   po::value<double>(&dt  )->default_value(2e-4), "timestep" )
      ( "tend", po::value<double>(&tend)->default_value(0.04 ), "end time" )
      ( "nx", po::value<int>(&npts[0])->default_value(32), "nx" )
      ( "ny", po::value<int>(&npts[1])->default_value(32), "ny" )
      ( "nz", po::value<int>(&npts[2])->default_value(32), "nz" )
      ( "Lx", po::value<double>(&length[0])->default_value(1.0),"Length in x")
      ( "Ly", po::value<double>(&length[1])->default_value(1.0),"Length in y")
      ( "Lz", po::value<double>(&length[2])->default_value(1.0),"Length in z")
      ( "nthreads", po::value<int>(&nthreads)->default_value(NTHREADS),"Number of threads (no effect unless configured with threads enabled)" )
      ( "timings-file-name", po::value<std::string>(&logFileName)->default_value("timings.log"), "Name for performance timings file" );

    po::variables_map args;
    po::store( po::parse_command_line(iarg,carg,desc), args );
    po::notify(args);

    if (args.count("help")) {
      cout << "\nSolves the 3D transient diffusion equation\n\n" << desc << "\n";
      return 1;
    }
  }

# ifdef ENABLE_THREADS
  set_soft_thread_count( nthreads );
# endif

  cout << " [nx,ny,nz] = [" << npts[0] << "," << npts[1] << "," << npts[2] << "]" << endl
       << " dt   = " << dt << endl
       << " tend = " << tend << endl
#      ifdef ENABLE_THREADS
       << " NTHREADS = " << get_soft_thread_count() << endl
#      endif
       << endl;

  // set mesh spacing (uniform, structured mesh)
  const DoubleVec spacing = length/npts;

  const Grid grid( npts, length );

  // build the spatial operators
  SpatialOps::OperatorDatabase sodb;
  SpatialOps::build_stencils( grid, sodb );

  const GradX& gradx = *sodb.retrieve_operator<GradX>();
  const GradY& grady = *sodb.retrieve_operator<GradY>();
  const GradZ& gradz = *sodb.retrieve_operator<GradZ>();

  const DivX& divx = *sodb.retrieve_operator<DivX>();
  const DivY& divy = *sodb.retrieve_operator<DivY>();
  const DivZ& divz = *sodb.retrieve_operator<DivZ>();

  const InterpX& interpx = *sodb.retrieve_operator<InterpX>();
  const InterpY& interpy = *sodb.retrieve_operator<InterpY>();
  const InterpZ& interpz = *sodb.retrieve_operator<InterpZ>();

  const DirichletXOpT& dirichletX = *sodb.retrieve_operator<DirichletXOpT>();
  const DirichletYOpT& dirichletY = *sodb.retrieve_operator<DirichletYOpT>();
  const DirichletZOpT& dirichletZ = *sodb.retrieve_operator<DirichletZOpT>();

  // build fields
  const GhostData ghost(1);
  const IntVec hasMinusBC(true,true,true), hasPlusBC(true,true,true);
  const BoundaryCellInfo cellBC = BoundaryCellInfo::build< CellField>(hasMinusBC,hasPlusBC);

  const MemoryWindow vwindow( get_window_with_ghost(npts,ghost,cellBC) );

  CellField temperature( vwindow, cellBC, ghost, NULL, InternalStorage, LOCATION );
  CellField thermCond  ( vwindow, cellBC, ghost, NULL, InternalStorage, LOCATION );
  CellField rhoCp      ( vwindow, cellBC, ghost, NULL, InternalStorage, LOCATION );
  CellField xcoord     ( vwindow, cellBC, ghost, NULL, InternalStorage, LOCATION );
  CellField ycoord     ( vwindow, cellBC, ghost, NULL, InternalStorage, LOCATION );
  CellField zcoord     ( vwindow, cellBC, ghost, NULL, InternalStorage, LOCATION );
  CellField rhs        ( vwindow, cellBC, ghost, NULL, InternalStorage, LOCATION );

  grid.set_coord<SpatialOps::XDIR>( xcoord );
  grid.set_coord<SpatialOps::YDIR>( ycoord );
  grid.set_coord<SpatialOps::ZDIR>( zcoord );

  const BoundaryCellInfo xBC = BoundaryCellInfo::build<XSideField>(hasMinusBC,hasPlusBC);
  const BoundaryCellInfo yBC = BoundaryCellInfo::build<YSideField>(hasMinusBC,hasPlusBC);
  const BoundaryCellInfo zBC = BoundaryCellInfo::build<ZSideField>(hasMinusBC,hasPlusBC);
  const MemoryWindow xwindow( get_window_with_ghost(npts,ghost,xBC) );
  const MemoryWindow ywindow( get_window_with_ghost(npts,ghost,yBC) );
  const MemoryWindow zwindow( get_window_with_ghost(npts,ghost,zBC) );

  XSideField xflux( xwindow, xBC, ghost, NULL, InternalStorage, LOCATION  );
  YSideField yflux( ywindow, yBC, ghost, NULL, InternalStorage, LOCATION  );
  ZSideField zflux( zwindow, zBC, ghost, NULL, InternalStorage, LOCATION  );

  //----------------------------------------------------------------------------
  // Build and initialize masks:
  std::vector<IntVec> xminusPts, xplusPts, yminusPts, yplusPts, zminusPts, zplusPts;
  initialize_mask_points( grid, xminusPts, xplusPts, yminusPts, yplusPts, zminusPts, zplusPts );

  SpatialMask<XSideField> xminus( xflux, xminusPts );
  SpatialMask<XSideField> xplus ( xflux, xplusPts  );
  SpatialMask<YSideField> yminus( yflux, yminusPts );
  SpatialMask<YSideField> yplus ( yflux, yplusPts  );
  SpatialMask<ZSideField> zminus( zflux, zminusPts );
  SpatialMask<ZSideField> zplus ( zflux, zplusPts  );

# ifdef ENABLE_CUDA
  // Masks are created on CPU so we need to explicitly transfer them to GPU
  xminus.add_consumer( GPU_INDEX );
  xplus .add_consumer( GPU_INDEX );
  yminus.add_consumer( GPU_INDEX );
  yplus .add_consumer( GPU_INDEX );
  zminus.add_consumer( GPU_INDEX );
  zplus .add_consumer( GPU_INDEX );
# endif
  //----------------------------------------------------------------------------

  //----------------------------------------------------------------------------
  // initial conditions
  thermCond   <<= xcoord + ycoord + zcoord;
  rhoCp       <<= 1.0;
  temperature <<= sin( xcoord ) + cos( ycoord ) + sin( zcoord );
  dirichletX( xminus, temperature,  1.0, true  );
  dirichletX( xplus,  temperature, -1.0, false );
  dirichletY( yminus, temperature,  1.0, true  );
  dirichletY( yplus,  temperature, -1.0, false );
  dirichletZ( zminus, temperature,  1.0, true  );
  dirichletZ( zplus,  temperature, -1.0, false );
  //----------------------------------------------------------------------------

  try{
    cout << "beginning timestepping" << endl;

    TimeLogger logger(logFileName);

    // mimic the effects of solving this PDE in time.
    double t = 0;
    while( t < tend ){

      logger.start("flux");
      thermCond <<= 0.1;
      if( npts[0]>1 ) xflux <<= -interpx(thermCond) * gradx(temperature);
      if( npts[1]>1 ) yflux <<= -interpy(thermCond) * grady(temperature);
      if( npts[2]>1 ) zflux <<= -interpz(thermCond) * gradz(temperature);
      logger.stop("flux");

      logger.start("rhs");
      if( npts[0] >1 && npts[1] > 1 && npts[2] > 1 ){
        rhs <<= ( - divx(xflux) - divy(yflux) - divz(zflux) ) / rhoCp;
      }
      else{
        rhs <<= 0.0;
//      if( npts[0]>1 ) rhs <<= rhs + divx( interpx(thermCond) * gradx(temperature) ) / rhoCp;
//      if( npts[1]>1 ) rhs <<= rhs + divy( interpy(thermCond) * grady(temperature) ) / rhoCp;
//      if( npts[2]>1 ) rhs <<= rhs + divz( interpz(thermCond) * gradz(temperature) ) / rhoCp;
        if( npts[0]>1 ) rhs <<= rhs - divx( xflux ) / rhoCp;
        if( npts[1]>1 ) rhs <<= rhs - divy( yflux ) / rhoCp;
        if( npts[2]>1 ) rhs <<= rhs - divz( zflux ) / rhoCp;
      }
      logger.stop("rhs");

      logger.start("update");
      temperature <<= temperature + dt * rhs;
      logger.stop("update");

      logger.start("bc");
      dirichletX( xminus, temperature,  1.0, true  );
      dirichletX( xplus,  temperature, -1.0, false );
      dirichletY( yminus, temperature,  1.0, true  );
      dirichletY( yplus,  temperature, -1.0, false );
      dirichletZ( zminus, temperature,  1.0, true  );
      dirichletZ( zplus,  temperature, -1.0, false );
      logger.stop("bc");

      t += dt;
    }

    cout << "done" << endl << endl
         << "\n------------------------------------------------------------------\n"
         << "Timing Summary:\n"
         << "---------------\n"
         << std::setw(10) << "Flux"
         << std::setw(10)  << "RHS"
         << std::setw(10)  << "update"
         << std::setw(10)  << "bc"
         << std::setw(10) << "TOTAL"
         << std::endl
         << std::setw(10) << logger.timer("flux").elapsed_time()
         << std::setw(10) << logger.timer("rhs").elapsed_time()
         << std::setw(10) << logger.timer("update").elapsed_time()
         << std::setw(10) << logger.timer("bc").elapsed_time()
         << std::setw(10) << logger.total_time()
         << "\n------------------------------------------------------------------\n"
         << endl << endl;

    return 0;
  }
  catch( std::exception& err ){
    cout << "ERROR!" << endl;
  }
  return -1;
}


#ifndef DLIB_INPUT_HPP
#define DLIB_INPUT_HPP

#include <dlib/dnn/tensor.h>
#include <dlib/dnn/input.h>
#include <dlib/opencv/cv_image.h>
#include <limits>

namespace dlib
{
  template <>
  class input<cv::Mat>
  {
  public:
    using input_type = cv::Mat;
    const static unsigned int sample_expansion_factor = 1;

    template <typename input_iterator>
    void to_tensor(
      input_iterator begin,
      input_iterator end,
      resizable_tensor& data
    ) const
    {
      DLIB_CASSERT(std::distance(begin, end) > 0, "");

      switch (begin->depth())
      {
      case CV_8S:
        to_tensor_<char>(begin, end, data);
        break;
      case CV_8U:
        to_tensor_<uchar>(begin, end, data);
        break;
      case CV_16S:
        to_tensor_<short>(begin, end, data);
        break;
      case CV_16U:
        to_tensor_<unsigned short>(begin, end, data);
        break;
      case CV_32S:
        to_tensor_<int>(begin, end, data);
        break;
      case CV_32F:
        to_tensor_<float>(begin, end, data);
        break;
      case CV_64F:
        to_tensor_<double>(begin, end, data);
        break;
      default:
        throw;
      }
    }

    friend void serialize(const input& item, std::ostream& out)
    {
      serialize("input<cv::Mat>", out);
    }

    friend void deserialize(input& item, std::istream& in)
    {
      std::string version;
      deserialize(version, in);
      if (version != "input<cv::Mat>")
        throw serialization_error("Unexpected version found while deserializing dlib::input.");
    }

    friend std::ostream& operator<<(std::ostream& out, const input& item)
    {
      out << "input<cv::Mat>";
      return out;
    }

    friend void to_xml(const input& item, std::ostream& out)
    {
      out << "<input/>";
    }

  private:
    /**
  	* \brief enumerable<cv::Mat> のイテレータをtensor型に変換する．
  	* \param [in] begin
  	* \param [in] end
  	*/
    template <typename Tp, typename input_iterator>
    void to_tensor_(
      input_iterator begin,
      input_iterator end,
      resizable_tensor& data
    ) const
    {
      using value_type = Tp;
      cv::Mat mat = *begin;

      // initialize data to the right size to contain the stuff in the iterator range.
      data.set_size(std::distance(begin, end), mat.channels(), mat.rows, mat.cols);

      const size_t offset = mat.step * mat.rows;
      auto ptr = data.host();
      for (auto it = begin; it != end; ++it)
      {
        for (int y = 0; y < mat.rows; ++y)
        {
          for (int x = 0; x < mat.cols; ++x)
          {
            auto tmp = it->ptr<value_type>(y, x);
            auto p = ptr++;
            for (int c = 0; c < mat.channels(); ++c)
            {
              *p = static_cast<float>(tmp[c]) / (std::numeric_limits<value_type>::max());
              p += offset;
            }
          }
        }

        ptr += offset * (mat.channels() - 1);
      }
    }
  };
}

#endif //!DLIB_INPUT_HPP


// Copyright (c) 2017 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE LargeSplitRegionsTest
#include <boost/test/unit_test.hpp>
#include <poplar/Interval.hpp>
#include <poputil/Util.hpp>

using namespace poputil;
using namespace poplar;

BOOST_AUTO_TEST_CASE(largeSplitRegionsTest) {
  auto N = 100000000UL;
  auto split = splitRegions({{0, N}}, 1, 6);
  for (const auto &intervals : split)
    for (const auto &interval : intervals)
      BOOST_CHECK_LE(interval.end(), N);
}

unsigned
maxRegionElements(std::vector<std::vector<std::vector<Interval>>> &regions) {
  unsigned maxElements = 0;
  for (unsigned i = 0; i < regions.size(); i++) {
    for (unsigned j = 0; j < regions[i].size(); j++) {
      const unsigned regionElements = std::accumulate(
          regions[i][j].begin(), regions[i][j].end(), 0,
          [](std::size_t a, Interval b) { return a + b.size(); });
      maxElements = std::max(maxElements, regionElements);
    }
  }
  return maxElements;
}

BOOST_AUTO_TEST_CASE(splitRegionsForRepeatTest1) {
  std::vector<std::vector<Interval>> regions(1);
  unsigned x = 0;
  const unsigned expectedRegions[] = {3, 2, 2};
  for (unsigned maxElements = 49999; maxElements < 50002; maxElements++, x++) {
    regions.resize(1);
    regions[0].resize(1);
    regions[0] = {{0, 100000}};
    auto result = splitRegions(regions, 4, 1, 0, UINT_MAX, maxElements);
    BOOST_CHECK_LE(maxRegionElements(result), maxElements);
    BOOST_CHECK_EQUAL(result[0].size(), expectedRegions[x]);
  }
}

BOOST_AUTO_TEST_CASE(splitRegionsForRepeatTest2) {
  std::vector<std::vector<Interval>> regions(1);
  regions[0].resize(2);
  regions[0] = {{0, 30000}, {40000, 60001}};
  unsigned maxElements = 50000;
  auto result = splitRegions(regions, 4, 1, 0, UINT_MAX, maxElements);

  BOOST_CHECK_LE(maxRegionElements(result), maxElements);
  BOOST_CHECK_EQUAL(result[0].size(), 2);
}

BOOST_AUTO_TEST_CASE(splitRegionsForRepeatTest3) {
  std::vector<std::vector<Interval>> regions(2);

  regions[0].resize(2);
  regions[1].resize(2);
  regions[0] = {{0, 30000}, {40000, 70000}};
  regions[1] = {{0, 20000}, {40000, 60000}};
  unsigned maxElements = 50000;
  auto result = splitRegions(regions, 4, 1, 0, UINT_MAX, maxElements);

  BOOST_CHECK_LE(maxRegionElements(result), maxElements);
  BOOST_CHECK_EQUAL(result[0].size(), 3);
}

BOOST_AUTO_TEST_CASE(splitRegionsForRepeatTest4) {
  std::vector<std::vector<Interval>> regions(1);
  unsigned x = 0;
  const unsigned expectedRegions[] = {1, 1, 2};
  unsigned maxElements = 50000;
  for (unsigned regionSize = 49999; regionSize < 50002; regionSize++, x++) {
    regions.resize(1);
    regions[0].resize(1);
    regions[0] = {{0, regionSize}};
    auto result = splitRegions(regions, 4, 1, 0, UINT_MAX, maxElements);

    BOOST_CHECK_LE(maxRegionElements(result), maxElements);
    BOOST_CHECK_EQUAL(result[0].size(), expectedRegions[x]);
  }
}

BOOST_AUTO_TEST_CASE(splitRegionsForRepeatTest5) {
  std::vector<std::vector<Interval>> regions(2);

  regions[0].resize(2);
  regions[1].resize(2);
  regions[0] = {{0, 300}, {400, 700}};
  regions[1] = {{0, 200}, {400, 600}};
  unsigned maxElements = 32;
  auto result = splitRegions(regions, 4, 6, 0, UINT_MAX, maxElements);

  BOOST_CHECK_LE(maxRegionElements(result), maxElements);
  for (unsigned i = 0; i < result.size(); i++) {
    BOOST_CHECK_GE(result[i].size(), 2);
  }
}


/*
 * 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_BINARY_TREE_BINARY_SEARCH_TREE_HPP
#define AMA_BINARY_TREE_BINARY_SEARCH_TREE_HPP 1

#include <boost/assert.hpp>
#include <ama/binary_tree/detail/binary_tree.hpp>
#include <ama/binary_tree/detail/binary_search_tree_node.hpp>

namespace ama
{

  template <typename VALUE>
  class binary_search_tree:
      public binary_tree_::binary_tree< binary_tree_::binary_search_tree_node<VALUE> >
  {
  public:
    typedef binary_tree_::binary_search_tree_node<VALUE> node_type;
    typedef typename node_type::node_handle node_handle;

    typedef VALUE value_type;

  protected:
    typedef node_type * node_pointer;
    typedef binary_tree_::binary_tree<node_type> base_type;

  public:
    /* insert a new value */
    node_handle insert(value_type const & val)
    {
      node_handle r;

      if(this->root().null())
      {
        this->reset(new node_type(val));
        r = this->root();
      }
      else
        r = this->insert(val, this->root());

      return r;
    }

    /* search for a value and return the relative node handler */
    node_handle search(value_type const & val)
    {
      BOOST_ASSERT(!this->root().null());
      return this->search(val, this->root());
    }

    /* retrieve the node that store the minimum */
    node_handle minimum()
    {
      BOOST_ASSERT(!this->root().null());
      return this->minimum(this->root());
    }

    /* retrieve the node that store the maximum */
    node_handle maximum()
    {
      BOOST_ASSERT(!this->root().null());
      return this->maximum(this->root());
    }

  public:
    /* insert a new value in the tree with root node */
    static node_handle insert(value_type const & val,
                              node_handle const & node)
    {
      node_handle x(node), y;

      while(!x.null())
      {
        y = x;
        x = (val < x->value()) ? x->left() : x->right();
      }

      node_pointer p = new node_type(y, val);
      (val < y->value()) ? y->reset_left(p) : y->reset_right(p);
      return node_handle(p);
    }

    /* search a value in the tree with root node */
    static node_handle search(value_type const & val,
                              node_handle const & node)
    {
      node_handle x = node;

      while(!x.null() and (val != x->value()))
        x = (val < x->value()) ? x->left() : x->right() ;

      return x;
    }

    /* retrieve the minimum of tree */
    static node_handle minimum(node_handle const & node)
    {
      node_handle x(node);
      while(!x->left().null())
        x = x->left();
      return x;
    }

    /* retrieve the maximum of tree */
    static node_handle maximum(node_handle const & node)
    {
      node_handle x(node);
      while(!x->right().null())
        x = x->right();
      return x;
    }

    /* retrieve the successor of tree */
    static node_handle successor(node_handle const & node)
    {
      node_handle x(node), y(node->parent());

      if(!x->right().null())
        return minimum(x->right());

      while(!y.null() and (x == y->right()))
      {
        x = y;
        y = y->parent();
      }

      return y;
    }

    /* retrieve the predecessor of tree */
    static node_handle predecessor(node_handle const & node)
    {
      node_handle x(node), y(node->parent());

      if(!x->left().null())
        return minimum(x->left());

      while(!y.null() and (x == y->left()))
      {
        x = y;
        y = y->parent();
      }

      return y;
    }

  };

}

#endif /* AMA_BINARY_TREE_BINARY_SEARCH_TREE_HPP */


//object2d_image.cpp
/*
* the 2d object image related features impl
* (C) 2004 olegabr. All rights reserved.
*/
#include <Magick++.h>
#include <boost/lambda/lambda.hpp>
#include <limits>
#include "object2d.h"

#include <iostream>
using std::cout; using std::endl;

//debug
object2d::object2d() : point_size_(1)
{
	size_ = size2d_t(32, 32);
	data_.resize(size_.width*size_.height);

	for (std::size_t i(0); i < size_.height*size_.width; ++i)
	{
		point2d p = pixel2point_(index2pixel_(i));
		data_[i] = 1;//p.x*p.x + p.y*p.y <= 160 ? 1 : 0;
	}
}

//construct from image
object2d::object2d(std::string image_name) : point_size_(1)
{
	//cout << "object2d(image_name)" << endl;
	Magick::Image image;
	image.read(image_name);
	size_ = size2d_t(image.columns(), image.rows());
	//copy data to object
	Magick::PixelPacket* pxls = image.getPixels(0, 0, image.columns(), image.rows());
	data_.resize(size_.width*size_.height);

	for (std::size_t i(0); i < size_.height*size_.width; ++i)
	{
		Magick::PixelPacket* pxl = pxls + i;
		data_[i] = (pxl->blue + pxl->green + pxl->red);
	}

	//normalize values in order to reduce the chance for numerical overflow
	value_type max_val = 3*(std::numeric_limits<Magick::Quantum>::max() - 1);
	std::transform
	(
		data_.begin(), data_.end(),
		data_.begin(),
		boost::lambda::_1/max_val
	);
}

void object2d::save_to_image(std::string img_name, std::pair<value_type, value_type> const &min_max) const
{
	Magick::Image img;
	img.size(Magick::Geometry(size_.width, size_.height));
	Magick::PixelPacket* pxls = img.getPixels(0, 0, size_.width, size_.height);

	value_type max_val(min_max.second), min_val(min_max.first);

	for (std::size_t i(0); i < size_.height*size_.width; ++i)
	{
		Magick::PixelPacket* pxl = pxls + i;
		Magick::Quantum color =
			static_cast<Magick::Quantum>
			(
				(
					(data_[i] - min_val) *
					static_cast<double>(std::numeric_limits<Magick::Quantum>::max() - 1)
				) / (max_val - min_val + 1)
			);
		pxl->blue = color;
		pxl->red = color;
		pxl->green = color;
	}

	img.syncPixels();
	img.type(Magick::GrayscaleType);
	img.write(img_name);
}

void object2d::load_from_image(const std::string& img_name)
{
	Magick::Image img;
	img.read(img_name);

	size_ = size2d_t(img.columns(), img.rows());
	point_size_ = 1;

	const std::size_t n = norm(size_);
	data_.resize(n);

	Magick::PixelPacket* pxl = img.getPixels(0, 0, size_.width, size_.height);
	for (std::size_t i(0); i < n; ++i, pxl += 1)
		data_[i] = (1./3.)*(pxl->red + pxl->green + pxl->blue);
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 Delft University of Technology 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      101125    D. Dirkx          First version of file
 *      110127    D. Dirkx          Finalized for code check.
 *      110206    J. Melman         Minor formatting issues.
 *      110905    S. Billemont      Reorganized includes.
 *                                  Moved (con/de)structors and getter/setters to header.
 *      120323    D. Dirkx          Removed set functions; moved functionality to constructor,
 *                                  removed raw pointer arrays
 *    References
 *      An example of a heritage code which uses such a mesh is found in:
 *          The Mark IV Supersonic-Hypersonic Arbitrary Body Program, Volume
 *          II-Program Formulation, Douglas Aircraft Company, AFFDL-TR-73-159,
 *          Volume II.
 *
 *    Notes
 *      The numberOfLines_ and numberOfPoints_ member variables denote the number of mesh points.
 *      The number of panels in the mesh will be numberOfLines_ - 1 by numberOfPoints_ - 1.
 *
 */

#include <boost/multi_array.hpp>
#include <limits>
#include <Eigen/Geometry>

#include "Tudat/Mathematics/GeometricShapes/quadrilateralMeshedSurfaceGeometry.h"

namespace tudat
{
namespace geometric_shapes
{

//! Calculate panel characteristics.
void QuadrilateralMeshedSurfaceGeometry::performPanelCalculations( )
{
    // Allocate memory for panel properties.
    panelCentroids_.resize(boost::extents[ numberOfLines_ - 1 ][ numberOfPoints_ - 1 ]);
    panelSurfaceNormals_.resize(boost::extents[ numberOfLines_ - 1 ][ numberOfPoints_ - 1 ]);
    panelAreas_.resize(boost::extents[ numberOfLines_ - 1 ][ numberOfPoints_ - 1 ]);

    // Declare local variables for normal and area determination.
    Eigen::Vector3d crossVector1;
    Eigen::Vector3d crossVector2;

    // Reset total area.
    totalArea_ = 0.0;

    // Loop over all panels to determine properties.
    for ( int i = 0; i < numberOfLines_ - 1; i++ )
    {
        for ( int j = 0; j < numberOfPoints_ - 1; j++ )
        {
            // Set panel centroid.
            panelCentroids_[ i ][ j ] = ( meshPoints_[ i ][ j ] +
                                          meshPoints_[ i + 1 ][ j ] +
                                          meshPoints_[ i ][ j + 1 ] +
                                          meshPoints_[ i + 1 ][ j + 1 ] ) / 4;

            // Set panel cross vectors.
            crossVector1 = meshPoints_[ i + 1 ][ j + 1 ] - meshPoints_[ i ][ j ];
            crossVector2 = meshPoints_[ i + 1 ][ j ] - meshPoints_[ i ][ j + 1 ];

            // Set panel normal (not yet normalized).
            panelSurfaceNormals_[ i ][ j ] = crossVector1.cross( crossVector2 );

            // Set panel area (not yet correct size).
            panelAreas_[ i ][ j ] = panelSurfaceNormals_[ i ][ j ].norm( );
            if ( panelAreas_[ i ][ j ] < std::numeric_limits< double >::epsilon( ) )
            {
                std::cerr << "WARNING panel area is zero in part at panel" << i
                          << ", " << j << std::endl;
            }

            // Normalize panel normal and, if necessary, invert normal direction.
            panelSurfaceNormals_[ i ][ j ] *= reversalOperator_;
            panelSurfaceNormals_[ i ][ j ].normalize( );

            // Set panel area to correct size.
            panelAreas_[ i ][ j ] *= 0.5;

            // Add panel area to total area.
            totalArea_ += panelAreas_[ i ][ j ];
        }
    }
}

//! Set reversal operator.
void QuadrilateralMeshedSurfaceGeometry::setReversalOperator( const bool isMeshInverted )
{
    if ( isMeshInverted == 0 )
    {
        reversalOperator_ = 1;
    }

    else
    {
        reversalOperator_ = -1;
    }
}

//! Get boolean denoting if the mesh is inverted.
bool QuadrilateralMeshedSurfaceGeometry::getReversalOperator( )
{
    bool isMeshInverted;
    if ( reversalOperator_ == 1 )
    {
        isMeshInverted = 0;
    }

    else
    {
        isMeshInverted = 1;
    }

    return isMeshInverted;
}

//! Overload ostream to print class information.
std::ostream& operator<<( std::ostream& stream,
                          QuadrilateralMeshedSurfaceGeometry& quadrilateralMeshedSurfaceGeometry )
{
    stream << "This is a quadrilateral meshed surface geometry"
           << " of a single part." << std::endl;
    stream << "The number of lines ( contours ) is: "
           << quadrilateralMeshedSurfaceGeometry.numberOfLines_ << std::endl;
    stream << "The number of points per line is: "
           << quadrilateralMeshedSurfaceGeometry.numberOfPoints_ << std::endl;

    // Return stream.
    return stream;
}

} // namespace geometric_shapes
} // namespace tudat