#include <algorithm>
#include <ctime>
#include <fstream>
#include <iostream>
#include <memory>
#include <sstream>
#include <string>

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <cv_bridge/cv_bridge.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <ros/package.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <pcl/PCLPointCloud2.h>

#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl_ros/point_cloud.h>

#include "Thirdparty/g2o/g2o/core/block_solver.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h"
#include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"

#include <object_slam/Frame.h>
#include <object_slam/Object_landmark.h>
#include <object_slam/g2o_Object.h>

#include <quadric_slam/Frame.h>
#include <quadric_slam/Object_landmark.h>
#include <quadric_slam/g2o_Object.h>

#include "detect_3d_cuboid/detect_3d_cuboid.h"
#include "detect_3d_cuboid/matrix_utils.h"

#include "line_lbd/line_lbd_allclass.h"

#include "distribution.hpp"
using namespace std;
using namespace Eigen;

typedef pcl::PointCloud<pcl::PointXYZRGB> CloudXYZRGB;
#define MAX_DETECTION 10

// global variable
std::string base_folder;
bool online_detect_mode;
bool save_results_to_txt;
cv::Mat_<float> matx_to3d_, maty_to3d_;

// for converting depth image to point cloud.
void set_up_calibration(const Eigen::Matrix3f& calibration_mat,
                        const int im_height,
                        const int im_width) {
  matx_to3d_.create(im_height, im_width);
  maty_to3d_.create(im_height, im_width);
  float center_x = calibration_mat(0, 2);      // cx
  float center_y = calibration_mat(1, 2);      // cy
  float fx_inv = 1.0 / calibration_mat(0, 0);  // 1/fx
  float fy_inv = 1.0 / calibration_mat(1, 1);  // 1/fy
  for (int v = 0; v < im_height; v++) {
    for (int u = 0; u < im_width; u++) {
      matx_to3d_(v, u) = (u - center_x) * fx_inv;
      maty_to3d_(v, u) = (v - center_y) * fy_inv;
    }
  }
}

// depth img is already in m unit.
void depth_to_cloud(const cv::Mat& rgb_img,
                    const cv::Mat& depth_img,
                    const Eigen::Matrix4f transToWorld,
                    CloudXYZRGB::Ptr& point_cloud,
                    bool downsample = false) {
  pcl::PointXYZRGB pt;
  pcl::ApproximateVoxelGrid<pcl::PointXYZRGB> vox_grid_;
  float close_depth_thre = 0.1;
  float far_depth_thre = 3.0;
  far_depth_thre = 3;
  int im_width = rgb_img.cols;
  int im_height = rgb_img.rows;
  for (int32_t i = 0; i < im_width * im_height; i++) {  // row by row
    int ux = i % im_width;
    int uy = i / im_width;
    float pix_depth = depth_img.at<float>(uy, ux);
    if (pix_depth > close_depth_thre && pix_depth < far_depth_thre) {
      pt.z = pix_depth;
      pt.x = matx_to3d_(uy, ux) * pix_depth;
      pt.y = maty_to3d_(uy, ux) * pix_depth;
      Eigen::VectorXf global_pt = homo_to_real_coord_vec<float>(
          transToWorld *
          Eigen::Vector4f(pt.x, pt.y, pt.z, 1));  // change to global position
      pt.x = global_pt(0);
      pt.y = global_pt(1);
      pt.z = global_pt(2);
      pt.r = rgb_img.at<cv::Vec3b>(uy, ux)[2];
      pt.g = rgb_img.at<cv::Vec3b>(uy, ux)[1];
      pt.b = rgb_img.at<cv::Vec3b>(uy, ux)[0];
      point_cloud->points.push_back(pt);
    }
  }
  if (downsample) {
    vox_grid_.setLeafSize(0.02, 0.02, 0.02);
    vox_grid_.setDownsampleAllData(true);
    vox_grid_.setInputCloud(point_cloud);
    vox_grid_.filter(*point_cloud);
  }
}

// one cuboid need front and back markers...
void cuboid_corner_to_marker(const Matrix38d& cube_corners,
                             visualization_msgs::Marker& marker,
                             int bodyOrfront) {
  Eigen::VectorXd edge_pt_ids;
  if (bodyOrfront == 0) {  // body edges
    edge_pt_ids.resize(16);
    edge_pt_ids << 1, 2, 3, 4, 1, 5, 6, 7, 8, 5, 6, 2, 3, 7, 8, 4;
    edge_pt_ids.array() -= 1;
  } else {  // front face edges
    edge_pt_ids.resize(5);
    edge_pt_ids << 1, 2, 6, 5, 1;
    edge_pt_ids.array() -= 1;
  }
  marker.points.resize(edge_pt_ids.rows());
  for (int pt_id = 0; pt_id < edge_pt_ids.rows(); pt_id++) {
    marker.points[pt_id].x = cube_corners(0, edge_pt_ids(pt_id));
    marker.points[pt_id].y = cube_corners(1, edge_pt_ids(pt_id));
    marker.points[pt_id].z = cube_corners(2, edge_pt_ids(pt_id));
  }
}

// one cuboid need front and back markers...  rgbcolor is 0-1 based
visualization_msgs::MarkerArray cuboids_to_marker(object_landmark* obj_landmark,
                                                  Vector3d rgbcolor) {
  visualization_msgs::MarkerArray plane_markers;
  visualization_msgs::Marker marker;
  if (obj_landmark == nullptr)
    return plane_markers;

  marker.header.frame_id = "/world";
  marker.header.stamp = ros::Time::now();
  marker.id = 0;  // 0
  marker.type = visualization_msgs::Marker::LINE_STRIP;
  marker.action = visualization_msgs::Marker::ADD;
  marker.color.r = rgbcolor(0);
  marker.color.g = rgbcolor(1);
  marker.color.b = rgbcolor(2);
  marker.color.a = 1.0;
  marker.scale.x = 0.02;

  g2o::cuboid cube_opti = obj_landmark->cube_vertex->estimate();
  Eigen::MatrixXd cube_corners = cube_opti.compute3D_BoxCorner();

  for (int ii = 0; ii < 2;
       ii++)  // each cuboid needs two markers!!! one for all edges, one for
              // front facing edge, could with different color.
  {
    marker.id++;
    cuboid_corner_to_marker(cube_corners, marker, ii);
    plane_markers.markers.push_back(marker);
  }
  return plane_markers;
}

geometry_msgs::Pose posenode_to_geomsgs(const g2o::SE3Quat& pose_Twc) {
  geometry_msgs::Pose pose_msg;
  Eigen::Vector3d pose_trans = pose_Twc.translation();
  pose_msg.position.x = pose_trans(0);
  pose_msg.position.y = pose_trans(1);
  pose_msg.position.z = pose_trans(2);
  Eigen::Quaterniond pose_quat = pose_Twc.rotation();
  pose_msg.orientation.x = pose_quat.x();
  pose_msg.orientation.y = pose_quat.y();
  pose_msg.orientation.z = pose_quat.z();
  pose_msg.orientation.w = pose_quat.w();
  return pose_msg;
}

nav_msgs::Odometry posenode_to_odommsgs(const g2o::SE3Quat& pose_Twc,
                                        const std_msgs::Header& img_header) {
  nav_msgs::Odometry odom_msg;
  odom_msg.pose.pose = posenode_to_geomsgs(pose_Twc);
  odom_msg.header = img_header;
  return odom_msg;
}

// publish every frame's raw and optimized results
void publish_all_poses(std::vector<tracking_frame*> all_frames,
                       std::vector<object_landmark*> cube_landmarks_history,
                       std::vector<object_landmark*> all_frame_rawcubes,
                       Eigen::MatrixXd& truth_frame_poses) {
  ros::NodeHandle n;
  ros::Publisher pub_slam_all_poses =
      n.advertise<geometry_msgs::PoseArray>("/slam_pose_array", 10);
  ros::Publisher pub_slam_odompose =
      n.advertise<nav_msgs::Odometry>("/slam_odom_pose", 10);
  ros::Publisher pub_truth_all_poses =
      n.advertise<geometry_msgs::PoseArray>("/truth_pose_array", 10);
  ros::Publisher pub_truth_odompose =
      n.advertise<nav_msgs::Odometry>("/truth_odom_pose", 10);
  ros::Publisher pub_slam_path =
      n.advertise<nav_msgs::Path>("/slam_pose_paths", 10);
  ros::Publisher pub_truth_path =
      n.advertise<nav_msgs::Path>("/truth_pose_paths", 10);
  ros::Publisher pub_final_opti_cube =
      n.advertise<visualization_msgs::MarkerArray>(
          "/cubes_opti", 10);  // final unique cube pose
  ros::Publisher pub_history_opti_cube =
      n.advertise<visualization_msgs::MarkerArray>(
          "/cubes_opti_hist",
          10);  // landmark cube pose after each optimization
  ros::Publisher pub_frame_raw_cube =
      n.advertise<visualization_msgs::MarkerArray>("/cubes_raw_frame", 10);
  ros::Publisher pub_2d_cuboid_project =
      n.advertise<sensor_msgs::Image>("/cuboid_project_img", 10);
  ros::Publisher raw_cloud_pub =
      n.advertise<CloudXYZRGB>("/raw_point_cloud", 50);

  int total_frame_number = all_frames.size();

  // prepare all paths messages.
  geometry_msgs::PoseArray all_pred_pose_array;
  std::vector<nav_msgs::Odometry> all_pred_pose_odoms;
  geometry_msgs::PoseArray all_truth_pose_array;
  std::vector<nav_msgs::Odometry> all_truth_pose_odoms;
  nav_msgs::Path path_truths, path_preds;
  std_msgs::Header pose_header;
  pose_header.frame_id = "/world";
  pose_header.stamp = ros::Time::now();
  path_preds.header = pose_header;
  path_truths.header = pose_header;
  for (int i = 0; i < total_frame_number; i++) {
    all_pred_pose_array.poses.push_back(
        posenode_to_geomsgs(all_frames[i]->cam_pose_Twc));
    all_pred_pose_odoms.push_back(
        posenode_to_odommsgs(all_frames[i]->cam_pose_Twc, pose_header));

    geometry_msgs::PoseStamped postamp;
    postamp.pose = posenode_to_geomsgs(all_frames[i]->cam_pose_Twc);
    postamp.header = pose_header;
    path_preds.poses.push_back(postamp);
  }
  if (truth_frame_poses.rows() > 0) {
    for (int i = 0; i < total_frame_number; i++) {
      geometry_msgs::Pose pose_msg;
      pose_msg.position.x = truth_frame_poses(i, 1);
      pose_msg.position.y = truth_frame_poses(i, 2);
      pose_msg.position.z = truth_frame_poses(i, 3);
      pose_msg.orientation.x = truth_frame_poses(i, 4);
      pose_msg.orientation.y = truth_frame_poses(i, 5);
      pose_msg.orientation.z = truth_frame_poses(i, 6);
      pose_msg.orientation.w = truth_frame_poses(i, 7);
      all_truth_pose_array.poses.push_back(pose_msg);
      nav_msgs::Odometry odom_msg;
      odom_msg.pose.pose = pose_msg;
      odom_msg.header = pose_header;
      all_truth_pose_odoms.push_back(odom_msg);

      geometry_msgs::PoseStamped postamp;
      postamp.pose = pose_msg;
      postamp.header = pose_header;
      path_truths.poses.push_back(postamp);
    }
  }
  all_pred_pose_array.header.stamp = ros::Time::now();
  all_pred_pose_array.header.frame_id = "/world";
  all_truth_pose_array.header.stamp = ros::Time::now();
  all_truth_pose_array.header.frame_id = "/world";

  if (save_results_to_txt)  // record cam pose and object pose
  {
    ofstream resultsFile;
    string resultsPath = base_folder + "output_cam_poses.txt";
    cout << "resultsPath  " << resultsPath << endl;
    resultsFile.open(resultsPath.c_str());
    resultsFile << "# timestamp tx ty tz qx qy qz qw"
                << "\n";
    for (int i = 0; i < total_frame_number; i++) {
      double time_string = truth_frame_poses(i, 0);
      ros::Time time_img(time_string);
      resultsFile << time_img << "  ";
      resultsFile << all_frames[i]->cam_pose_Twc.toVector().transpose() << "\n";
    }
    resultsFile.close();

    ofstream objresultsFile;
    string objresultsPath = base_folder + "output_obj_poses.txt";
    objresultsFile.open(objresultsPath.c_str());
    for (size_t j = 0; j < cube_landmarks_history.size(); j++) {
      g2o::cuboid cube_opti =
          cube_landmarks_history[j]->cube_vertex->estimate();
      // transform it to local ground plane.... suitable for matlab processing.
      objresultsFile << cube_opti.toMinimalVector().transpose() << " "
                     << "\n";
    }
    objresultsFile.close();
  }

  // sensor parameter for TUM cabinet data!
  Eigen::Matrix3f calib;
  float depth_map_scaling = 5000;
  calib << 535.4, 0, 320.1, 0, 539.2, 247.6, 0, 0, 1;
  set_up_calibration(calib, 480, 640);

  visualization_msgs::MarkerArray finalcube_markers =
      cuboids_to_marker(cube_landmarks_history.back(), Vector3d(0, 1, 0));

  bool show_truth_cloud =
      true;  // show point cloud using camera pose. for visualization purpose

  pcl::PCLPointCloud2 pcd_cloud2;

  ros::Rate loop_rate(5);  // 5
  int frame_number = -1;
  while (n.ok()) {
    frame_number++;

    if (0)  // directly show final results
    {
      pub_slam_all_poses.publish(all_pred_pose_array);
      pub_truth_all_poses.publish(all_truth_pose_array);
      pub_slam_path.publish(path_preds);
      pub_truth_path.publish(path_truths);
    }
    //    pub_final_opti_cube.publish(finalcube_markers);

    if (frame_number < total_frame_number) {
      //      // publish cuboid landmarks, after each frame's g2o optimization
      //      if (cube_landmarks_history[frame_number] != nullptr)
      //        pub_history_opti_cube.publish(cuboids_to_marker(
      //            cube_landmarks_history[frame_number], Vector3d(1, 0, 0)));

      //      // publish raw detected cube in each frame, before optimization
      //      if (all_frame_rawcubes.size() > 0 &&
      //          all_frame_rawcubes[frame_number] != nullptr)
      //        pub_frame_raw_cube.publish(cuboids_to_marker(
      //            all_frame_rawcubes[frame_number], Vector3d(0, 0, 1)));

      // publish camera pose estimation of this frame
      //      pub_slam_odompose.publish(all_pred_pose_odoms[frame_number]);
      pub_truth_odompose.publish(all_truth_pose_odoms[frame_number]);

      // 	    std::cout<<"Frame position x/y:   "<<frame_number<<"
      // "<<all_pred_pose_odoms[frame_number].pose.pose.position.x<<"  "<<
      // 			  all_pred_pose_odoms[frame_number].pose.pose.position.y
      // <<std::endl;

      //      char frame_index_c[256];
      //      sprintf(frame_index_c, "%04d", frame_number);  // format into 4
      //      digit

      //      cv::Mat cuboid_2d_proj_img =
      //      all_frames[frame_number]->cuboids_2d_img;

      //      std::string raw_rgb_img_name = base_folder + "raw_imgs/" +
      //                                     std::string(frame_index_c) +
      //                                     "_rgb_raw.jpg";
      //      cv::Mat raw_rgb_img = cv::imread(raw_rgb_img_name, 1);

      //      if (show_truth_cloud && (truth_frame_poses.rows() > 0))
      //        if (frame_number % 2 == 0)  // show point cloud every N frames
      //        {
      //          std::string raw_depth_img_name = base_folder + "depth_imgs/" +
      //                                           std::string(frame_index_c) +
      //                                           "_depth_raw.png";
      //          cv::Mat raw_depth_img =
      //              cv::imread(raw_depth_img_name, CV_LOAD_IMAGE_ANYDEPTH);
      //          raw_depth_img.convertTo(raw_depth_img, CV_32FC1,
      //                                  1.0 / depth_map_scaling, 0);

      //          CloudXYZRGB::Ptr point_cloud(new CloudXYZRGB());
      //          Eigen::Matrix4f truth_pose_matrix =
      //              g2o::SE3Quat(truth_frame_poses.row(frame_number).segment<7>(1))
      //                  .to_homogeneous_matrix()
      //                  .cast<float>();

      //          depth_to_cloud(raw_rgb_img, raw_depth_img, truth_pose_matrix,
      //                         point_cloud,
      //                         true);  // need to downsample cloud, otherwise
      //                         too many
      //          ros::Time curr_time = ros::Time::now();

      //          point_cloud->header.frame_id = "/world";
      //          point_cloud->header.stamp = (curr_time.toNSec() / 1000ull);
      //          raw_cloud_pub.publish(point_cloud);
      //        }

      //      cv_bridge::CvImage out_image;
      //      out_image.header.stamp = ros::Time::now();
      //      out_image.image = cuboid_2d_proj_img;
      //      out_image.encoding = sensor_msgs::image_encodings::TYPE_8UC3;
      //      pub_2d_cuboid_project.publish(out_image.toImageMsg());
    }

    if (frame_number == int(all_pred_pose_odoms.size())) {
      cout << "Finish all visulialization!" << endl;
    }

    ros::spinOnce();
    loop_rate.sleep();
  }
}

