// Copyright (c) 2017 Shota SUGIHARA
// Distributed under the MIT License.
#include "particles.h"
#include <cmath>
#include <fstream>
#include <iostream>
#include <sstream>
#include <boost/format.hpp>
#include <Eigen/LU>

namespace tiny_mps {

Particles::Particles(int size, const Condition& condition)
    : condition_(condition), dimension(condition.dimension), inflow_stride(0.0) {
  initialize(size);
  setInitialParticleNumberDensity();
  setLaplacianLambda();
}

Particles::Particles(const std::string& path, const Condition& condition)
    : condition_(condition), dimension(condition.dimension), inflow_stride(0.0) {
  readGridFile(path, condition);
  updateParticleNumberDensity();
  setInitialParticleNumberDensity();
  setLaplacianLambda();
  checkSurfaceParticles();
}

Particles::Particles(const Particles& other)
    : condition_(other.condition_),
      dimension(other.dimension) {
  size = other.size;
  ghost_stack = other.ghost_stack;
  initial_particle_number_density = other.initial_particle_number_density;
  laplacian_lambda_pressure = other.laplacian_lambda_pressure;
  laplacian_lambda_viscosity = other.laplacian_lambda_viscosity;
  initial_neighbor_particles = other.initial_neighbor_particles;
  inflow_stride = other.inflow_stride;
  position = other.position;
  velocity = other.velocity;
  pressure = other.pressure;
  particle_number_density = other.particle_number_density;
  temporary_position = other.temporary_position;
  temporary_velocity = other.temporary_velocity;
  correction_velocity = other.correction_velocity;
  particle_types = other.particle_types;
  boundary_types = other.boundary_types;
  neighbor_particles = other.neighbor_particles;
  source_term = other.source_term;
  voxel_ratio = other.voxel_ratio;
}

Particles& Particles::operator=(const Particles& other) {
  if (this != &other) {
    size = other.size;
    ghost_stack = other.ghost_stack;
    initial_particle_number_density = other.initial_particle_number_density;
    laplacian_lambda_pressure = other.laplacian_lambda_pressure;
    laplacian_lambda_viscosity = other.laplacian_lambda_viscosity;
    initial_neighbor_particles = other.initial_neighbor_particles;
    inflow_stride = other.inflow_stride;
    position = other.position;
    velocity = other.velocity;
    pressure = other.pressure;
    particle_number_density = other.particle_number_density;
    temporary_position = other.temporary_position;
    temporary_velocity = other.temporary_velocity;
    correction_velocity = other.correction_velocity;
    particle_types = other.particle_types;
    boundary_types = other.boundary_types;
    neighbor_particles = other.neighbor_particles;
    source_term = other.source_term;
    voxel_ratio = other.voxel_ratio;
  }
  return *this;
}

Particles::~Particles() {}

void Particles::initialize(int size) {
  this->size = size;
  position = Eigen::MatrixXd::Zero(3, size);
  velocity = Eigen::MatrixXd::Zero(3, size);
  pressure = Eigen::VectorXd::Zero(size);
  particle_number_density = Eigen::VectorXd::Zero(size);
  temporary_position = Eigen::MatrixXd::Zero(3, size);
  temporary_velocity = Eigen::MatrixXd::Zero(3, size);
  particle_types = Eigen::VectorXi::Zero(size);
  boundary_types = Eigen::VectorXi::Zero(size);
  correction_velocity = Eigen::MatrixXd::Zero(3, size);
  neighbor_particles = Eigen::VectorXi::Zero(size);
  source_term = Eigen::VectorXd::Zero(size);
  voxel_ratio = Eigen::VectorXd::Zero(size);
}

void Particles::readGridFile(const std::string& path, const Condition& condition) {
  std::ifstream ifs(path);
  if (ifs.fail()) {
    std::cerr << "Error: in readGridFile() in particles.cpp" << std::endl;
    std::cerr << "Failed to read files: " << path << std::endl;
    throw std::ios_base::failure("Error: in readGridFile() in particles.cpp.");
  }
  std::string tmp_str;
  getline(ifs, tmp_str);      //Line 0: Start time
  getline(ifs, tmp_str);      //Line 1: particles_number
  int ptcl_num = 0;
  {
    std::stringstream ss;
    ss.str(tmp_str);
    ss >> ptcl_num;
    initialize(ptcl_num + condition.extra_ghost_particles);
  }
  int i_counter = 0;
  while(getline(ifs, tmp_str)) {
    std::stringstream ss;
    ss.str(tmp_str);
    ss >> particle_types(i_counter);
    for(int i_dim = 0; i_dim < 3; ++i_dim) {
      double tmp;
      ss >> tmp;
      if (i_dim < dimension) position(i_dim, i_counter) = tmp;
    }
    for(int i_dim = 0; i_dim < 3; ++i_dim) {
      double tmp;
      ss >> tmp;
      if (i_dim < dimension) velocity(i_dim, i_counter) = tmp;
    }
    ss >> pressure(i_counter);
    ++i_counter;
  }
  for (int i_particle = ptcl_num; i_particle < size; ++i_particle) {
    particle_types(i_particle) = ParticleType::GHOST;
    ghost_stack.push(i_particle);
  }
  std::cout << "Succeed in reading grid file: " << path << std::endl;
}

void Particles::writeVtkFile(const std::string& path, const std::string& title) const {
  std::ofstream ofs(path);
  if(ofs.fail()) {
    std::cerr << "Error: in writeVtkFile() in particles.cpp." << std::endl;
    throw std::ios_base::failure("Error: in writeVtkFile() in particles.cpp.");
  }
  ofs << "# vtk DataFile Version 2.0" << std::endl;
  ofs << title << std::endl;
  ofs << "ASCII" << std::endl;
  ofs << "DATASET UNSTRUCTURED_GRID" << std::endl;
  ofs << std::endl;
  ofs << "POINTS " << size << " double" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << position(0, i) << " " << position(1, i) << " " << position(2, i) << std::endl;
  }
  ofs << std::endl;
  ofs << "CELLS " << size << " " << size * 2 << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << 1 << " " << i << std::endl;
  }
  ofs << std::endl;
  ofs << "CELL_TYPES " << size << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << 1 << std::endl;
  }
  ofs << std::endl;
  ofs << "POINT_DATA " << size << std::endl;
  ofs << "SCALARS Pressure double" << std::endl;
  ofs << "LOOKUP_TABLE Pressure" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << pressure(i) << std::endl;
  }
  ofs << std::endl;
  ofs << "VECTORS Velocity double" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << velocity(0, i) << " " << velocity(1, i) << " " << velocity(2, i) << std::endl;
  }
  ofs << std::endl;
  ofs << "SCALARS Type int" << std::endl;
  ofs << "LOOKUP_TABLE Type" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << particle_types(i) << std::endl;
  }
  ofs << std::endl;
  ofs << "SCALARS ParticleNumberDensity double" << std::endl;
  ofs << "LOOKUP_TABLE ParticleNumberDensity" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << particle_number_density(i) << std::endl;
  }
  ofs << std::endl;
  ofs << "SCALARS NeighborParticles int" << std::endl;
  ofs << "LOOKUP_TABLE NeighborParticles" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << neighbor_particles(i) << std::endl;
  }
  ofs << std::endl;
  ofs << "SCALARS BoundaryCondition int" << std::endl;
  ofs << "LOOKUP_TABLE BoundaryCondition" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << boundary_types(i) << std::endl;
  }
  ofs << std::endl;
  ofs << "VECTORS CorrectionVelocity double" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << correction_velocity(0, i) << " " << correction_velocity(1, i) << " " << correction_velocity(2, i) << std::endl;
  }
  ofs << std::endl;
  ofs << "SCALARS SourceTerm double" << std::endl;
  ofs << "LOOKUP_TABLE SourceTerm" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << source_term(i) << std::endl;
  }
  ofs << std::endl;
  ofs << "SCALARS VoxelsRatio double" << std::endl;
  ofs << "LOOKUP_TABLE VoxelsRatio" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << voxel_ratio(i) << std::endl;
  }
  std::cout << "Succeed in writing vtk file: " << path << std::endl;
}

bool Particles::saveInterval(const std::string& path, const Timer& timer) const {
  if (!timer.isOutputTime()) return false;
  std::string output_index = (boost::format("%04d") % timer.getOutputCount()).str();
  writeVtkFile((boost::format(path) % output_index).str(), (boost::format("Time: %s") % timer.getCurrentTime()).str());
  return true;
}

void Particles::setInitialParticleNumberDensity() {
  const int xy_max = (int)condition_.pnd_influence;
  const int z_max = (dimension == 3)? xy_max : 0;
  double pnd = 0;
  int count = 0;
  for (int i_z = -z_max; i_z <= z_max; ++i_z) {
    for (int i_y = -xy_max; i_y <= xy_max; ++i_y) {
      for (int i_x = -xy_max; i_x <= xy_max; ++i_x) {
        if (i_x == 0 && i_y == 0 && i_z == 0) continue;
        Eigen::Vector3d vec(i_x, i_y, i_z);
        vec *= condition_.average_distance;
        pnd += weightForParticleNumberDensity(vec);
        count += weightCount(vec, condition_.pnd_weight_radius);
      }
    }
  }
  initial_particle_number_density = pnd;
  initial_neighbor_particles = count;
  std::cout << "Initial particle number density: " << initial_particle_number_density << std::endl;
  std::cout << "Initial neighbor particles: " << initial_neighbor_particles << std::endl;
}

void Particles::setLaplacianLambda() {
  {
    const int xy_max = (int)condition_.laplacian_pressure_influence;
    const int z_max = (dimension == 3)? xy_max : 0;
    double numerator = 0.0;
    double denominator = 0.0;
    for (int i_z = -z_max; i_z <= z_max; ++i_z) {
      for (int i_y = -xy_max; i_y <= xy_max; ++i_y) {
        for (int i_x = -xy_max; i_x <= xy_max; ++i_x) {
          if (i_x == 0 && i_y == 0 && i_z == 0) continue;
          Eigen::Vector3d vec(i_x, i_y, i_z);
          vec *= condition_.average_distance;
          double w = weightForLaplacianPressure(vec);
          numerator += vec.squaredNorm() * w;
          denominator += w;
        }
      }
    }
    laplacian_lambda_pressure = numerator / denominator;
  }
  {
    const int xy_max = (int)condition_.laplacian_viscosity_influence;
    const int z_max = (dimension == 3)? xy_max : 0;
    double numerator = 0.0;
    double denominator = 0.0;
    for (int i_z = -z_max; i_z <= z_max; ++i_z) {
      for (int i_y = -xy_max; i_y <= xy_max; ++i_y) {
        for (int i_x = -xy_max; i_x <= xy_max; ++i_x) {
          if (i_x == 0 && i_y == 0 && i_z == 0) continue;
          Eigen::Vector3d vec(i_x, i_y, i_z);
          vec *= condition_.average_distance;
          double w = weightForLaplacianViscosity(vec);
          numerator += vec.squaredNorm() * w;
          denominator += w;
        }
      }
    }
    laplacian_lambda_viscosity = numerator / denominator;
  }
  std::cout << "Laplacian lambda for Pressure: " << laplacian_lambda_pressure << std::endl;
  std::cout << "Laplacian lambda for Viscosity: " << laplacian_lambda_viscosity << std::endl;
  std::cout << "Relaxation coefficient of PND: " << condition_.relaxation_coefficient_pnd << std::endl;
  std::cout << "Relaxation coefficient of velocity divergence: " << condition_.relaxation_coefficient_vel_div << std::endl;
}

