// Implementation of the GCPR 2016 Paper "Joint Object Pose Estimation and
// Shape Reconstruction in Urban Street Scenes Using 3D Shape Priors" by Engelmann et al.
// Copyright (C) 2016  Francis Engelmann - Visual Computing Institute RWTH Aachen University
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.

// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

// Eigen includes
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Geometry>
#include <Eigen/SVD>

// C/C++ includes
#include <iostream>
#include <tuple>

// Own includes
#include "geometry.h"

namespace gvl {

// See http://www.cs.virginia.edu/~gfx/Courses/1999/intro.fall99.html/lookat.html
// for implementation details.
Eigen::Matrix4d computeLookAtMatrix(const Eigen::Vector3d& I,
                                    const Eigen::Vector3d& E,
                                    const Eigen::Vector3d& U)
{
  Eigen::Vector3d F = I-E;
  Eigen::Vector3d f = F/F.norm();
  Eigen::Vector3d u = U/U.norm();
  Eigen::Vector3d s = f.cross(u);
  Eigen::Vector3d w = s.cross(f);

  Eigen::Matrix<double,4,4> M = Eigen::Matrix<double,4,4>::Identity();
  M(0,0) = s(0); M(0,1) = s(1); M(0,2) = s(2);
  M(1,0) = w(0); M(1,1) = w(1); M(1,2) = w(2);
  M(2,0) = -f(0); M(2,1) = -f(1); M(2,2) = -f(2);

  Eigen::Matrix<double,4,4> T = Eigen::Matrix<double,4,4>::Identity();
  T(0,3) = -E(0); T(1,3) = -E(1); T(2,3) = -E(2);

  return M*T;
}

Eigen::Matrix3d computeIntrinsicMatrix(const double f_x,
                                       const double f_y,
                                       const double p_x,
                                       const double p_y,
                                       const double s)
{
  Eigen::Matrix<double,3,3> K = Eigen::Matrix<double,3,3>::Identity();
  K(0,0) = f_x;
  K(1,1) = f_y;
  K(0,2) = p_x;
  K(1,2) = p_y;
  K(0,1) = s;
  return K;
}

// See http://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
Eigen::Matrix4d computeTransformationFromPlane(const double a,
                                               const double b,
                                               const double c,
                                               const double d)
{
  Eigen::Vector3d n; n << a,b,c;
  Eigen::Vector3d m; m << 0,-1,0;
  Eigen::Vector3d v; v = n.cross(m);
  double sin = v.norm(); // sine of angle
  double ccos = n.dot(m); // cosine of angle
  Eigen::Matrix3d v_x; v_x << 0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0;
  Eigen::Matrix3d R; R = Eigen::Matrix3d::Identity() + v_x + v_x*v_x*(1-ccos)/(sin*sin);
  Eigen::Vector3d t; t << 0, -d, 0;
  Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
  T.block<3,3>(0,0) = R;
  T.block<3,1>(0,3) = t;
  return T;
}

void computeVerticesFromDisparity(const cv::Mat& disparity,
                                  const Eigen::Matrix3d& K,
                                  const double b,
                                  cv::Mat& vertices)
{
  // Init variables
  const double max_depth = 1000;
  const double min_depth = 1;
  const double& baseline = b;
  const double& f = K(0,0);
  Eigen::Matrix3d Kinv = K.inverse();
  vertices = cv::Mat(disparity.rows, disparity.cols, CV_64FC3, cv::Scalar(0,0,0));

  // Iterate over disparity pixels, compute corresponding vertex
  for (int v=0; v<disparity.rows; v++) {
    for (int u=0; u<disparity.cols; u++) {
      // Compute depth
      double disp = (double)((uint16_t)(disparity.at<unsigned short>(v,u)))/256.0;
      double depth = (baseline*f)/(disp); //256 according to spspstereo
      if (depth < min_depth || depth > max_depth) continue;

      // Compute vertice
      //Eigen::Vector3d v3d = depth*Kinv*Eigen::Vector3d(u,v,1.0);
      vertices.at<cv::Vec3d>(v,u)[0] = depth*(Kinv(0,0)*u + Kinv(0,1)*v + Kinv(0,2));
      vertices.at<cv::Vec3d>(v,u)[1] = depth*(Kinv(1,0)*u + Kinv(1,1)*v + Kinv(1,2));
      vertices.at<cv::Vec3d>(v,u)[2] = depth*(Kinv(2,0)*u + Kinv(2,1)*v + Kinv(2,2));
    }
  }
}

void computeDisparityFromVertices(const cv::Mat& vertices,
                                  const Eigen::Matrix3d& K,
                                  const double b,
                                  cv::Mat& disparity)
{

  // Init variables
  double max_depth = 1000;
  double min_depth = 1;
  const double& baseline = b;
  const double& f = K(0,0);

  // Iterate over disparity pixels, compute corresponding vertex
  for (int v=0; v<vertices.rows; v++) {
    for (int u=0; u<vertices.cols; u++) {
      cv::Vec3d vertex = vertices.at<cv::Vec3d>(v,u);
      const double& depth = vertex[2];
      if (depth < min_depth || depth > max_depth) continue;
      double disp = (baseline*f)/depth;
      //unsigned short disp_old = disparity.at<unsigned short>(v,u);
      unsigned short disp_new = disp*256;
      disparity.at<unsigned short>(v,u) = disp_new;
    }
  }
}

void computeNormalsFromVertices(const cv::Mat& vertices, cv::Mat& normals)
{
  double neighbor_dist_threshold = 0.5;
  normals = cv::Mat(vertices.rows, vertices.cols, CV_64FC3, cv::Scalar(0,0,1) );

  // Iterate over vertices in image
  for (int v=1; v<normals.rows-1; v++) {
    for (int u=1; u<normals.cols-1; u++) {

      // Go over 8-neighborhood to compute #neighbors
      cv::Vec3d b = vertices.at<cv::Vec3d>(v,u); // center
      //unsigned char neighbor_mask = 0;  // binary mask to mark neighbors within distance threshold
      unsigned char neighbor_count = 0; // number of neighbors within distance threshold
      char neighbor_curr = -1;  // index of current neighor that is checked, used for mask
      cv::Vec3d centroid(0,0,0); // Accumulator to compute centroid
      Eigen::MatrixXd data_matrix(9,3);

      // Iterate over 8-neighborhood
      for (int x=u-1; x<=u+1; x++) {
        for (int y=v-1; y<=v+1; y++) {
          neighbor_curr++;
          cv::Vec3d a = vertices.at<cv::Vec3d>(y,x);
          cv::Vec3d d = a-b;
          double squared_dist = (d).dot(d);

          // Only accept points within distance threshold
          if (squared_dist < neighbor_dist_threshold*neighbor_dist_threshold) {
            data_matrix(neighbor_count, 0) = a[0];
            data_matrix(neighbor_count, 1) = a[1];
            data_matrix(neighbor_count, 2) = a[2];
            neighbor_count++;
            //neighbor_mask = neighbor_mask | (1<<neighbor_curr);
            centroid += a;
          }
        }
      }

      // if hte number of neighbors is too small, we need at least 3 vertices
      if (neighbor_count < 3) continue; // default normal (0,0,1) is assigned, see init

      // Compute centroid and resize data_matrix
      centroid /= (double)neighbor_count;
      data_matrix.resize(neighbor_count, 3);

      // Subtract mean from data_matrix
      for (int i=0; i<neighbor_count; i++) {
        data_matrix(i,0) -= centroid[0];
        data_matrix(i,1) -= centroid[1];
        data_matrix(i,2) -= centroid[2];
      }

      // Last eigenvector corresponds to normal
      Eigen::JacobiSVD<Eigen::MatrixXd> svd(data_matrix, Eigen::ComputeThinU | Eigen::ComputeFullV);

      /*double ev0 = svd.singularValues()[0];
      double ev1 = svd.singularValues()[1];
      double ratio = ev0/ev1;
      if (ratio>16) continue; // default normal (0,0,1) is assigned, see init*/

      Eigen::Vector3d n = svd.matrixV().col(2);
      n /= n.norm();

      // Get the correct orientation, assuming all points are visible from origin
      Eigen::Vector3d vector; vector << b[0],b[1],b[2];
      vector/=vector.norm();
      double dot = vector.dot(n);
      if (dot>0) n*=-1;

      cv::Vec3d normal;
      normal[0] = n[0];
      normal[1] = n[1];
      normal[2] = n[2];
      normals.at<cv::Vec3d>(v,u) = normal;

    }
  }
}

void computeNormalsFromVerticesSimple(const cv::Mat& vertices, cv::Mat& normals)
{
  normals = cv::Mat(vertices.rows, vertices.cols, CV_64FC3, cv::Scalar(0,0,0) );
  for (int v=1; v<normals.rows-1; v++) {
    for (int u=1; u<normals.cols-1; u++) {

      cv::Vec3d v1 = vertices.at<cv::Vec3d>(v+1,u);
      cv::Vec3d v2 = vertices.at<cv::Vec3d>(v,u);
      cv::Vec3d v3 = vertices.at<cv::Vec3d>(v,u+1);

      Eigen::Vector3d v1_; v1_ << v1[0],v1[1],v1[2];
      Eigen::Vector3d v2_; v2_ << v2[0],v2[1],v2[2];
      Eigen::Vector3d v3_; v3_ << v3[0],v3[1],v3[2];
      Eigen::Vector3d n = (v1_-v2_).cross(v3_-v2_);
      n /= n.norm();
      cv::Vec3d normal; normal[0] = n[0]; normal[1] = n[1]; normal[2] = n[2];
      normals.at<cv::Vec3d>(v,u) = normal;
    }
  }
}

void computePointcloudFromVerticesAndColor(const cv::Mat &vertices,
                                           const cv::Mat &colors,
                                           gvl::Pointcloud& pointcloud)
{
  // Check that vertices and colors have the same size
  assert(vertices.cols == colors.cols && vertices.rows == colors.rows);

  // Allocate as many points as pixels in the image,
  // this allows to address the 3d-points by 2d-coordinates
  pointcloud.points.resize(vertices.cols*vertices.rows);

  for (int v=0; v<vertices.rows; v++) {
    for (int u=0; u<vertices.cols; u++) {
      gvl::Point point;
      point.x = (double)vertices.at<cv::Vec3d>(v,u)[0];
      point.y = (double)vertices.at<cv::Vec3d>(v,u)[1];
      point.z = (double)vertices.at<cv::Vec3d>(v,u)[2];
      point.r = (unsigned char)colors.at<cv::Vec3b>(v,u)[2];
      point.g = (unsigned char)colors.at<cv::Vec3b>(v,u)[1];
      point.b = (unsigned char)colors.at<cv::Vec3b>(v,u)[0];
      point.u = u;
      point.v = v;
      int index = v*vertices.cols + u;
      pointcloud.points.at(index) = point;
    }
  }
}

void computeBoundingBoxFromPointcloud(const Pointcloud &pointcloud, BoundingBox& bb)
{
  double min_x=900, max_x=-900;
  double min_y=900, max_y=-900;
  double min_z=900, max_z=-900;
  for (auto p : pointcloud.points) {
    if (p.x > max_x) max_x=p.x;
    if (p.y > max_y) max_y=p.y;
    if (p.z > max_z) max_z=p.z;
    if (p.x < min_x) min_x=p.x;
    if (p.y < min_y) min_y=p.y;
    if (p.z < min_z) min_z=p.z;
  }
  bb.height = max_y - min_y;
  bb.width = max_x - min_x;
  bb.length = max_z - min_z;
  bb.x = (max_x + min_x)/2.0;
  bb.y = max_y;
  bb.z = (max_z + min_z)/2.0;
  bb.rotation_y=0;
}

double computeSquaredDistance(const Point& p1, const Point& p2)
{
  double d1 = (p1.x-p2.x);
  double d2 = (p1.y-p2.y);
  double d3 = (p1.z-p2.z);
  return d1*d1+d2*d2+d3*d3;
}

double bilinearInterpolation(const Eigen::Vector4d& values,
                             const Eigen::Vector2d& position) {
  const double& u = position[0];
  const double& v = position[1];
  return (1-u)*(1-v)*values[0] +
         (0+u)*(1-v)*values[1] +
         (1-u)*(0+v)*values[2] +
         (0+u)*(0+v)*values[3];
}

Eigen::Matrix4d computePoseFromRotTransScale(const double rotation_y,
                                             const Eigen::Vector3d& translation,
                                             const double scale)
{
  //Eigen::AngleAxisd aa(rotation_y, Eigen::Vector3d(0,1,0)); aa.matrix();
  Eigen::Matrix3d rotation = Eigen::Matrix3d::Identity();
  rotation(0,0) = std::cos(rotation_y);   rotation(0,2) = std::sin(rotation_y);
  rotation(2,0) = -std::sin(rotation_y);  rotation(2,2) = std::cos(rotation_y);
  Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
  pose.block<3,3>(0, 0) = rotation*scale;
  pose.block<3,1>(0, 3) = translation;
  return pose;
}

void computeBoundingBoxFromAnnotation(const Annotation &annotation,
                                      const Eigen::Vector3f &color,
                                      BoundingBox &bb)
{
  bb.height = 1.7;
  bb.width = 2.0;
  bb.length = 4.5;
  bb.x = annotation.translation[0];
  bb.y = annotation.translation[1];
  bb.z = annotation.translation[2];
  bb.rotation_y = annotation.rotation_y;
  bb.r = color[0];
  bb.g = color[1];
  bb.b = color[2];
  bb.score = 1.0;
}

double computeMedian(std::vector<double>& values) {
  std::sort(values.begin(), values.end());
  std::cout << "Median >> Values: "; for (auto t: values) std::cout << t << " "; std::cout << std::endl;
  double median;
  if (values.size()%2 == 1) {
    std::cout << "Median >> Size: " << values.size() << std::endl;
    median = values.at(values.size()/2);
  } else {
    median = 0.5*values.at(values.size()/2 - 1)+0.5*values.at(values.size()/2);;
  }
  std::cout << "Median: " << median << std::endl;
  return median;
}

double computeVerticalDistanceToPlane(const double a,
                                      const double b,
                                      const double c,
                                      const double d,
                                      const double x,
                                      const double z)
{
  double y = (-d-c*z-a*x)/b;
  return y;
}

// From: http://stackoverflow.com/questions/7685495/transforming-a-3d-plane-by-4x4-matrix
// This does NOT seem to work!!!
void transform_plane(const Eigen::Matrix4d &trafo, Eigen::Vector4d &p)
{
  std::cerr << "Transform_plane does NOT seem to work!!!" << std::endl;
  Eigen::Vector4d O = Eigen::Vector4d(p[0]*p[3], p[1]*p[3], p[2]*p[3], 1);
  Eigen::Vector4d N = Eigen::Vector4d(p[0], p[1], p[2], 0.0);
  O = trafo*O;
  N = (trafo.inverse()).transpose() * N;

  Eigen::Vector3d n = Eigen::Vector3d(N[0],N[1],N[2]);
  Eigen::Vector3d o = Eigen::Vector3d(O[0],O[1],O[2]);
  p[0] = N[0];
  p[1] = N[1];
  p[2] = N[2];
  p[3] = o.dot(n);

  //vector4 O = (xyz * d, 1)
  //vector4 N = (xyz, 0)
  //O = M * O
  //N = transpose(invert(M)) * N
  //xyz = N.xyz
  //d = dot(O.xyz, N.xyz)
}

}