void publish_all_poses_quadric(
    const std::vector<std::shared_ptr<tracking_frame_quadric>>& all_frames,
    const Eigen::MatrixXd& truth_frame_poses,
    const std::map<int, std::shared_ptr<Quadric_landmark>>& all_landmarks) {
  ros::NodeHandle n;
  ros::Publisher pub_slam_odompose =
      n.advertise<nav_msgs::Odometry>("/slam_odom_pose", 10);
  ros::Publisher pub_truth_odompose =
      n.advertise<nav_msgs::Odometry>("/truth_odom_pose", 10);

  ros::Publisher pub_landmark =
      n.advertise<visualization_msgs::MarkerArray>("/visualization_marker", 10);

  std_msgs::Header pose_header;
  pose_header.frame_id = "/world";
  pose_header.stamp = ros::Time::now();
  std::vector<nav_msgs::Odometry> all_pred_pose_odoms;
  std::vector<nav_msgs::Odometry> all_truth_pose_odoms;

  int landmarkNum = int(all_landmarks.size());
  visualization_msgs::MarkerArray markers;
  //  markers.markers.push_back(marker);
  cout << "landmarkNum" << landmarkNum << endl;
  for (int i = 0; i < landmarkNum; ++i) {
    visualization_msgs::Marker marker;
    marker.header.frame_id = "/world";
    marker.header.stamp = ros::Time();
    marker.id = i;
    marker.type = visualization_msgs::Marker::SPHERE;
    marker.action = visualization_msgs::Marker::ADD;
    marker.color.a = 1.0;
    marker.lifetime = ros::Duration();
    switch (i) {
      case 0:
        marker.color.r = 0.0;
        marker.color.g = 1.0;
        marker.color.b = 0.0;
        break;
      case 1:
        marker.color.r = 1.0;
        marker.color.g = 0.0;
        marker.color.b = 0.0;
        break;
      case 2:
        marker.color.r = 0.0;
        marker.color.g = 0.0;
        marker.color.b = 1.0;
        break;
      case 3:
        marker.color.r = 1.0;
        marker.color.g = 0.0;
        marker.color.b = 1.0;
        break;
    }
    markers.markers.push_back(marker);
  }

  for (size_t i = 0; i < all_frames.size(); i++) {
    geometry_msgs::Pose pose_msg;
    pose_msg.position.x = truth_frame_poses(i, 0);
    pose_msg.position.y = truth_frame_poses(i, 1);
    pose_msg.position.z = truth_frame_poses(i, 2);
    pose_msg.orientation.x = truth_frame_poses(i, 3);
    pose_msg.orientation.y = truth_frame_poses(i, 4);
    pose_msg.orientation.z = truth_frame_poses(i, 5);
    pose_msg.orientation.w = truth_frame_poses(i, 6);
    nav_msgs::Odometry odom_msg;
    odom_msg.pose.pose = pose_msg;
    odom_msg.header = pose_header;

    all_truth_pose_odoms.push_back(odom_msg);
  }

  for (size_t i = 0; i < all_frames.size(); i++) {
    all_pred_pose_odoms.push_back(
        posenode_to_odommsgs(all_frames[i]->cam_pose_Twc, pose_header));
  }

  ros::Rate loop_rate(5);  // 5
  int frame_number = -1;
  while (n.ok()) {
    frame_number++;
    if (frame_number == int(all_pred_pose_odoms.size())) {
      cout << "Finish all visulialization!" << endl;
    }
    // publish landmark
    if (frame_number < int(all_pred_pose_odoms.size())) {
      for (auto landmark = all_landmarks.begin();
           landmark != all_landmarks.end(); ++landmark) {
        if (landmark->second->isDetected) {
          cout << "class_id  " << landmark->second->class_id << endl;
          cout << "scale  " << landmark->second->Quadric_meas.scale << endl;
          cout << "pose  " << landmark->second->Quadric_meas.pose << endl;
          markers.markers[landmark->second->class_id].pose.position.x =
              landmark->second->Quadric_meas.pose.translation()(0);
          markers.markers[landmark->second->class_id].pose.position.y =
              landmark->second->Quadric_meas.pose.translation()(1);
          markers.markers[landmark->second->class_id].pose.position.z =
              landmark->second->Quadric_meas.pose.translation()(2);
          markers.markers[landmark->second->class_id].pose.orientation.x =
              landmark->second->Quadric_meas.pose.rotation().x();
          markers.markers[landmark->second->class_id].pose.orientation.y =
              landmark->second->Quadric_meas.pose.rotation().y();
          markers.markers[landmark->second->class_id].pose.orientation.z =
              landmark->second->Quadric_meas.pose.rotation().z();
          markers.markers[landmark->second->class_id].pose.orientation.w =
              landmark->second->Quadric_meas.pose.rotation().w();
          markers.markers[landmark->second->class_id].scale.x =
              landmark->second->Quadric_meas.scale(0);
          markers.markers[landmark->second->class_id].scale.y =
              landmark->second->Quadric_meas.scale(1);
          markers.markers[landmark->second->class_id].scale.z =
              landmark->second->Quadric_meas.scale(2);
        }
      }
      // publish camera pose estimation of this frame
      cout << "visulialization camera pose!" << endl;
      pub_slam_odompose.publish(all_pred_pose_odoms[frame_number]);
      pub_truth_odompose.publish(all_truth_pose_odoms[frame_number]);
      pub_landmark.publish(markers);
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
}
// NOTE offline_pred_objects and init_frame_poses are not used in
// online_detect_mode! truth cam pose of first frame is used.
void incremental_build_graph(Eigen::MatrixXd& offline_pred_frame_objects,
                             Eigen::MatrixXd& init_frame_poses,
                             Eigen::MatrixXd& truth_frame_poses) {
  Eigen::Matrix3d calib;
  calib << 535.4, 0, 320.1,  // for TUM cabinet data.
      0, 539.2, 247.6, 0, 0, 1;
  int total_frame_number = truth_frame_poses.rows();

  // detect all frames' cuboids.
  detect_3d_cuboid detect_cuboid_obj;
  detect_cuboid_obj.whether_plot_detail_images = false;
  detect_cuboid_obj.whether_plot_final_images = false;
  detect_cuboid_obj.print_details = false;  // false  true
  detect_cuboid_obj.set_calibration(calib);
  detect_cuboid_obj.whether_sample_bbox_height = false;
  detect_cuboid_obj.nominal_skew_ratio = 2;
  detect_cuboid_obj.whether_save_final_images = true;

  line_lbd_detect line_lbd_obj;
  line_lbd_obj.use_LSD = true;
  line_lbd_obj.line_length_thres = 15;  // remove short edges

  // graph optimization.
  // NOTE in this example, there is only one object!!! perfect association
  g2o::SparseOptimizer graph;
  g2o::BlockSolverX::LinearSolverType* linearSolver;
  linearSolver =
      new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
  g2o::BlockSolverX* solver_ptr = new g2o::BlockSolverX(linearSolver);
  g2o::OptimizationAlgorithmLevenberg* solver =
      new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
  graph.setAlgorithm(solver);
  graph.setVerbose(false);

  // only first truth pose is used. to directly visually compare with truth
  // pose. also provide good roll/pitch
  g2o::SE3Quat fixed_init_cam_pose_Twc(truth_frame_poses.row(0).tail<7>());

  // save optimization results of each frame
  std::vector<object_landmark*> cube_pose_opti_history(
      total_frame_number,
      nullptr);  // landmark pose after each frame's optimization
  std::vector<object_landmark*> cube_pose_raw_detected_history(
      total_frame_number,
      nullptr);  // raw detected cuboid frame each frame. before optimization

  int offline_cube_obs_row_id = 0;

  std::vector<tracking_frame*> all_frames(total_frame_number);
  g2o::VertexCuboid* vCube;

  // process each frame online and incrementally
  for (int frame_index = 0; frame_index < total_frame_number; frame_index++) {
    g2o::SE3Quat curr_cam_pose_Twc;
    g2o::SE3Quat odom_val;  // from previous frame to current frame

    if (frame_index == 0)
      curr_cam_pose_Twc = fixed_init_cam_pose_Twc;
    else {
      g2o::SE3Quat prev_pose_Tcw = all_frames[frame_index - 1]->cam_pose_Tcw;
      if (frame_index > 1)  // from third frame, use constant motion model to
                            // initialize camera.
      {
        g2o::SE3Quat prev_prev_pose_Tcw =
            all_frames[frame_index - 2]->cam_pose_Tcw;
        odom_val = prev_pose_Tcw * prev_prev_pose_Tcw.inverse();
      }
      curr_cam_pose_Twc = (odom_val * prev_pose_Tcw).inverse();
    }

    tracking_frame* currframe = new tracking_frame();
    currframe->frame_seq_id = frame_index;
    all_frames[frame_index] = currframe;

    bool has_detected_cuboid = false;
    g2o::cuboid cube_local_meas;
    double proposal_error = 1.0;
    char frame_index_c[256];
    sprintf(frame_index_c, "%04d", frame_index);  // format into 4 digit

    // read or detect cuboid
    if (online_detect_mode) {
      // start detect cuboid
      cv::Mat raw_rgb_img = cv::imread(
          base_folder + "raw_imgs/" + frame_index_c + "_rgb_raw.jpg", 1);

      // edge detection
      cv::Mat all_lines_mat;
      line_lbd_obj.detect_filter_lines(raw_rgb_img, all_lines_mat);
      Eigen::MatrixXd all_lines_raw(all_lines_mat.rows, 4);
      for (int rr = 0; rr < all_lines_mat.rows; rr++)
        for (int cc = 0; cc < 4; cc++)
          all_lines_raw(rr, cc) = all_lines_mat.at<float>(rr, cc);

      // read cleaned yolo 2d object detection
      Eigen::MatrixXd raw_2d_objs(10,
                                  5);  // 2d rect [x1 y1 width height], and prob
      if (!read_all_number_txt(base_folder + "/filter_2d_obj_txts/" +
                                   frame_index_c + "_yolo2_0.15.txt",
                               raw_2d_objs))
        return;
      raw_2d_objs.leftCols<2>().array() -=
          1;  // change matlab coordinate to c++, minus 1

      Matrix4d transToWolrd;
      detect_cuboid_obj.whether_sample_cam_roll_pitch =
          (frame_index != 0);  // first frame doesn't need to sample cam pose.
                               // could also sample. doesn't matter much
      if (detect_cuboid_obj
              .whether_sample_cam_roll_pitch)  // sample around
                                               // first frame's pose
        transToWolrd = fixed_init_cam_pose_Twc.to_homogeneous_matrix();
      else
        transToWolrd = curr_cam_pose_Twc.to_homogeneous_matrix();

      std::vector<ObjectSet>
          frames_cuboids;  // each 2d bbox generates an ObjectSet, which is
                           // vector of sorted proposals
      detect_cuboid_obj.detect_cuboid(raw_rgb_img, transToWolrd, raw_2d_objs,
                                      all_lines_raw, frames_cuboids);
      currframe->cuboids_2d_img = detect_cuboid_obj.cuboids_2d_img;

      has_detected_cuboid =
          frames_cuboids.size() > 0 && frames_cuboids[0].size() > 0;
      if (has_detected_cuboid)  // prepare object measurement
      {
        cuboid* detected_cube =
            frames_cuboids[0][0];  // NOTE this is a simple dataset, only one
                                   // landmark

        g2o::cuboid cube_ground_value;  // cuboid in the local ground frame.
        Vector9d cube_pose;
        cube_pose << detected_cube->pos(0), detected_cube->pos(1),
            detected_cube->pos(2), 0, 0, detected_cube->rotY,
            detected_cube->scale(0), detected_cube->scale(1),
            detected_cube->scale(2);  // xyz roll pitch yaw scale
        cube_ground_value.fromMinimalVector(cube_pose);
        cube_local_meas = cube_ground_value.transform_to(
            curr_cam_pose_Twc);  // measurement is in local camera frame

        if (detect_cuboid_obj
                .whether_sample_cam_roll_pitch)  // if camera roll/pitch is
                                                 // sampled, transform to the
                                                 // correct camera frame.
        {
          Vector3d new_camera_eulers =
              detect_cuboid_obj.cam_pose_raw.euler_angle;
          new_camera_eulers(0) += detected_cube->camera_roll_delta;
          new_camera_eulers(1) += detected_cube->camera_pitch_delta;
          Matrix3d rotation_new = euler_zyx_to_rot<double>(
              new_camera_eulers(0), new_camera_eulers(1), new_camera_eulers(2));
          Vector3d trans = transToWolrd.col(3).head<3>();
          g2o::SE3Quat curr_cam_pose_Twc_new(rotation_new, trans);
          cube_local_meas =
              cube_ground_value.transform_to(curr_cam_pose_Twc_new);
        }
        proposal_error = detected_cube->normalized_error;
      }
    } else {
      int cube_obs_frame_id =
          int(offline_pred_frame_objects(offline_cube_obs_row_id, 0));
      has_detected_cuboid = cube_obs_frame_id == frame_index;
      if (has_detected_cuboid)  // prepare object measurement   not all frame
                                // has observation!!
      {
        VectorXd measure_data =
            offline_pred_frame_objects.row(offline_cube_obs_row_id);
        g2o::cuboid cube_ground_value;
        Vector9d cube_pose;
        cube_pose << measure_data(1), measure_data(2), measure_data(3), 0, 0,
            measure_data(4), measure_data(5), measure_data(6),
            measure_data(7);  // xyz roll pitch yaw scale
        cube_ground_value.fromMinimalVector(cube_pose);
        Eigen::VectorXd cam_pose_vec = init_frame_poses.row(frame_index);
        g2o::SE3Quat cam_val_Twc(
            cam_pose_vec.segment<7>(1));  // time x y z qx qy qz qw
        cube_local_meas = cube_ground_value.transform_to(
            cam_val_Twc);  // measurement is in local camera frame
        proposal_error = measure_data(8);

        // read offline saved 2d image
        std::string detected_cube_2d_img_name =
            base_folder + "pred_3d_obj_overview/" + std::string(frame_index_c) +
            "_best_objects.jpg";
        currframe->cuboids_2d_img = cv::imread(detected_cube_2d_img_name, 1);

        offline_cube_obs_row_id++;  // switch to next row  NOTE at most one
                                    // object one frame in this data
      }
    }

    if (has_detected_cuboid) {
      object_landmark* localcuboid = new object_landmark();

      localcuboid->cube_meas = cube_local_meas;
      localcuboid->meas_quality = (1 - proposal_error + 0.5) /
                                  2;  // initial error 0-1, higher worse,  now
                                      // change to [0.5,1] higher, better
      currframe->observed_cuboids.push_back(localcuboid);
    }

    // set up g2o cube vertex. only one in this dataset
    if (frame_index == 0) {
      g2o::cuboid init_cuboid_global_pose =
          cube_local_meas.transform_from(curr_cam_pose_Twc);
      vCube = new g2o::VertexCuboid();
      vCube->setEstimate(init_cuboid_global_pose);

      //      vCube->setFixed(false);
      graph.addVertex(vCube);
      vCube->setId(0);
    }

    // set up g2o camera vertex
    g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap();
    currframe->pose_vertex = vSE3;
    vSE3->setId(frame_index + 1);
    graph.addVertex(vSE3);

    vSE3->setEstimate(
        curr_cam_pose_Twc
            .inverse());  // g2o vertex usually stores world to camera pose.
    vSE3->setFixed(frame_index == 0);

    // add g2o camera-object measurement edges, if there is
    if (currframe->observed_cuboids.size() > 0) {
      object_landmark* cube_landmark_meas =
          all_frames[frame_index]->observed_cuboids[0];
      g2o::EdgeSE3Cuboid* e = new g2o::EdgeSE3Cuboid();
      e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(vSE3));
      e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(vCube));
      e->setMeasurement(cube_landmark_meas->cube_meas);
      e->setId(frame_index);
      Vector9d inv_sigma;
      inv_sigma << 1, 1, 1, 1, 1, 1, 1, 1, 1;
      inv_sigma = inv_sigma * 2.0 * cube_landmark_meas->meas_quality;
      Matrix9d info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal();
      e->setInformation(info);
      graph.addEdge(e);
    }

    // camera vertex, add cam-cam odometry edges
    if (frame_index > 0) {
      g2o::EdgeSE3Expmap* e = new g2o::EdgeSE3Expmap();
      e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                          all_frames[frame_index - 1]->pose_vertex));
      e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                          all_frames[frame_index]->pose_vertex));
      e->setMeasurement(odom_val);

      e->setId(total_frame_number + frame_index);
      Vector6d inv_sigma;
      inv_sigma << 1, 1, 1, 1, 1, 1;
      inv_sigma = inv_sigma * 1.0;
      Matrix6d info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal();
      e->setInformation(info);
      graph.addEdge(e);
    }
    graph.initializeOptimization();
    graph.optimize(5);  // do optimization!

    // retrieve the optimization result, for debug visualization
    for (int j = 0; j <= frame_index; j++) {
      all_frames[j]->cam_pose_Tcw = all_frames[j]->pose_vertex->estimate();
      all_frames[j]->cam_pose_Twc = all_frames[j]->cam_pose_Tcw.inverse();
    }
    object_landmark* current_landmark = new object_landmark();
    current_landmark->cube_vertex = new g2o::VertexCuboid();
    current_landmark->cube_vertex->setEstimate(vCube->estimate());
    cube_pose_opti_history[frame_index] = current_landmark;

    if (all_frames[frame_index]->observed_cuboids.size() > 0) {
      object_landmark* cube_landmark_meas =
          all_frames[frame_index]->observed_cuboids[0];
      g2o::cuboid local_cube = cube_landmark_meas->cube_meas;

      g2o::cuboid global_cube =
          local_cube.transform_from(all_frames[frame_index]->cam_pose_Twc);
      object_landmark* tempcuboids2 = new object_landmark();
      tempcuboids2->cube_vertex = new g2o::VertexCuboid();
      tempcuboids2->cube_vertex->setEstimate(global_cube);
      cube_pose_raw_detected_history[frame_index] = tempcuboids2;
    } else
      cube_pose_raw_detected_history[frame_index] = nullptr;
  }

  cout << "Finish all optimization! Begin visualization." << endl;

  publish_all_poses(all_frames, cube_pose_opti_history,
                    cube_pose_raw_detected_history, truth_frame_poses);
}