bool Particles::nextLoop(const std::string& path, Timer& timer) {
  std::cout << std::endl;
  timer.limitCurrentDeltaTime(getMaxSpeed(), condition_);
  timer.printCompuationTime();
  timer.printTimeInfo();
  showParticlesInfo();
  std::cout << boost::format("Max velocity: %f") % getMaxSpeed() << std::endl;
  saveInterval(path, timer);
  if (checkNeedlessCalculation()) {
    std::cerr << "Error: All particles have become ghost." << std::endl;
    writeVtkFile((boost::format(path) % "err").str(), (boost::format("Time: %s") % timer.getCurrentTime()).str());
    throw std::range_error("Error: All particles have become ghost.");
  }
  if (timer.isUnderMinDeltaTime()) {
    std::cerr << "Error: Delta time has become so small." << std::endl;
    writeVtkFile((boost::format(path) % "err").str(), (boost::format("Time: %s") % timer.getCurrentTime()).str());
    throw std::range_error("Error: Delta time has become so small.");
  }
  if (!timer.hasNextLoop()) {
    std::cout << std::endl << "Total ";
    timer.printCompuationTime();
    std::cout << "Succeed in simulation." << std::endl;
    return false;
  }
  timer.update();
  temporary_velocity = velocity;
  temporary_position = position;
  return true;
}

bool Particles::checkNeedlessCalculation() const {
  if (pressure.hasNaN() || velocity.hasNaN() || position.hasNaN()) {
    std::cerr << "Error: Data contains NaN." << std::endl;
    return true;
  }
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (particle_types(i_particle) == ParticleType::NORMAL) return false;
  }
  std::cerr << "Error: No normal particles." << std::endl;
  return true;
}

void Particles::extendStorage(int extra_size) {
  position.conservativeResize(3, size + extra_size);
  velocity.conservativeResize(3, size + extra_size);
  temporary_position.conservativeResize(3, size + extra_size);
  temporary_velocity.conservativeResize(3, size + extra_size);
  correction_velocity.conservativeResize(3, size + extra_size);
  pressure.conservativeResize(size + extra_size);
  particle_number_density.conservativeResize(size + extra_size);
  neighbor_particles.conservativeResize(size + extra_size);
  boundary_types.conservativeResize(size + extra_size);
  particle_types.conservativeResize(size + extra_size);
  source_term.conservativeResize(size + extra_size);
  voxel_ratio.conservativeResize(size + extra_size);

  position.block(0, size, 3, extra_size)            = Eigen::MatrixXd::Zero(3, extra_size);
  velocity.block(0, size, 3, extra_size)            = Eigen::MatrixXd::Zero(3, extra_size);
  temporary_position.block(0, size, 3, extra_size)  = Eigen::MatrixXd::Zero(3, extra_size);
  temporary_velocity.block(0, size, 3, extra_size)  = Eigen::MatrixXd::Zero(3, extra_size);
  correction_velocity.block(0, size, 3, extra_size) = Eigen::MatrixXd::Zero(3, extra_size);
  pressure.segment(size, extra_size)                = Eigen::VectorXd::Zero(extra_size);
  particle_number_density.segment(size, extra_size) = Eigen::VectorXd::Zero(extra_size);
  neighbor_particles.segment(size, extra_size)      = Eigen::VectorXi::Zero(extra_size);
  source_term.segment(size, extra_size)             = Eigen::VectorXd::Zero(extra_size);
  voxel_ratio.segment(size, extra_size)             = Eigen::VectorXd::Zero(extra_size);
  for (int i_particle = size; i_particle < size + extra_size; ++i_particle) {
    particle_types(i_particle) = ParticleType::GHOST;
    boundary_types(i_particle) = BoundaryType::OTHERS;
    ghost_stack.push(i_particle);
  }
  size += extra_size;
  std::cout << "Added ghost particles: " << extra_size << std::endl;
}

int Particles::addParticle() {
  if (ghost_stack.empty()) {
    if (condition_.additional_ghost_particles > 0) {
      extendStorage(condition_.additional_ghost_particles);
      int new_index = ghost_stack.top();
      ghost_stack.pop();
      particle_types(new_index) = ParticleType::NORMAL;
      return new_index;
    } else {
      std::cerr << "Error: Can't make new particles." << std::endl
                << "Extra ghost particles has run out." << std::endl
                << "Additional ghost particles: " << condition_.additional_ghost_particles << std::endl;
      throw std::bad_array_new_length();
      exit(EXIT_FAILURE);
    }
  } else {
    int new_index = ghost_stack.top();
    ghost_stack.pop();
    particle_types(new_index) = ParticleType::NORMAL;
    return new_index;
  }
}

void Particles::setGhostParticle(int index) {
  if (index < 0 || index >= size) {
    std::cerr << "Error: Index is out of range." << std::endl;
    std::cerr << "Size: " << size << ", Index: " << std::endl;
    throw std::out_of_range("Error: Index is out of range.");
  }
  particle_types(index) = ParticleType::GHOST;
  boundary_types(index) = BoundaryType::OTHERS;
  position.col(index).setZero();
  velocity.col(index).setZero();
  pressure(index) = 0.0;
  particle_number_density(index) = 0.0;
  neighbor_particles(index) = 0.0;
  temporary_position.col(index).setZero();
  temporary_velocity.col(index).setZero();
  correction_velocity.col(index).setZero();
  source_term(index) = 0.0;
  voxel_ratio(index) = 0.0;
  ghost_stack.push(index);
  std::cout << "Changed ghost particle: " << index << std::endl;
}

void Particles::removeOutsideParticles(const Eigen::Vector3d& minpos, const Eigen::Vector3d& maxpos) {
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (particle_types(i_particle) == ParticleType::GHOST) continue;
    for (int i_dim = 0; i_dim < dimension; ++i_dim) {
      if (position.col(i_particle)(i_dim) < minpos(i_dim) || position.col(i_particle)(i_dim) > maxpos(i_dim)) {
        setGhostParticle(i_particle);
      }
    }
  }
}

void Particles::removeFastParticles(double max_speed) {
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (particle_types(i_particle) == ParticleType::GHOST) continue;
    if (velocity.col(i_particle).norm() > max_speed) setGhostParticle(i_particle);
  }
}

void Particles::calculateTemporaryParticleNumberDensity() {
  Grid grid(condition_.pnd_weight_radius, temporary_position, particle_types.array() != ParticleType::GHOST, dimension);
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (particle_types(i_particle) == ParticleType::GHOST) {
      particle_number_density(i_particle) = 0.0;
      neighbor_particles(i_particle) = 0;
      continue;
    }
    Grid::Neighbors neighbors;
    grid.getNeighbors(i_particle, neighbors);
    double pnd = 0.0;
    int count = 0;
    for (int j_particle : neighbors) {
      if (particle_types(i_particle) == ParticleType::GHOST) continue;
      Eigen::Vector3d r_ij = temporary_position.col(j_particle) - temporary_position.col(i_particle);
      pnd += weightForParticleNumberDensity(r_ij);
      ++count;
    }
    particle_number_density(i_particle) = pnd;
    neighbor_particles(i_particle) = count;
  }
}

void Particles::updateParticleNumberDensity() {
  Grid grid(condition_.pnd_weight_radius, position, particle_types.array() != ParticleType::GHOST, dimension);
  updateParticleNumberDensity(grid);
}

void Particles::updateParticleNumberDensity(const Grid& grid) {
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (particle_types(i_particle) == ParticleType::GHOST) {
      particle_number_density(i_particle) = 0.0;
      neighbor_particles(i_particle) = 0;
      continue;
    }
    Grid::Neighbors neighbors;
    grid.getNeighbors(i_particle, neighbors);
    double pnd = 0.0;
    int count = 0;
    for (int j_particle : neighbors) {
      if (particle_types(i_particle) == ParticleType::GHOST) continue;
      Eigen::Vector3d r_ij = position.col(j_particle) - position.col(i_particle);
      pnd += weightForParticleNumberDensity(r_ij);
      ++count;
    }
    particle_number_density(i_particle) = pnd;
    neighbor_particles(i_particle) = count;
  }
}

void Particles::updateVoxelRatio(int width, const Grid& grid) {
  if (condition_.dimension == 2) {
    for (int i_particle = 0; i_particle < size; ++i_particle) {
      if (particle_types(i_particle) == ParticleType::GHOST
       || boundary_types(i_particle) == BoundaryType::OTHERS) {
        voxel_ratio(i_particle) = 0;
        continue;
      }
      Grid::Neighbors neighbors;
      grid.getNeighborsInBox(i_particle, neighbors);
      const int voxels_size = width * width;
      std::vector<int> voxels(voxels_size);
      double shift = width * condition_.average_distance / 2.0;
      for (int idx = 0; idx < voxels_size; ++idx) voxels[idx] = 0;
      double center_hash = voxels_size / 2;
      if (boundary_types(i_particle) == BoundaryType::SURFACE) {
        voxels[center_hash] = 1;
      } else {
        if (voxels[center_hash] != 1) voxels[center_hash] = 2;
      }
      bool near_surface = false;
      for (int j_particle : neighbors) {
        if (particle_types(j_particle) == ParticleType::GHOST) continue;
        Eigen::Vector3d r_ij = temporary_position.col(j_particle) - temporary_position.col(i_particle);
        int x_idx = std::floor((r_ij(0) + shift) / condition_.average_distance);
        int y_idx = std::floor((r_ij(1) + shift) / condition_.average_distance);
        if (x_idx < 0 || x_idx >= width || y_idx < 0 || y_idx >= width) continue;
        int hash = x_idx + y_idx * width;
        if (hash < 0 || hash >= voxels_size) continue;
        if (boundary_types(j_particle) == BoundaryType::SURFACE) {
          voxels[hash] = 1;
          near_surface = true;
        } else {
          if (voxels[hash] != 1) voxels[hash] = 2;
        }
      }
      double val_sum = 0;
      for (int idx = 0; idx < voxels_size; ++idx) {
        val_sum += voxels[idx];
      }
      voxel_ratio(i_particle) = (near_surface)? val_sum / (voxels_size * 2) : 1.0;
    }
  } else {
    throw std::out_of_range("Not implemented for 3d.");
  }
}