// GAME - Geometric Algebra Multivector Estimation
//
// Copyright (c) 2015, Norwegian University of Science and 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 GAME 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) HOWEVE 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.

#include "ceres/autodiff_local_parameterization.h"
#include <Eigen/Core>
#include <ceres/autodiff_cost_function.h>
#include <game/vsr/cga_op.h>
#include <glog/logging.h>

using namespace vsr::cga;

const double kPi = 3.141592653589793238462643383279;

struct VectorCorrespondencesCostFunctor {
  VectorCorrespondencesCostFunctor(const Vec &a, const Vec &b) : a_(a), b_(b) {}

  template <typename T>
  auto operator()(const T *const rotor, T *residual) const -> bool {
    Rotor<T> R(rotor);
    Vector<T> a(a_);
    Vector<T> b(b_);
    Vector<T> c = a.spin(R);

    for (int i = 0; i < 3; ++i) {
      residual[i] = c[i] - b[i];
    }

    return true;
  }

private:
  const Vec a_;
  const Vec b_;
};

struct RotorPlus {
  template <typename T>
  bool operator()(const T *x, const T *delta, T *x_plus_delta) const {
    const T squared_norm_delta =
        delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
    T r_delta[4];
    if (squared_norm_delta > T(0.0)) {
      T norm_delta = sqrt(squared_norm_delta);
      const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
      r_delta[0] = cos(norm_delta);
      r_delta[1] = sin_delta_by_delta * delta[0];
      r_delta[2] = sin_delta_by_delta * delta[1];
      r_delta[3] = sin_delta_by_delta * delta[2];
    } else {
      // We do not just use r_delta = [1,0,0,0] here because that is a
      // constant and when used for automatic differentiation will
      // lead to a zero derivative. Instead we take a first order
      // approximation and evaluate it at zero.
      r_delta[0] = T(1.0);
      r_delta[1] = delta[0];
      r_delta[2] = delta[1];
      r_delta[3] = delta[2];
    }

    Rotor<T> rotor = Rotor<T>{r_delta[0], r_delta[1], r_delta[2], r_delta[3]} *
                     Rotor<T>{x[0], x[1], x[2], x[3]};

    for (int i = 0; i < 4; ++i)
      x_plus_delta[i] = rotor[i];

    return true;
  }
};

int main(int argc, char **argv) {

  google::InitGoogleLogging(argv[0]);

  double theta_half{kPi / 6.0};
  Rot rotor{cos(theta_half), -sin(theta_half), 0.0, 0.0};
  Vec a{1.0, 0.0, 0.0};
  Vec b{0.0, 1.0, 0.0};

  Eigen::Matrix<double, 3, 4, Eigen::RowMajor> global_jacobian;
  Eigen::Matrix<double, 4, 3, Eigen::RowMajor> local_jacobian;
  Eigen::Matrix<double, 3, 3, Eigen::RowMajor> jacobian;
  Eigen::Matrix<double, 1, 3> result;

  const double *parameters[1] = {rotor.begin()};
  double *global_jacobian_array[1] = {global_jacobian.data()};

  ceres::AutoDiffCostFunction<VectorCorrespondencesCostFunctor, 3, 4>(
      new VectorCorrespondencesCostFunctor(a, b))
      .Evaluate(parameters, result.data(), global_jacobian_array);

  ceres::AutoDiffLocalParameterization<RotorPlus, 4, 3>(new RotorPlus())
      .ComputeJacobian(rotor.begin(), local_jacobian.data());

  jacobian = global_jacobian * local_jacobian;

  std::cout << "Jacobian of the function F = R * a * ~R - b where" << std::endl;
  std::cout << "R is a Euclidean rotor with coefficients:" << std::endl;
  std::cout << "R: " << rotor << std::endl;
  std::cout << "and a and b are vectors with coefficients:" << std::endl;
  std::cout << "a: " << a << std::endl;
  std::cout << "b: " << b << std::endl;
  std::cout << "The resulting vector have coefficients:" << std::endl;
  std::cout << result << std::endl;
  std::cout << std::endl;
  std::cout << "The resulting 3x4 global jacobian:" << std::endl;
  std::cout << global_jacobian << std::endl;
  std::cout << "The 3x3 local jacobian:" << std::endl;
  std::cout << local_jacobian << std::endl;
  std::cout << "The final 3x3 jacobian:" << std::endl;
  std::cout << jacobian << std::endl;

  return 0;
}


#include <iostream>
#include <fstream>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "ceres/ceres.h"
#include "glog/logging.h"
#include <pangolin/pangolin.h>

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solve;
using ceres::Solver;
using Eigen::Isometry3d;
using Eigen::Vector3d;
using Eigen::Quaterniond;
using Eigen::Matrix3d;
using std::vector;

const double DT = 1.0 / 18;
// const Eigen::Vector3d GRAVITY{0, 0, 0};
const Eigen::Vector3d GRAVITY{0, 0, -9.8};

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);
void DrawTrajectoryComparison(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>,
                              vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);
struct State {
  Vector3d pos = Vector3d::Random();
  Vector3d vel = Vector3d::Random();
  Quaterniond q = Quaterniond::UnitRandom();
  Vector3d bias = Vector3d::Zero();
  State() {}

  State(Vector3d& pos, Vector3d& vel, Quaterniond& q, Vector3d& bias)
    : pos(pos), vel(vel), q(q), bias(bias) {}

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

struct Measurement{
  Matrix3d Rwr;
  Quaterniond qwr;
  Vector3d twr;
  Vector3d acc;
  Vector3d omega;

  Measurement(Matrix3d Rwr,
              Quaterniond qwr,
              Vector3d twr,
              Vector3d acc,
              Vector3d omega)
    : Rwr(Rwr), qwr(qwr), twr(twr), acc(acc), omega(omega) {}

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

struct StateError {
  StateError(const Vector3d& pos_state,
						 const Vector3d& vel_state,
             const Quaterniond& q_state,
             const Vector3d& bias_state,
             const Eigen::Matrix<double, 12, 12>& sqrt_cov)
    : pos_state_(pos_state), vel_state_(vel_state), q_state_(q_state), bias_state_(bias_state), sqrt_cov_(sqrt_cov) {}

  template <typename T>
  bool operator()(const T* const pos_hat_ptr,
                  const T* const vel_hat_ptr,
                  const T* const q_hat_ptr,
                  const T* const bias_hat_ptr,
                  T* residuals_ptr) const {
    Eigen::Matrix<T, 12, 1> residuals;

    Eigen::Matrix<T, 3, 1> pos_hat(pos_hat_ptr);
    Eigen::Matrix<T, 3, 1> vel_hat(vel_hat_ptr);
    Eigen::Quaternion<T> q_hat(q_hat_ptr);
    Eigen::Matrix<T, 3, 1> bias_hat(bias_hat_ptr);

    // pos error
    residuals.template block<3, 1>(0, 0) = pos_hat - pos_state_.template cast<T>();

    // vel error
    residuals.template block<3, 1>(3, 0) = vel_hat - vel_state_.template cast<T>();

    // quat error
    Eigen::Quaternion<T> q_delta = q_state_.conjugate().template cast<T>() * q_hat;
    residuals.template block<3, 1>(6, 0) = q_delta.vec();

    // bias error
    double bias_weight = 1.5;
    residuals.template block<3, 1>(9, 0) = T(bias_weight) * (bias_hat - bias_state_.template cast<T>());

    // marginal factor
    residuals = sqrt_cov_ * residuals;
    for (int i = 0; i < residual_size; i++) {
      residuals_ptr[i] = residuals[i];
    }
    return true;
  }

  static CostFunction* Create(const Vector3d& pos_state,
															const Vector3d& vel_state,
															const Quaterniond& q_state,
                              const Vector3d& bias_state,
                              const Eigen::Matrix<double, 12, 12>& sqrt_cov) {
    return new AutoDiffCostFunction<StateError, 12, 3, 3, 4, 3>(
      new StateError(pos_state, vel_state, q_state, bias_state, sqrt_cov));
  }

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  int residual_size = 12;
  const Vector3d pos_state_;
  const Vector3d vel_state_;
  const Quaterniond q_state_;
  const Vector3d bias_state_;
  const Eigen::Matrix<double, 12, 12> sqrt_cov_;
};

struct PoseError {
  PoseError(const Eigen::Vector3d& pos_measured,
            const Eigen::Quaterniond& q_measured)
    : pos_measured_(pos_measured), q_measured_(q_measured) {}

  template <typename T>
  bool operator()(const T* const pos_hat_ptr,
                  const T* const q_hat_ptr,
                  T* residuals_ptr) const {
    Eigen::Matrix<T, 6, 1> residuals;

    Eigen::Matrix<T, 3, 1> pos_hat(pos_hat_ptr);
    Eigen::Quaternion<T> q_hat(q_hat_ptr);

    // pos error
    Eigen::Matrix<T, 3, 1> pos_delta;
    residuals.template block<3, 1>(0, 0) = pos_hat - pos_measured_.template cast<T>();

    // quat error
    Eigen::Quaternion<T> q_delta = q_measured_.conjugate().template cast<T>() * q_hat;
    residuals.template block<3, 1>(3, 0) = q_delta.vec();
    for (int i = 0; i < 6; i++) {
      residuals_ptr[i] = residuals[i];
    }
    return true;
  }

  static CostFunction* Create(const Eigen::Vector3d& pos_measured,
                              const Eigen::Quaterniond& q_measured) {
    return new AutoDiffCostFunction<PoseError, 6, 3, 4>(
      new PoseError(pos_measured, q_measured));
  }

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  const Eigen::Vector3d pos_measured_;
  const Eigen::Quaterniond q_measured_;
};

struct PredictionError{
  PredictionError(const Eigen::Vector3d& acc_measured,
                  const Eigen::Vector3d& omega_measured)
    : acc_measured_(acc_measured), omega_measured_(omega_measured) {}

  template <typename T>
  bool operator()(const T* const pos_b_ptr,
                  const T* const vel_b_ptr,
                  const T* const q_b_ptr,
                  const T* const bias_b_ptr,
                  const T* const pos_e_ptr,
                  const T* const vel_e_ptr,
                  const T* const q_e_ptr,
                  const T* const bias_e_ptr,
                  T* residuals_ptr) const {
    Eigen::Matrix<T, 12, 1> residuals;

    const Eigen::Matrix<T, 3, 1> pos_b(pos_b_ptr);
    const Eigen::Matrix<T, 3, 1> pos_e(pos_e_ptr);
    const Eigen::Matrix<T, 3, 1> vel_b(vel_b_ptr);
    const Eigen::Matrix<T, 3, 1> vel_e(vel_e_ptr);
    const Eigen::Quaternion<T> q_b(q_b_ptr);
    const Eigen::Quaternion<T> q_e(q_e_ptr);
    const Eigen::Matrix<T, 3, 1> bias_b(bias_b_ptr);
    const Eigen::Matrix<T, 3, 1> bias_e(bias_e_ptr);

    // pos error
    residuals.template block<3, 1>(0, 0) = pos_b + vel_b * DT - pos_e;

    // vel error
    residuals.template block<3, 1>(3, 0) = vel_b + (q_b * (acc_measured_.template cast<T>() - bias_b) - GRAVITY) * DT - vel_e;

    // quat errorsqrt_cov
    Eigen::Quaternion<T> q_new;
    Eigen::Quaternion<T> q_add;

    // // https://gamedev.stackexchange.com/questions/108920/applying-angular-velocity-to-quaternion
    // Eigen::Quaternion<T> q_omega;
    // // q_omega.w() = 0;
    // q_omega.vec() = omega_measured_.template cast<T>() * DT * 0.5;
    // q_add = q_omega * q_b;
    // q_new.w() = q_b.w() + q_add.w();
    // q_new.vec() = q_b.vec() + q_add.vec();

    Eigen::Vector3d rotated = omega_measured_ * DT;
    double angle = rotated.norm();
    Eigen::Vector3d axis = rotated.normalized();
    q_add = Eigen::AngleAxisd(angle, axis).template cast<T>();
    q_new = q_b * q_add;

    Eigen::Quaternion<T> q_delta = q_e.conjugate() * q_new;
    residuals.template block<3, 1>(6, 0) = q_delta.vec();

    // bias error
    residuals.template block<3, 1>(9, 0) = bias_e - bias_b;

    for (int i = 0; i < residual_size; i++) {
      residuals_ptr[i] = residuals[i];
    }
    return true;
  }