void incremental_build_graph_quadric(
    const Eigen::MatrixXd& offline_pred_frame_objects,
    const Eigen::MatrixXd& init_frame_poses,
    const Eigen::MatrixXd& truth_frame_poses,
    const Eigen::MatrixXd& detect_result_bbox) {
  Eigen::Matrix3d calib;
  //  calib << 535.4, 0, 320.1,  // for TUM cabinet data.
  //      0, 539.2, 247.6, 0, 0, 1;
  calib << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1;
  const int total_frame_number = int(truth_frame_poses.rows());

  // graph optimization.
  //  perfect association
  g2o::SparseOptimizer graph;
  //  g2o::BlockSolverX::LinearSolverType* linearSolver;
  //  linearSolver =
  //      new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
  std::shared_ptr<g2o::BlockSolverX::LinearSolverType> linearSolver =
      std::make_shared<
          g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>>();
  std::shared_ptr<g2o::BlockSolverX> solver_ptr =
      std::make_shared<g2o::BlockSolverX>(linearSolver.get());
  std::shared_ptr<g2o::OptimizationAlgorithmLevenberg> solver =
      std::make_shared<g2o::OptimizationAlgorithmLevenberg>(solver_ptr.get());
  graph.setAlgorithm(solver.get());
  graph.setVerbose(false);

  // only first truth pose is used. to directly visually compare with truth
  // pose. also provide good roll/pitch
  g2o::SE3Quat fixed_init_cam_pose_Twc(truth_frame_poses.row(0).tail<7>());

  // int offline_cube_obs_row_id = 0; //used in offline mode

  std::vector<std::shared_ptr<tracking_frame_quadric>> all_frames;
  std::map<int, std::shared_ptr<Quadric_landmark>> all_landmark;
  int totall_landmark = 0;
  int vertexID = 0;
  int edgeID = 0;

  cout << "total_frame_number: " << total_frame_number << endl;
  cout << "fixed_init_cam_pose_Twc: " << fixed_init_cam_pose_Twc << endl;

  // DP process for the d
  vector<std::shared_ptr<ds::DrProcess>> dps;
  for (int i = 0; i < TOTALL_CLASS; ++i) {
    dps.emplace_back(
        std::make_shared<ds::DrProcess>(0.1));  // 0.1 is a hyper-parameter
  }

  // process each frame online and incrementally
  for (int frame_index = 0; frame_index < total_frame_number; frame_index++) {
    cout << "loop begin" << endl;
    g2o::SE3Quat curr_cam_pose_Twc;  //  camera to world!!!
    g2o::SE3Quat odom_val;           // from previous frame to current frame

    if (frame_index == 0)
      curr_cam_pose_Twc = fixed_init_cam_pose_Twc;
    else {
      g2o::SE3Quat temp_cam_pose_Twc(truth_frame_poses.row(frame_index)
                                         .tail<7>());  // use true pose to debug

      g2o::SE3Quat prev_pose_Tcw = all_frames[frame_index - 1]->cam_pose_Tcw;
      if (frame_index > 1)  // from third frame, useconstant motion model to
                            // initialize camera.
      {
        g2o::SE3Quat prev_prev_pose_Tcw =
            all_frames[frame_index - 2]->cam_pose_Tcw;
        odom_val = temp_cam_pose_Twc.inverse() * prev_pose_Tcw.inverse();
      }
      curr_cam_pose_Twc = temp_cam_pose_Twc;
      //      curr_cam_pose_Twc =
      //          (odom_val * prev_pose_Tcw)
      //              .inverse();  // predict cam position using last transition
    }
    auto currframe = std::make_shared<tracking_frame_quadric>(frame_index);
    all_frames.push_back(currframe);

    char frame_index_c[256];
    sprintf(frame_index_c, "%04d", frame_index);  // format into 4 digit

    cout << "start detection" << endl;

    if (online_detect_mode) {
      cv::Mat raw_rgb_img = cv::imread(
          base_folder + "raw_imgs/" + frame_index_c + "_rgb_raw.jpg", 1);
      // read cleaned yolo 2d object detection, !!! assume only one bbox
      Eigen::MatrixXd raw_2d_objs(MAX_DETECTION,
                                  5);  // 2d rect [x1 y1 width height],and prob
      if (!read_all_number_txt(base_folder + "/filter_2d_obj_txts/" +
                                   frame_index_c + "_yolo2_0.15.txt",
                               raw_2d_objs))
        return;
      raw_2d_objs.leftCols<2>().array() -=
          1;  // change matlab coordinate to c++, minus 1
      cout << "change matlab coordinate to c++" << endl;
      for (int i = 0; i < 4 /*raw_2d_objs.rows()*/; i++) {
        // Vector5d temp = raw_2d_objs.block(i, 0, 1, 5).transpose();  // cube
        // data

        // our date
        Vector6d temp;
        temp.block(0, 0, 5, 1) =
            detect_result_bbox.block(frame_index * 4 + i, 0, 1, 5).transpose();
        temp(5) = i;
        // change to [x,y,width,height,prop]
        double centreX = (temp(0) + temp(2)) / 2;
        double centreY = (temp(1) + temp(3)) / 2;
        double width = temp(2) - temp(0);
        double height = temp(3) - temp(1);
        temp(0) = centreX;
        temp(1) = centreY;
        temp(2) = width;
        temp(3) = height;

        cout << "detect_result_bbox" << temp << endl;
        auto tempDR =
            std::make_shared<Detection_result>(temp.head(5), frame_index);
        currframe->detect_results.push_back(tempDR);

        cout << "data association" << endl;
        int associaId = -1;
        double maxPro = 0.0;

        for (auto landmark = all_landmark.begin();
             landmark != all_landmark.end(); ++landmark) {
          const double pro = dps[landmark->second->class_id]->calProb(
                                 landmark->second->totall_id) *
                             landmark->second->ds.calProb(
                                 int(temp(5))) /* *calPositionPro */;  // Todo:
          if (pro > maxPro) {
            maxPro = pro;
            associaId = landmark->second->totall_id;
          }
        }
        if (maxPro <
            dps[int(temp(5))]->calProb(dps[int(temp(5))]->totallClass)) {
          auto newLandmark =
              std::make_shared<Quadric_landmark>(totall_landmark);
          newLandmark->class_id = int(temp(5));
          tempDR->total_id = totall_landmark;
          all_landmark.insert(pair<int, std::shared_ptr<Quadric_landmark>>(
              totall_landmark, newLandmark));
          totall_landmark++;
          newLandmark->landmark_id = dps[newLandmark->class_id]->totallClass;
          dps[newLandmark->class_id]->newClass();
        } else {
          all_landmark[associaId]->ds.update(int(temp(5)));
          int maxCoee;
          double maxPro;
          all_landmark[associaId]->ds.maxPro(maxCoee, maxPro);
          if (maxCoee != all_landmark[associaId]->class_id) {
            dps[maxCoee]->newClass();
            dps[all_landmark[associaId]->class_id]->update(
                all_landmark[associaId]->landmark_id, ds::DECREASE);

            all_landmark[associaId]->class_id = maxCoee;
            all_landmark[associaId]->landmark_id =
                dps[maxCoee]->totallClass - 1;
            tempDR->total_id = associaId;
          } else {
            dps[all_landmark[associaId]->class_id]->update(
                all_landmark[associaId]->landmark_id, ds::INCREASE);
          }
        }
      }
      cout << "totall class" << totall_landmark << endl;
      // detect_cuboid_obj.detect_cuboid(raw_rgb_img, transToWolrd,raw_2d_objs,
      //                                 all_lines_raw, frames_cuboids);
      // currframe->cuboids_2d_img = detect_cuboid_obj.cuboids_2d_img;
    } else {
      // Todo:offline mode
      // int cube_obs_frame_id =
      //     offline_pred_frame_objects(offline_cube_obs_row_id, 0);
      // has_detected_cuboid = cube_obs_frame_id == frame_index;
      // if (has_detected_cuboid)
      // prepare object measurement   not all frame
      //                           // has observation!!
      // {
      //   VectorXd measure_data =
      //       offline_pred_frame_objects.row(offline_cube_obs_row_id);
      //   g2o::cuboid cube_ground_value;
      //   Vector9d cube_pose;
      //   cube_pose << measure_data(1), measure_data(2), measure_data(3),0, 0,
      //       measure_data(4), measure_data(5), measure_data(6),
      //       measure_data(7);  // xyz roll pitch yaw scale
      //   cube_ground_value.fromMinimalVector(cube_pose);
      //   Eigen::VectorXd cam_pose_vec = init_frame_poses.row(frame_index);
      //   g2o::SE3Quat cam_val_Twc(
      //       cam_pose_vec.segment<7>(1));  // time x y z qx qy qz qw
      //   cube_local_meas = cube_ground_value.transform_to(
      //       cam_val_Twc);  // measurement is in local camera frame
      //   proposal_error = measure_data(8);

      //   // read offline saved 2d image
      //   std::string detected_cube_2d_img_name =
      //       base_folder + "pred_3d_obj_overview/" +
      //       std::string(frame_index_c)
      //       +
      //       "_best_objects.jpg";
      //   currframe->cuboids_2d_img =cv::imread(detected_cube_2d_img_name, 1);

      //   offline_cube_obs_row_id++;  // switch to next row  NOTE at most one
      //                               // object one frame in this data
      // }
    }

    cout << "set up g2o camera vertex" << endl;
    // set up g2o camera vertex
    std::shared_ptr<g2o::VertexSE3Expmap> vSE3 =
        std::make_shared<g2o::VertexSE3Expmap>();  // Todo: delete
    currframe->pose_vertex = vSE3;
    vSE3->setId(vertexID++);
    graph.addVertex(vSE3.get());
    vSE3->setEstimate(
        curr_cam_pose_Twc
            .inverse());  // g2o vertex usually stores world to camera pose.
    vSE3->setFixed(frame_index == 0);

    cout << "add cam-cam odometry edges" << endl;
    // camera vertex, add cam-cam odometry edges
    if (frame_index > 0) {
      g2o::EdgeSE3Expmap* e = new g2o::EdgeSE3Expmap();
      e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                          all_frames[frame_index - 1]->pose_vertex.get()));
      e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                          all_frames[frame_index]->pose_vertex.get()));
      e->setMeasurement(odom_val);

      e->setId(edgeID++);

      Vector6d inv_sigma;
      inv_sigma << 1, 1, 1, 1, 1, 1;
      inv_sigma = inv_sigma * 1.0;

      Matrix6d info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal();
      e->setInformation(info);
      graph.addEdge(e);
      // do optimization!
      graph.initializeOptimization();
      graph.optimize(5);
    }
    // update camera pose
    for (int j = 0; j <= frame_index; j++) {
      all_frames[j]->cam_pose_Tcw = all_frames[j]->pose_vertex->estimate();
      all_frames[j]->cam_pose_Twc = all_frames[j]->cam_pose_Tcw.inverse();
    }
    cout << "update landmark" << endl;
    // update landmark
    bool landmarkUpdate = false;  // if find now consrtain

    for (auto bbox = currframe->detect_results.begin();
         bbox != currframe->detect_results.end(); ++bbox) {
      auto landmark = all_landmark.find((*bbox)->total_id);
      landmark->second->quadric_tracking.push_back(*bbox);
      vector<Eigen::Matrix<double, 3, 4>,
             Eigen::aligned_allocator<Eigen::Matrix<double, 3, 4>>>
          projection_matrix;

      for (auto matrix = landmark->second->quadric_tracking.begin();
           matrix != landmark->second->quadric_tracking.end(); ++matrix) {
        projection_matrix.push_back(all_frames[(*matrix)->frame_seq_id]
                                        ->cam_pose_Tcw.to_homogeneous_matrix()
                                        .block(0, 0, 3, 4));
      }
      cout << "start quadric detection" << endl;
      landmark->second->quadric_detection(calib, projection_matrix);
      cout << "end quadric detection" << endl;
      if (landmark->second->isDetected == NEW_QUADRIC) {  // new v
        cout << "new quadric vertex" << endl;
        graph.addVertex(landmark->second->quadric_vertex.get());
        landmark->second->quadric_vertex->setId(vertexID++);

        for (auto deteResult = landmark->second->quadric_tracking.begin();
             deteResult != landmark->second->quadric_tracking.end();
             ++deteResult) {
          auto e = std::make_shared<g2o::EdgeSE3QuadricProj>();
          e->calib = calib;
          e->setVertex(
              0,
              dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                  all_frames[(*deteResult)->frame_seq_id]->pose_vertex.get()));
          e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                              landmark->second->quadric_vertex.get()));
          e->setMeasurement((*deteResult)->bbox);
          e->setId(edgeID++);
          Vector4d inv_sigma;
          inv_sigma << 1, 1, 1, 1;
          inv_sigma = inv_sigma * 1.0 * landmark->second->meas_quality *
                      (*deteResult)->prop;
          Matrix4d info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal();
          e->setInformation(info);
          graph.addEdge(e.get());
        }
      } else if (landmark->second->isDetected == UPDATE_QUADRIC) {
        landmarkUpdate = true;
        cout << "update quadric vertex" << endl;
        auto e = std::make_shared<g2o::EdgeSE3QuadricProj>();
        e->calib = calib;
        e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                            currframe->pose_vertex.get()));
        e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
                            landmark->second->quadric_vertex.get()));
        e->setMeasurement((*bbox)->bbox);
        e->setId(edgeID++);
        Vector4d inv_sigma;
        inv_sigma << 1, 1, 1, 1;
        inv_sigma =
            inv_sigma * 1.0 * landmark->second->meas_quality * (*bbox)->prop;
        Matrix4d info = inv_sigma.cwiseProduct(inv_sigma).asDiagonal();
        e->setInformation(info);
        graph.addEdge(e.get());
      }
    }

    cout << "do optimization" << endl;
    // do optimization!

    if (landmarkUpdate) {
      cout << "landmarkUpdate " << frame_index << endl;
      graph.initializeOptimization();
      graph.optimize(1);
    }
    cout << "update camera pose" << endl;
    // update camera pose and quadric
    for (int j = 0; j <= frame_index; j++) {
      all_frames[j]->cam_pose_Tcw = all_frames[j]->pose_vertex->estimate();
      all_frames[j]->cam_pose_Twc = all_frames[j]->cam_pose_Tcw.inverse();
    }
    cout << "update quadric" << endl;
    for (auto landmark = all_landmark.begin(); landmark != all_landmark.end();
         ++landmark) {
      if (landmark->second->isDetected != NO_QUADRIC)
        landmark->second->Quadric_meas =
            landmark->second->quadric_vertex->estimate();
    }

    cout << "finished frame " << frame_index << endl;
  }

  cout << "Finish all optimization! Begin visualization." << endl;
  cout << "landmark size: " << all_landmark.size() << endl;
  cout << "truth_frame_poses.size: " << truth_frame_poses.size() << endl;
  cout << "all_frames.size: " << all_frames.size() << endl;
  assert(int(all_frames.size()) == int(truth_frame_poses.rows()));
  publish_all_poses_quadric(all_frames, truth_frame_poses, all_landmark);
}
int main(int argc, char* argv[]) {
  ros::init(argc, argv, "object_slam");

  ros::NodeHandle nh;

  nh.param("/base_folder", base_folder,
           ros::package::getPath("object_slam") + "/data/");
  nh.param("/online_detect_mode", online_detect_mode, true);
  nh.param("/save_results_to_txt", save_results_to_txt, false);

  cout << "" << endl;
  cout << "base_folder   " << base_folder << endl;
  if (online_detect_mode)
    ROS_WARN_STREAM("Online detect object mode !!\n");
  else
    ROS_WARN_STREAM("Offline read object mode !!\n");

  // NOTE important
  // in online mode, pred_frame_objects and init_frame_poses are not used. only
  // first frame of truth_frame_poses is used.

  std::string pred_objs_txt =
      base_folder +
      "detect_cuboids_saved.txt";  // saved cuboids in local ground frame.
  std::string init_camera_pose =
      base_folder +
      "pop_cam_poses_saved.txt";  // offline camera pose for cuboids detection
                                  // (x y yaw=0, truth roll/pitch/height)
  //  std::string truth_camera_pose = base_folder + "truth_cam_poses.txt";
  std::string truth_camera_pose = base_folder + "trajectories35.txt";
  std::string detect_result = base_folder + "detection35.txt";

  Eigen::MatrixXd pred_frame_objects(
      100, 10);  // 100 is some large row number, each row in txt has 10 numbers
  Eigen::MatrixXd init_frame_poses(100, 8);
  Eigen::MatrixXd truth_frame_poses(100, 7 /*8*/);
  Eigen::MatrixXd detect_result_bbox(150, 6);
  if (!read_all_number_txt(pred_objs_txt, pred_frame_objects))
    return -1;
  if (!read_all_number_txt(init_camera_pose, init_frame_poses))
    return -1;
  if (!read_all_number_txt(truth_camera_pose, truth_frame_poses))
    return -1;
  if (!read_all_number_txt(detect_result, detect_result_bbox))
    return -1;
  std::cout << "read data size:  " << pred_frame_objects.rows() << "  "
            << init_frame_poses.rows() << "  " << truth_frame_poses.rows()
            << std::endl;

  //  incremental_build_graph(pred_frame_objects, init_frame_poses,
  //                          truth_frame_poses);
  incremental_build_graph_quadric(pred_frame_objects, init_frame_poses,
                                  truth_frame_poses, detect_result_bbox);
  return 0;
}