void Particles::moveInflowParticles(const Timer& timer) {
  inflow_stride += condition_.inflow_velocity.norm() * timer.getCurrentDeltaTime();
  Eigen::Vector3d inflow_normalized = condition_.inflow_velocity.normalized();
  if (inflow_stride >= condition_.average_distance) {
    for (int i_particle = 0; i_particle < size; ++i_particle) {
      if (particle_types(i_particle) == ParticleType::INFLOW) {
        int new_index = addParticle();
        particle_types(new_index) = ParticleType::NORMAL;
        boundary_types(new_index) = boundary_types(i_particle);
        position.col(new_index) = position.col(i_particle);
        velocity.col(new_index) = velocity.col(i_particle);
        pressure(new_index) = pressure(i_particle);
        particle_number_density(new_index) = particle_number_density(i_particle);
        neighbor_particles(new_index) = neighbor_particles(i_particle);
        temporary_position.col(new_index) = temporary_position.col(i_particle);
        temporary_velocity.col(new_index) = temporary_velocity.col(i_particle);
        correction_velocity.col(new_index) = correction_velocity.col(i_particle);
        temporary_velocity.col(i_particle) = condition_.inflow_velocity;
        velocity.col(i_particle) = condition_.inflow_velocity;
        temporary_position.col(i_particle) -= inflow_normalized  * condition_.average_distance;
        position.col(i_particle) -= inflow_normalized  * condition_.average_distance;
      }
      if (particle_types(i_particle) == ParticleType::DUMMY_INFLOW) {
        temporary_velocity.col(i_particle) = condition_.inflow_velocity;
        velocity.col(i_particle) = condition_.inflow_velocity;
        temporary_position.col(i_particle) -= inflow_normalized * condition_.average_distance;
        position.col(i_particle) -= inflow_normalized * condition_.average_distance;
      }
    }
    inflow_stride -= condition_.average_distance;
  } else {
    for (int i_particle = 0; i_particle < size; ++i_particle) {
      if (particle_types(i_particle) == ParticleType::INFLOW || particle_types(i_particle) == ParticleType::DUMMY_INFLOW ) {
        temporary_velocity.col(i_particle) = condition_.inflow_velocity;
        velocity.col(i_particle) = condition_.inflow_velocity;
      }
    }
  }
}

void Particles::calculateTemporaryVelocity(const Eigen::Vector3d& force, const Timer& timer) {
  Grid grid(condition_.laplacian_viscosity_weight_radius, position,
            particle_types.array() == ParticleType::NORMAL || particle_types.array() == ParticleType::INFLOW,
            condition_.dimension);
  calculateTemporaryVelocity(force, timer, grid);
}

void Particles::calculateTemporaryVelocity(const Eigen::Vector3d& force, const Timer& timer, Grid& grid) {
  double delta_time = timer.getCurrentDeltaTime();
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (particle_types(i_particle) == ParticleType::NORMAL) {
      temporary_velocity.col(i_particle) += delta_time * force;
      if (condition_.viscosity_calculation) {
        Grid::Neighbors neighbors;
        grid.getNeighbors(i_particle, neighbors);
        Eigen::Vector3d lap_vec(0.0, 0.0, 0.0);
        for (int j_particle : neighbors) {
          Eigen::Vector3d u_ij = velocity.col(j_particle) - velocity.col(i_particle);
          Eigen::Vector3d r_ij = position.col(j_particle) - position.col(i_particle);
          lap_vec += u_ij * weightForLaplacianViscosity(r_ij) * 2 * dimension / (laplacian_lambda_viscosity * initial_particle_number_density);
        }
        temporary_velocity.col(i_particle) += lap_vec * condition_.kinematic_viscosity * delta_time;
      }
    }
  }
}

void Particles::updateTemporaryPosition(const Timer& timer) {
  temporary_position = position + timer.getCurrentDeltaTime() * temporary_velocity;
}

void Particles::solvePressurePoisson(const Timer& timer) {
  Grid grid(condition_.laplacian_pressure_weight_radius, temporary_position, boundary_types.array() != BoundaryType::OTHERS, condition_.dimension);
  using T = Eigen::Triplet<double>;
  double lap_r = grid.getGridWidth()/condition_.average_distance;
  int n_size = (int)(std::pow(lap_r * 2, dimension));
  double delta_time = timer.getCurrentDeltaTime();
  Eigen::SparseMatrix<double> p_mat(size, size);
  source_term.setZero();
  std::vector<T> coeffs(size * n_size);
  std::vector<int> neighbors(n_size * 2);
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (boundary_types(i_particle) == BoundaryType::OTHERS) {
      coeffs.push_back(T(i_particle, i_particle, 1.0));
      continue;
    } else if (boundary_types(i_particle) == BoundaryType::SURFACE) {
      coeffs.push_back(T(i_particle, i_particle, 1.0));
      continue;
    }
    grid.getNeighbors(i_particle, neighbors);
    double sum = 0.0;
    double div_vel = 0.0;
    for (int j_particle : neighbors) {
      if (boundary_types(j_particle) == BoundaryType::OTHERS) continue;
      Eigen::Vector3d r_ij = temporary_position.col(j_particle) - temporary_position.col(i_particle);
      double mat_ij = weightForLaplacianPressure(r_ij) * 2 * dimension
              / (laplacian_lambda_pressure * initial_particle_number_density);
      sum -= mat_ij;
      div_vel += (temporary_velocity.col(j_particle) - temporary_velocity.col(i_particle)).dot(r_ij)
              * weightForLaplacianPressure(r_ij) * condition_.dimension / (r_ij.squaredNorm() * initial_particle_number_density);
      if (boundary_types(j_particle) == BoundaryType::INNER) {
        coeffs.push_back(T(i_particle, j_particle, mat_ij));
      }
    }
    sum -= condition_.weak_compressibility * condition_.mass_density / (delta_time * delta_time);
    coeffs.push_back(T(i_particle, i_particle, sum));
    source_term(i_particle) = div_vel * condition_.mass_density * condition_.relaxation_coefficient_vel_div / delta_time
                - (particle_number_density(i_particle) - initial_particle_number_density)
                * condition_.relaxation_coefficient_pnd * condition_.mass_density / (delta_time * delta_time * initial_particle_number_density);
  }
  p_mat.setFromTriplets(coeffs.begin(), coeffs.end()); // Finished setup matrix
  solveConjugateGradient(p_mat);
}

void Particles::solvePressurePoissonTanakaMasunaga(const Timer& timer) {
  Grid grid(condition_.laplacian_pressure_weight_radius, position, boundary_types.array() != BoundaryType::OTHERS, condition_.dimension);
  using T = Eigen::Triplet<double>;
  double lap_r = grid.getGridWidth()/condition_.average_distance;
  int n_size = (int)(std::pow(lap_r * 2, dimension));
  double delta_time = timer.getCurrentDeltaTime();
  Eigen::SparseMatrix<double> p_mat(size, size);
  std::vector<T> coeffs(size * n_size);
  std::vector<int> neighbors(n_size * 2);
  source_term.setZero();
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (boundary_types(i_particle) == BoundaryType::OTHERS) {
      coeffs.push_back(T(i_particle, i_particle, 1.0));
      continue;
    } else if (boundary_types(i_particle) == BoundaryType::SURFACE) {
      coeffs.push_back(T(i_particle, i_particle, 1.0));
      continue;
    }
    grid.getNeighbors(i_particle, neighbors);
    double sum = 0.0;
    double div_vel = 0.0;
    for (int j_particle : neighbors) {
      if (boundary_types(j_particle) == BoundaryType::OTHERS) continue;
      Eigen::Vector3d r_ij = position.col(j_particle) - position.col(i_particle);
      double mat_ij = weightForLaplacianPressure(r_ij) * 2 * dimension
              / (laplacian_lambda_pressure * initial_particle_number_density);
      sum -= mat_ij;
      div_vel += (temporary_velocity.col(j_particle) - temporary_velocity.col(i_particle)).dot(r_ij)
              * weightForLaplacianPressure(r_ij) * condition_.dimension / (r_ij.squaredNorm() * initial_particle_number_density);
      if (boundary_types(j_particle) == BoundaryType::INNER) {
        coeffs.push_back(T(i_particle, j_particle, mat_ij));
      }
    }
    sum -= condition_.weak_compressibility * condition_.mass_density / (delta_time * delta_time);
    coeffs.push_back(T(i_particle, i_particle, sum));
    source_term(i_particle) = div_vel * condition_.mass_density * condition_.relaxation_coefficient_vel_div / delta_time
                - (particle_number_density(i_particle) - initial_particle_number_density)
                * condition_.relaxation_coefficient_pnd * condition_.mass_density / (delta_time * delta_time * initial_particle_number_density);
  }
  p_mat.setFromTriplets(coeffs.begin(), coeffs.end()); // Finished setup matrix
  solveConjugateGradient(p_mat);
}

void Particles::solvePressurePoissonTamai(const Timer& timer) {
  Grid grid(condition_.laplacian_pressure_weight_radius, temporary_position, boundary_types.array() != BoundaryType::OTHERS, condition_.dimension);
  using T = Eigen::Triplet<double>;
  double lap_r = grid.getGridWidth()/condition_.average_distance;
  int n_size = (int)(std::pow(lap_r * 2, dimension));
  double delta_time = timer.getCurrentDeltaTime();
  Eigen::SparseMatrix<double> p_mat(size, size);
  std::vector<T> coeffs(size * n_size);
  std::vector<int> neighbors(n_size * 2);
  source_term.setZero();
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (boundary_types(i_particle) == BoundaryType::OTHERS) {
      coeffs.push_back(T(i_particle, i_particle, 1.0));
      continue;
    } else if (boundary_types(i_particle) == BoundaryType::SURFACE) {
      coeffs.push_back(T(i_particle, i_particle, 1.0));
      continue;
    }
    grid.getNeighbors(i_particle, neighbors);
    double sum = 0.0;
    double div_vel = 0.0;
    double div_tmp_vel = 0.0;
    for (int j_particle : neighbors) {
      if (boundary_types(j_particle) == BoundaryType::OTHERS) continue;
      Eigen::Vector3d r_ij = temporary_position.col(j_particle) - temporary_position.col(i_particle);
      double mat_ij = weightForLaplacianPressure(r_ij) * 2 * dimension
              / (laplacian_lambda_pressure * initial_particle_number_density);
      sum -= mat_ij;
      div_vel += (velocity.col(j_particle) - velocity.col(i_particle)).dot(r_ij)
              * weightForLaplacianPressure(r_ij) * condition_.dimension / (r_ij.squaredNorm() * initial_particle_number_density);
      div_tmp_vel += (temporary_velocity.col(j_particle) - temporary_velocity.col(i_particle)).dot(r_ij)
              * weightForLaplacianPressure(r_ij) * condition_.dimension / (r_ij.squaredNorm() * initial_particle_number_density);
              if (boundary_types(j_particle) == BoundaryType::INNER) {
        coeffs.push_back(T(i_particle, j_particle, mat_ij));
      }
    }
    sum -= condition_.weak_compressibility * condition_.mass_density / (delta_time * delta_time);
    coeffs.push_back(T(i_particle, i_particle, sum));
    double pnd_diff = (particle_number_density(i_particle) - initial_particle_number_density * voxel_ratio(i_particle)) / (initial_particle_number_density * voxel_ratio(i_particle));
    source_term(i_particle) = (div_tmp_vel + std::abs(pnd_diff) * div_vel + std::abs(div_vel) * pnd_diff) * condition_.mass_density / delta_time;
  }
  p_mat.setFromTriplets(coeffs.begin(), coeffs.end()); // Finished setup matrix
  solveConjugateGradient(p_mat);
}