  static CostFunction* Create(const Eigen::Vector3d& acc_measured,
                              const Eigen::Vector3d& omega_measured) {
    return new AutoDiffCostFunction<PredictionError, 12, 3, 3, 4, 3, 3, 3, 4, 3>(
      new PredictionError(acc_measured, omega_measured));
  }

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  int residual_size = 12;
  const Eigen::Vector3d acc_measured_;
  const Eigen::Vector3d omega_measured_;
};

void print_state(const State& state) {
  Eigen::AngleAxisd ori(state.q);

  std::cout << "state pos: \n" << state.pos << "\n"
            << "state vel: \n" << state.vel << "\n"
            << "state ori: \n" << ori.angle() << " " << ori.axis() << "\n"
            << "state bias: \n" << state.bias << std::endl;
}

class FixLagSmoother {
public:
	std::vector<State, Eigen::aligned_allocator<State>> get_all_states() {
		return all_states;
	}

	bool step(Measurement& measurement) {
		all_states.push_back(State());
		int state_num = all_states.size();
		int marginal_idx = state_num - wind_size;
    std::cout << "current state_num: " << state_num << "marginal_idx: " << marginal_idx << std::endl;

    Problem problem;
		ceres::LossFunction* loss_function = nullptr;
		ceres::LocalParameterization* quaternion_local_parameterization =
				new ceres::EigenQuaternionParameterization;

		ceres::CostFunction* marginal_cost_function = StateError::Create(all_states[marginal_idx].pos,
																																		 all_states[marginal_idx].vel,
																																		 all_states[marginal_idx].q,
																																		 all_states[marginal_idx].bias,
                                                                     sqrt_cov);
		problem.AddResidualBlock(marginal_cost_function,
														 loss_function,
														 all_states[marginal_idx].pos.data(),
														 all_states[marginal_idx].vel.data(),
														 all_states[marginal_idx].q.coeffs().data(),
														 all_states[marginal_idx].bias.data());
    problem.SetParameterization(all_states[marginal_idx].q.coeffs().data(),
                                quaternion_local_parameterization);
		for (int i = marginal_idx + 1; i < state_num; i++) {
			ceres::CostFunction* pos_cost_function = PoseError::Create(measurement.twr,
																																 measurement.qwr);
			problem.AddResidualBlock(pos_cost_function,
															 loss_function,
															 all_states[i].pos.data(),
															 all_states[i].q.coeffs().data());
			problem.SetParameterization(all_states[i].q.coeffs().data(),
																	quaternion_local_parameterization);
			ceres::CostFunction* pred_cost_function = PredictionError::Create(measurement.acc,
																																				measurement.omega);
			problem.AddResidualBlock(pred_cost_function,
															 loss_function,
															 all_states[i - 1].pos.data(),
															 all_states[i - 1].vel.data(),
															 all_states[i - 1].q.coeffs().data(),
															 all_states[i - 1].bias.data(),
															 all_states[i].pos.data(),
															 all_states[i].vel.data(),
															 all_states[i].q.coeffs().data(),
															 all_states[i].bias.data());
			problem.SetParameterization(all_states[i - 1].q.coeffs().data(),
																  quaternion_local_parameterization);
			problem.SetParameterization(all_states[i].q.coeffs().data(),
																	quaternion_local_parameterization);
		};

		ceres::Solver::Options options;
		options.max_num_iterations = 200;
		options.linear_solver_type = ceres::DENSE_SCHUR;
		// options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
		options.minimizer_progress_to_stdout = true;

		ceres::Solver::Summary summary;
		ceres::Solve(options, &problem, &summary);
		std::cout << "Iteration: " << marginal_idx + 1 << "\n" << summary.FullReport() << "\n";
    print_state(all_states[marginal_idx]);

		// ceres covariance matrix estimation
		update_marginal_llt(marginal_idx + 1, problem);
	}

	bool update_marginal_llt(int marginal_idx, ceres::Problem& problem) {
		ceres::Covariance::Options options;
		ceres::Covariance covariance(options);

		std::vector<std::pair<const double*, const double*>> covariance_blocks;
		covariance_blocks.push_back(std::make_pair(all_states[marginal_idx].pos.data(), all_states[marginal_idx].pos.data()));
		covariance_blocks.push_back(std::make_pair(all_states[marginal_idx].pos.data(), all_states[marginal_idx].vel.data()));
		covariance_blocks.push_back(std::make_pair(all_states[marginal_idx].pos.data(), all_states[marginal_idx].q.coeffs().data()));
		covariance_blocks.push_back(std::make_pair(all_states[marginal_idx].pos.data(), all_states[marginal_idx].bias.data()));
		covariance_blocks.push_back(std::make_pair(all_states[marginal_idx].vel.data(), all_states[marginal_idx].vel.data()));
		covariance_blocks.push_back(std::make_pair(all_states[marginal_idx].vel.data(), all_states[marginal_idx].q.coeffs().data()));
		covariance_blocks.push_back(std::make_pair(all_states[marginal_idx].vel.data(), all_states[marginal_idx].bias.data()));
		covariance_blocks.push_back(std::make_pair(all_states[marginal_idx].q.coeffs().data(), all_states[marginal_idx].q.coeffs().data()));
		covariance_blocks.push_back(std::make_pair(all_states[marginal_idx].q.coeffs().data(), all_states[marginal_idx].bias.data()));
		covariance_blocks.push_back(std::make_pair(all_states[marginal_idx].bias.data(), all_states[marginal_idx].bias.data()));

		ceres::LocalParameterization* quaternion_local_parameterization =
				new ceres::EigenQuaternionParameterization;
    problem.SetParameterization(all_states[marginal_idx].q.coeffs().data(),
                                quaternion_local_parameterization);
		CHECK(covariance.Compute(covariance_blocks, &problem));

    double covariance_pp[3 * 3];
    double covariance_pv[3 * 3];
    double covariance_pq[3 * 3];
    double covariance_pb[3 * 3];
    double covariance_vv[3 * 3];
    double covariance_vq[3 * 3];
    double covariance_vb[3 * 3];
    double covariance_qq[3 * 3];
    double covariance_qb[3 * 3];
    double covariance_bb[3 * 3];
		covariance.GetCovarianceBlock(all_states[marginal_idx].pos.data(), all_states[marginal_idx].pos.data(), covariance_pp);
		covariance.GetCovarianceBlock(all_states[marginal_idx].pos.data(), all_states[marginal_idx].vel.data(), covariance_pv);
		covariance.GetCovarianceBlockInTangentSpace(all_states[marginal_idx].pos.data(), all_states[marginal_idx].q.coeffs().data(), covariance_pq);
		covariance.GetCovarianceBlock(all_states[marginal_idx].pos.data(), all_states[marginal_idx].bias.data(), covariance_pb);
		covariance.GetCovarianceBlock(all_states[marginal_idx].vel.data(), all_states[marginal_idx].vel.data(), covariance_vv);
		covariance.GetCovarianceBlockInTangentSpace(all_states[marginal_idx].vel.data(), all_states[marginal_idx].q.coeffs().data(), covariance_vq);
		covariance.GetCovarianceBlock(all_states[marginal_idx].vel.data(), all_states[marginal_idx].bias.data(), covariance_vb);
		covariance.GetCovarianceBlockInTangentSpace(all_states[marginal_idx].q.coeffs().data(), all_states[marginal_idx].q.coeffs().data(), covariance_qq);
		covariance.GetCovarianceBlockInTangentSpace(all_states[marginal_idx].q.coeffs().data(), all_states[marginal_idx].bias.data(), covariance_qb);
		covariance.GetCovarianceBlock(all_states[marginal_idx].bias.data(), all_states[marginal_idx].bias.data(), covariance_bb);

		Eigen::Matrix<double, 12, 12> cov;
		cov.block<3, 3>(0, 0) = Eigen::Matrix3d(covariance_pp);
		cov.block<3, 3>(0, 3) = Eigen::Matrix3d(covariance_pv);
		cov.block<3, 3>(0, 6) = Eigen::Matrix3d(covariance_pq);
		cov.block<3, 3>(0, 9) = Eigen::Matrix3d(covariance_pb);
		cov.block<3, 3>(3, 0) = Eigen::Matrix3d(covariance_pv).transpose();
		cov.block<3, 3>(3, 3) = Eigen::Matrix3d(covariance_vv);
		cov.block<3, 3>(3, 6) = Eigen::Matrix3d(covariance_vq);
		cov.block<3, 3>(3, 9) = Eigen::Matrix3d(covariance_vb);
		cov.block<3, 3>(6, 0) = Eigen::Matrix3d(covariance_pq).transpose();
		cov.block<3, 3>(6, 3) = Eigen::Matrix3d(covariance_pv).transpose();
		cov.block<3, 3>(6, 6) = Eigen::Matrix3d(covariance_qq);
		cov.block<3, 3>(6, 9) = Eigen::Matrix3d(covariance_qb);
		cov.block<3, 3>(9, 0) = Eigen::Matrix3d(covariance_pb).transpose();
		cov.block<3, 3>(9, 3) = Eigen::Matrix3d(covariance_vb).transpose();
		cov.block<3, 3>(9, 6) = Eigen::Matrix3d(covariance_qb).transpose();
		cov.block<3, 3>(9, 9) = Eigen::Matrix3d(covariance_bb);

    std::cout << "cov: \n" << cov << std::endl;
    std::cout << "cov_inv: \n" << cov.inverse() << std::endl;

    // (x_2 - x_2_hat).T * cov_inv(H) * (x_2 - x_2_hat)
    // cov_inv = U.T * U = L * L.T
    // b = U * (x_2 - x_2_hat)
    // cost = b.T * b
    Eigen::LLT<Eigen::Matrix<double, 12, 12>> lltOfcovinv(cov.inverse()); // compute the Cholesky decomposition
    sqrt_cov = lltOfcovinv.matrixU();

    // sqrt_cov.block<3, 3>(9, 9) = Matrix3d::Identity();
    std::cout << "sqrt_cov: \n" << sqrt_cov << std::endl;
    return true;
	}