/*
 * Adapt3DInterpolator.cpp
 *
 *  Created on: Oct 29, 2010
 *      Author: dberrios
 */

#include "Adapt3DInterpolator.h"
#include "Adapt3D.h" //include the smart values structure
#include "StringConstants.h"
#include "MathHelper.h"
#include <stdio.h>
#include <iostream>
//#include <boost/thread.hpp>
#define MIN_RANGE    -1e9
#define MAX_RANGE    +1e9
#define NNODE_ADAPT3D 4
#define LINEAR_INTERPOL

//#define DEBUG
//#define DEBUGS
namespace ccmc
{
	/**
	 * @param modelReader Pointer to the Model object containing the appropriate variable maps.  Adapt3DInterpolator
	 * should be returned by a Adapt3D::createNewInterpolator() call.
	 */
	Adapt3DInterpolator::Adapt3DInterpolator(Model * modelReader)
	{
		// TODO Auto-generated constructor stub
		this->modelReader = modelReader;
		/***  Open should have failed previosly, so they should exist! ***/

		this->grid_reg_no = (modelReader->getGlobalAttribute(ccmc::strings::variables::grid_reg_no_)).getAttributeInt();
		this->ndimn = (modelReader->getGlobalAttribute(ccmc::strings::variables::ndimn_)).getAttributeInt();
		this->npoin = (modelReader->getGlobalAttribute(ccmc::strings::variables::npoin_)).getAttributeInt();
		this->nelem = (modelReader->getGlobalAttribute(ccmc::strings::variables::nelem_)).getAttributeInt();
		//->nboun = (modelReader->getGlobalAttribute(ccmc::strings::variables::nboun_)).getAttributeInt();
		//this->nconi = (modelReader->getGlobalAttribute(ccmc::strings::variables::nconi_)).getAttributeInt();
		this->coord = modelReader->getVariableFromMap(ccmc::strings::variables::coord_);
		this->intmat = modelReader->getIntVariableFromMap(ccmc::strings::variables::intmat_);
		//this->unkno = (modelReader->getVariableFromMap(ccmc::strings::variables::unkno_));

		this->smartSearchValues = ((Adapt3D*)(modelReader))->getSmartGridSearchValues();
		//indx = new int[nelem];
		//esup1 = new int[nelem*4];
		//esup2 = new int[npoin+1];
		this->nnode = NNODE_ADAPT3D;
		//this->unstructured_grid_setup_done = this->setupUnstructuredGridSearch();
		//this->smartSearchSetup();
		this->smartSearchValues->last_element_found = -1;
//		std::cout << "created Adapt3DInterpolator object" << std::endl;

	}

	/**
	 * @param variable
	 * @param c0 X component of the position
	 * @param c1 Y component of the position
	 * @param c2 Z component of the position
	 * @return
	 */
	float Adapt3DInterpolator::interpolate(const std::string& variable, const float& c0, const float& c1,
			const float& c2)
	{

		float dc0, dc1, dc2;
		//long variable_id = modelReader->getVariableID(variable);
		return interpolate(variable, c0, c1, c2, dc0, dc1, dc2);

	}