void Particles::solveConjugateGradient(Eigen::SparseMatrix<double> p_mat) {
  Eigen::ConjugateGradient<Eigen::SparseMatrix<double>> cg;
  cg.compute(p_mat);
  if (cg.info() != Eigen::ComputationInfo::Success) {
    std::cerr << "Error: Failed decompostion." << std::endl;
  }
  pressure = cg.solve(source_term);
  if (cg.info() != Eigen::ComputationInfo::Success) {
    std::cerr << "Error: Failed solving." << std::endl;
  }
  std::cout << "Solver - iterations: " << cg.iterations() << ", estimated error: " << cg.error() << std::endl;
}

void Particles::setZeroOnNegativePressure(){
  for (int i = 0; i < size; ++i) {
    if (pressure(i) < 0) pressure(i) = 0;
  }
}

void Particles::correctVelocity(const Timer& timer) {
  Grid grid(condition_.gradient_radius, temporary_position, boundary_types.array() != BoundaryType::OTHERS, condition_.dimension);
  correctVelocity(timer, grid);
}

void Particles::correctVelocity(const Timer& timer, const Grid& grid) {
  correction_velocity.setZero();
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (particle_types(i_particle) != ParticleType::NORMAL) continue;
    if (boundary_types(i_particle) == BoundaryType::OTHERS) continue;
    Grid::Neighbors neighbors;
    grid.getNeighbors(i_particle, neighbors);
    double p_min = pressure(i_particle);
    for (int j_particle : neighbors) {
      if (boundary_types(j_particle) == BoundaryType::OTHERS) continue;
      p_min = std::min(pressure(j_particle), p_min);
    }
    Eigen::Vector3d tmp(0.0, 0.0, 0.0);
    for (int j_particle : neighbors) {
      if (boundary_types(j_particle) == BoundaryType::OTHERS) continue;
      Eigen::Vector3d r_ij = temporary_position.col(j_particle) - temporary_position.col(i_particle);
      tmp += r_ij * (pressure(j_particle) - p_min) * weightForGradientPressure(r_ij) / r_ij.squaredNorm();
    }
    if (dimension == 2) tmp(2) = 0;
    correction_velocity.col(i_particle) -= tmp * dimension * timer.getCurrentDeltaTime() / (initial_particle_number_density * condition_.mass_density);
  }
  temporary_velocity += correction_velocity;
}

void Particles::correctVelocityExplicitly(const Timer& timer) {
  Grid grid(condition_.gradient_radius, position, boundary_types.array() != BoundaryType::OTHERS, condition_.dimension);
  correction_velocity.setZero();
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (particle_types(i_particle) != ParticleType::NORMAL) continue;
    if (boundary_types(i_particle) == BoundaryType::OTHERS) continue;
    Grid::Neighbors neighbors;
    grid.getNeighbors(i_particle, neighbors);
    double p_min = pressure(i_particle);
    for (int j_particle : neighbors) {
      if (boundary_types(j_particle) == BoundaryType::OTHERS) continue;
      p_min = std::min(pressure(j_particle), p_min);
    }
    Eigen::Vector3d tmp(0.0, 0.0, 0.0);
    for (int j_particle : neighbors) {
      if (boundary_types(j_particle) == BoundaryType::OTHERS) continue;
      Eigen::Vector3d r_ij = position.col(j_particle) - position.col(i_particle);
      tmp += r_ij * (pressure(j_particle) - p_min) * weightForGradientPressure(r_ij) / r_ij.squaredNorm();
    }
    if (dimension == 2) tmp(2) = 0;
    correction_velocity.col(i_particle) -= tmp * dimension * timer.getCurrentDeltaTime() / (initial_particle_number_density * condition_.mass_density);
  }
  temporary_velocity += correction_velocity;
}

void Particles::correctTanakaMasunagaVelocity(const Timer& timer) {
  Grid grid(condition_.gradient_radius, position, boundary_types.array() != BoundaryType::OTHERS, condition_.dimension);
  correction_velocity.setZero();
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (particle_types(i_particle) != ParticleType::NORMAL) continue;
    if (boundary_types(i_particle) == BoundaryType::OTHERS) continue;
    Grid::Neighbors neighbors;
    grid.getNeighbors(i_particle, neighbors);
    Eigen::Vector3d tmp(0.0, 0.0, 0.0);
    for (int j_particle : neighbors) {
      if (boundary_types(j_particle) == BoundaryType::OTHERS) continue;
      Eigen::Vector3d r_ij = position.col(j_particle) - position.col(i_particle);
      tmp += r_ij * (pressure(j_particle) + pressure(i_particle)) * weightForGradientPressure(r_ij) / r_ij.squaredNorm();
    }
    if (dimension == 2) tmp(2) = 0;
    correction_velocity.col(i_particle) -= tmp * dimension * timer.getCurrentDeltaTime() / (initial_particle_number_density * condition_.mass_density);
  }
  temporary_velocity += correction_velocity;
}

void Particles::correctVelocityWithTensor(const Timer& timer) {
  Grid grid(condition_.gradient_radius, temporary_position, boundary_types.array() != BoundaryType::OTHERS, condition_.dimension);
  correction_velocity.setZero();
  int tensor_count = 0;
  int not_tensor_count = 0;
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (particle_types(i_particle) != ParticleType::NORMAL) continue;
    if (boundary_types(i_particle) == BoundaryType::OTHERS) continue;
    Grid::Neighbors neighbors;
    grid.getNeighbors(i_particle, neighbors);
    Eigen::Matrix3d tensor = Eigen::Matrix3d::Zero();
    Eigen::Vector3d tmp_vel(0.0, 0.0, 0.0);
    for (int j_particle : neighbors) {
      if (boundary_types(j_particle) == BoundaryType::OTHERS) continue;
      Eigen::Vector3d r_ij = temporary_position.col(j_particle) - temporary_position.col(i_particle);
      Eigen::Vector3d n_ij = r_ij.normalized();
      Eigen::Matrix3d tmp_tensor = Eigen::Matrix3d::Zero();
      tmp_tensor << n_ij(0) * n_ij(0), n_ij(0) * n_ij(1), n_ij(0) * n_ij(2),
                    n_ij(1) * n_ij(0), n_ij(1) * n_ij(1), n_ij(1) * n_ij(2),
                    n_ij(2) * n_ij(0), n_ij(2) * n_ij(1), n_ij(2) * n_ij(2);
      tensor += tmp_tensor * weightForGradientPressure(r_ij) / initial_particle_number_density;
      tmp_vel += r_ij * (pressure(j_particle) - pressure(i_particle)) * weightForGradientPressure(r_ij) / r_ij.squaredNorm();
    }
    if (dimension == 2) {
      tmp_vel(2) = 0;
      tensor(2, 2) = 1.0;
    }
    if (tensor.determinant() > 1.0e-10) {
      correction_velocity.col(i_particle) -= tensor.inverse() * tmp_vel * timer.getCurrentDeltaTime() / (initial_particle_number_density * condition_.mass_density);
      ++tensor_count;
    } else {
      correction_velocity.col(i_particle) -= tmp_vel * dimension * timer.getCurrentDeltaTime() / (initial_particle_number_density * condition_.mass_density);
      ++not_tensor_count;
      // std::cout << abs(tensor.determinant()) << std::endl <<  tensor << std::endl;
    }
  }
  std::cout << "Tensor: " << tensor_count << ", Not Tensor: " << not_tensor_count << std::endl;

  temporary_velocity += correction_velocity;
}

void Particles::correctVelocityTanakaMasunagaWithTensor(const Timer& timer) {
  Grid grid(condition_.gradient_radius, position, boundary_types.array() != BoundaryType::OTHERS, condition_.dimension);
  correction_velocity.setZero();
  int tensor_count = 0;
  int not_tensor_count = 0;
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (particle_types(i_particle) != ParticleType::NORMAL) continue;
    if (boundary_types(i_particle) == BoundaryType::OTHERS) continue;
    Grid::Neighbors neighbors;
    grid.getNeighbors(i_particle, neighbors);
    Eigen::Matrix3d tensor = Eigen::Matrix3d::Zero();
    Eigen::Vector3d tmp_vel(0.0, 0.0, 0.0);
    for (int j_particle : neighbors) {
      if (boundary_types(j_particle) == BoundaryType::OTHERS) continue;
      Eigen::Vector3d r_ij = position.col(j_particle) - position.col(i_particle);
      Eigen::Vector3d n_ij = r_ij.normalized();
      Eigen::Matrix3d tmp_tensor = Eigen::Matrix3d::Zero();
      tmp_tensor << n_ij(0) * n_ij(0), n_ij(0) * n_ij(1), n_ij(0) * n_ij(2),
                    n_ij(1) * n_ij(0), n_ij(1) * n_ij(1), n_ij(1) * n_ij(2),
                    n_ij(2) * n_ij(0), n_ij(2) * n_ij(1), n_ij(2) * n_ij(2);
      tensor += tmp_tensor * weightForGradientPressure(r_ij) / initial_particle_number_density;
      tmp_vel += r_ij * (pressure(j_particle) - pressure(i_particle)) * weightForGradientPressure(r_ij) / r_ij.squaredNorm();
    }
    if (dimension == 2) {
      tmp_vel(2) = 0;
      tensor(2, 2) = 1.0;
    }
    if (tensor.determinant() > 1.0e-10) {
      correction_velocity.col(i_particle) -= tensor.inverse() * tmp_vel * timer.getCurrentDeltaTime() / (initial_particle_number_density * condition_.mass_density);
      ++tensor_count;
    } else {
      correction_velocity.col(i_particle) -= tmp_vel * dimension * timer.getCurrentDeltaTime() / (initial_particle_number_density * condition_.mass_density);
      ++not_tensor_count;
      // std::cout << abs(tensor.determinant()) << std::endl <<  tensor << std::endl;
    }
  }
  std::cout << "Tensor: " << tensor_count << ", Not Tensor: " << not_tensor_count << std::endl;

  temporary_velocity += correction_velocity;
}

void Particles::updateVelocityAndPosition() {
  velocity = temporary_velocity;
  position = temporary_position;
}

void Particles::checkSurfaceParticles() {
  for(int i = 0; i < getSize(); ++i) {
    if (particle_types(i) == ParticleType::NORMAL || particle_types(i) == ParticleType::WALL
        || particle_types(i) == ParticleType::INFLOW) {
      if (particle_number_density(i) < condition_.surface_threshold_pnd * initial_particle_number_density
              && neighbor_particles(i) < condition_.surface_threshold_number * initial_neighbor_particles) {
        boundary_types(i) = BoundaryType::SURFACE;
      } else {
        boundary_types(i) = BoundaryType::INNER;
      }
    } else {
      boundary_types(i) = BoundaryType::OTHERS;
    }
  }
}