	bool initialize(const int& wind_size, const State& init_state) {
		this->wind_size = wind_size;
    all_states.push_back(init_state);
    return true;
	}

private:
	int wind_size = 2;
  Eigen::Matrix<double, 12, 12> sqrt_cov = Eigen::Matrix<double, 12, 12>::Identity();
	Eigen::Vector3d bias = Eigen::Vector3d::Zero();
	Eigen::Matrix3d state_hessian;
	Eigen::Matrix3d state_b;
	std::vector<State, Eigen::aligned_allocator<State>> all_states;
};

void save_states(const std::string& filename,
                 std::vector<State, Eigen::aligned_allocator<State>>& states) {
  std::fstream outfile;
  outfile.open(filename.c_str(), std::istream::out);

  for (auto& state : states) {
    Eigen::Matrix3d rot = state.q.matrix();
    outfile << rot << "\n" << state.pos.transpose() << "\n" << state.vel.transpose() << "\n\n";
  }
}

std::vector<Measurement, Eigen::aligned_allocator<Measurement>> readSensorData(std::string path) {
  std::vector<Measurement, Eigen::aligned_allocator<Measurement>> ret;

  std::ifstream csvFile;
  csvFile.open(path);

  std::string line;
  while(std::getline(csvFile, line)) {
    std::vector<double> row;
    // std::cout << "line:" << line << std::endl;
    std::istringstream s(line);
    std::string field;
    while (std::getline(s, field,',')) {
      // std::cout << "field: " << field << std::endl;
      row.push_back(std::stod(field));
    }
    Eigen::Matrix3d Rwr;
    Eigen::Quaterniond qwr;
    Eigen::Vector3d twr;
    Eigen::Vector3d acc;
    Eigen::Vector3d omega;
    Rwr << row[0], row[1], row[2],
          row[4], row[5], row[6],
          row[8], row[9], row[10];
    qwr = Rwr;
    twr << row[3], row[7], row[11];
    acc << row[16], row[17], row[18];
    omega << row[19], row[20], row[21];
    // std::cout << "Rwr: " << Rwr << std::endl;
    // std::cout << "qwr: " << qwr.w() << " " << qwr.vec() << std::endl;
    // std::cout << "twr: " << twr << std::endl;
    // std::cout << "acc: " << acc << std::endl;
    // std::cout << "omega: " << omega << std::endl;

    ret.push_back(Measurement(Rwr, qwr, twr, acc, omega));
  }

  return ret;
}

double abs_pos_error(const std::vector<State, Eigen::aligned_allocator<State>>& states,
                     const std::vector<State, Eigen::aligned_allocator<State>>& gt_states) {
  double err = 0.0;
  std::cout << "abs_pos_err by step: ";
  for (int i = 0; i < states.size(); i++) {
    double step_err = (states[i].pos - gt_states[i].pos).norm();
    err += step_err;
    std::cout << step_err << " ";
  }
  std::cout << std::endl;
  return err;
}

int main(int argc, char** argv) {
  if(argc < 2) {
    std::cout << "missing arg for the csv file" << std::endl;
  }

  std::string path = argv[1];
  std::vector<Measurement, Eigen::aligned_allocator<Measurement>> data = readSensorData(path);
  int cnt = data.size();
  // cnt = 15;
	int wind_size = 2;

  std::vector<State, Eigen::aligned_allocator<State>> gt_states(cnt);
  std::cout << "states size: " << gt_states.size() << std::endl;

  for (int i = 0; i < gt_states.size(); i++) {
    gt_states[i].pos = data[i].twr;
    gt_states[i].vel = Eigen::Vector3d::Zero();
    gt_states[i].q = data[i].qwr;
  }
  gt_states[0].vel = Eigen::Vector3d({0, 94.25, 0});
	FixLagSmoother smoother;

  Vector3d init_vel({0, 0, 0});
  Vector3d init_bias({0, 0, 0});
  State init_state(data[0].twr, init_vel, data[0].qwr, init_bias);
	smoother.initialize(wind_size, init_state);

	for (int i = wind_size - 1; i < cnt; i++) {
		smoother.step(data[i]);
	}

  std::vector<State, Eigen::aligned_allocator<State>> states = smoother.get_all_states();

  std::string est_filename = "./results/increm_states.txt";
  save_states(est_filename, states);

  vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
  vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> gt_poses;
  for (int i = 0; i < cnt; i++) {
    Isometry3d Twr(states[i].q.matrix());
    Twr.pretranslate(states[i].pos / 20); // manually divided by 20 to zoom out
    poses.push_back(Twr);
  }
  for (int i = 0; i < cnt; i++) {
    Isometry3d Twr(gt_states[i].q.matrix());
    Twr.pretranslate(gt_states[i].pos / 20); // manually divided by 20 to zoom out
    gt_poses.push_back(Twr);
  }

  double err = abs_pos_error(states, gt_states);
  std::cout << "absolute position error: " << err << std::endl;
  DrawTrajectoryComparison(poses, gt_poses);
	return 0;
}

void DrawTrajectoryComparison(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses,
                              vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> gt_poses) {
    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
    .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
    .SetHandler(new pangolin::Handler3D(s_cam));

  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    glLineWidth(2);
    for (size_t i = 0; i < poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = poses[i].translation();
      Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < poses.size(); i++) {
      glColor3f(1.0, 0.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = poses[i], p2 = poses[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }

    for (size_t i = 0; i < gt_poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = gt_poses[i].translation();
      Vector3d Xw = gt_poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = gt_poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = gt_poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < gt_poses.size(); i++) {
      glColor3f(0.0, 1.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = gt_poses[i], p2 = gt_poses[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }
}

/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {
  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
    .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
    .SetHandler(new pangolin::Handler3D(s_cam));

  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    glLineWidth(2);
    for (size_t i = 0; i < poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = poses[i].translation();
      Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < poses.size(); i++) {
      glColor3f(0.0, 0.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = poses[i], p2 = poses[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }
}


#include <iostream>
#include <cmath>
#include <math.h>
#include <vector>
#include <array>
#include "GeneralizedHeat.hpp"
#include "TriDiagMatrix.hpp"
#include "MassMatrix.hpp"
#include "StiffnessMatrix.hpp"
#include <fstream>
#include <string>
#include <functional>
#include <boost/math/quadrature/gauss.hpp>
using namespace std;
using namespace boost::math::quadrature;

void GeneralHeat::EnergyNorm()
{
mpEnergyNorm.clear();
BuildGradientVec(mpx, mpsmesh, FEMGradient);

auto SquaredError = [this](double x)
    { return pow(GradientFunction(x) - mppde->AnalyticGradientWRTx(x, mptmesh.ReadTimeStep(mpcurrenTimeStep)), 2); };

for(int i=0; i<mpsmesh.meshsize(); i++)
{
mpEnergyNorm.push_back( gauss<double, 7>::integrate(SquaredError,
                                        mpsmesh.ReadSpaceNode(i), mpsmesh.ReadSpaceNode(i+1)) );

}
        double globalError=0;
        for(auto k: mpEnergyNorm)
            globalError = globalError + k;
        //std::cout << sqrt(globalError);
}

void GeneralHeat::AddVectors(std::vector<double>func1, std::vector<double> func2, std::vector<double>& result)
{
    if(func1.size()==func2.size())
    {
    result.clear();
     for ( int i=0; i<func1.size(); i++ )
     {
         result.push_back(func1.at(i)+func2.at(i));
     }
    }
    else
    {
        std::cout<< " your vectors are different sizes";
        std::cout<<"\n";
    }
}

void GeneralHeat::SetSpaceTimeMesh( SpaceMesh smesh, TimeMesh tmesh, APDE& apde )
{
    mpsmesh = smesh;
    mptmesh = tmesh;
    mppde = &apde;
    k_0 = mppde->k_0;
    k_L = mppde->k_L;
    g_0 = mppde->g_0;
    g_L = mppde->g_L;
    mpa = mppde->a;

    //std::cout<<apde.k_0<<"\n"<<apde.k_L<<"\n"<<mpa<<"\n"<<g_0<<"\n"<<g_L;
}

double GeneralHeat::ContinuousAnalyticSolution( double x, double t )
{
     return mppde->ContinuousAnalyticSolution( x, t );
}

void GeneralHeat::StationaryHeatEquation()
{
buildfvec( mpsmesh );

stiff.SetParameters(k_0, k_L, mpa);

stiff.BuildGeneralStiffnessMatrix ( mpsmesh );

BuiltbrVec();
AddVectors(br, f_vec, br);

stiff.MatrixSolver( br, mpx );
}

void GeneralHeat::buildfvec( SpaceMesh& a_smesh)
{
    double my_var = 0.5*a_smesh.ReadSpaceMesh(0);
    f_vec = {mppde->EllipticalRHSfunction(0)*my_var};

for(int i =1; i<mpsmesh.meshsize(); i++)
{
    my_var = 0.5*(a_smesh.ReadSpaceMesh(i)+a_smesh.ReadSpaceMesh(i-1));
    f_vec.push_back( mppde->EllipticalRHSfunction(a_smesh.ReadSpaceNode(i))*my_var );
}

my_var = 0.5*mpsmesh.ReadSpaceMesh(mpsmesh.meshsize()-1);
f_vec.push_back( mppde->EllipticalRHSfunction(a_smesh.ReadSpaceNode(a_smesh.meshsize()))*my_var );
}

void GeneralHeat::BuiltbrVec()
{
    br.assign(mpsmesh.meshsize()+1, 0);
    br.at(0) = k_0*g_0;
    br.at(mpsmesh.meshsize()) = k_L*g_L;
}

void GeneralHeat::AnalyticSolutionVec( )
{
    mpAnalyticSolution.clear();
     for (int i = 0; i<mpsmesh.meshsize()+1; i++)
{
    mpAnalyticSolution.push_back(ContinuousAnalyticSolution( mpsmesh.ReadSpaceNode(i),
                                                                        mptmesh.ReadTimeStep(mpcurrenTimeStep)));
}
}

void GeneralHeat::PrintSolution( )
{
        BuildGradientVec(mpx, mpsmesh, FEMGradient);
        GradientRecoveryFunction( mpsmesh, FEMGradient, GradientRecovery );
        BuildErrorEstimate();
        BuildErrorMesh();
        AnalyticSolutionVec();
//        AnalyticGradientVec();
        EnergyNorm();
        double globalError=0;
        for(auto k: ErrorEstimate)
            globalError = globalError + k;

        std::cout << "FEM Approximation:     ";
        PrintVector(mpx);
        std::cout << "Analytic Solution:     ";
        PrintVector(mpAnalyticSolution);
        std::cout << "Error Mesh:            ";
        PrintVector(mpErrorMesh);
        std::cout << "Global Error           ";
        GlobalSpaceError();
        std::cout << "FEM Grad approx:       ";
        PrintVector(FEMGradient);
        std::cout << "ErrorEstimate:         ";
        PrintVector(ErrorEstimate);
        std::cout << "GlobalErrorEstimate:   ";
        std::cout << sqrt(globalError)<<" \n";
        std::cout << "EnergyError:           ";
        PrintVector(mpEnergyNorm);
        std::cout << "Global Energy Error    ";
        GlobalEnergyError();
        std::cout << "\n";
}

void GeneralHeat::BuildErrorMesh()
{
mpErrorMesh.clear();

auto SquaredError = [this](double x)
    { return pow(GeneralInterpolant(x, mpx, mpsmesh ) -
    ContinuousAnalyticSolution(x, mptmesh.ReadTimeStep(mpcurrenTimeStep)), 2); };

double Q;
for(int i=0; i<mpsmesh.meshsize(); i++)
{
Q = gauss<double, 7>::integrate(SquaredError, mpsmesh.ReadSpaceNode(i), mpsmesh.ReadSpaceNode(i+1));
mpErrorMesh.push_back( Q );
}
}

double GeneralHeat::GlobalSpaceError()
{
    BuildErrorMesh();
    double globalError=0;
    for(auto k: mpErrorMesh)
        globalError = globalError + k;

    std::cout << sqrt(globalError);
    std::cout << " \n";
    return sqrt(globalError);
}

double GeneralHeat::GlobalEnergyError()
{
    EnergyNorm();
    double globalError=0;
    for(auto k: mpEnergyNorm)
        globalError = globalError + k;

    std::cout << sqrt(globalError);
    std::cout << " \n";
    return sqrt(globalError);

}

void GeneralHeat::PrintErrorMesh()
{
    BuildErrorMesh();
    PrintVector(mpErrorMesh);
}

void GeneralHeat::SolveWithBCs()
{
mpcurrenTimeStep = 0;
mpcurrentMeshIndex = 0;
//AnalyticSolutionVec();
//mpPreviousSolution = mpAnalyticSolution;
mppde->InitialCondition(mpsmesh, mpPreviousSolution);
stiff.SetParameters(k_0, k_L, mpa);

ofstream myfile;
ofstream myfile1;
myfile.open ("solution.csv");
myfile1.open ("X.csv");
for (auto k: mpPreviousSolution)
    myfile << k << ", ";
myfile << "\n";
for (auto k: mpsmesh.mpSpaceNodes)
    myfile1 << k << ", ";
myfile1 << "\n";

int m = mptmesh.NumberOfTimeSteps();
for(int j = 0; j<m; j++)
{
mpcurrenTimeStep = j+1;
mpcurrentMeshIndex = j;
stiff.BuildGeneralStiffnessMatrix ( mpsmesh );
stiff.MultiplyByScalar( mptmesh.ReadTimeMesh(mpcurrentMeshIndex) );
mass.BuildGeneralMassMatrix(mpsmesh);

LHS.AddTwoMatrices( mass, stiff );
mass.MatrixVectorMultiplier( mpPreviousSolution, mpRHS );

g_0 = mppde->FirstBoundary(mptmesh.ReadTimeStep(mpcurrenTimeStep));
g_L =mppde->SecondBoundary(mptmesh.ReadTimeStep(mpcurrenTimeStep));

BuiltbrVec();
VectorTimesScalar( br, mptmesh.ReadTimeMesh(mpcurrentMeshIndex) );
AddVectors( br, mpRHS, mpRHS );
LHS.MatrixSolver( mpRHS, mpx );

for (auto k: mpx)
    myfile << k << ", ";
myfile << "\n";
for (auto k: mpsmesh.mpSpaceNodes)
    myfile1 << k << ", ";
myfile1 << "\n";

mpPreviousSolution = mpx;

if (j==int(0.5*m))
{
}
}
myfile.close();
myfile1.close();

}

double GeneralHeat::GeneralInterpolant( double x, std::vector<double>& funct, SpaceMesh& relevantMesh )
{
    std::array<double, 2> firstpoint;
    std::array<double, 2> secondpoint;

    int upperindex = relevantMesh.IndexAbove( x );

    if((upperindex==1)||(upperindex==0))
    {
    firstpoint.at(0)= relevantMesh.ReadSpaceNode(0);
    firstpoint.at(1) = funct.at(0);

    secondpoint[0] = relevantMesh.ReadSpaceNode(1);
    secondpoint.at(1) = funct.at(1);
    }
    else if (upperindex == relevantMesh.meshsize())
    {
    firstpoint.at(0)= relevantMesh.ReadSpaceNode(upperindex-1);
    firstpoint.at(1) = funct.at(upperindex-1);

    secondpoint[0] = relevantMesh.ReadSpaceNode(upperindex);
    secondpoint.at(1) = funct.at(upperindex);
    }
    else
    {
    firstpoint.at(0)= relevantMesh.ReadSpaceNode(upperindex-1);
    firstpoint.at(1) = funct.at(upperindex-1);

    secondpoint[0] = relevantMesh.ReadSpaceNode(upperindex);
    secondpoint.at(1) = funct.at(upperindex);
    }

    long double m = (firstpoint[1]-secondpoint[1])/(firstpoint[0]-secondpoint[0]);

    return m*(x - firstpoint[0])+firstpoint[1];
}

void GeneralHeat::BuildGradientVec( std::vector<double>& funct, SpaceMesh& relevantMesh, std::vector<double>& gradvec )
{
    gradvec.clear();
    std::array<double, 2> firstpoint;
    std::array<double, 2> secondpoint;
    long double m;

    for(int i = 0; i<relevantMesh.meshsize(); i++)
    {
    firstpoint.at(0)= relevantMesh.ReadSpaceNode(i);
    firstpoint.at(1) = funct.at(i);

    secondpoint[0] = relevantMesh.ReadSpaceNode(i+1);
    secondpoint.at(1) = funct.at(i+1);

    m = (firstpoint[1]-secondpoint[1])/(firstpoint[0]-secondpoint[0]);

    gradvec.push_back(m);
    }
}

void GeneralHeat::GradientRecoveryFunction( SpaceMesh& relevantMesh,
                                             std::vector<double>& gradvec, std::vector<double>& gradrecovery )
{
    gradrecovery.clear();

    double x_0 = 0.5*(relevantMesh.ReadSpaceNode(1)+relevantMesh.ReadSpaceNode(0));
    double y_0 = gradvec.at(0);
    double x_1 = relevantMesh.ReadSpaceNode(1);
    double y_1 = 0.5*(gradvec.at(1)+gradvec.at(0));

    gradrecovery.push_back(y_0+(relevantMesh.ReadSpaceNode(0)-x_0)*(y_1-y_0)/(x_1-x_0));

    for(int i = 0; i<relevantMesh.meshsize()-1; i++)
    {
        gradrecovery.push_back(0.5*(gradvec.at(i)+gradvec.at(i+1)));
    }

    x_0 = relevantMesh.ReadSpaceNode(mpsmesh.meshsize()-1);
    y_0 = gradrecovery.back();
    x_1 = 0.5*(relevantMesh.ReadSpaceNode(relevantMesh.meshsize())+relevantMesh.ReadSpaceNode(relevantMesh.meshsize()-1));
    y_1 = gradvec.back();

    gradrecovery.push_back(y_0+(mpsmesh.ReadSpaceNode(mpsmesh.meshsize())-x_0)*(y_1-y_0)/(x_1-x_0));
}

void GeneralHeat::BuildErrorEstimate(  )
{
    ErrorEstimate.clear();

    auto GradSquaredError = [this](double x)
        { return pow(GeneralInterpolant(x, GradientRecovery, mpsmesh ) - GradientFunction(x), 2); };

    double dummy_var;
    for(int i=0; i<mpsmesh.meshsize(); i++)
    {
    ErrorEstimate.push_back( gauss<double, 7>::integrate(GradSquaredError, mpsmesh.ReadSpaceNode(i), mpsmesh.ReadSpaceNode(i+1)) );
    }
}

    //discontinuous function which throws exceptions at undefined points
double GeneralHeat::GradientFunction ( double x )
{
    int upperindex = mpsmesh.IndexAbove( x );
    if (mpsmesh.Contained(x))
    {
        std::cout<< "FEM gradient undefined at this point"<<"\n";
        return 0;
    }
    else
    {
       return FEMGradient.at(upperindex-1);
    }
}

void GeneralHeat::PrintVector( std::vector<double> aVector)
{
        for (auto k: aVector)
        std::cout << k << ", ";
        std::cout << " \n";
}

void GeneralHeat::VectorTimesScalar( std::vector<double>& func1, double scalar)
{
         for ( int i=0; i<func1.size(); i++ )
     {
         func1.at(i)= scalar*func1.at(i);
     }
}

double GeneralHeat::H_1Norm()
{
    EnergyNorm();
    BuildErrorMesh();
    double globalError=0;
    for(int i=0;i<mpErrorMesh.size(); i++)
    {
        globalError =mpEnergyNorm.at(i)+mpErrorMesh.at(i)+globalError;
    }

    std::cout << sqrt(globalError);
    std::cout << " \n";
    return sqrt(globalError);

}

void GeneralHeat::UnitTest1 ()
{
    BuildGradientVec(mpx, mpsmesh, FEMGradient);
    GradientRecoveryFunction( mpsmesh, FEMGradient, GradientRecovery );
    BuildErrorEstimate();

    double globalError=0;
    for(int i=0;i<ErrorEstimate.size(); i++)
    {
        globalError =ErrorEstimate.at(i)+globalError;
    }
    std::cout << "Error estimate:           ";
    std::cout << sqrt(globalError)<<"\n";

    EnergyNorm();
    std::cout << "Global Energy Error       ";
    GlobalEnergyError();
    std::cout << "\n";

    //PrintVector(ErrorEstimate);
}

Solver class generalised for all boundary conditions/GeneralizedHeat.cpp


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

#pragma once

//libraries
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

namespace math {

template<typename ScalarMajor, typename ScalarMinor>
Eigen::Matrix<ScalarMajor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
scale(const Eigen::Matrix<ScalarMajor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& container, ScalarMinor factor);

template<typename ScalarMajor, typename ScalarMinor>
Eigen::Tensor<ScalarMajor, 3, Eigen::ColMajor>
scale(const Eigen::Tensor<ScalarMajor, 3, Eigen::ColMajor>& container, ScalarMinor factor);

template<typename ScalarMajor, typename ScalarMinor>
Eigen::Matrix<ScalarMajor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
cwise_product(
		const Eigen::Matrix<ScalarMajor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& container_a,
		const Eigen::Matrix<ScalarMinor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& container_b);

template<typename ScalarMajor, typename ScalarMinor>
Eigen::Tensor<ScalarMajor, 3, Eigen::ColMajor>
cwise_product(
		const Eigen::Tensor<ScalarMajor, 3, Eigen::ColMajor>& container_a,
		const Eigen::Tensor<ScalarMinor, 3, Eigen::ColMajor>& container_b);

template<typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
cwise_add_constant(const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& container, Scalar constant);

template<typename Scalar>
Eigen::Tensor<Scalar, 3, Eigen::ColMajor>
cwise_add_constant(const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& container, Scalar constant);

template<typename ScalarMajor, typename ScalarMinor>
inline
Eigen::Matrix<ScalarMajor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
cwise_add(
		const Eigen::Matrix<ScalarMajor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& container_a,
		const Eigen::Matrix<ScalarMinor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& container_b){
	return (container_a.array() + container_b.array()).matrix();
}

template<typename ScalarMajor, typename ScalarMinor>
inline
Eigen::Tensor<ScalarMajor, 3, Eigen::ColMajor>
cwise_add(
		const Eigen::Tensor<ScalarMajor, 3, Eigen::ColMajor>& container_a,
		const Eigen::Tensor<ScalarMinor, 3, Eigen::ColMajor>& container_b){
	return container_a + container_b;
}

template<typename ScalarMajor, typename ScalarMinor>
inline
Eigen::Matrix<ScalarMajor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>
cwise_subtract(
		const Eigen::Matrix<ScalarMajor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& container_a,
		const Eigen::Matrix<ScalarMinor, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& container_b){
	return (container_a.array() - container_b.array()).matrix();
}

template<typename ScalarMajor, typename ScalarMinor>
inline
Eigen::Tensor<ScalarMajor, 3, Eigen::ColMajor>
cwise_subtract(
		const Eigen::Tensor<ScalarMajor, 3, Eigen::ColMajor>& container_a,
		const Eigen::Tensor<ScalarMinor, 3, Eigen::ColMajor>& container_b){
	return container_a - container_b;
}

}  // namespace math


#include <algorithm>
#include <cmath>
#include <Eigen/Dense>
#include "../../common/kernel.hpp"
#include "../../common/unary.hpp"

extern "C"
{
  static void add_bias(float *c, float *y, int m, int n) {
    for (int row = 0; row < m; row++)
    {
      for (int col = 0; col < n; col++)
      {
        y[row * n + col] += c[col];
      }
    }
  }

  static void do_gemm_transa0_transb0(float *a, float *b, float *y, int m, int n, int k)
  {
    // 'const' float *a raises compile error
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > a_mat(a, m, k);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > b_mat(b, k, n);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > y_mat(y, m, n);

    y_mat.noalias() = a_mat * b_mat;
  }

  void WEBDNN_KERNEL kernel_gemm_transa0_transb0(float *a, float *b, float *y, int m, int n, int k)
  {
    do_gemm_transa0_transb0(a, b, y, m, n, k);
  }

  void WEBDNN_KERNEL kernel_gemm_transa0_transb0_c(float *a, float *b, float *c, float *y, int m, int n, int k)
  {
    do_gemm_transa0_transb0(a, b, y, m, n, k);
    add_bias(c, y, m, n);
  }

  static void do_gemm_transa0_transb1(float *a, float *b, float *y, int m, int n, int k)
  {
    // 'const' float *a raises compile error
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > a_mat(a, m, k);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> > b_mat(b, k, n);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > y_mat(y, m, n);

    y_mat.noalias() = a_mat * b_mat;
  }

  void WEBDNN_KERNEL kernel_gemm_transa0_transb1(float *a, float *b, float *y, int m, int n, int k)
  {
    do_gemm_transa0_transb1(a, b, y, m, n, k);
  }

  void WEBDNN_KERNEL kernel_gemm_transa0_transb1_c(float *a, float *b, float *c, float *y, int m, int n, int k)
  {
    do_gemm_transa0_transb1(a, b, y, m, n, k);
    add_bias(c, y, m, n);
  }

  static void do_gemm_transa1_transb0(float *a, float *b, float *y, int m, int n, int k)
  {
    // 'const' float *a raises compile error
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> > a_mat(a, m, k);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > b_mat(b, k, n);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > y_mat(y, m, n);

    y_mat.noalias() = a_mat * b_mat;
  }

  void WEBDNN_KERNEL kernel_gemm_transa1_transb0(float *a, float *b, float *y, int m, int n, int k)
  {
    do_gemm_transa1_transb0(a, b, y, m, n, k);
  }

  void WEBDNN_KERNEL kernel_gemm_transa1_transb0_c(float *a, float *b, float *c, float *y, int m, int n, int k)
  {
    do_gemm_transa1_transb0(a, b, y, m, n, k);
    add_bias(c, y, m, n);
  }

  static void do_gemm_transa1_transb1(float *a, float *b, float *y, int m, int n, int k)
  {
    // 'const' float *a raises compile error
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> > a_mat(a, m, k);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> > b_mat(b, k, n);
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > y_mat(y, m, n);

    y_mat.noalias() = a_mat * b_mat;
  }

  void WEBDNN_KERNEL kernel_gemm_transa1_transb1(float *a, float *b, float *y, int m, int n, int k)
  {
    do_gemm_transa1_transb1(a, b, y, m, n, k);
  }

  void WEBDNN_KERNEL kernel_gemm_transa1_transb1_c(float *a, float *b, float *c, float *y, int m, int n, int k)
  {
    do_gemm_transa1_transb1(a, b, y, m, n, k);
    add_bias(c, y, m, n);
  }
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <Eigen/Dense>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkPolyData.h>
#include <vtkDoubleArray.h>
#include <vtkSmartPointer.h>
#include <functional>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point;
typedef CGAL::Triangulation_vertex_base_with_info_2<unsigned,K> Vb;
typedef CGAL::Triangulation_data_structure_2<Vb> Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds> Delaunay;
typedef Delaunay::Face_circulator Face_circulator;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> Map3Xd;

// New structure to store first ring neighbors of a vertex
struct vertex_first_ring{
    size_t vertex_id;
    std::vector< std::pair<unsigned,unsigned> > faces;
    std::vector< unsigned > edges;
};

int main(){
    clock_t t1;
    t1 = clock();
    for(auto i=0; i < 1; ++i){
        // *************************** Triangulation *************************//
        auto reader = vtkSmartPointer<vtkPolyDataReader>::New();
        reader->SetFileName("T7.vtk");
        reader->Update();
        auto poly = reader->GetOutput();
        auto N = poly->GetNumberOfPoints();
        auto pts = (double*) poly->GetPoints()->GetData()->GetVoidPointer(0);
        Map3Xd points(pts,3,N);

        // Project points to unit sphere
        points.colwise().normalize();

        // Reset the center of the sphere to origin by translating
        Vector3d center = points.rowwise().mean();
        points = points.colwise() - center;

        // Rotate all points so that the point in 0th column is along z-axis
        Vector3d c = points.col(0);
        double_t cos_t = c(2);
        double_t sin_t = std::sqrt( 1 - cos_t*cos_t );
        Vector3d axis;
        axis << c(1), -c(0), 0.;
        Matrix3d rotMat, axis_cross, outer;
        axis_cross << 0. , -axis(2), axis(1),
                        axis(2), 0., -axis(0),
                        -axis(1), axis(0), 0.;

        outer.noalias() = axis*axis.transpose();

        rotMat = cos_t*Matrix3d::Identity() + sin_t*axis_cross + (1-cos_t)*outer;
        Matrix3Xd rPts(3,N);
        rPts = rotMat*points; // The points on a sphere rotated

        // Calculate the stereographic projections
        Vector3d p0;
        Map3Xd l0( &(rPts(0,1)), 3, N-1 );
        Matrix3Xd l(3,N-1), proj(3,N-1);
        p0 << 0,0,-1;
        c = rPts.col(0);
        l = (l0.colwise() - c).colwise().normalized();
        for( auto j=0; j < N-1; ++j ){
            proj.col(j) = ((p0(2) - l0(2,j))/l(2,j))*l.col(j) + l0.col(j);
        }
        // Insert the projected points in a CGAL vertex_with_info vector
        std::vector< std::pair< Point, unsigned> > verts;
        for( auto j=0; j < N-1; ++j )
            verts.push_back(std::make_pair(Point(proj(0,j),proj(1,j)),j+1));

        Delaunay dt( verts.begin(), verts.end() );

        // Write the finite faces of the triangulation to a VTK file
        vtkNew<vtkCellArray> triangles;
        for( auto ffi = dt.finite_faces_begin(); ffi != dt.finite_faces_end(); ++ffi){
            triangles->InsertNextCell(3);
            for(auto j=2; j >= 0; --j)
                triangles->InsertCellPoint(ffi->vertex(j)->info());
        }

        // Iterate over infinite faces
        Face_circulator fc = dt.incident_faces(dt.infinite_vertex()), done3(fc);
        if (fc != 0) {
            do{
                triangles->InsertNextCell(3);
                for(auto j=2; j >= 0; --j){
                    auto vh = fc->vertex(j);
                    auto id = dt.is_infinite(vh)? 0 : vh->info();
                    triangles->InsertCellPoint(id);
                }
            }while(++fc != done3);
        }
        poly->SetPolys(triangles);

        // Write to VTK file
        vtkNew<vtkPolyDataWriter> writer;
        writer->SetFileName("CGALStereoMesh.vtk");
        writer->SetInputData(poly);
        writer->Write();

        // *************************** Our data structure *************************//
        std::vector<vertex_first_ring> first_ring;
        std::set<std::set<unsigned>> tri, edges;

        // Iterate over all vertices and collect first ring neighbors
        for(auto fvi = dt.all_vertices_begin(); fvi != dt.all_vertices_end(); ++fvi){

            vertex_first_ring vfr;
            std::vector<Delaunay::Vertex_handle> rvh;
            auto vid = dt.is_infinite(fvi)? 0 : fvi->info();
            vfr.vertex_id = vid;
            Delaunay::Edge_circulator ec = dt.incident_edges(fvi), done(ec);

            // Lambda function to get the vertex id for the edge
            auto getVertexId = [](int a, int b){
                std::set<int> index{0,1,2};
                index.erase(a);
                index.erase(b);
                return *index.begin();
            };

            if( ec != 0){
                do{

                    auto fh = ec->first;
                    auto edgeIndex = getVertexId(fh->index(fvi),ec->second);
                    auto verH = fh->vertex(edgeIndex);
                    auto edgeId = dt.is_infinite(verH)? 0 : verH->info();
                    std::set<unsigned> edge{vid,edgeId};
                    auto tryInsertEdge = edges.insert(edge);
                    if(tryInsertEdge.second){
                        vfr.edges.push_back(edgeId);
                        rvh.push_back(verH);
                    }

                }while(++ec != done);
            }

            // Check which edges form a unique face
            auto numEdges = rvh.size();
            for( auto k = 0; k < numEdges; ++k){
                auto next = (k+1) % numEdges;
                // Check if face is formed
                if(dt.is_face(fvi,rvh[k],rvh[next] )){
                    // Check if face is unique
                    std::set<unsigned> face{vid,vfr.edges[k],vfr.edges[next]};
                    auto tryInsertFace = tri.insert(face);
                    if(tryInsertFace.second)
                        vfr.faces.push_back(std::make_pair(k,next));
                }
            }

            first_ring.push_back(vfr);
        }

        //*************************** Print first ring ******************************//
        auto edgeNum = 0;
        auto faceNum = 0;
        for( const auto & vfr : first_ring){
            std::cout<<" Center Point = " << vfr.vertex_id << std::endl;
            std::cout<<"\t Faces = "<< std::endl;
            for(const auto & face : vfr.faces){
                std::cout<<"\t\t"<< face.first << " " << face.second << std::endl;
                faceNum++;
            }
            std::cout<<"\t Edges = "<< std::endl;
            for(const auto & edge : vfr.edges){
                std::cout<<"\t\t"<< edge << std::endl;
                edgeNum++;
            }
        }
        std::cout<< "Number of faces = " << faceNum << std::endl;
        std::cout<< "Number of edges = " << edgeNum << std::endl;
    }
    float diff((float)clock() - (float)t1);
    std::cout << "Time elapsed : " << diff / CLOCKS_PER_SEC
              << " seconds" << std::endl;
    return 0;
}


#include <vector>
#include "..\include\myfluidBox.h"
#include <Eigen/Dense>

using namespace std;

namespace particleSystem{
	unsigned int myFluidBox::ID_gen = 0;

	myFluidBox::myFluidBox() :ID(++ID_gen) {}//std::cout<<"building fluid box ID : "<<ID<<std::endl;	}
	myFluidBox::myFluidBox(int _numCellX, int _numCellY, int _numCellZ, double _diffusion, double _viscosity, double _deltaT, int _sviters, const Eigen::Ref<const Eigen::Vector3d>& _cellSz) :
		ID(++ID_gen), numCellX(_numCellX), numCellY(_numCellY), numCellZ(_numCellZ), diff(_diffusion), visc(_viscosity), slvIters(_sviters), startLoc(0, 0, 0), sphereBnds(), ctrSzHalfNC(0, 0, 0), //sphereExtBnds(), 				//precalculated (center - sz*halfNumCell)/sz for force query for particles
		deltaT(_deltaT), center(0, 0, 0), numCellXY(numCellX * numCellY), numCells(numCellX * numCellY * numCellZ), cellSz(_cellSz), isMesh(false), radSq(0) {

		vortEps = deltaT * .01;			//TODO allow for UI input
		initVecs();

		halfNmCellX = (numCellX / 2);
		halfNmCellY = (numCellY / 2);
		halfNmCellZ = (numCellZ / 2);

		//corresponds to x,y,z idx of "internal" array (inside single cube boundary layer
		sx1i = numCellX - 1;
		sy1i = numCellY - 1;
		sz1i = numCellZ - 1;
		//corresponds to x,y,z size of internal cube array of nodes used for sim
		sx2i = numCellX - 2;
		sy2i = numCellY - 2;
		sz2i = numCellZ - 2;
		hO2Sxd = 1.0 / (2.0* sx2i); hO2Syd = 1.0 / (2.0* sy2i); hO2Szd = 1.0 / (2.0* sz2i);
		hSxd = 0.5f* sx2i; hSyd = 0.5f* sy2i; hSzd = 0.5f* sz2i;
		memSetNumElems = sizeof(Vx0[0]) * numCells;
		//location to start rendering
		setStartLoc();
	}

	myFluidBox::~myFluidBox() {
		delete[] oldDensity;
		delete[] density;
		delete[] isOOB;
		delete[] Vx;
		delete[] Vy;
		delete[] Vz;
		delete[] Vx0;
		delete[] Vy0;
		delete[] Vz0;
	}
	void myFluidBox::set_bndCube(int b, double* x) {
		//int N = size;//for reading ease
		//int sz1 = numCellZ - 1, sz2 = numCellZ - 2,
		//	sx1 = numCellX - 1, sx2 = numCellX - 2,
		//	sy1 = sy1i, sy2 = numCellY - 2;
		if (b == 0) {//diffusion
			for (unsigned int j = 1; j < sy1i; ++j) {for (unsigned int i = 1; i < sx1i; ++i) {	x[IX(i, j, 0)] = x[IX(i, j, 1)]; x[IX(i, j, sz1i)] = x[IX(i, j, sz2i)];}}
			for (unsigned int k = 1; k < sz1i; ++k) {for (unsigned int i = 1; i < sx1i; ++i) {	x[IX(i, 0, k)] = x[IX(i, 1, k)]; x[IX(i, sy1i, k)] = x[IX(i, sy2i, k)];}}
			for (unsigned int k = 1; k < sz1i; ++k) {for (unsigned int j = 1; j < sy1i; ++j) {	x[IX(0, j, k)] = x[IX(1, j, k)]; x[IX(sx1i, j, k)] = x[IX(sx2i, j, k)];}}
		}
		else if (b == 1) {//reflecting in x
			for (unsigned int j = 1; j < sy1i; ++j) {for (unsigned int i = 1; i < sx1i; ++i) {	x[IX(i, j, 0)] = x[IX(i, j, 1)];  x[IX(i, j, sz1i)] = x[IX(i, j, sz2i)]; } }
			for (unsigned int k = 1; k < sz1i; ++k) {for (unsigned int i = 1; i < sx1i; ++i) {	x[IX(i, 0, k)] = x[IX(i, 1, k)];  x[IX(i, sy1i, k)] = x[IX(i, sy2i, k)]; } }
			for (unsigned int k = 1; k < sz1i; ++k) {for (unsigned int j = 1; j < sy1i; ++j) {	x[IX(0, j, k)] = -x[IX(1, j, k)]; x[IX(sx1i, j, k)] = -x[IX(sx2i, j, k)];} }
		}
		else if (b == 2) {//reflecting in y
			for (unsigned int j = 1; j < sy1i; ++j) {for (unsigned int i = 1; i < sx1i; ++i) {	x[IX(i, j, 0)] = x[IX(i, j, 1)]; x[IX(i, j, sz1i)] = x[IX(i, j, sz2i)]; } }
			for (unsigned int k = 1; k < sz1i; ++k) {for (unsigned int i = 1; i < sx1i; ++i) {	x[IX(i, 0, k)] = -x[IX(i, 1, k)];x[IX(i, sy1i, k)] = -x[IX(i, sy2i, k)];} }
			for (unsigned int k = 1; k < sz1i; ++k) {for (unsigned int j = 1; j < sy1i; ++j) {	x[IX(0, j, k)] = x[IX(1, j, k)]; x[IX(sx1i, j, k)] = x[IX(sx2i, j, k)]; } }
		}
		else if (b == 3) {//refelecting in z
			for (unsigned int j = 1; j < sy1i; ++j) {for (unsigned int i = 1; i < sx1i; ++i) {	x[IX(i, j, 0)] = -x[IX(i, j, 1)]; x[IX(i, j, sz1i)] = -x[IX(i, j, sz2i)]; } }
			for (unsigned int k = 1; k < sz1i; ++k) {for (unsigned int i = 1; i < sx1i; ++i) {	x[IX(i, 0, k)] = x[IX(i, 1, k)];  x[IX(i, sy1i, k)] = x[IX(i, sy2i, k)]; } }
			for (unsigned int k = 1; k < sz1i; ++k) {for (unsigned int j = 1; j < sy1i; ++j) {	x[IX(0, j, k)] = x[IX(1, j, k)];  x[IX(sx1i, j, k)] = x[IX(sx2i, j, k)]; } }
		}

		// edges
		for (unsigned int i = 1; i < sx1i; ++i) {
			x[IX(i, 0, 0)] = 0.5f * (x[IX(i, 1, 0)] + x[IX(i, 0, 1)]);
			x[IX(i, sy1i, 0)] = 0.5f * (x[IX(i, sy2i, 0)] + x[IX(i, sy1i, 1)]);
			x[IX(i, 0, sz1i)] = 0.5f * (x[IX(i, 1, sz1i)] + x[IX(i, 0, sz2i)]);
			x[IX(i, sy1i, sz1i)] = 0.5f * (x[IX(i, sy2i, sz1i)] + x[IX(i, sy1i, sz2i)]);
		}

		for (unsigned int j = 1; j < sy1i; ++j) {
			x[IX(0, j, 0)] = 0.5f * (x[IX(1, j, 0)] + x[IX(0, j, 1)]);
			x[IX(sx1i, j, 0)] = 0.5f * (x[IX(sx2i, j, 0)] + x[IX(sx1i, j, 1)]);
			x[IX(0, j, sz1i)] = 0.5f * (x[IX(1, j, sz1i)] + x[IX(0, j, sz2i)]);
			x[IX(sx1i, j, sz1i)] = 0.5f * (x[IX(sx2i, j, sz1i)] + x[IX(sx1i, j, sz2i)]);
		}

		for (unsigned int k = 1; k < sz1i; ++k) {
			x[IX(0, 0, k)] = 0.5f * (x[IX(0, 1, k)] + x[IX(1, 0, k)]);
			x[IX(0, sy1i, k)] = 0.5f * (x[IX(0, sy2i, k)] + x[IX(1, sy1i, k)]);
			x[IX(sx1i, 0, k)] = 0.5f * (x[IX(sx1i, 1, k)] + x[IX(sx2i, 0, k)]);
			x[IX(sx1i, sy1i, k)] = 0.5f * (x[IX(sx1i, sy2i, k)] + x[IX(sx2i, sy1i, k)]);
		}

		// corners
		double calcVal = (1 / 3.0);
		x[IX(0, 0, 0)] = calcVal*(x[IX(0, 1, 0)] + x[IX(1, 0, 0)] + x[IX(0, 0, 1)]);
		x[IX(sx1i, 0, 0)] = calcVal*(x[IX(sx2i, 0, 0)] + x[IX(sx1i, 1, 0)] + x[IX(sx1i, 0, 1)]);
		x[IX(0, sy1i, 0)] = calcVal*(x[IX(0, sy2i, 0)] + x[IX(1, sy1i, 0)] + x[IX(0, sy1i, 1)]);
		x[IX(sx1i, sy1i, 0)] = calcVal*(x[IX(sx1i, sy2i, 0)] + x[IX(sx2i, sy1i, 0)] + x[IX(sx1i, sy1i, 1)]);

		x[IX(0, 0, sz1i)] = calcVal*(x[IX(0, 1, sz1i)] + x[IX(1, 0, sz1i)] + x[IX(0, 0, sz2i)]);
		x[IX(sx1i, 0, sz1i)] = calcVal*(x[IX(sx2i, 0, sz1i)] + x[IX(sx1i, 1, sz1i)] + x[IX(sx1i, 0, sz2i)]);
		x[IX(0, sy1i, sz1i)] = calcVal*(x[IX(0, sy2i, sz1i)] + x[IX(1, sy1i, sz1i)] + x[IX(0, sy1i, sz2i)]);
		x[IX(sx1i, sy1i, sz1i)] = calcVal*(x[IX(sx1i, sy2i, sz1i)] + x[IX(sx2i, sy1i, sz1i)] + x[IX(sx1i, sy1i, sz2i)]);
	}

	//handle advection for passed arrays of velocities
	void myFluidBox::advect(int b, double* d, double* d0, double* velocX, double* velocY, double* velocZ) {
		double s0, s1, t0, t1, u0, u1;
		//double tmp1, tmp2, tmp3;
		double x, y, z;

		//double idouble = 1, jdouble = 1, kdouble = 1;
		int i0, i1, j0, j1, k0, k1;

		double dtx = deltaT * (sx2i),
			dty = deltaT * (sy2i),
			dtz = deltaT * (sz2i);

		int IXidx;

		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					IXidx = IX(i, j, k);

					x = i - dtx * velocX[IXidx];
					y = j - dty * velocY[IXidx];
					z = k - dtz * velocZ[IXidx];
					x = forceIDXBndD(x, sx1i + 0.5, .5);
					i0 = floor(x);	i1 = i0 + 1.0;  s1 = x - i0;	s0 = 1.0 - s1;
					y = forceIDXBndD(y, sy1i + 0.5, .5);
					j0 = floor(y);	j1 = j0 + 1.0;	t1 = y - j0;	t0 = 1.0 - t1;
					z = forceIDXBndD(z, sz1i + 0.5, .5);
					k0 = floor(z);	k1 = k0 + 1.0;	u1 = z - k0;	u0 = 1.0 - u1;

					d[IXidx] =
						s0 * (t0 * (u0 * d0[IX(i0, j0, k0)]
						+ u1 * d0[IX(i0, j0, k1)])
						+ (t1 * (u0 * d0[IX(i0, j1, k0)]
						+ u1 * d0[IX(i0, j1, k1)])))
						+ s1 * (t0 * (u0 * d0[IX(i1, j0, k0)]
						+ u1 * d0[IX(i1, j0, k1)])
						+ (t1 * (u0 * d0[IX(i1, j1, k0)]
						+ u1 * d0[IX(i1, j1, k1)])));
				}//for i
			}//for j
		}//for k
		set_bndCube(b, d);
	}

	void myFluidBox::project(double* velocX, double* velocY, double* velocZ, double* p, double* div) {
		//double sx2 = sx2i, sy2 = numCellY - 2, sz2 = numCellZ - 2;
		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					div[IX(i, j, k)] = -(
						  (velocX[IX(i + 1, j, k)] - velocX[IX(i - 1, j, k)])*hO2Sxd
						+ (velocY[IX(i, j + 1, k)] - velocY[IX(i, j - 1, k)])*hO2Syd
						+ (velocZ[IX(i, j, k + 1)] - velocZ[IX(i, j, k - 1)])*hO2Szd
						);
					p[IX(i, j, k)] = 0;
				}
			}
		}
		set_bndCube(0, div);
		//set_bnd(0, p);
		lin_solve(0, p, div, 1, 6);

		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					velocX[IX(i, j, k)] -= hSxd *(p[IX(i + 1, j, k)] - p[IX(i - 1, j, k)]);
					velocY[IX(i, j, k)] -= hSyd *(p[IX(i, j + 1, k)] - p[IX(i, j - 1, k)]);
					velocZ[IX(i, j, k)] -= hSzd *(p[IX(i, j, k + 1)] - p[IX(i, j, k - 1)]);
				}
			}
		}
		set_bndCube(1, velocX);
		set_bndCube(2, velocY);
		set_bndCube(3, velocZ);
	}