	/**
	 * @param variable_id
	 * @param c0 X component of the position
	 * @param c1 Y component of the position
	 * @param c2 Z component of the position
	 * @return
	 */
	float Adapt3DInterpolator::interpolate(const long& variable_id, const float& c0, const float& c1, const float& c2)
	{

		float dc0, dc1, dc2;
		return interpolate(variable_id, c0, c1, c2, dc0, dc1, dc2);

	}

	/**
	 * Interpolation method. Note that using the variable ID is significantly faster than using the variable string.
	 * @param variable_id A long representing the variable ID.
	 * @param c0 X component of the position
	 * @param c1 Y component of the position
	 * @param c2 Z component of the position
	 * @param dc0 Reference to a variable to store the delta for component 0
	 * @param dc1 Reference to a variable to store the delta for component 1
	 * @param dc2 Reference to a variable to store the delta for component 2
	 * @return The interpolated value at position (c0,c1,c2) with deltas (dc0,dc1,dc2)
	 */
	float Adapt3DInterpolator::interpolate(const long& variable_id, const float& c0, const float& c1, const float& c2,
			float& dc0, float& dc1, float& dc2)
	{

		std::string variable = this->modelReader->getVariableName(variable_id);
		return interpolate(variable, c0,c1,c2,dc0,dc1,dc2);
	}

	/**
	 * Interpolation method. Note that using the variable ID is significantly faster than using the variable string.
	 * @param variable The input variable.
	 * @param c0 X component of the position
	 * @param c1 Y component of the position
	 * @param c2 Z component of the position
	 * @param dc0 Reference to a variable to store the delta for component 0
	 * @param dc1 Reference to a variable to store the delta for component 1
	 * @param dc2 Reference to a variable to store the delta for component 2
	 * @return The interpolated value at position (c0,c1,c2) with deltas (dc0,dc1,dc2)
	 */
	float Adapt3DInterpolator::interpolate(const std::string& variable, const float& c0, const float& c1,
			const float& c2, float& dc0, float& dc1, float& dc2)
	{
		float missingValue = this->modelReader->getMissingValue();
		//this->last_element_found = -1;
		if (!point_within_grid(c0,c1,c2))
			return missingValue;
		//float rsun_in_meters = 7.0e8;
		//float unkno_local[9];

		long counts[1] = { 0 };
		long intervals[1] = { 1 };

		float interpolated_value;

		int clear_cache;

		int     array_size;
		int     istatus;
		int     ielem;

		//char variable_name0[] = "intmat";
		//char variable_name1[] = "coord";

		/**
		* TODO: figure out what to do about the dc0,dc1,dc2 values
		*/
		dc0=dc1=dc2=.02f;

		/** lets see if required variables are in memory **/

		/****     since the cdf data is stored in r[meters], radians, radians or r, phi theta
		*         but we are accepting input as r[AU], lon, lat - we must do some coordiante transformations
		*/

		/* convert rsun to meters */
		/*
		   coord1[0] = X * rsun_in_meters;
		   coord1[1] = Y * rsun_in_meters;
		   coord1[2] = Z * rsun_in_meters;
		*/

		counts[0] = 0; /*reset values after once through */
		intervals[0] = 1;

		/* for field line tracing,etc..., select appropriate variable number ie. bx_cdfNum|by_cdfNum|bz_cdfNum based on *variable_string */
		/* also select appropriate position arrays x, y , z for bx1, by1,bz1 */

		/* set default grid arrays and change if neccessary */

		//char *filename="adapt3d_kameleon_soln.cdf";

		if ( variable == ccmc::strings::variables::bx_ || variable == ccmc::strings::variables::b1_)
		{
		  //unkno_index=5;
		}
		else if ( variable == "by" || variable == "b2")
		{
		 // unkno_index=6;
		}
		else if ( variable == "bz" || variable == "b3")
		{
		 // unkno_index=7;
		}
		else if ( variable == ccmc::strings::variables::ux_ || variable == "u1")
		{
		  //unkno_index=1;
		}
		else if ( variable == ccmc::strings::variables::uy_ || variable == "u2")
		{
		  //unkno_index=2;
		}
		else if ( variable == ccmc::strings::variables::uz_ || variable == "u3")
		{
		  //unkno_index=3;
		}
		else if (variable == ccmc::strings::variables::rho_)
		{
		  //unkno_index=0;
		}
		else if (variable == ccmc::strings::variables::p_ || variable == ccmc::strings::variables::e_)
		{
		  //unkno_index=4;
		}
		else if ( variable == ccmc::strings::variables::temp_ )
		{
		  //unkno_index=8;
		}
		else
		{
		  printf(
				"ERROR:\tcould not find cdf variable number for %s\n",
				variable.c_str());
		  printf(
				"Valid Variable Names for ADAPT3D :\n bx OR b1 \n by OR b2 \n bz OR b3\n ux OR u1 \n uy OR u2 \n uz OR u3 \n p OR e \n OR temp\n----------------------------------------------\n"
				);
		  return 0;
		}

		clear_cache = 0;
			/* locate the grid element that contains the point coord1 */
		//ielem = smartSearch(c0,c1,c2);
		ielem = findElement(c0,c1,c2, clear_cache);
		//Element * element = this->smartSearchValues->parent->findElement(c0, c1, c2);
		//if (element != NULL)
		//	ielem = this->smartSearchValues->parent->findElement(c0, c1, c2)->getIndex();
		//else ielem = -1;
#ifdef DEBUG
	   std::cerr << "ielem: " << ielem << " for position " << c0 << "," << c1 << "," << c2 << std::endl;
#endif
		interpolated_value = missingValue;     /* test value */

		if(ielem > -1)
		{
		   interpolated_value = interpolate_adapt3d_solution(c0, c1, c2, ielem, variable);
#ifdef DEBUG
		   std::cout << "MIN_RANGE: " << MIN_RANGE << " this->missingValue: " << this->missingValue << " interpolated_value: " << interpolated_value << std::endl;
#endif
		   this->smartSearchValues->last_element_found = ielem;
		} else {
			//printf("Failed to find point in grid\n");
			this->smartSearchValues->last_element_found = -1;
		}

		/*  return interpolated_value  */
		if (interpolated_value >= MIN_RANGE && interpolated_value <= MAX_RANGE && interpolated_value != missingValue)
		{

		  //std::cerr << "position: " << c0 << "," << c1 << "," << c2 << ": " << (float)interpolated_value << std::endl;
		  return (float)interpolated_value;

		}
		else
		{
		  //std::cerr << "position: " << c0 << "," << c1 << "," << c2 << ": " << this->missingValue << std::endl;
		  return missingValue;
		}
	}

	int Adapt3DInterpolator::smartSearch(const float& c0, const float& c1, const float& c2)
	{
//#define DEBUGS
		int lfound, mask[NNODE_ADAPT3D], try_grid_search;

		int  i,j,k,ielem,inode,jnode ;
		int  ifound, jelem, kelem;
		int  node_order[NNODE_ADAPT3D];
		int  i_node, j_node, k_node, k_node_hi;
		int  next_node, i_order;

		int  nelems_checked;
		int  clear_cache;

		float  shapex[NNODE_ADAPT3D];
		float  radius;

		float  distance[NNODE_ADAPT3D];

		float size_of_last_element;

#ifdef DELAUNEY_SEARCH
       int   iteration, iteration_max;
       int   next_element, next_element0;
       int   in0,in1,in2,in3,iselect;
       int   jnext,jk,jrand;
       int   i_s,j_s,k_s;
       float x_last_element,y_last_element,z_last_element,distance0;
       float x,y,z,r,t,p;
#endif

//std::cout << "smart search" << std::endl;
/*----------------------------------------------------------------
!
! Step A
!
! First check the last_element_found to see if the new point is still
! inside it. If yes, then set ifound=.true'
*/
		ifound = -1;
		if( this->smartSearchValues->last_element_found >= 0 )
		{
#ifdef DEBUGS
			printf("Checkin if still in last element \n");
#endif

			ifound = chkineln(c0,c1,c2, this->smartSearchValues->last_element_found ,shapex);
			nelems_checked = 1;
		}
//std::cout << "ifound: " << ifound << std::endl;
/*--------*/
		if( ifound == 0 )
		{
/*--------*/

			#ifdef DEBUGS
			printf("Point is still in starting element! \n");
			std::cerr << "this->smartSearchValues->last_element_found: " << this->smartSearchValues->last_element_found << std::endl;
			#endif
			kelem = this->smartSearchValues->last_element_found;
			//this->smartSearchValues->still_in_same_element++;

/*--------*/
		} else
		{
/*--------*/

/* If we have a starting_element number set to begin the search  */
			if( this->smartSearchValues->last_element_found >= 0 )
			{

//std::cout << "starting search" << std::endl;
#ifdef DELAUNEY_SEARCH
/* Delauney algorithm */
           ifound=1;
           iteration=0;
           next_element=this->smartSearchValues->last_element_found;
           iteration_max=DELAUNEY_ITER_MAX;
           while ((ifound != 0) && (iteration < iteration_max)) {
             kelem = chkineln(c0,c1,c2,next_element ,shapex);

             in0=(*intmat)[ index_2d_to_1d(next_element,0,4) ];
             in1=(*intmat)[ index_2d_to_1d(next_element,1,4) ];
             in2=(*intmat)[ index_2d_to_1d(next_element,2,4) ];
             in3=(*intmat)[ index_2d_to_1d(next_element,3,4) ];
             x_last_element = 0.25*(
                    (*coord)[ index_2d_to_1d(in0,0,3) ]
                   +(*coord)[ index_2d_to_1d(in1,0,3) ]
                   +(*coord)[ index_2d_to_1d(in2,0,3) ]
                   +(*coord)[ index_2d_to_1d(in3,0,3) ] );
             y_last_element = 0.25*(
                    (*coord)[ index_2d_to_1d(in0,1,3) ]
                   +(*coord)[ index_2d_to_1d(in1,1,3) ]
                   +(*coord)[ index_2d_to_1d(in2,1,3) ]
                   +(*coord)[ index_2d_to_1d(in3,1,3) ] );
             z_last_element = 0.25*(
                    (*coord)[ index_2d_to_1d(in0,2,3) ]
                   +(*coord)[ index_2d_to_1d(in1,2,3) ]
                   +(*coord)[ index_2d_to_1d(in2,2,3) ]
                   +(*coord)[ index_2d_to_1d(in3,2,3) ] );
             distance0=std::sqrt( (x_last_element-c0)*(x_last_element-c0)
                           + (y_last_element-c1)*(y_last_element-c1)
                           + (z_last_element-c2)*(z_last_element-c2) );

/* If the distance  from last element found is too great compared with the element size then force use of the structured grid */

#ifdef DEBUGS
             printf("Delauney iteration no %d\n",iteration);
             radius=sqrt(x_last_element*x_last_element+y_last_element*y_last_element+z_last_element*z_last_element);
             printf("Center of last element %d in search : radius %e\n",next_element,radius);
             printf("Center of last element %d in search %e %e %e\n",next_element,x_last_element,y_last_element,z_last_element);
        x=x_last_element;
        y=y_last_element;
        z=z_last_element;
        Math::convert_xyz_to_rthetaphi(x,y,z,&r,&t,&p);
             printf("Center of last element (rtp) %d in search %e %e %e\n",next_element,r,t,p);

           i_s = (int)( (r-this->smartSearchValues->xl_sg)/this->smartSearchValues->dx_sg );
           j_s = (int)( (t-this->smartSearchValues->yl_sg)/this->smartSearchValues->dy_sg );
           k_s = (int)( (p-this->smartSearchValues->zl_sg)/this->smartSearchValues->dz_sg );
           printf("Located in structured cell %d %d %d\n",i_s,j_s,k_s);

             printf("Distance from last element to search point is %e\n",distance0);
             printf("Search pt coords %e %e %e \n",c0,c1,c2);
             printf("Search pt radius %e\n",std::sqrt(c0*c0+c1*c1+c2*c2));
             fflush(stdout);
#endif
             next_element0=next_element;
             jrand=(int)(3.0001 *(float)rand() / (float)RAND_MAX );
             jk=0;
             iselect=-1;
             while ((iselect == -1) && (jk < 4)) {
               jnext=(jk+jrand)%4;
               if(shapex[ jnext ] < 0.) {
                 next_element=(*this->smartSearchValues->facing_elements)[ index_2d_to_1d(next_element0,jnext,4) ];
                 if(next_element > -1) {
                   iselect=jnext;
#ifdef DEBUGS
                 } else {
                  printf("Loop %d: Face %d is a boundary with next_element=%d. Skip to next face\n",jk,jnext,next_element);
                  printf("shapex %e %e %e %e\n",shapex[0],shapex[1],shapex[2],shapex[3]);
                  fflush(stdout);
#endif
                 }
#ifdef DEBUGS
                 printf("jrand=%d\n",jrand);
                 printf("iselect=%d\n",iselect);
                 fflush(stdout);
#endif
               }
               jk=jk+1;
             }

             iteration++;

/* If only face in direction of search point is a boundary then end search and reset last-element_found */
             if(next_element == -999) {
                iteration=iteration_max;
                this->smartSearchValues->last_element_found=-1;
                kelem=-1;
               // this->smartSearchValues->outside_grid += 1;
             }

             if(kelem == 0) {
               ifound=0;
#ifdef DEBUGS
                 printf("Delauney search successful : found in element %d \n",next_element);
    //             fflush(stdout);
#endif
                 if(iteration-1 < DELAUNEY_ITER_MAX) (*this->smartSearchValues->delauney_search_iteration_profile)[iteration-1] += 1;
             }
           }
           if(ifound == 0)  kelem=next_element;

/* End of Delauney algorithm */
#endif /* ifdef DELAUNEY_SEARCH */

       }      /*   if( last_element_found .ge. 0 )  */

/*--------*/
       }      /*   if( ifound .eq. 0)  */
/*--------*/

#ifdef DEBUGS
       if( ifound != 0) {
          printf("Smart search failed! \n");
          printf("search_point_coords %e %e %e \n",c0,c1,c2);
                 fflush(stdout);

          if(kelem > 0) {
              printf("Found in element %i \n",kelem);
              fflush(stdout);
          } else {
              printf("Failed to locate element in grid \n");
              fflush(stdout);
          }
       }
#endif

		return kelem;

	}