void Particles::checkSurfaceParticlesRemovingIsolated() {
  for(int i = 0; i < getSize(); ++i) {
    if (particle_types(i) == ParticleType::NORMAL || particle_types(i) == ParticleType::WALL
        || particle_types(i) == ParticleType::INFLOW) {
      if (particle_number_density(i) < condition_.surface_threshold_pnd * initial_particle_number_density
              && neighbor_particles(i) < condition_.surface_threshold_number * initial_neighbor_particles) {
        boundary_types(i) = BoundaryType::SURFACE;
      } else {
        if (neighbor_particles(i) <= 3) boundary_types(i) = BoundaryType::SURFACE;
        else boundary_types(i) = BoundaryType::INNER;
      }
    } else {
      boundary_types(i) = BoundaryType::OTHERS;
    }
  }
}

void Particles::giveCollisionRepulsionForce() {
  giveCollisionRepulsionForce(condition_.collision_influence, condition_.restitution_coefficent);
}

void Particles::giveCollisionRepulsionForce(double influence_ratio, double restitution_coefficient) {
  Grid grid(influence_ratio * condition_.average_distance, temporary_position, boundary_types.array() != BoundaryType::OTHERS, condition_.dimension);
  Eigen::Matrix3Xd impulse_vel = Eigen::MatrixXd::Zero(3, size);
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (particle_types(i_particle) != ParticleType::NORMAL) continue;
    if (boundary_types(i_particle) == BoundaryType::OTHERS) continue;
    Grid::Neighbors neighbors;
    grid.getNeighbors(i_particle, neighbors);
    for (int j_particle : neighbors) {
      if (boundary_types(j_particle) == BoundaryType::OTHERS) continue;
      Eigen::Vector3d n_ij = (temporary_position.col(j_particle) - temporary_position.col(i_particle)).normalized();
      Eigen::Vector3d u_ij = temporary_velocity.col(j_particle) - temporary_velocity.col(i_particle);
      impulse_vel.col(i_particle) += n_ij * u_ij.dot(n_ij) * (restitution_coefficient + 1) / 2;
    }
  }
  temporary_velocity += impulse_vel;
}

void Particles::shiftParticles(double influence_ratio, double alpha) {
  double influence_radius = influence_ratio * condition_.average_distance;
  Grid grid(influence_radius, temporary_position, particle_types.array() != ParticleType::GHOST, condition_.dimension);
  Eigen::Matrix3Xd shift_vec = Eigen::MatrixXd::Zero(3, size);
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (particle_types(i_particle) != ParticleType::NORMAL) continue;
    if (boundary_types(i_particle) == BoundaryType::OTHERS) continue;
    Grid::Neighbors neighbors;
    grid.getNeighbors(i_particle, neighbors);
    for (int j_particle : neighbors) {
      if (particle_types(j_particle) == ParticleType::GHOST) continue;
      Eigen::Vector3d r_ij = temporary_position.col(j_particle) - temporary_position.col(i_particle);
      shift_vec.col(i_particle) += r_ij * weightStandard(r_ij, influence_radius) * influence_radius / r_ij.squaredNorm();
    }
  }
  temporary_position += shift_vec * alpha * condition_.average_distance;
}

double Particles::weightForParticleNumberDensity(const Eigen::Vector3d& vec) const {
  return weightStandard(vec, condition_.pnd_weight_radius);
}

double Particles::weightForGradientPressure(const Eigen::Vector3d& vec) const {
  return weightStandard(vec, condition_.gradient_radius);
}

double Particles::weightForLaplacianPressure(const Eigen::Vector3d& vec) const {
  return weightStandard(vec, condition_.laplacian_pressure_weight_radius);
}

double Particles::weightForLaplacianViscosity(const Eigen::Vector3d& vec) const {
  return weightStandard(vec, condition_.laplacian_viscosity_weight_radius);
}

void Particles::showParticlesInfo() {
  int inner = 0;
  int surface = 0;
  int ghost = 0;
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (boundary_types(i_particle) == BoundaryType::INNER) ++inner;
    if (boundary_types(i_particle) == BoundaryType::SURFACE) ++surface;
    if (particle_types(i_particle) == ParticleType::GHOST) ++ghost;
  }
  std::cout << "Particles - "
            << "inners: " << inner << ", surfaces: " << surface
            << ", others: " << size - (inner + surface) << " (ghosts: " << ghost << ")" << std::endl;
}

} // namespace tiny_mps


#pragma once

#include <string>
#include <vector>
#include <iostream>
#include <boost/unordered_map.hpp>
#include <boost/algorithm/string.hpp>
#include <math.h>

#include "query.hpp"
#include "data_statistic.hpp"
#include "mymath.hpp"
#include "timer.hpp"

using namespace std;

#define COST_THRESHOLD 350

struct plan {
  double cost;      // now min cost
  vector<int64_t> orders; // now best orders
  double result_num;     // record intermediate results
};

struct select_record{
  int p;
  int d;
  double v;
  bool operator > (const select_record & other) const {
    return v > other.v;
  }
  bool operator < (const select_record & other) const {
    return v < other.v;
  }
  bool operator == (const select_record & other) const {
    return v == other.v;
  }
};

template <class T>
class Minimum_maintenance {
  private:
    T * _stack[2]; // 0 store Descending sequence, 1 store other's
    int _iter[2];    // store iter
    int * _order;    // for push and pop
    int * _fakedeepcopy ;
  public:
    Minimum_maintenance(){
      cout<<"NOT SUPPORT"<<endl;
      assert(0);
    }
    Minimum_maintenance(int s){
      if(s < 1)//protect
        s = 1;
      _iter[0] = 0;
      _iter[1] = 0;
      _stack[0] = new T[s];
      _stack[1] = new T[s];
      _order = new int[s];
      //cout<<"NEW    [] +++++++++++++++++++"<<endl;
    }
    Minimum_maintenance(const Minimum_maintenance& copy){
      _stack[0]   = copy._stack[0];
      _stack[1]   = copy._stack[1];
      _iter [0]   = copy._iter [0];
      _iter [1]   = copy._iter [1];
      _order      = copy._order;
    }
    ~Minimum_maintenance(){
      delete [] _stack[0];
      delete [] _stack[1];
      delete [] _order;
      //cout<<"DELETE [] -------------------"<<endl;
    }
    void push(const T & ms){
      int i;
      if(_iter[0] == 0 || _stack[0][_iter[0] - 1] > ms){
        i = 0;
      }else{
        i = 1;
      }
      _stack[i][_iter[i]] = ms;

      _order[_iter[0]+_iter[1]] = i;
      _iter[i] ++ ;
    }
    void pop(){
      if(_iter[0]+_iter[1]){
        _iter[_order[ _iter[0] + _iter[1] - 1 ]]--;
      }
    }
    bool top(T & ms){
      if(!(_iter[0]+_iter[1]))
        return false;
      int o = _order[ _iter[0] + _iter[1] - 1];
      ms = _stack[0][ _iter[0] - 1];
      return true;
    }
    bool const empty(){
      return _iter[0] == 0;
    }
};

class Planner {
    // members
    data_statistic * statistic ;
    vector<int64_t> triples;
    double min_cost;
    vector<int64_t> path;
    bool is_empty;            // help identify empty queries

    // for dfs
    vector<int64_t> min_path;
    int _chains_size_div_4 ;
    int * min_select_record ;
    unordered_map<int, shared_ptr<Minimum_maintenance<select_record>> > * min_select;

    // store all orders for DP
    vector<int> subgraph[11]; // for 2^10 all orders

    // functions
    // dfs traverse , traverse all the valid orders
    bool com_traverse(unsigned int pt_bits, double cost, double pre_results) {
      if(pt_bits == ( 1 << _chains_size_div_4 ) - 1){
        //cout << "estimated cost : " << cost << endl;
        bool ctn = true;
        if (min_cost == std::numeric_limits<double>::max() && cost < COST_THRESHOLD && (path[0] >= (1 << NBITS_IDX)) ) {
          ctn = false;  // small query
          cout << "small query and use heuristic.\n";
        }
        if (cost < min_cost) {
          min_cost = cost;
          min_path = path;
        }
        return ctn;
      }
      for(int pt_pick = 0; pt_pick < _chains_size_div_4; pt_pick++){
        if( pt_bits & ( 1 << pt_pick ) )
          continue ;
        int i = 4 * pt_pick;
        double add_cost = 0;
        int o1 = triples[i];
        int p = triples[i+1];
        int d = triples[i+2];
        int o2 = triples[i+3];
        if (path.size() == 0) {
          if (o1 < 0 && o2 < 0) {
            //continue;
            // use index vertex
            path.push_back(p);
            path.push_back(0);
            path.push_back(IN);
            path.push_back(o1);
            add_cost = double(statistic->global_pscount[p]) / statistic->world->size(); //find subjects
            // next iteration
            //cout << "pick : " << o1 << " " << p << " " << d << " " << o2 << endl;
            //cout << "add_cost : " << add_cost << endl;
            double new_cost = cost + add_cost;
            if (new_cost > min_cost) {
              path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();
              continue;
            }
            double select_num_s = (double)statistic->global_pscount[p];
            select_record srs = {p,d,select_num_s};
            (*min_select)[o1] = std::unique_ptr<Minimum_maintenance<select_record> >
              ( new Minimum_maintenance<select_record>(2 * _chains_size_div_4));
            (*min_select)[o1]->push(srs);
            bool ctn = com_traverse(pt_bits, new_cost, add_cost);
            (*min_select)[o1]->pop();
            if (!ctn) return ctn;
            path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();

            // different direction
            path.push_back(p);
            path.push_back(0);
            path.push_back(OUT);
            path.push_back(o2);
            add_cost = double(statistic->global_pocount[p]) / statistic->world->size(); //find objects
            // next iteration
            //cout << "pick : " << o1 << " " << p << " " << d << " " << o2 << endl;
            //cout << "add_cost : " << add_cost << endl;
            new_cost = cost + add_cost;
            if (new_cost > min_cost) {
              path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();
              continue;
            }
            double select_num_o = (double)statistic->global_pocount[p];
            select_record sro = {p,IN,select_num_o};
            (*min_select)[o2] = std::unique_ptr<Minimum_maintenance<select_record> >
              ( new Minimum_maintenance<select_record> (2 * _chains_size_div_4));
            (*min_select)[o2]->push(sro);
            ctn = com_traverse(pt_bits, new_cost, add_cost);
            (*min_select)[o2]->pop();
            if (!ctn) return ctn;
            path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();
          }
          if (o1 > 0) {
            path.push_back(o1);
            path.push_back(p);
            path.push_back(d);
            path.push_back(o2);
            add_cost = double(statistic->global_ptcount[p]) / statistic->global_pscount[p];

            // next iteration
            //cout << "pick : " << o1 << " " << p << " " << d << " " << o2 << endl;
            //cout << "add_cost : " << add_cost << endl;
            double new_cost = cost + add_cost;
            if (new_cost > min_cost) {
              path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();
              continue;
            }
            new_add_selectivity(o1, p, o2);
            bool ctn = com_traverse(pt_bits | (1 << pt_pick), new_cost, add_cost);
            unadd_selectivity();
            if (!ctn) return ctn;
            path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();

          } else if (o2 > 0) {
            path.push_back(o2);
            path.push_back(p);
            path.push_back(IN);
            path.push_back(o1);
            // for type triples
            if (p == TYPE_ID) {
              add_cost = statistic->global_pscount[o2];
              //cout << "type " << o2 << " : " << add_cost << endl;
            } else { // normal triples
              add_cost = double(statistic->global_ptcount[p]) / statistic->global_pocount[p];
            }

            // next iteration
            //cout << "pick : " << o1 << " " << p << " " << d << " " << o2 << endl;
            //cout << "add_cost : " << add_cost << endl;
            double new_cost = cost + add_cost;
            if (new_cost > min_cost) {
              path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();
              continue;
            }
            new_add_selectivity(o1, p, o2);
            bool ctn = com_traverse(pt_bits | (1 << pt_pick), new_cost, add_cost);
            unadd_selectivity();
            if (!ctn) return ctn;
            path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();

          }
        } else {
          if (min_select->find(o1) != min_select->end() && !(*min_select)[o1]->empty()) {
            path.push_back(o1);
            path.push_back(p);
            path.push_back(d);
            path.push_back(o2);
            double prune_result;
            select_record sr ;
            if(!(*min_select)[o1]->top(sr))
              cout<<"ERROR o1 on .top\n";
            int pre_p = sr.p;
            int pre_d = sr.d;
            // prune based on correlation and constant
            if (p == TYPE_ID && o2 > 0) {
              prune_result = com_prune(pre_results, pre_p, pre_d, o2, OUT);
              add_cost = prune_result;
            } else if (o2 >= 0){
              prune_result = com_prune(pre_results, pre_p, pre_d, p, OUT);
              prune_result = double(prune_result) / statistic->global_pocount[p];
              add_cost = prune_result * (double(statistic->global_ptcount[p]) / statistic->global_pscount[p]);
            } else {
              prune_result = com_prune(pre_results, pre_p, pre_d, p, OUT);
              add_cost = prune_result * (double(statistic->global_ptcount[p]) / statistic->global_pscount[p]);
            }

            // next iteration
            //cout << "pick : " << o1 << " " << p << " " << d << " " << o2 << endl;
            //cout << "add_cost : " << add_cost << endl;
            double new_cost = cost + add_cost;
            if (new_cost > min_cost) {
              path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();
              continue;
            }
            new_add_selectivity(o1, p, o2);
            bool ctn = com_traverse(pt_bits | (1 << pt_pick), new_cost, add_cost);
            unadd_selectivity();
            if (!ctn) return ctn;
            path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();

          }
          if (min_select->find(o2) != min_select->end() && !(*min_select)[o2]->empty() ) {
            path.push_back(o2);
            path.push_back(p);
            path.push_back(IN);
            path.push_back(o1);
            double prune_result;
            select_record sr ;
            if(!(*min_select)[o2]->top(sr))
              cout<<"ERROR o2 on .top\n";
            int pre_p = sr.p;
            int pre_d = sr.d;
            // prune based on correlation and constant
            prune_result = com_prune(pre_results, pre_p, pre_d, p, IN);
            if (o1 >= 0) {
              prune_result = double(prune_result) / statistic->global_pscount[p];
            }
            add_cost = prune_result * (double(statistic->global_ptcount[p]) / statistic->global_pocount[p]);

            // next iteration
            //cout << "pick : " << o1 << " " << p << " " << d << " " << o2 << endl;
            //cout << "add_cost : " << add_cost << endl;
            double new_cost = cost + add_cost;
            if (new_cost > min_cost) {
              path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();
              continue;
            }
            new_add_selectivity(o1, p, o2);
            bool ctn = com_traverse(pt_bits | (1 << pt_pick), new_cost, add_cost);
            unadd_selectivity();
            if (!ctn) return ctn;
            path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();

          }
        }
      }

      return true;
    }