	//void myFluidBox::diffuse(int b, double* x, double* xOld, double viscdiff, int _numCells) {
	//	double delVisc = (deltaT*viscdiff*(numCells));
	//	lin_solve(b, x, xOld, delVisc, 1 + 6 * delVisc);
	//}

	void myFluidBox::myFluidBoxTimeStep() {
		addSource(Vx, Vx0);
		addSource(Vy, Vy0);
		addSource(Vz, Vz0);
		addSource(density, oldDensity);
		//if (isMesh) { myFluidSphereTimeStep(); return; }

		std::swap(Vx, Vx0); std::swap(Vy, Vy0); std::swap(Vz, Vz0);

		diffuse(1, Vx, Vx0, visc, numCellX);
		diffuse(2, Vy, Vy0, visc, numCellY);
		diffuse(3, Vz, Vz0, visc, numCellZ);

		project(Vx, Vy, Vz, Vx0, Vy0);

		std::swap(Vx, Vx0); std::swap(Vy, Vy0); std::swap(Vz, Vz0);

		advect(1, Vx, Vx0, Vx0, Vy0, Vz0);
		advect(2, Vy, Vy0, Vx0, Vy0, Vz0);
		advect(3, Vz, Vz0, Vx0, Vy0, Vz0);

		project(Vx, Vy, Vz, Vx0, Vy0);

		std::swap(density, oldDensity);
		diffuse(0, density, oldDensity, diff, numCellX);
		std::swap(density, oldDensity);
		advect(0, density, oldDensity, Vx0, Vy0, Vz0);
		resetOldVals();
	}//myFluidBoxTimeStep