	int Adapt3DInterpolator::findElement(const float& c0, const float& c1, const float& c2, int clear_cache)
	{

//#define DEBUGS

		//std::cout << "entered findElement" << std::endl;

		int			ielem,kelem, inode;
		int         i_s,j_s,k_s,i,j,k,indx_start,indx_end;
		int         indx1,ifound,just_found,jelem;
		float       x,y,z;
        //float* shapex = new float[nnode];
		int			next_element, next_element0, iselect;

		int        in0,in1,in2,in3,iteration;
		int        delta_i;
		float      x_last_element,y_last_element,z_last_element;
		float      distance;
		float      ddx,ddy,ddz,ss,xx,yy,zz,radius;
		int        new_del,i_new,j_new,k_new;
		float      r, t, p;
		float      distance0,size_of_last_element;

		int      i_min,i_max;
		int      j_min,j_max;
		int      k_min,k_max;
		int      j0,k0;

		kelem=-1;
		ielem=-1;
		ifound=-1;
		if ( point_within_grid(c0, c1, c2) == 1)
		{

	         if(clear_cache == 1) this->smartSearchValues->last_element_found=-1;
	#ifdef DEBUG
	       printf("0find_element: (*coord)[0][0-2] : %e %e %e \n",(*coord)[ index_2d_to_1d(0,0,npoin) ],(*coord)[ index_2d_to_1d(0,1,npoin) ],(*coord)[ index_2d_to_1d(0,2,npoin) ]);
	#endif

	       /* If available, use the last element found to begin the search */
	       if(this->smartSearchValues->last_element_found != -1) {
	    	   in0=(*intmat)[ index_2d_to_1d(this->smartSearchValues->last_element_found,0,4) ];
	    	   in1=(*intmat)[ index_2d_to_1d(this->smartSearchValues->last_element_found,1,4) ];
	    	   x_last_element=(*coord)[ index_2d_to_1d(in0,0,3) ];
	    	   y_last_element=(*coord)[ index_2d_to_1d(in0,1,3) ];
	    	   z_last_element=(*coord)[ index_2d_to_1d(in0,2,3) ];
	    	   distance0=std::sqrt( (x_last_element-c0)*(x_last_element-c0)
	    			   	   	      + (y_last_element-c1)*(y_last_element-c1)
	    			   	   	      + (z_last_element-c2)*(z_last_element-c2));
	    	   size_of_last_element=std::sqrt(
	    			   ( (*coord)[ index_2d_to_1d(in0,0,3) ] -(*coord)[ index_2d_to_1d(in1,0,3) ] )*
	    			   ( (*coord)[ index_2d_to_1d(in0,0,3) ] -(*coord)[ index_2d_to_1d(in1,0,3) ] )
	    			   +( (*coord)[ index_2d_to_1d(in0,1,3) ] -(*coord)[ index_2d_to_1d(in1,1,3) ] )*
	    			   ( (*coord)[ index_2d_to_1d(in0,1,3) ] -(*coord)[ index_2d_to_1d(in1,1,3) ] )
	    			   +( (*coord)[ index_2d_to_1d(in0,2,3) ] -(*coord)[ index_2d_to_1d(in1,2,3) ] )*
	    			   ( (*coord)[ index_2d_to_1d(in0,2,3) ] -(*coord)[ index_2d_to_1d(in1,2,3) ] ) );

#ifdef DEBUGS
	    	   printf("coords %e %e %e %e %e %e\n", (*coord)[ index_2d_to_1d(in0,0,3) ],(*coord)[ index_2d_to_1d(in1,0,3) ],
	    			   (*coord)[ index_2d_to_1d(in0,1,3) ] ,(*coord)[ index_2d_to_1d(in1,1,3) ],
	    			   (*coord)[ index_2d_to_1d(in0,2,3) ] ,(*coord)[ index_2d_to_1d(in1,2,3) ] );
	    	   printf("distance to element  : %e\n",distance0);
	    	   printf("element size  : %e\n",size_of_last_element);
	    	   printf("Ratio of distance to element size  : %e\n",distance0/size_of_last_element);
#endif

	    	   /* If the distance  from last element found is too great compared with the element size then force use of the structured grid */
	    	   if (distance0/size_of_last_element > 50.) {
	    		   this->smartSearchValues->last_element_found = -1;
#ifdef DEBUGS
	    		   printf("Force new structured grid search\n");
#endif
	    	   }

	       }
	       /* If necessary generate a first guess element to begin search */
	       if(this->smartSearchValues->last_element_found == -1) {

#ifdef CARTESIAN_S_GRID
	    	   x = cintp[0];
	    	   y = cintp[1];
	    	   z = cintp[2];
#endif /* CARTESIAN_S_GRID */
#ifdef SPHERICAL_S_GRID
	    	   Math::convert_xyz_to_rthetaphi(c0,c1,c2,&r,&t,&p);
	    	   x=r;
	    	   y=t;
	    	   z=p;
#endif /* SPHERICAL_S_GRID */

#ifdef DEBUGS
	    	   printf("find_element: Searching for point x y z = %e %e %e\n",x,y,z);
#endif
	    	   i_s = (int)( (x-this->smartSearchValues->xl_sg)/this->smartSearchValues->dx_sg );
	    	   j_s = (int)( (y-this->smartSearchValues->yl_sg)/this->smartSearchValues->dy_sg );
	    	   k_s = (int)( (z-this->smartSearchValues->zl_sg)/this->smartSearchValues->dz_sg );
	    	   /* pmn fix section */
	    	   i_s = std::min(i_s,nx_sg-1);
	    	   i_s = std::max(i_s,0);
	    	   j_s = std::min(j_s,ny_sg-1);
	    	   j_s = std::max(j_s,0);
	    	   k_s = std::min(k_s,nz_sg-1);
	    	   k_s = std::max(k_s,0);
	    	   /* end pmn fix section */
#ifdef DEBUGS
	    	   printf("Located in structured cell %d %d %d\n",i_s,j_s,k_s);
#endif
	    	   if(this->smartSearchValues->nelems_in_cell[k_s][j_s][i_s] > 0) {
	    		   indx_start = this->smartSearchValues->start_index[k_s][j_s][i_s];
	    		   indx_end   = this->smartSearchValues->end_index[k_s][j_s][i_s];
	    		   delta_i=(int)( (float)(indx_end-indx_start) * ( (float)rand() / (float)RAND_MAX ) );
	    		   this->smartSearchValues->last_element_found = (*this->smartSearchValues->indx)[indx_start + delta_i];
#ifdef DEBUGS
	    		   printf("Reseting last_element_found from structured grid to %d delta_i %d\n",this->smartSearchValues->last_element_found,indx_end-indx_start);
#endif
	    	   } else {
	    		   this->smartSearchValues->last_element_found = -1;
#ifdef DEBUGS
	    		   printf("Structured cell has no elements - Trying the node list now\n");
	    		   printf("nnodes_in_cell %d %d %d is %d \n",i_s,j_s,k_s,this->smartSearchValues->nnodes_in_cell[k_s][j_s][i_s]);
	    		   fflush(stdout);
#endif
	    		   if(this->smartSearchValues->nnodes_in_cell[k_s][j_s][i_s] > 0) {
	    			   indx_start = this->smartSearchValues->start_index_nodes[k_s][j_s][i_s];
	    			   indx_end = this->smartSearchValues->end_index_nodes[k_s][j_s][i_s];
#ifdef DEBUGS
	    			   printf("nnodes_in_cell %d %d %d is %d \n",i_s,j_s,k_s,this->smartSearchValues->nnodes_in_cell[k_s][j_s][i_s]);
	    			   printf("indx_start indx_end %d %d\n",indx_start,indx_end);
	    			   fflush(stdout);
#endif
	    			   delta_i=(int)( (float)(indx_end-indx_start) * ( (float)rand() / (float)RAND_MAX ) );
	    			   last_node_found = (*this->smartSearchValues->indx_nodes)[indx_start + delta_i];
#ifdef DEBUGS
	    			   printf("last_node_found=%d\n",last_node_found);
	    			   printf("coords of last node are %e %e %e\n",(*coord)[last_node_found*3],
	    					   (*coord)[last_node_found*3+1],(*coord)[last_node_found*3+2]);
	    			   fflush(stdout);
#endif
	    			   this->smartSearchValues->last_element_found = this->smartSearchValues->esup1->at( this->smartSearchValues->esup2->at(last_node_found) );
#ifdef DEBUGS
	    			   printf("Node search found element %d as starting point for search\n", this->smartSearchValues->last_element_found);
	    			   //fflush(stdout);
#endif
	    		   } else {

#ifdef DEBUGS
	    			   printf("Structured cell is empty - Node search also failed\n");
	    			   printf("Start scanning neighbor cells using elements\n");
#endif
	    			   /* search immediate neighbor cells for search starting point */

	    			   /* Locate a neighboring cell of the structured grid that contains a node */
	    			   iselect = 1 ;

	    			   i_min = std::max(0,i_s-1);
	    			   i_max = std::min(nx_sg-1,i_s+1);
#ifdef CARTESIAN_S_GRID
	    			   j_min = max(0,j_s-1);
	    			   j_max = min(ny_sg-1,j_s+1);
	    			   k_min = max(0,k_s-1);
	    			   k_max = min(nz_sg-1,k_s+1);
#endif  /* CARTESIAN_S_GRID */
#ifdef SPHERICAL_S_GRID
	    			   j_min = std::max(j_s-1, 0);			/* pmn fix */
	    			   j_max = std::min(j_s+1, ny_sg-1); 	/* pmn fix */
	    			   k_min = k_s-1;
	    			   k_max = k_s+1;
#endif  /* SPHERICAL_S_GRID */

	    			   /* if using elements for search */
	    			   for(k=k_min;k<k_max+1;k++) {
	    				   k0=k;
#ifdef SPHERICAL_S_GRID
	    				   if(k==-1) k0=nz_sg-1;
	    				   if(k==nz_sg) k0=0;
#endif  /* SPHERICAL_S_GRID */
	    				   for(j=j_min;j<j_max+1;j++) {
	    					   j0=j;
#ifdef SPHERICAL_S_GRID
	    					   if(j==ny_sg) {
	    						   k0 = (k0+nz_sg/2)%(nz_sg-1);
	    						   j0 = ny_sg-1;
	    					   }
	    					   if(j==-1) {
	    						   k0 = (k0+nz_sg/2)%(nz_sg-1);
	    						   j0 = 0;
	    					   }
#endif  /* SPHERICAL_S_GRID */
	    					   for(i=i_min;i<i_max+1;i++) {
#ifdef DEBUGS
	    						   printf("cell %d %d %d nelems_in_cell %d\n",i,j0,k0,this->smartSearchValues->nelems_in_cell[k0][j0][i]);
#endif
	    						   if(this->smartSearchValues->nelems_in_cell[k0][j0][i]>0) {
	    							   iselect=0;
	    							   i_s=i;
	    							   j_s=j0;
	    							   k_s=k0;
	    						   }
	    					   }}}
	    			   if(iselect==0)  {
#ifdef DEBUGS
	    				   printf("Found a starting element in neighbor %d %d %d\n",i_s,j_s,k_s);
#endif
	    				   indx_start = this->smartSearchValues->start_index[k_s][j_s][i_s];
	    				   indx_end   = this->smartSearchValues->end_index[k_s][j_s][i_s];
	    				   delta_i=(int)( (float)(indx_end-indx_start) * ( (float)rand() / (float)RAND_MAX ) );
	    				   this->smartSearchValues->last_element_found = (*this->smartSearchValues->indx)[indx_start + delta_i];
#ifdef DEBUGS
	    				   printf("Element is %d\n",this->smartSearchValues->last_element_found);
#endif
	    			   }

	    			   /* if using nodes for search */
	    			   if(iselect == 1) {
#ifdef DEBUGS
	    				   printf("Neighbor search using elements failed also\n");
	    				   printf("Start scanning neighbor cells using nodes\n");
#endif
	    				   for(k=k_min;k<k_max+1;k++) {
	    					   k0=k;
#ifdef SPHERICAL_S_GRID
	    					   if(k==-1) k0=nz_sg-1;
	    					   if(k==nz_sg) k0=0;
#endif  /* SPHERICAL_S_GRID */
	    					   for(j=j_min;j<j_max+1;j++) {
	    						   j0=j;
#ifdef SPHERICAL_S_GRID
	    						   if(j==ny_sg) {
	    							   k0 = (k0+nz_sg/2)%(nz_sg-1);
	    							   j0 = ny_sg-1;
	    						   }
	    						   if(j==-1) {
	    							   k0 = (k0+nz_sg/2)%(nz_sg-1);
	    							   j0 = 0;
	    						   }
#endif  /* SPHERICAL_S_GRID */
	    						   for(i=i_min;i<i_max+1;i++) {
#ifdef DEBUGS
	    							   printf("cell %d %d %d nnodes_in_cell %d \n",i,j0,k0,this->smartSearchValues->nnodes_in_cell[k0][j0][i]);
#endif
	    							   if(this->smartSearchValues->nnodes_in_cell[k0][j0][i]>0) {
	    								   iselect=0;
	    								   i_s=i;
	    								   j_s=j0;
	    								   k_s=k0;
	    							   }
	    						   }}}
	    				   if(iselect==0)  {
#ifdef DEBUGS
	    					   printf("Found a starting node in neighbor %d %d %d\n",i_s,j_s,k_s);
#endif
	    					   indx_start = this->smartSearchValues->start_index_nodes[k_s][j_s][i_s];
	    					   indx_end = this->smartSearchValues->end_index_nodes[k_s][j_s][i_s];
	    					   delta_i=(int)( (float)(indx_end-indx_start) * ( (float)rand() / (float)RAND_MAX ) );
	    					   last_node_found = (*this->smartSearchValues->indx_nodes)[indx_start + delta_i];
#ifdef DEBUGS
	    					   printf("Node is %d\n",last_node_found);
#endif
	    					   this->smartSearchValues->last_element_found = this->smartSearchValues->esup1->at( this->smartSearchValues->esup2->at(last_node_found) );
#ifdef DEBUGS
	    					   printf("Element is %d\n",this->smartSearchValues->last_element_found);
	    				   } else {
	    					   printf("\n\n Final part of search failed\n\n\n");
#endif
	    				   }

	    				   /* end of immediate neighbor search */

	    				   /* If still not found search up to 2 neighbors away */
	    				   if(iselect == 1) {

	    					   i_min = std::max(0,i_s-2);
	    					   i_max = std::min(nx_sg-1,i_s+2);
#ifdef CARTESIAN_S_GRID
	    					   j_min = max(0,j_s-2);
	    					   j_max = min(ny_sg-1,j_s+2);
	    					   k_min = max(0,k_s-2);
	    					   k_max = min(nz_sg-1,k_s+2);
#endif  /* CARTESIAN_S_GRID */
#ifdef SPHERICAL_S_GRID
	    					   j_min = std::max(j_s-2, 0);
	    					   j_max = std::min(j_s+2, ny_sg-1);
	    					   k_min = k_s-2;
	    					   k_max = k_s+2;
#endif  /* SPHERICAL_S_GRID */

	    					   /* if using elements for search */
	    					   for(k=k_min;k<k_max+1;k++) {
	    						   k0=k;
#ifdef SPHERICAL_S_GRID
	    						   if(k==-1) k0=nz_sg-1;
	    						   if(k==nz_sg) k0=0;
	    						   if(k==-2) k0=nz_sg-2;
	    						   if(k==nz_sg+1) k0=1;
#endif  /* SPHERICAL_S_GRID */
	    						   for(j=j_min;j<j_max+1;j++) {
	    							   j0=j;
#ifdef SPHERICAL_S_GRID
	    							   if(j==ny_sg) {
	    								   k0 = (k0+nz_sg/2)%(nz_sg-1);
	    								   j0 = ny_sg-1;
	    							   }
	    							   if(j==ny_sg+1) {
	    								   k0 = (k0+nz_sg/2)%(nz_sg-1);
	    								   j0 = ny_sg-2;
	    							   }
	    							   if(j==-1) {
	    								   k0 = (k0+nz_sg/2)%(nz_sg-1);
	    								   j0 = 0;
	    							   }
	    							   if(j==-2) {
	    								   k0 = (k0+nz_sg/2)%(nz_sg-1);
	    								   j0 = 1;
	    							   }
#endif  /* SPHERICAL_S_GRID */
	    							   for(i=i_min;i<i_max+1;i++) {
#ifdef DEBUGS
	    								   printf("cell %d %d %d nelems_in_cell %d\n",i,j0,k0,this->smartSearchValues->nelems_in_cell[k0][j0][i]);
#endif
	    								   if(this->smartSearchValues->nelems_in_cell[k0][j0][i]>0) {
	    									   iselect=0;
	    									   i_s=i;
	    									   j_s=j0;
	    									   k_s=k0;
	    								   }
	    							   }}}
	    					   if(iselect==0)  {
#ifdef DEBUGS
	    						   printf("Found a starting element in neighbor %d %d %d\n",i_s,j_s,k_s);
#endif
	    						   indx_start = this->smartSearchValues->start_index[k_s][j_s][i_s];
	    						   indx_end   = this->smartSearchValues->end_index[k_s][j_s][i_s];
	    						   delta_i=(int)( (float)(indx_end-indx_start) * ( (float)rand() / (float)RAND_MAX ) );
	    						   this->smartSearchValues->last_element_found = (*this->smartSearchValues->indx)[indx_start + delta_i];
#ifdef DEBUGS
	    						   printf("Element is %d\n",this->smartSearchValues->last_element_found);
#endif
	    					   }

	    				   }
	    				   /* end of 2 neighbor search */

	    			   }
	    			   /* next cell to pick from should be i_s,j_s,k_s */

	    		   }
	    	   }

	       }
//	       std::cout << "last_element_found = " << this->smartSearchValues->last_element_found << std::endl;
	       /* If available, use the last element found to begin the search */
	       if(this->smartSearchValues->last_element_found != -1) kelem = smartSearch(c0,c1,c2);

	       this->smartSearchValues->last_element_found=kelem;
	       previous_c0=c0;
	       previous_c1=c1;
	       previous_c2=c2;

#ifdef DEBUGS
	       printf("Exiting find element with last_element_found=%d\n",this->smartSearchValues->last_element_found);
#endif

		} else
		{
#ifdef DEBUGS
			//this->smartSearchValues->outside_grid += 1;
			this->smartSearchValues->last_element_found = -1;
			previous_c0 = 0.f;
			previous_c1 = 0.f;
			previous_c2 = 0.f;
#endif
		}
	         return kelem;

	/*       end subroutine find_element  */
	}