    // dynamic programming planner
    void dp_plan() {
      int count = triples.size() / 4; // number of triples
      int size = pow(2,count);    // size of DP state

      // initial the DP array
      plan *state = new plan[size];
      plan initial;
      initial.cost = std::numeric_limits<double>::max();
      initial.result_num = 0;
      state[0].cost = 0;
      state[0].result_num = 0;
      for (int i = 1; i < size; i++)
        state[i] = initial;

      //cout << "count:" << count << " size : " << size << endl;
      // DP
      for (int i = 1; i < count+1; i++) {
        for (int k = 0,klimit=subgraph[i].size() ; k < klimit; k++) {
          int bit_state = subgraph[i][k];
          if (bit_state >= size) continue;
          plan& best_plan = state[bit_state];
          int best_m = -1;  // record best one
          int best_dir = OUT;
          // f[i][x] = min {f[i-1][x1] + cost, ...}
          // to compute from all the i-1 state
          for (int m = 0; m < count; m++) {
            if ((bit_state & (1 << m)) > 0) {
              int son_state = bit_state ^ (1 << m);
              double pre_cost = state[son_state].cost;
              if (pre_cost == std::numeric_limits<double>::max()) continue;  // previous order invalid
              double pre_results = state[son_state].result_num;
              vector<pair<double, int> > cost_dir;
              cost_add(pre_results, son_state, m*4, cost_dir);
              if (cost_dir.size() == 0) continue;  // order invalid
              //cout << "pre_results : " << pre_results << endl;
              if (cost_dir.size() == 1) {  // one direction
                double new_cost = cost_dir[0].first + pre_cost;
                //cout << "new_cost : " << new_cost << endl;
                if (best_plan.cost > new_cost) {
                  best_plan.cost = new_cost;
                  best_plan.result_num = cost_dir[0].first;
                  best_plan.orders = state[son_state].orders;
                  best_m = m;
                  best_dir = cost_dir[0].second;
                }
              } else if (cost_dir.size() == 2) { // both direction
                double new_cost0 = cost_dir[0].first + pre_cost;
                double new_cost1 = cost_dir[1].first + pre_cost;
                //cout << "new_cost0 : " << new_cost0 << endl;
                //cout << "new_cost1 : " << new_cost1 << endl;
                if (new_cost0 < new_cost1) {
                  if (best_plan.cost > new_cost0) {
                    best_plan.cost = new_cost0;
                    best_plan.result_num = cost_dir[0].first;
                    best_plan.orders = state[son_state].orders;
                    best_m = m;
                    best_dir = cost_dir[0].second;
                  }
                } else {
                  if (best_plan.cost > new_cost1) {
                    best_plan.cost = new_cost1;
                    best_plan.result_num = cost_dir[1].first;
                    best_plan.orders = state[son_state].orders;
                    best_m = m;
                    best_dir = cost_dir[1].second;
                  }
                }
                // if i == 1 then is index vertex
                // best_m = -1
                // push something
                if (i == 1) {
                  best_m = -1;
                  int vertex1,vertex2;
                  if (best_dir == OUT) {
                    vertex1 = triples[4*m + 3];
                    vertex2 = triples[4*m];
                  }
                  else {
                    vertex1 = triples[4*m];
                    vertex2 = triples[4*m + 3];
                  }
                  // push index execution
                  best_plan.orders.push_back(triples[4*m+1]);
                  best_plan.orders.push_back(0);
                  best_plan.orders.push_back(best_dir);
                  best_plan.orders.push_back(vertex1);
                  // push the other direction
                  best_plan.orders.push_back(vertex1);
                  best_plan.orders.push_back(triples[4*m+1]);
                  best_plan.orders.push_back(1-best_dir);
                  best_plan.orders.push_back(vertex2);
                  best_plan.result_num = statistic->global_ptcount[triples[4*m+1]]; // revise the result num
                }

              }
            }
          }
          // already find the best order
          if (best_m >= 0) {
            if (best_dir == OUT) {
              best_plan.orders.push_back(triples[4*best_m]);
              best_plan.orders.push_back(triples[4*best_m+1]);
              best_plan.orders.push_back(triples[4*best_m+2]);
              best_plan.orders.push_back(triples[4*best_m+3]);
            } else {
              best_plan.orders.push_back(triples[4*best_m+3]);
              best_plan.orders.push_back(triples[4*best_m+1]);
              best_plan.orders.push_back(IN);
              best_plan.orders.push_back(triples[4*best_m]);
            }
          }

        }
      }
      cout << "DP query planning is finished.\n";

      // state[size - 1] has the final best plan
      min_path = state[size - 1].orders;
      min_cost = state[size - 1].cost;

    }

    int bitcount(int i) {
      i = i - ((i >> 1) & 0x55555555);
      i = (i & 0x33333333) + ((i >> 2) & 0x33333333);
      return (((i + (i >> 4)) & 0x0F0F0F0F) * 0x01010101) >> 24;
    }

    void cost_add(double pre_results, int bit_set, int next, vector<pair<double, int> >& cost_dir) {
      unordered_map<int, vector<int> > selec;
      double cost = 0;
      for (int i = 0, ilimit = triples.size(); i < ilimit; i = i + 4) {
        int tmp = i / 4;
        if ( (bit_set & (0x1 << tmp)) == false ) continue;
        add_selectivity(selec, triples[i], triples[i+1], triples[i+3]);
      }

      int o1 = triples[next];
      int p = triples[next+1];
      int o2 = triples[next+3];

      // if selec empty
      if (selec.empty()) {
        if (o1 < 0 && o2 < 0) {
          // s < 0 , o < 0, two direction
          // begin from s
          cost = double(statistic->global_pscount[p]) / statistic->world->size(); //find subjects
          cost += statistic->global_ptcount[p];
          if (cost == 0) cost = 1;
          cost_dir.push_back(make_pair(cost, IN));
          // begin from o
          cost = double(statistic->global_pocount[p]) / statistic->world->size(); //find objects
          cost += statistic->global_ptcount[p];
          if (cost == 0) cost = 1;
          cost_dir.push_back(make_pair(cost, OUT));
        }
        if (o1 > 0 && o2 < 0) {
          // s > 0, o < 0
          cost = double(statistic->global_ptcount[p]) / statistic->global_pscount[p];
          cost_dir.push_back(make_pair(cost, OUT));
        }
        if (o1 < 0 && o2 > 0) {
          // s < 0, o > 0
          if (p == TYPE_ID) {
            cost = statistic->global_pscount[o2];
          } else { // normal triples
            cost = double(statistic->global_ptcount[p]) / statistic->global_pocount[p];
          }
          cost_dir.push_back(make_pair(cost, IN));
        }
      }
      // not empty
      else {
        if (selec.find(o1) != selec.end()) {
          // s match previous
          double prune_result;
          int pre_p = selec[o1][0];
          int pre_d = selec[o1][1];
          if (p == TYPE_ID && o2 > 0) {
            prune_result = com_prune(pre_results, pre_p, pre_d, o2, OUT);
            cost = prune_result;
          } else if (o2 >= 0){
            prune_result = com_prune(pre_results, pre_p, pre_d, p, OUT);
            prune_result = double(prune_result) / statistic->global_pocount[p];
            cost = prune_result * (double(statistic->global_ptcount[p]) / statistic->global_pscount[p]);
          } else {
            prune_result = com_prune(pre_results, pre_p, pre_d, p, OUT);
            cost = prune_result * (double(statistic->global_ptcount[p]) / statistic->global_pscount[p]);
          }
          if (cost == 0) cost = 1;
          cost_dir.push_back(make_pair(cost, OUT));
        }

        if (selec.find(o2) != selec.end()) {
          // o match previous
          double prune_result;
          int pre_p = selec[o2][0];
          int pre_d = selec[o2][1];
          prune_result = com_prune(pre_results, pre_p, pre_d, p, IN);
          if (o1 >= 0) {
            prune_result = double(prune_result) / statistic->global_pscount[p];
          }
          cost = prune_result * (double(statistic->global_ptcount[p]) / statistic->global_pocount[p]);
          if (cost == 0) cost = 1;
          cost_dir.push_back(make_pair(cost, IN));
        }
      }

    }