	/////////////sphere stuff

	 //timestepping for sphere, to handle 3d bounds together instead of 1 dim at a time
	void myFluidBox::myFluidSphereTimeStep() {
		//addSource(Vx, Vx0);
		//addSource(Vy, Vy0);
		//addSource(Vz, Vz0);
		//addSource(density, oldDensity);

		std::swap(Vx, Vx0); std::swap(Vy, Vy0); std::swap(Vz, Vz0);
		double delVisc = (deltaT*visc*numCells);
		//diffusion of velocity
		lin_solveSphere(Vx, Vx0, Vy, Vy0, Vz, Vz0, delVisc, 1 + 6 * delVisc);

		projectSphere(Vx, Vy, Vz, Vx0, Vy0);

		std::swap(Vx, Vx0); std::swap(Vy, Vy0); std::swap(Vz, Vz0);
		advectSphere(Vx, Vx0, Vy, Vy0, Vz, Vz0);

		projectSphere(Vx, Vy, Vz, Vx0, Vy0);

		std::swap(density, oldDensity);
		diffSphDens(density, oldDensity, diff);
		std::swap(density, oldDensity);
		advSphDens(density, oldDensity, Vx, Vy, Vz);
		//vort confine here
		vorticityConfinement(oldDensity);
		//vorticity particle method
		//vorticityParticles();

		resetOldVals();
	}//myFluidSphereTimeStep