	int Adapt3DInterpolator::index_2d_to_1d( int i1, int i2, int n2)
	{
	/* converts a 2D array index into a flat 1D index */
	      int idx = n2*i1 + i2;

	      return idx;
	}

	int Adapt3DInterpolator::point_within_grid( const float& c0, const float& c1, const float& c2)
	{
		/*
		!
		! This function test to see if the given point (coord) is inside
		! the grid bounds. This function requires specific knowledge of the
		! grid type and range. It will need a customized function for each model
		! used with this search.
		*/

	      float  radius;
	      int within_bounds = 1;

	      radius=std::sqrt(c0*c0+c1*c1+c2*c2);
	      if(c0 < this->smartSearchValues->xl_gr)
	      {
	    	  return 0;

	      } else if(c0 > this->smartSearchValues->xr_gr)
	      {
	    	  return 0;

	      } else if (c1 < this->smartSearchValues->yl_gr)
	      {
	    	  return 0;

	      } else if (c1 > this->smartSearchValues->yr_gr)
	      {
	    	  return 0;

	      } else if(c2 < this->smartSearchValues->zl_gr)
	      {
	    	  return 0;

	      } else if(c2 > this->smartSearchValues->zr_gr)
	      {
	    	  return 0;

	      }
	      if(radius < INNER_RADIUS) return 0;
	      if(radius > OUTER_RADIUS) return 0;

	      return within_bounds;

	}

	int Adapt3DInterpolator::point_within_grid( const float * scoord )
	{
		/*
		!
		! This function test to see if the given point (coord) is inside
		! the grid bounds. This function requires specific knowledge of the
		! grid type and range. It will need a customized function for each model
		! used with this search.
		*/

	      float  radius;
	      int within_bounds = 1;

	      radius=std::sqrt(scoord[0]*scoord[0]+scoord[1]*scoord[1]+scoord[2]*scoord[2]);
	      if(scoord[0] < this->smartSearchValues->xl_gr)
	      {
	    	  within_bounds = 0;

	    	  std::cerr << "scoord[0]: " << scoord[0] << " < " << this->smartSearchValues->xl_gr << std::endl;
	      } else if(scoord[0] > this->smartSearchValues->xr_gr)
	      {
	    	  within_bounds = 0;
	    	  std::cerr << "scoord[0]: " << scoord[0] << " > " << this->smartSearchValues->xr_gr << std::endl;
	      } else if (scoord[1] < this->smartSearchValues->yl_gr)
	      {
	    	  within_bounds = 0;
	    	  std::cerr << "scoord[1]: " << scoord[1] << " < " << this->smartSearchValues->yl_gr << std::endl;
	      } else if (scoord[1] > this->smartSearchValues->yr_gr)
	      {
	    	  within_bounds = 0;
	    	  std::cerr << "scoord[1]: " << scoord[1] << " > " << this->smartSearchValues->yr_gr << std::endl;

	      } else if(scoord[2] < this->smartSearchValues->zl_gr)
	      {
	    	  within_bounds = 0;
	    	  std::cerr << "scoord[2]: " << scoord[2] << " < " << this->smartSearchValues->zl_gr << std::endl;

	      } else if(scoord[2] > this->smartSearchValues->zr_gr)
	      {
	    	  within_bounds = 0;
	    	  std::cerr << "scoord[2]: " << scoord[2] << " > " << this->smartSearchValues->zr_sg << std::endl;

	      }
	      if(radius < INNER_RADIUS) within_bounds = 0;
	      if(radius > OUTER_RADIUS) within_bounds = 0;

	      return within_bounds;

	}

	void Adapt3DInterpolator::calculation1(const float& a, const float& b, const float& c, const float& d, const float& e, float& result)
	{
		result = a*(b*c-d*e);

	}

    int Adapt3DInterpolator::chkineln( const float& c0, const float& c1, const float& c2, int ielem , float * shapex)
	{

    	//
//#define DEBUG
	/*
	!...  mesh arrays
	*/
	/*
		integer,intent(in) ::  ndimn,npoin,nnode,nelem
		integer,intent(in) ::  intmat(nnode,nelem)
		integer,intent(in) ::  ielem
		real*8,intent(in)  ::  coord(ndimn,npoin),cintp(ndimn)

		real*8,intent(out) ::  shapex(nnode)
	*/

		int   ipa,ipb,ipc,ipd;
		int   ierro;
		float xa,ya,za,xba,yba,zba,xca,yca,zca,xda,yda,zda;
		float xpa,ypa,zpa;
		float deter,detin,shmin,shmax;
		float rin11,rin12,rin13;
		float rin21,rin22,rin23;
		float rin31,rin32,rin33;

	/*
	!       data tolow/ -0.005 /
	!       data tolhi/  1.005 /
	!       data   c00/  0.0   /
	!       data   c10/  1.0   /
	!
	!...  this sub sees if element ielem contains point cintp,
	!     writing the shape-function values into ==> shape
	!
	!...  find the local coordinates
	!
	*/
		ipa = intmat->at(index_2d_to_1d(ielem,0,4));
		ipb = intmat->at(index_2d_to_1d(ielem,1,4));
		ipc = intmat->at(index_2d_to_1d(ielem,2,4));
		ipd = intmat->at(index_2d_to_1d(ielem,3,4));

		//std::cerr << "npoin: " << npoin << " ndimn: " << ndimn << " ipa: " << ipa << " ipb: " << ipb << " ipc: " << ipc << " ipd: " << ipd << std::endl;
		xa  = (*coord)[index_2d_to_1d(ipa,0,3)];
		ya  = (*coord)[index_2d_to_1d(ipa,1,3)];
		za  = (*coord)[index_2d_to_1d(ipa,2,3)];
		xba = (*coord)[index_2d_to_1d(ipb,0,3)] - xa;
		yba = (*coord)[index_2d_to_1d(ipb,1,3)] - ya;
		zba = (*coord)[index_2d_to_1d(ipb,2,3)] - za;
		xca = (*coord)[index_2d_to_1d(ipc,0,3)] - xa;
		yca = (*coord)[index_2d_to_1d(ipc,1,3)] - ya;
		zca = (*coord)[index_2d_to_1d(ipc,2,3)] - za;
		xda = (*coord)[index_2d_to_1d(ipd,0,3)] - xa;
		yda = (*coord)[index_2d_to_1d(ipd,1,3)] - ya;
		zda = (*coord)[index_2d_to_1d(ipd,2,3)] - za;

		float t1 = 0.f;
		float t2 = 0.f;
		float t3 = 0.f;
		t1 = xba*(yca*zda - zca*yda);
		t2 = yba*(xca*zda - zca*xda);
		t3 = zba*(xca*yda - yca*xda);
		//a*(b*c-d*e);
//		boost::thread thread1(&Adapt3DInterpolator::calculation1,boost::ref(xba),boost::ref(yca),boost::ref(zda),boost::ref(zca),boost::ref(yda), boost::ref(t1));
//		boost::thread thread2(&Adapt3DInterpolator::calculation1,boost::ref(yba),boost::ref(xca),boost::ref(zda),boost::ref(zca),boost::ref(xda), boost::ref(t2));
//		boost::thread thread3(&Adapt3DInterpolator::calculation1,boost::ref(zba),boost::ref(xca),boost::ref(yda),boost::ref(yca),boost::ref(xda), boost::ref(t3));
//		thread1.join();
//		thread2.join();
//		thread3.join();
		//calculation1(xba,yca,zda,zca,yda, t1);
		//calculation1(yba,xca,zda,zca,xda, t2);
		//calculation1(zba,xca,yda,yca,xda, t3);
		//float deter = xba*(yca*zda-zca*yda) - yba*(xca*zda-zca*xda) + zba*(xca*yda-yca*xda);
		deter = t1 - t2 + t3;

	#ifdef DEBUG
//		std::cerr << "xa: " << xa << " ya: " << ya << " za: " << za << " xba: " << xba;
//		std::cerr << " yba: " << yba << " zba: " << zba << " xca: " << xca << " yca: " << yca;
//		std::cerr << " zca: " << zca << " xda: " << xda << " yda: " << yda << " zda: " << zda << std::endl;
		  printf("(*coord)[ipa]= %d %e %e %e \n",ipa,(*coord)[index_2d_to_1d(ipa,0, 3)],(*coord)[index_2d_to_1d(ipa,1,3)],(*coord)[index_2d_to_1d(ipa,2,3)]);
		  std::cerr << "deter= " << deter << std::endl;
	#endif
	/*       detin = c10/deter */
		detin = 1.0/deter;

		rin11 = detin*(yca*zda-zca*yda);
		rin12 =-detin*(xca*zda-zca*xda);
		rin13 = detin*(xca*yda-yca*xda);
		rin21 =-detin*(yba*zda-zba*yda);
		rin22 = detin*(xba*zda-zba*xda);
		rin23 =-detin*(xba*yda-yba*xda);
		rin31 = detin*(yba*zca-zba*yca);
		rin32 =-detin*(xba*zca-zba*xca);
		rin33 = detin*(xba*yca-yba*xca);

		xpa = c0-xa;
		ypa = c1-ya;
		zpa = c2-za;
	/*
	!...  local coordinates & shape-function values
	*/
		shapex[1] = rin11*xpa + rin12*ypa + rin13*zpa;
		shapex[2] = rin21*xpa + rin22*ypa + rin23*zpa;
		shapex[3] = rin31*xpa + rin32*ypa + rin33*zpa;
		shapex[0] = 1.0 - shapex[1] - shapex[2] - shapex[3];
	#ifdef DEBUG
//		  printf("cintp= %e %e %e \n",cintp[0],cintp[1],cintp[2]);
//		  printf("xa-za= %e %e %e \n",xa,ya,za);
		  //std::cerr << "rin11: " << rin11 << " xpa: " << xpa << " rin12: " << rin12 << " ypa: " << ypa << " rin13: " << rin13 << " zpa: " << zpa << std::endl;
		std::cerr << "cintp: " << c0 << "," << c1 << "," << c2 << std::endl;
		std::cerr << "ielem: " << ielem << " shapex = " << shapex[0] << " " << shapex[1] << " " << shapex[2] << " " << shapex[3] << std::endl;
	#endif
	/*       shape(1) = c10 - shape(2) - shape(3) - shape(4)
	!
	!...  max/min of these shape-functions
	!
	*/
		//float t = shapex[0] + shapex[1] + shapex[2] + shapex[3];
		shmin = ccmc::Math::ffindmin(shapex,4);
		shmax = ccmc::Math::ffindmax(shapex,4);
	/*
	!...  see if in the element
	!
	!       if(shmin .ge. tolow .and. shmax .le. tolhi) then
	*/
		if ( (shmin > 0.) && (shmax <= 1.0) && !std::isnan(shapex[0])) {
			   ierro = 0;
		} else {
			   ierro = 1;
		}

	/*
	!...  control output
	!     write(*,*)' ielem,shmin,shmax,ierro=',ielem,shmin,shmax,ierro
	*/

	#ifdef DEBUG
		if( ierro == 0) {
		  printf("shmin= %e \n",shmin);
		  printf("shmax= %e \n",shmax);
		  printf("ierro= %d \n",ierro);
		  printf("cintp= %e %e %e \n",c0,c1,c2);
		  printf("node 1 = %e %e %e %d \n",(*coord)[index_2d_to_1d(ipa,0,3)],(*coord)[index_2d_to_1d(ipa,1,3)],(*coord)[index_2d_to_1d(ipa,2,3)],ipa);
		  printf("node 2 = %e %e %e %d \n",(*coord)[index_2d_to_1d(ipb,0,3)],(*coord)[index_2d_to_1d(ipb,1,3)],(*coord)[index_2d_to_1d(ipb,2,3)],ipb);
		  printf("node 3 = %e %e %e %d \n",(*coord)[index_2d_to_1d(ipc,0,3)],(*coord)[index_2d_to_1d(ipc,1,3)],(*coord)[index_2d_to_1d(ipc,2,3)],ipc);
		  printf("node 4 = %e %e %e %d \n",(*coord)[index_2d_to_1d(ipd,0,3)],(*coord)[index_2d_to_1d(ipd,1,3)],(*coord)[index_2d_to_1d(ipd,2,3)],ipd);
		 }
	#endif

		 return ierro;

	/*       end subroutine chkineln */
	}