    // prune using correlation estimation
    double com_prune(double pre_results, int pre_p, int pre_d, int p, int d) {
      double result_num;
      double x,y;
      // handle corner case first
      if (pre_p == p) {
        return pre_results;
      }

      // out out correlation
      if (pre_d == OUT && d == OUT) {
        if (pre_p < p){
          x = double(statistic->global_ppcount[make_pair(pre_p,p)].out_out);
        } else {
          x = double(statistic->global_ppcount[make_pair(p,pre_p)].out_out);
        }
        y = statistic->global_pscount[pre_p];
      }

      // in out correlation
      if (pre_d == IN && d == OUT) {
        if (pre_p < p){
          x = double(statistic->global_ppcount[make_pair(pre_p,p)].in_out);
        } else {
          x = double(statistic->global_ppcount[make_pair(p,pre_p)].out_in);
        }
        y = statistic->global_pocount[pre_p];
      }

      // in in correlation
      if (pre_d == IN && d == IN) {
        if (pre_p < p){
          x = double(statistic->global_ppcount[make_pair(pre_p,p)].in_in);
        } else {
          x = double(statistic->global_ppcount[make_pair(p,pre_p)].in_in);
        }
        y = statistic->global_pocount[pre_p];
      }

      // out in correlation
      if (pre_d == OUT && d == IN) {
        if (pre_p < p){
          x = double(statistic->global_ppcount[make_pair(pre_p,p)].out_in);
        } else {
          x = double(statistic->global_ppcount[make_pair(p,pre_p)].in_out);
        }
        y = statistic->global_pscount[pre_p];
      }

      //cout << "prune ratio (x/y) : " << x << " / " << y  << endl;
      result_num = pre_results * (x/y);
      if (x == 0) is_empty = true;
      return result_num;
    }

    void new_add_selectivity(int o1, int p, int o2) {
      if (o1 < 0 && o2 > 0) {
        double select_num;
        if (p == TYPE_ID) {
          select_num = (double)statistic->global_pscount[o2];
          p = o2;
        } else {
          select_num = double(statistic->global_pscount[p]) / statistic->global_pocount[p];
        }
        select_record sr = {p,OUT,select_num};
        if (min_select->find(o1) == min_select->end())
          (*min_select)[o1] = std::unique_ptr<Minimum_maintenance<select_record> >
            ( new Minimum_maintenance<select_record> (2 * _chains_size_div_4));
        (*min_select)[o1]->push(sr);
        min_select_record[min_select_record[0] + 1] = o1;
        min_select_record[min_select_record[0] + 2] = 1;
        min_select_record[0] += 2 ;
      }else if (o2 < 0 && o1 > 0) {
        double select_num = double(statistic->global_pocount[p]) / statistic->global_pscount[p];
        select_record sr = {p,IN,select_num};
        if (min_select->find(o2) == min_select->end())
          (*min_select)[o2] = std::unique_ptr<Minimum_maintenance<select_record> >
            ( new Minimum_maintenance<select_record> (2 * _chains_size_div_4));
        (*min_select)[o2]->push(sr);

        min_select_record[min_select_record[0] + 1] = o2;
        min_select_record[min_select_record[0] + 2] = 1;
        min_select_record[0] += 2 ;
      }else if (o1 < 0 && o2 < 0) {
        double select_num_s = statistic->global_pscount[p];
        double select_num_o = statistic->global_pocount[p];
        select_record sro1 = {p,OUT,select_num_s};
        if (min_select->find(o1) == min_select->end())
          (*min_select)[o1] = std::unique_ptr<Minimum_maintenance<select_record> >
            ( new Minimum_maintenance<select_record> (2 * _chains_size_div_4));
        (*min_select)[o1]->push(sro1);

        select_record sro2 = {p,IN,select_num_o};
        if (min_select->find(o2) == min_select->end())
          (*min_select)[o2] = std::unique_ptr<Minimum_maintenance<select_record> >
            ( new Minimum_maintenance<select_record> (2 * _chains_size_div_4));
        (*min_select)[o2]->push(sro2);

        min_select_record[min_select_record[0] + 1] = o1;
        min_select_record[min_select_record[0] + 2] = o2;
        min_select_record[min_select_record[0] + 3] = 2;
        min_select_record[0] += 3 ;
      }else{
        min_select_record[min_select_record[0] + 1] = 0;
        min_select_record[0] += 1 ;
      }
    }

    void unadd_selectivity(){
      int lastpos = min_select_record[0];
      if(lastpos == 0)
        return ;
      int lastnum = min_select_record[lastpos];
      assert(lastpos > lastnum);
      int i;
      for(i = 1 ; i <= lastnum ; i++){
        (*min_select)[min_select_record[lastpos - i]]->pop();
      }
      min_select_record[0] -= lastnum + 1;
    }

    void add_selectivity(unordered_map<int, vector<int> >& tmp_selec, int o1, int p, int o2) {
      if (o1 < 0 && o2 > 0) {
        int select_num;
        if (p == TYPE_ID) {
          select_num = statistic->global_pscount[o2];
          p = o2;
        } else {
          select_num = double(statistic->global_pscount[p]) / statistic->global_pocount[p];
        }
        if (tmp_selec.find(o1) == tmp_selec.end()) {
          tmp_selec[o1].push_back(p);
          tmp_selec[o1].push_back(OUT);
          tmp_selec[o1].push_back(select_num);
        } else {
          int pre_select_num = tmp_selec[o1][2];
          if (pre_select_num > select_num) {
            vector<int> new_tmp;
            new_tmp.push_back(p);
            new_tmp.push_back(OUT);
            new_tmp.push_back(select_num);
            tmp_selec[o1] = new_tmp;
          }
        }
      }
      if (o2 < 0 && o1 > 0) {
        int select_num = double(statistic->global_pocount[p]) / statistic->global_pscount[p];
        if (tmp_selec.find(o2) == tmp_selec.end()) {
          tmp_selec[o2].push_back(p);
          tmp_selec[o2].push_back(IN);
          tmp_selec[o2].push_back(select_num);
        } else {
          int pre_select_num = tmp_selec[o2][2];
          if (pre_select_num > select_num) {
            vector<int> new_tmp;
            new_tmp.push_back(p);
            new_tmp.push_back(IN);
            new_tmp.push_back(select_num);
            tmp_selec[o2] = new_tmp;
          }
        }
      }
      if (o1 < 0 && o2 < 0) {
        int select_num_s = statistic->global_pscount[p];
        int select_num_o = statistic->global_pocount[p];
        if (tmp_selec.find(o1) == tmp_selec.end()) {
          tmp_selec[o1].push_back(p);
          tmp_selec[o1].push_back(OUT);
          tmp_selec[o1].push_back(select_num_s);
        } else {
          int pre_select_num = tmp_selec[o1][2];
          if (pre_select_num > select_num_s) {
            vector<int> new_tmp;
            new_tmp.push_back(p);
            new_tmp.push_back(OUT);
            new_tmp.push_back(select_num_s);
            tmp_selec[o1] = new_tmp;
          }
        }
        if (tmp_selec.find(o2) == tmp_selec.end()) {
          tmp_selec[o2].push_back(p);
          tmp_selec[o2].push_back(IN);
          tmp_selec[o2].push_back(select_num_o);
        } else {
          int pre_select_num = tmp_selec[o2][2];
          if (pre_select_num > select_num_o) {
            vector<int> new_tmp;
            new_tmp.push_back(p);
            new_tmp.push_back(IN);
            new_tmp.push_back(select_num_o);
            tmp_selec[o2] = new_tmp;
          }
        }
      }
    }