	//handle advection for passed arrays of velocities
	void myFluidBox::advectSphere(double* _velx, double* _velx0, double* _vely, double* _vely0, double* _velz, double* _velz0) {
		double s0, s1, t0, t1, u0, u1,x, y, z;

		int i0, i1, j0, j1, k0, k1;
		//precalced idx's
		int idx0, idx1, idx2, idx3, idx4, idx5, idx6, idx7;

		double dtx = deltaT * (sx2i),
			dty = deltaT * (sy2i),
			dtz = deltaT * (sz2i);
		int IXidx;

		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					IXidx = IX(i, j, k);
					if (isOOB[IXidx]) { continue; }

					x = i - dtx * _velx0[IXidx];
					x = forceIDXBndD(x, sx2i + 0.5, .5);
					i0 = floor(x);	i1 = i0 + 1.0;  s1 = x - i0;	s0 = 1.0 - s1;

					y = j - dty * _vely0[IXidx];
					y = forceIDXBndD(y, sy2i + 0.5, .5);
					j0 = floor(y);	j1 = j0 + 1.0;	t1 = y - j0;	t0 = 1.0 - t1;

					z = k - dtz * _velz0[IXidx];
					z = forceIDXBndD(z, sz2i + 0.5, .5);
					k0 = floor(z);	k1 = k0 + 1.0;	u1 = z - k0;	u0 = 1.0 - u1;

					idx0 = IX(i0, j0, k0);idx1 = IX(i0, j0, k1);idx2 = IX(i0, j1, k0);idx3 = IX(i0, j1, k1);
					idx4 = IX(i1, j0, k0);idx5 = IX(i1, j0, k1);idx6 = IX(i1, j1, k0);idx7 = IX(i1, j1, k1);

					_velx[IXidx] =
						s0 * (t0 * (u0 * _velx0[idx0] + u1 * _velx0[idx1]) + (t1 * (u0 * _velx0[idx2] + u1 * _velx0[idx3]))) +
						s1 * (t0 * (u0 * _velx0[idx4] + u1 * _velx0[idx5]) + (t1 * (u0 * _velx0[idx6] + u1 * _velx0[idx7])));

					_vely[IXidx] =
						s0 * (t0 * (u0 * _vely0[idx0] + u1 * _vely0[idx1]) + (t1 * (u0 * _vely0[idx2] + u1 * _vely0[idx3]))) +
						s1 * (t0 * (u0 * _vely0[idx4] + u1 * _vely0[idx5]) + (t1 * (u0 * _vely0[idx6] + u1 * _vely0[idx7])));

					_velz[IXidx] =
						s0 * (t0 * (u0 * _velz0[idx0] + u1 * _velz0[idx1]) + (t1 * (u0 * _velz0[idx2] + u1 * _velz0[idx3]))) +
						s1 * (t0 * (u0 * _velz0[idx4] + u1 * _velz0[idx5]) + (t1 * (u0 * _velz0[idx6] + u1 * _velz0[idx7])));
				}//for i
			}//for j
		}//for k
		set_bndSphere3(_velx, _vely, _velz);
	}//advectSphere

	//diffuse density - 1 d but use sphere bnds
	void myFluidBox::diffSphDens(double* x, double* x0, double viscdiff) {
		double a = (deltaT*viscdiff*(numCells)), c = (1 + 6 * a);
		int idx;
		if(a==0){
			std::memcpy(&x, &x0, sizeof x0);
			set_bndDiffSphere(x);
		} else {
			for (unsigned int itr = 0; itr < slvIters; ++itr) {
				for (unsigned int k = 1; k < sz1i; ++k) {
					for (unsigned int j = 1; j < sy1i; ++j) {
						for (unsigned int i = 1; i < sx1i; ++i) {
							idx = IX(i, j, k);
							if (isOOB[idx]) { continue; }
							x[idx] = (x0[idx] + a *
								(x[IX(i + 1, j, k)]
									+ x[IX(i - 1, j, k)]
									+ x[IX(i, j + 1, k)]
									+ x[IX(i, j - 1, k)]
									+ x[IX(i, j, k + 1)]
									+ x[IX(i, j, k - 1)])) / c;
						}//for i
					}//for j
				}//for k
				set_bndDiffSphere(x);
			}
		}//for itr
	}//diffSphDens
	//advect density through sphere, using sphere bounds
	void myFluidBox::advSphDens(double* d, double* d0, double* velocX, double* velocY, double* velocZ) {
		double s0, s1, t0, t1, u0, u1, x, y, z, dtx = deltaT * (sx2i),dty = deltaT * (sy2i),dtz = deltaT * (sz2i);
		unsigned int i0, i1, j0, j1, k0, k1, i, j, k, IXidx;

		for (k = 1; k < sz1i; ++k) {
			for (j = 1; j < sy1i; ++j) {
				for (i = 1; i < sx1i; ++i) {
					IXidx = IX(i, j, k);
					if (isOOB[IXidx]) { continue; }
					x = i - dtx * velocX[IXidx];
					y = j - dty * velocY[IXidx];
					z = k - dtz * velocZ[IXidx];
					x = forceIDXBndD(x, sx2i + 0.5, .5);
					i0 = floor(x);	i1 = i0 + 1.0;  s1 = x - i0;	s0 = 1.0 - s1;

					y = forceIDXBndD(y, sy2i + 0.5, .5);
					j0 = floor(y);	j1 = j0 + 1.0;	t1 = y - j0;	t0 = 1.0 - t1;

					z = forceIDXBndD(z, sz2i + 0.5, .5);
					k0 = floor(z);	k1 = k0 + 1.0;	u1 = z - k0;	u0 = 1.0 - u1;

					d[IXidx] =
						s0 * (t0 * (u0 * d0[IX(i0, j0, k0)]	+ u1 * d0[IX(i0, j0, k1)]) + (t1 * (u0 * d0[IX(i0, j1, k0)] + u1 * d0[IX(i0, j1, k1)]))) +
						s1 * (t0 * (u0 * d0[IX(i1, j0, k0)]	+ u1 * d0[IX(i1, j0, k1)]) + (t1 * (u0 * d0[IX(i1, j1, k0)]	+ u1 * d0[IX(i1, j1, k1)])));
				}//for i
			}//for j
		}//for k
		set_bndDiffSphere(d);
	}//advSphDens

	void myFluidBox::lin_solveSphere(double* x, double* x0, double* y, double* y0, double* z, double* z0, double a, double c) {
		if (a == 0) {
			std::memcpy(&x, &x0, sizeof x0);
			std::memcpy(&y, &y0, sizeof y0);
			std::memcpy(&z, &z0, sizeof z0);
			set_bndSphere3(x, y, z);
		}
		else {
			unsigned int idx0, idx1, idx2, idx3, idx4, idx5, idx6;
			for (unsigned int itr = 0; itr < slvIters; ++itr) {
				for (unsigned int k = 1; k < sz1i; ++k) {
					for (unsigned int j = 1; j < sy1i; ++j) {
						for (unsigned int i = 1; i < sx1i; ++i) {
							idx0 = IX(i, j, k);
							if (isOOB[idx0]) { continue; }
							idx1 = IX(i + 1, j, k);
							idx2 = IX(i - 1, j, k);
							idx3 = IX(i, j + 1, k);
							idx4 = IX(i, j - 1, k);
							idx5 = IX(i, j, k + 1);
							idx6 = IX(i, j, k - 1);
							x[idx0] = (x0[idx0] + a * (x[idx1] + x[idx2] + x[idx3] + x[idx4] + x[idx5] + x[idx6])) / c;
							y[idx0] = (y0[idx0] + a * (y[idx1] + y[idx2] + y[idx3] + y[idx4] + y[idx5] + y[idx6])) / c;
							z[idx0] = (z0[idx0] + a * (z[idx1] + z[idx2] + z[idx3] + z[idx4] + z[idx5] + z[idx6])) / c;
						}//for i
					}//for j
				}//for k
				set_bndSphere3(x, y, z);
			}//for itr
		}//if a != 0
	}//lin_solveSphere

	//vorticity confinement - add back vorticity details lost through numerical dissipation
	//vortN is unused array to hold calcs
	void myFluidBox::vorticityConfinement(double* vortN) {
		unsigned int idx, idx_ijp1k, idx_ijm1k, idx_ip1jk, idx_im1jk, idx_ijkm1, idx_ijkp1;
		//double vortEps = deltaT * .01;	//TODO change to allow for user input
		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					idx = IX(i, j, k);
					if (isOOB[idx]) { continue; }
					idx_ip1jk = IX(i + 1, j, k); idx_im1jk = IX(i - 1, j, k); idx_ijp1k = IX(i, j + 1, k); idx_ijm1k = IX(i, j - 1, k); idx_ijkp1 = IX(i, j, k + 1); idx_ijkm1 = IX(i, j, k - 1);
					//curl operation del cross u -> partial z w/respect to y is the finite diff of the z vels across the y coords
					vortVec[idx] <<
						((Vy[idx_ijkp1] - Vy[idx_ijkm1]) * hO2Syd) - ((Vz[idx_ijp1k] - Vz[idx_ijm1k]) * hO2Szd),
						((Vz[idx_ip1jk] - Vz[idx_im1jk]) * hO2Szd) - ((Vx[idx_ijkp1] - Vx[idx_ijkm1]) * hO2Sxd),
						((Vx[idx_ijp1k] - Vx[idx_ijm1k]) * hO2Sxd) - ((Vy[idx_ip1jk] - Vy[idx_im1jk]) * hO2Syd);
				}
			}
		}
		set_bndDiffVec(vortVec);
		for (idx = 0; idx < numCells; ++idx) { vortN[idx] = (isOOB[idx]) ?  0 : vortVec[idx].norm(); }

		Eigen::Vector3d eta, vf; eta.setZero();	vf.setZero();

		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					idx = IX(i, j, k);
					if (vortN[idx] < .0000001) {	continue;}
					vortVec[idx].normalize();
					eta << ((vortN[IX(i + 1, j, k)] - vortN[IX(i - 1, j, k)]) * hO2Sxd), ((vortN[IX(i, j + 1, k)] - vortN[IX(i, j - 1, k)]) * hO2Syd), ( (vortN[IX(i, j, k + 1)] - vortN[IX(i, j, k - 1)]) * hO2Szd);
					eta.normalize();
					vf = vortEps * (eta.cross(vortVec[idx]));
					//cout << "Vx " << idx << " before :  " << Vx[idx] << " vortN : " << vortN[idx] << " invDivX : " << invDivX << " eta : " << eta(0) << "," << eta(1) << "," << eta(2) << " vf : " << vf(0) << "," << vf(1) << "," << vf(2) << " vort : " << vort(0) << "," << vort(1) << "," << vort(2);
					Vx[idx] += vf(0) * sx2i;
					Vy[idx] += vf(1) * sy2i;
					Vz[idx] += vf(2) * sz2i;
					//cout << "Vx " << idx << " after :  " << Vx[idx]<<endl;
				}
			}
		}
		set_bndSphere3(Vx, Vy, Vz);
	}//vorticityConfinement

	//vorticity particle method TODO
	void myFluidBox::vorticityParticles() {
		int idx, idx_ijp1k, idx_ijm1k, idx_ip1jk, idx_im1jk, idx_ijkm1, idx_ijkp1;
		//find accelerations via finite diff
		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					idx = IX(i, j, k);
					if (isOOB[idx]) { continue; }
					idx_ip1jk = IX(i + 1, j, k);idx_im1jk = IX(i - 1, j, k);idx_ijp1k = IX(i, j + 1, k);idx_ijm1k = IX(i, j - 1, k);idx_ijkp1 = IX(i, j, k + 1);idx_ijkm1 = IX(i, j, k - 1);

					accelVecX[idx] << (Vx[idx_ip1jk] - Vx[idx_im1jk]) * hO2Sxd, (Vx[idx_ijp1k] - Vx[idx_ijm1k]) * hO2Syd, (Vx[idx_ijkp1] - Vx[idx_ijkm1]) * hO2Szd;//interpAccel(Vx, idx_ip1jk, idx_im1jk, idx_ijp1k, idx_ijm1k, idx_ijkp1, idx_ijkm1);
					accelVecY[idx] << (Vy[idx_ip1jk] - Vy[idx_im1jk]) * hO2Sxd, (Vy[idx_ijp1k] - Vy[idx_ijm1k]) * hO2Syd, (Vy[idx_ijkp1] - Vy[idx_ijkm1]) * hO2Szd;//interpAccel(Vy, idx_ip1jk, idx_im1jk, idx_ijp1k, idx_ijm1k, idx_ijkp1, idx_ijkm1);
					accelVecZ[idx] << (Vz[idx_ip1jk] - Vz[idx_im1jk]) * hO2Sxd, (Vz[idx_ijp1k] - Vz[idx_ijm1k]) * hO2Syd, (Vz[idx_ijkp1] - Vz[idx_ijkm1]) * hO2Szd;//interpAccel(Vz, idx_ip1jk, idx_im1jk, idx_ijp1k, idx_ijm1k, idx_ijkp1, idx_ijkm1);

				}
			}
		}
		set_bndDiffVec(accelVecX);
		set_bndDiffVec(accelVecY);
		set_bndDiffVec(accelVecZ);

		//vort particle code here TODO

	}//vorticityParticles

	void myFluidBox::projectSphere(double* velocX, double* velocY, double* velocZ, double* p, double* div) {
		unsigned int idx;
		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					idx = IX(i, j, k);
					p[idx] = 0;
					if (isOOB[idx]) {	div[idx] = 0;	continue; }
					//when using mac grid, change to * 1/sxi instead of 1/2sxi
					div[idx] = -(
						(velocX[IX(i + 1, j, k)] - velocX[IX(i - 1, j, k)]) * hO2Sxd
						+(velocY[IX(i, j + 1, k)] - velocY[IX(i, j - 1, k)]) * hO2Syd
						+(velocZ[IX(i, j, k + 1)] - velocZ[IX(i, j, k - 1)]) * hO2Szd
						);
				}
			}
		}
		set_bndDiffSphere(div);
		set_bndDiffSphere(p);
		for (unsigned int itr = 0; itr < slvIters; ++itr) {
			for (unsigned int k = 1; k < sz1i; ++k) {
				for (unsigned int j = 1; j < sy1i; ++j) {
					for (unsigned int i = 1; i < sx1i; ++i) {
						idx = IX(i, j, k);
						if (isOOB[idx]) { continue; }
						p[idx] = (div[idx] + (p[IX(i + 1, j, k)] + p[IX(i - 1, j, k)] + p[IX(i, j + 1, k)] + p[IX(i, j - 1, k)] + p[IX(i, j, k + 1)] + p[IX(i, j, k - 1)])) / 6.0;
					}//for i
				}//for j
			}//for k
			set_bndDiffSphere(p);
		}//for each iteration

		for (unsigned int k = 1; k < sz1i; ++k) {
			for (unsigned int j = 1; j < sy1i; ++j) {
				for (unsigned int i = 1; i < sx1i; ++i) {
					idx = IX(i, j, k);
					if (isOOB[idx]) { continue; }
					velocX[idx] -= hSxd *(p[IX(i + 1, j, k)] - p[IX(i - 1, j, k)]);
					velocY[idx] -= hSyd *(p[IX(i, j + 1, k)] - p[IX(i, j - 1, k)]);
					velocZ[idx] -= hSzd *(p[IX(i, j, k + 1)] - p[IX(i, j, k - 1)]);
				}
			}
		}
		set_bndSphere3(velocX, velocY, velocZ);
	}//projectSphere

	void myFluidBox::set_bndDiffSphere(double* x) {
		for (sphereBndMap::iterator it = sphereBnds.begin(); it != sphereBnds.end(); ++it) { x[it->first] *= it->second->mag; }//scale to amt of cube in bounds
	}
	void myFluidBox::set_bndDiffVec(eignVecTyp& egVec) {
		Eigen::Vector3d  velNorm(0, 0, 0);
		for (sphereBndMap::iterator it = sphereBnds.begin(); it != sphereBnds.end(); ++it) {
			//velNorm = (egVec[it->first].dot(it->second->norm)* it->second->mag)  *it->second->norm;			//velocity in the normal direction toward center of sphere
			velNorm = (egVec[it->first].dot(it->second->norm))  *it->second->norm;			//velocity in the normal direction toward center of sphere
			egVec[it->first] -= velNorm;													//remove velocity component in opposite direction of normal
			egVec[it->first] *= velNorm.norm();												//amplify remaining component by same amount - increase tangent velocity

		}
	}
	//address boundary layer values
	void myFluidBox::set_bndSphere3(double* x, double* y, double* z) {
		//int vIdx = b - 1, xIdx;
		Eigen::Vector3d velVec(0, 0, 0), velNorm(0, 0, 0);
		for (sphereBndMap::iterator it = sphereBnds.begin(); it != sphereBnds.end(); ++it) {
			velVec << x[it->first], y[it->first], z[it->first];
			//velNorm = (velVec.dot(it->second->norm)* it->second->mag)  *it->second->norm;			//velocity in the normal direction toward center of sphere
			velNorm = (velVec.dot(it->second->norm))  *it->second->norm;							//velocity in the normal direction toward center of sphere
																									//subtract velocity in direction of sphere wall
			x[it->first] -= velNorm(0);
			y[it->first] -= velNorm(1);
			z[it->first] -= velNorm(2);
			//scale result (tangent dir) by lost velocity magnitude - don't want to lose velocity, just redirect it
			x[it->first] *= velNorm.norm();
			y[it->first] *= velNorm.norm();
			z[it->first] *= velNorm.norm();
		}
	}//set_bndSphere

	void myFluidBox::myFluidBoxAddDensity(int x, int y, int z, double amount) { density[IX(x, y, z)] += amount; }

	void myFluidBox::resetOldVals() {
		//all elems same type
		//int numElems = sizeof(Vx0[0]) * numCells;
		memset(Vx0, 0, memSetNumElems);
		memset(Vy0, 0, memSetNumElems);
		memset(Vz0, 0, memSetNumElems);
		memset(oldDensity, 0, memSetNumElems);
	}

	void myFluidBox::myFluidBoxAddForce(const Eigen::Ref<const Eigen::Vector3d>& cellLoc, const Eigen::Ref<const Eigen::Vector3d>& amount) {
		//cout<<"force addition location in cube :("<< cellLoc (0)<<","<< cellLoc(1) <<","<< cellLoc(2) <<")"<<endl;

		int idx = IX(forceIDXBnd((int)(cellLoc(0)), sx1i,0),
					 forceIDXBnd((int)(cellLoc(1)), sy1i,0),
					 forceIDXBnd((int)(cellLoc(2)), sz1i,0));
		//static int iters = 0;
		//cout << "add force in fluidbox @idx : " << idx << " iter : " << iters++ << "\n";
		//Vx0[idx] = amount(0);
		//Vy0[idx] = amount(1);
		//Vz0[idx] = amount(2);
		Vx[idx] += amount(0);
		Vy[idx] += amount(1);
		Vz[idx] += amount(2);
	}

	//cellloc is a particle position - cell idx is going to be floor of each coord
	Eigen::Vector3d myFluidBox::getVelAtCell(const Eigen::Ref<const Eigen::Vector3d>& testLoc) {
		//ctrSzHalfNC == (ctr - (halfNumCell * cellSz))/cellSz
		//cout<<ctrSzHalfNC << "\n";
		Eigen::Vector3d tmpTestLoc = testLoc.cwiseQuotient(cellSz),
			testLocInFluid = tmpTestLoc - ctrSzHalfNC;

		//double locX = (testLoc(0) - center(0)) / cellSz(0) + halfNmCellX,
		//	locY = (testLoc(1) - center(1)) / cellSz(1) + halfNmCellY,
		//	locZ = (testLoc(2) - center(2)) / cellSz(2) + halfNmCellZ;
		int intLocX = (int)testLocInFluid(0),
			intLocY = (int)testLocInFluid(1),
			intLocZ = (int)testLocInFluid(2);

		double interpX = testLocInFluid(0) - intLocX, interpM1X = 1 - interpX,
			   interpY = testLocInFluid(1) - intLocY, interpM1Y = 1 - interpY,
			   interpZ = testLocInFluid(2) - intLocZ, interpM1Z = 1 - interpZ;
		//bound idx's
		//int tX[2], tY[2], tZ[2];
		//tX[0] = forceIDXBnd(intLocX, sx1i),
		//tY[0] = forceIDXBnd(intLocY, sy1i),
		//tZ[0] = forceIDXBnd(intLocZ, sz1i),
		//tX[1] = forceIDXBnd(intLocX + 1, sx1i),
		//tY[1] = forceIDXBnd(intLocY + 1, sy1i),
		//tZ[1] = forceIDXBnd(intLocZ + 1, sz1i);
	/*
		vector<int> idxs(8); int cnt = 0;
		for (unsigned int z = 0; z < 2; ++z) {for (unsigned int y = 0; y < 2; ++y) {for (unsigned int x = 0; x < 2; ++x) { idxs[cnt++] = IX(tX[x], tY[y], tZ[z]);}}}
	*/
		int idx000 = IX(forceIDXBnd(intLocX, sx1i, 0), forceIDXBnd(intLocY, sy1i, 0), forceIDXBnd(intLocZ, sz1i, 0)),
			idx111 = IX(forceIDXBnd(intLocX + 1, sx1i, 0), forceIDXBnd(intLocY + 1, sy1i, 0), forceIDXBnd(intLocZ + 1, sz1i, 0));

		//int idx000 = IX(forceIDXBnd(intLocX, sx2i, 1), forceIDXBnd(intLocY, sy2i, 1), forceIDXBnd(intLocZ, sz2i, 1)),
		//	idx111 = IX(forceIDXBnd(intLocX + 1, sx2i, 1), forceIDXBnd(intLocY + 1, sy2i, 1), forceIDXBnd(intLocZ + 1, sz2i, 1));

		//double valx = interpM1X * Vx[idx000] + interpX*Vx[idx111],
		//	   valy = interpM1Y * Vy[idx000] + interpY*Vy[idx111],
		//	   valz = interpM1Z * Vz[idx000] + interpZ*Vz[idx111];

		//int idx = IX(forceIDXBnd((int)((testLoc(0) - center(0)) / cellSz(0) + halfNmCellX), sx1i),
		//			 forceIDXBnd((int)((testLoc(1) - center(1)) / cellSz(1) + halfNmCellY), sy1i),
		//			 forceIDXBnd((int)((testLoc(2) - center(2)) / cellSz(2) + halfNmCellZ), sz1i));

		//return Eigen::Vector3d(Vx[idx], Vy[idx], Vz[idx]);
		return Eigen::Vector3d(interpM1X * Vx[idx000] + interpX*Vx[idx111], interpM1Y * Vy[idx000] + interpY*Vy[idx111], interpM1Z * Vz[idx000] + interpZ*Vz[idx111]);
	}//getVelAtCell

}//namespace particleSystem