    float Adapt3DInterpolator::interpolate_adapt3d_solution(const float& x, const float& y, const float& z, int ielem, const std::string& variable)
    {
    	float missingValue = this->modelReader->getMissingValue();
    /*
     * Interpolate values of unkno to position coord in element ielem
    */

           int ipa,ipb,ipc,ipd;
           int iv;
           float x1,y1,z1;
           float x2,y2,z2;
           float x3,y3,z3;
           float x4,y4,z4;
           float vol,vol6;
           float a1,b1,c1,d1;
           float a2,b2,c2,d2;
           float a3,b3,c3,d3;
           float a4,b4,c4,d4;
           float f1,f2,f3,f4;

           ipa = intmat->at(index_2d_to_1d(ielem,0,4));
           ipb = intmat->at(index_2d_to_1d(ielem,1,4));
           ipc = intmat->at(index_2d_to_1d(ielem,2,4));
           ipd = intmat->at(index_2d_to_1d(ielem,3,4));
           x1 = (*coord)[index_2d_to_1d(ipa,0,3)];
           y1 = (*coord)[index_2d_to_1d(ipa,1,3)];
           z1 = (*coord)[index_2d_to_1d(ipa,2,3)];
           x2 = (*coord)[index_2d_to_1d(ipb,0,3)];
           y2 = (*coord)[index_2d_to_1d(ipb,1,3)];
           z2 = (*coord)[index_2d_to_1d(ipb,2,3)];
           x3 = (*coord)[index_2d_to_1d(ipc,0,3)];
           y3 = (*coord)[index_2d_to_1d(ipc,1,3)];
           z3 = (*coord)[index_2d_to_1d(ipc,2,3)];
           x4 = (*coord)[index_2d_to_1d(ipd,0,3)];
           y4 = (*coord)[index_2d_to_1d(ipd,1,3)];
           z4 = (*coord)[index_2d_to_1d(ipd,2,3)];

    #ifdef TEST_CASE1
           x1=0.;
           y1=0.;
           z1=0.;
           x2=1.;
           y2=0.;
           z2=0.;
           x3=0.;
           y3=2.;
           z3=0.;
           x4=0.;
           y4=0.;
           z4=3.;
    #endif

//    #ifdef LINEAR_INTERPOL
           a1 = x2*(y3*z4-z3*y4)+y2*(z3*x4-z4*x3)+z2*(x3*y4-x4*y3);
           b1 = - ( y3*z4-z3*y4 + y2*(z3-z4) + z2*(y4-y3) );
           c1 = - ( x2*(z4-z3) + (z3*x4-z4*x3) + z2*(x3-x4) );
           d1 = - ( x2*(y3-y4) + y2*(x4-x3) + (x3*y4-y3*x4) );

           a2 = x3*(y4*z1-z4*y1)+y3*(z4*x1-z1*x4)+z3*(x4*y1-x1*y4);
           b2 = - ( y4*z1-z4*y1 + y3*(z4-z1) + z3*(y1-y4) );
           c2 = - ( x3*(z1-z4) + (z4*x1-z1*x4) + z3*(x4-x1) );
           d2 = - ( x3*(y4-y1) + y3*(x1-x4) + (x4*y1-y4*x1) );

           a3 = x4*(y1*z2-z1*y2)+y4*(z1*x2-z2*x1)+z4*(x1*y2-x2*y1);
           b3 = - ( y1*z2-z1*y2 + y4*(z1-z2) + z4*(y2-y1) );
           c3 = - ( x4*(z2-z1) + (z1*x2-z2*x1) + z4*(x1-x2) );
           d3 = - ( x4*(y1-y2) + y4*(x2-x1) + (x1*y2-y1*x2) );

           a4 = x1*(y2*z3-z2*y3)+y1*(z2*x3-z3*x2)+z1*(x2*y3-x3*y2);
           b4 = - ( y2*z3-z2*y3 + y1*(z2-z3) + z1*(y3-y2) );
           c4 = - ( x1*(z3-z2) + (z2*x3-z3*x2) + z1*(x2-x3) );
           d4 = - ( x1*(y2-y3) + y1*(x3-x2) + (x2*y3-y2*x3) );

           vol6 = a1 + x1*b1 + y1*c1 + z1*d1;
           vol  = vol6/6.;

    #ifdef TEST_CASE1
           printf("Volume = %e\n",vol);
           printf("Correct Volume should be 1.0\n");
    #endif

           const std::vector<float> * vData = modelReader->getVariableFromMap(variable);
           if (vData == NULL || vData->size() == 0)
           {
        	   std::cerr << "missing value" << std::endl;
        	   return missingValue;
           }
          /* for ( iv=0; iv<9; iv++) {

             unkno_local[iv] = 0.;
             f1 =  (a1 + b1*x + c1*y + d1*z)/vol6;
             f2 = -(a2 + b2*x + c2*y + d2*z)/vol6;
             f3 =  (a3 + b3*x + c3*y + d3*z)/vol6;
             f4 = -(a4 + b4*x + c4*y + d4*z)/vol6;

             unkno_local[iv] = f1*(*unkno)[ index_2d_to_1d(ipa,iv,npoin,NVARS_ADAPT3D))+f2*(*unkno)[ index_2d_to_1d(ipb,iv,npoin,NVARS_ADAPT3D) ]
                              +f3*(*unkno)[ index_2d_to_1d(ipc,iv,npoin,NVARS_ADAPT3D) ]+f4*(*unkno)[ index_2d_to_1d(ipd,iv,npoin,NVARS_ADAPT3D) ] ;

           }*/

           /*
            * int Adapt3DInterpolator::index_2d_to_1d( int i1, int i2, int n1, int n2)
			{

				  int idx = n2*i1 + i2;

				  return idx;
			}
    */
           f1 =  (a1 + b1*x + c1*y + d1*z)/vol6;
           f2 = -(a2 + b2*x + c2*y + d2*z)/vol6;
           f3 =  (a3 + b3*x + c3*y + d3*z)/vol6;
           f4 = -(a4 + b4*x + c4*y + d4*z)/vol6;
//           std::cout << "a1: " << a1 << " a2: " << a2 << " a3: " << a3 << " a4: " << a4 << std::endl;
//           std::cout << "b1: " << b1 << " b2: " << b2 << " b3: " << b3 << " b4: " << b4 << std::endl;
//           std::cout << "c1: " << c1 << " c2: " << c2 << " c3: " << c3 << " c4: " << c4 << std::endl;
//           std::cout << "d1: " << d1 << " d2: " << d2 << " d3: " << d3 << " d4: " << d4 << std::endl;
#ifdef DEBUG
           std::cout << "vol6: " << vol6 << " vol: " << vol << std::endl;
           std::cout << "f1: " << f1 << " f2: " << f2 << " f3: " << f3 << " f4: " << f4 << std::endl;
           std::cout << "ipa: " << ipa << " ipb: " << ipb << " ipc: " << ipc << " ipd: " << ipd << std::endl;
           std::cout << "(*vData)[" << ipa << "] " << (*vData)[ ipa ] << std::endl;
           std::cout << "(*vData)[" << ipb << "] " << (*vData)[ ipb ] << std::endl;
           std::cout << "(*vData)[" << ipc << "] " << (*vData)[ ipc ] << std::endl;
           std::cout << "(*vData)[" << ipd << "] " << (*vData)[ ipd ] << std::endl;
#endif

           return f1*(*vData)[ ipa ]+f2*(*vData)[ ipb ]
				 +f3*(*vData)[ ipc ]+f4*(*vData)[ ipd ] ;

    /*       end subroutine interpolate_solution  */
    }

	/**
	 * Destructor
	 */
	Adapt3DInterpolator::~Adapt3DInterpolator()
	{
		// TODO Auto-generated destructor stub

	}
}


#ifndef _ITERATOR_INDEXED_HPP_
#define _ITERATOR_INDEXED_HPP_

#include <boost/multi_array/storage_order.hpp>
#include <boost/range.hpp>
#include <boost/iterator/iterator_adaptor.hpp>
#include <boost/type_traits/add_reference.hpp>
#include <iterator>

#include "cxx/array.hpp"

namespace cxx {
namespace iterator {

    using boost::c_storage_order;
    using boost::fortran_storage_order;
    using boost::array;

    template<size_t N, class Order = boost::c_storage_order>
    struct multi_index {
	typedef boost::array<size_t,N> type;
	multi_index(const size_t (&dims)[N]) {
	    std::copy(dims, dims + N, dims_);
	    index1d_ = 0;
	    update();
	}
	size_t dim(size_t i) const {return dims_[i] ; }
	size_t operator[](size_t i) const { return index_[i]; }
	multi_index& operator++() { ++index1d_; update(); return *this; }
	operator const type&() const { return index_; }
	static type index(size_t index1d, const size_t (&dims)[N],
			  const boost::c_storage_order &order) {
	    type v;
	    size_t r = index1d;
	    for (size_t i = N-1; i > 0; --i) {
		size_t q = r/dims[i];
		v[i] = r - q*dims[i];
		r = q;
	    }
	    v[0] = r;
	    // std::cout << v << index1d  << boost::make_array(dims) << "\n";
	    return v;
	}
	static type index(size_t index1d, const size_t (&dims)[N],
			  const boost::fortran_storage_order &order) {
	    type v;
	    size_t r = index1d;
	    for (size_t i = 0; i < N-1; ++i) {
		size_t q = r/dims[i];
		v[i] = r - q*dims[i];
		r = q;
	    }
	    v[N-1] = r;
	    return v;
	}
    private:
	void update() { index_ = index(index1d_, dims_, Order()); }
	size_t dims_[N];
	type index_;
	size_t index1d_;
    };

    using boost::iterator_range;

    template<size_t N, class F>
    class indexed_iterator
	: public boost::iterator_facade<indexed_iterator<N,F>,
				 typename F::reference,
				 boost::forward_traversal_tag>
    {
    public:
	typedef typename F::storage_order_type storage_order_type;
	typedef array<size_t,N> index_array;
	indexed_iterator(const index_array &dims, F &f, size_t index = 0)
	    : dims_(dims), f_(f), index_(index), store()
	{
	    update();
	}
	const index_array& index() const { return index_n_; }
    private:
	friend class boost::iterator_core_access;
	index_array dims_;
	F &f_;
	mutable size_t index_;
	mutable index_array index_n_;
	const storage_order_type store;
	void increment() const { ++index_; update(); }
	bool equal(const indexed_iterator &other) const {
	    return index() == other.index();
	}
	typename F::reference dereference() { return f_(index()); }
	typename F::const_reference dereference() const { return f_(index()); }
	void update() const { index_n_ = index_n(index_, dims_, store); }
    };

    template<size_t N, class F>
    iterator_range<indexed_iterator<N,F> >
    indexed_range(const array<size_t,N> &dims, F &f, size_t begin, size_t end,
		  const c_storage_order &order = c_storage_order()) {
	typedef indexed_iterator<N,F> it;
	return make_iterator_range(it(dims, f, begin), it(dims, f, end));
    }

    template<class MA>
    iterator_range<indexed_iterator<MA::dimensionality,MA> >
    indexed_range(MA &A) {
	typedef indexed_iterator<MA::dimensionality,MA> iterator;
	return indexed_range(A.shape(), A, 0, A.num_elements());
    }

}
}

#endif /* _ITERATOR_INDEXED_HPP_ */