    // for testing purpose
    void get_all_plans(vector<int64_t> triples, unordered_map<int, vector<int> > max_selec,
        double cost, double pre_results) {
      if (triples.empty()) {
        cout << "estimated cost : " << cost << endl;
        if (cost < min_cost) {
          min_cost = cost;
          min_path = path;
        }
        // record this new order
        plan new_order;
        new_order.cost = cost;
        new_order.orders = path;
        boost::unordered_map<int,int> convert;
        for (int i = 0, ilimit = new_order.orders.size(); i < ilimit; i++) {
          if (new_order.orders[i] < 0 ){
            if (convert.find(new_order.orders[i]) == convert.end()) {
              int value =  -1 - convert.size();
              convert[new_order.orders[i]] = value;
              new_order.orders[i] = value;
            } else {
              new_order.orders[i] = convert[new_order.orders[i]];
            }
          }
        }
        all_plans.push_back(new_order);
        return;
      }
      for (int i = 0 , ilimit = triples.size(); i < ilimit; i = i+4) {
        double add_cost = 0;
        int o1 = triples[i];
        int p = triples[i+1];
        int d = triples[i+2];
        int o2 = triples[i+3];
        if (path.size() == 0) {
          if (o1 < 0 && o2 < 0) {
            //continue;
            // use index vertex
            path.push_back(p);
            path.push_back(0);
            path.push_back(IN);
            path.push_back(o1);
            add_cost = double(statistic->global_pscount[p]) / statistic->world->size(); //find subjects
            if (add_cost == 0) add_cost = 1;
            // next iteration
            double new_cost = cost + add_cost;
            unordered_map<int, vector<int> > tmp_selec = max_selec;
            int select_num_s = statistic->global_pscount[p];
            tmp_selec[o1].push_back(p);
            tmp_selec[o1].push_back(d);
            tmp_selec[o1].push_back(select_num_s);
            get_all_plans(triples, tmp_selec, new_cost, add_cost);
            path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();

            // different direction
            path.push_back(p);
            path.push_back(0);
            path.push_back(OUT);
            path.push_back(o2);
            add_cost = double(statistic->global_pocount[p]) / statistic->world->size(); //find objects
            if (add_cost == 0) add_cost = 1;
            // next iteration
            new_cost = cost + add_cost;
            tmp_selec = max_selec;
            int select_num_o = statistic->global_pocount[p];
            tmp_selec[o2].push_back(p);
            tmp_selec[o2].push_back(IN);
            tmp_selec[o2].push_back(select_num_o);
            get_all_plans(triples, tmp_selec, new_cost, add_cost);
            path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();
          }
          if (o1 > 0) {
            path.push_back(o1);
            path.push_back(p);
            path.push_back(d);
            path.push_back(o2);
            double inter_results = 0;
            add_cost = double(statistic->global_ptcount[p]) / statistic->global_pscount[p];
            inter_results = double(add_cost) / statistic->world->size();

            // next iteration
            double new_cost = cost + add_cost;
            vector<int64_t> tmp = triples;
            tmp.erase(tmp.begin() + i, tmp.begin() + i + 4);
            unordered_map<int, vector<int> > tmp_selec = max_selec;
            add_selectivity(tmp_selec, o1, p, o2);
            get_all_plans(tmp, tmp_selec, new_cost, inter_results);
            path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();

          } else if (o2 > 0) {
            path.push_back(o2);
            path.push_back(p);
            path.push_back(IN);
            path.push_back(o1);
            double inter_results = 0;
            // for type triples
            if (p == TYPE_ID) {
              add_cost = double(statistic->global_pscount[o2]) / statistic->world->size();
              inter_results = add_cost;
            } else { // normal triples
              add_cost = double(statistic->global_ptcount[p]) / statistic->global_pocount[p];
              inter_results = double(add_cost) / statistic->world->size();
            }

            // next iteration
            double new_cost = cost + add_cost;
            vector<int64_t> tmp = triples;
            tmp.erase(tmp.begin() + i, tmp.begin() + i + 4);
            unordered_map<int, vector<int> > tmp_selec = max_selec;
            add_selectivity(tmp_selec, o1, p, o2);
            get_all_plans(tmp, tmp_selec, new_cost, inter_results);
            path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();

          }
        } else {
          if (max_selec.find(o1) != max_selec.end()) {
            path.push_back(o1);
            path.push_back(p);
            path.push_back(d);
            path.push_back(o2);
            double prune_result;
            int pre_p = max_selec[o1][0];
            int pre_d = max_selec[o1][1];
            // prune based on correlation and constant
            if (p == TYPE_ID && o2 > 0) {
              prune_result = com_prune(pre_results, pre_p, pre_d, o2, OUT);
              add_cost = prune_result;
            } else if (o2 >= 0){
              prune_result = com_prune(pre_results, pre_p, pre_d, p, OUT);
              prune_result = double(prune_result) / statistic->global_pocount[p];
              add_cost = prune_result * (double(statistic->global_ptcount[p]) / statistic->global_pscount[p]);
            } else {
              prune_result = com_prune(pre_results, pre_p, pre_d, p, OUT);
              add_cost = prune_result * (double(statistic->global_ptcount[p]) / statistic->global_pscount[p]);
            }
            if (add_cost == 0) add_cost = 1;

            // next iteration
            double new_cost = cost + add_cost;
            vector<int64_t> tmp = triples;
            tmp.erase(tmp.begin() + i, tmp.begin() + i + 4);
            unordered_map<int, vector<int> > tmp_selec = max_selec;
            add_selectivity(tmp_selec, o1, p, o2);
            get_all_plans(tmp, tmp_selec, new_cost, add_cost);
            path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();

          }
          if (max_selec.find(o2) != max_selec.end()) {
            path.push_back(o2);
            path.push_back(p);
            path.push_back(IN);
            path.push_back(o1);
            double prune_result;
            int pre_p = max_selec[o2][0];
            int pre_d = max_selec[o2][1];
            // prune based on correlation and constant
            prune_result = com_prune(pre_results, pre_p, pre_d, p, IN);
            if (o1 >= 0) {
              prune_result = double(prune_result) / statistic->global_pscount[p];
            }
            add_cost = prune_result * (double(statistic->global_ptcount[p]) / statistic->global_pocount[p]);
            if (add_cost == 0) add_cost = 1;

            // next iteration
            double new_cost = cost + add_cost;
            vector<int64_t> tmp = triples;
            tmp.erase(tmp.begin() + i, tmp.begin() + i + 4);
            unordered_map<int, vector<int> > tmp_selec = max_selec;
            add_selectivity(tmp_selec, o1, p, o2);
            get_all_plans(tmp, tmp_selec, new_cost, add_cost);
            path.pop_back(); path.pop_back(); path.pop_back(); path.pop_back();

          }
        }
      }
    }

public:
    Planner() {
      for (int i = 0; i < 1024; i++) {
        int c = bitcount(i);
        subgraph[c].push_back(i);
      }
    }

    void choose_computing_device(request_or_reply& r, data_statistic* statistic){
        //TODO: write a metrics to use statistic to decide to use GPU or CPU
        if (r.start_from_index() && global_default_use_gpu_handle) {
            r.comp_dev = GPU_comp;
        } else {
            r.comp_dev = CPU_comp;
        }

    }

    bool generate_plan(request_or_reply& r, data_statistic* statistic) {
      this->statistic = statistic;
      min_path.clear();
      path.clear();
      is_empty = false;
      double cost = 0;
      min_cost = std::numeric_limits<double>::max();
      if (min_cost != std::numeric_limits<double>::max()) assert(false);

      uint64_t t_prepare1 = timer::get_usec();
      // prepare for heuristic
      for (int i = 0, ilimit = r.cmd_chains.size(); i < ilimit; i = i + 4) {
        if (r.cmd_chains[i] >= (1 << NBITS_IDX) || r.cmd_chains[i + 3] >= (1 << NBITS_IDX)) {
          if (i == 0) break;
          int ta,tb,tc,td;
          ta = r.cmd_chains[i];
          tb = r.cmd_chains[i + 1];
          tc = r.cmd_chains[i + 2];
          td = r.cmd_chains[i + 3];
          r.cmd_chains[i] = r.cmd_chains[0];
          r.cmd_chains[i + 1] = r.cmd_chains[1];
          r.cmd_chains[i + 2] = r.cmd_chains[2];
          r.cmd_chains[i + 3] = r.cmd_chains[3];
          r.cmd_chains[0] = ta;
          r.cmd_chains[1] = tb;
          r.cmd_chains[2] = tc;
          r.cmd_chains[3] = td;
          break;
        }
      }
      uint64_t t_prepare2 = timer::get_usec();
      //cout << "prepare time : " << t_prepare2 - t_prepare1 << " us" << endl;

      uint64_t t_traverse1 = timer::get_usec();
      this->triples = r.cmd_chains;
      this->min_select = new unordered_map<int, shared_ptr<Minimum_maintenance<select_record>>>;
      _chains_size_div_4 = r.cmd_chains.size()/4 ;
      min_select_record = new int [ 1 + 6 * _chains_size_div_4 ];
      min_select_record[0] = 0;
      com_traverse(0, cost, 0);
      delete [] min_select_record ;
      delete this->min_select;
      uint64_t t_traverse2 = timer::get_usec();
      //cout << "traverse time : " << t_traverse2 - t_traverse1 << " us" << endl;

      if (is_empty == true) {
        cout << "identified empty result query." << endl;
        cout << "query planning is finished." << endl;
        return false;
      }

      // check for middle heuristic, swap the direction
      /*if (min_path[0] == min_path[5]) {
        int count_s = statistic->global_pscount[min_path[0]];
        int count_o = statistic->global_pocount[min_path[0]];
        if ((min_path[2] == 0 && count_s < count_o) || (min_path[2] == 1 && count_s > count_o)) {
          min_path[2] = 1 - min_path[2];
          min_path[6] = 1 - min_path[6];
          min_path[3] = min_path[7];
          min_path[7] = min_path[4];
          min_path[4] = min_path[3];
        }
      }*/

      uint64_t t_convert1 = timer::get_usec();
      boost::unordered_map<int,int> convert;
      for (int i = 0, ilimit = min_path.size(); i < ilimit; i++) {
        if (min_path[i] < 0 ){
          if (convert.find(min_path[i]) == convert.end()) {
            int value =  -1 - convert.size();
            convert[min_path[i]] = value;
            min_path[i] = value;
          } else {
            min_path[i] = convert[min_path[i]];
          }
        }
      }
      uint64_t t_convert2 = timer::get_usec();
      //cout << "convert time : " << t_convert2 - t_convert1 << " us" << endl;

      //for (int i = 0, ilimit = r.cmd_chains.size(); i < ilimit; i = i + 4)
      //  cout << "cmd_chain " << " : " << r.cmd_chains[i] << " "
      //    << r.cmd_chains[i+1] << " "
      //    << r.cmd_chains[i+2] << " "
      //    << r.cmd_chains[i+3] << " "
      //    << "ps : " << statistic->global_pscount[r.cmd_chains[i+1]]
      //    << " pt : " << statistic->global_ptcount[r.cmd_chains[i+1]]
      //    << " po : " << statistic->global_pocount[r.cmd_chains[i+1]]
      //    << endl;

      //for (int i = 0, ilimit = min_path.size(); i < ilimit; i = i + 4)
      //  cout << "min_path " << " : " << min_path[i] << " "
      //    << min_path[i+1] << " "
      //    << min_path[i+2] << " "
      //    << min_path[i+3] << endl;
      cout << "query planning is finished." << endl;
      cout << "estimated cost: " << min_cost << endl;

      r.cmd_chains = min_path;
      return true;

    }

    bool generate_dp_plan(request_or_reply& r, data_statistic* statistic) {
      this->statistic = statistic;
      min_path.clear();
      min_cost = std::numeric_limits<double>::max();
      is_empty = false;

      cout << "using DP query planning." << endl;
      this->triples = r.cmd_chains;
      dp_plan();
      //cout << "min_path size : " << min_path.size() << endl;

      if (is_empty == true) return false;

      //cout << "before convert: " << endl;
      //for (int i = 0, ilimit = min_path.size(); i < ilimit ; i = i + 4)
      //  cout << "min_path " << " : " << min_path[i] << " "
      //    << min_path[i+1] << " "
      //    << min_path[i+2] << " "
      //    << min_path[i+3] << endl;

      boost::unordered_map<int,int> convert;
      for (int i = 0, ilimit = min_path.size(); i < ilimit ; i++) {
        if (min_path[i] < 0 ){
          if (convert.find(min_path[i]) == convert.end()) {
            int value =  -1 - convert.size();
            convert[min_path[i]] = value;
            min_path[i] = value;
          } else {
            min_path[i] = convert[min_path[i]];
          }
        }
      }
      //for (int i = 0, ilimit = r.cmd_chains.size(); i < ilimit ; i = i + 4)
      //  cout << "cmd_chain " << " : " << r.cmd_chains[i] << " "
      //    << r.cmd_chains[i+1] << " "
      //    << r.cmd_chains[i+2] << " "
      //    << r.cmd_chains[i+3] << " "
      //    << "ps : " << statistic->global_pscount[r.cmd_chains[i+1]]
      //    << " pt : " << statistic->global_ptcount[r.cmd_chains[i+1]]
      //    << " po : " << statistic->global_pocount[r.cmd_chains[i+1]]
      //    << endl;
      //for (int i = 0, ilimit = min_path.size(); i < ilimit ; i = i + 4)
      //  cout << "min_path " << " : " << min_path[i] << " "
      //    << min_path[i+1] << " "
      //    << min_path[i+2] << " "
      //    << min_path[i+3] << endl;
      cout << "estimated cost: " << min_cost << endl;

      r.cmd_chains = min_path;
      return true;
    }

    // for test
    vector<plan> all_plans;
    void test_planner(request_or_reply& r, data_statistic* statistic) {
      this->statistic = statistic;
      min_path.clear();
      path.clear();
      all_plans.clear();
      is_empty = false;
      double cost = 0;
      min_cost = std::numeric_limits<double>::max();
      unordered_map<int, vector<int> > max_selec;

      get_all_plans(r.cmd_chains, max_selec, cost, 0);
      cout << "plan space size: " << all_plans.size() << endl;

      cout << "get_all_plans is finish.\n";
    }

};