#include <iostream>
#include <fstream>

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

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

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

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

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

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

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

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

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

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

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

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

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

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

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

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

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

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

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

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

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

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

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

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

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

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

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

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

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

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

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

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

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

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

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

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

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

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

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

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

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

  return ret;
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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


#pragma once
#include <Eigen/Dense>
#include <vector>
#include <iostream>
#include <fstream>

using namespace std;
using namespace Eigen;

void split(const string& s, char c, vector<string>& v) {
    string::size_type i = 0;
    string::size_type j = s.find(c);

    while (j != string::npos) {
        v.push_back(s.substr(i, j-i));
        i = ++j;
        j = s.find(c, j);
    }
    if (j == string::npos) v.push_back(s.substr(i, s.length( )));
}

inline double string2double(const std::string& s){ std::istringstream i(s); double x = 0; i >> x; return x; }

MatrixXd read_matrix_file(string filename, char sep) {
    cerr << "Loading " << filename << endl;
    ifstream myfile(filename.c_str());
    stringstream ss;

    vector<vector<double> > M;
    if (myfile.is_open()) {
        string line;

        while ( getline(myfile,line) ) {
            //split string based on "," and store results into vector
            vector<string> fields;
            split(line, sep, fields);

            vector<double>row(fields.size());
            for( int i=0; i < fields.size(); i++ ) {
                row[i] = string2double(fields[i]);
            }
            M.push_back(row);
        }
    }

    MatrixXd X( (int) M.size(), (int) M[0].size() );
    for(int i=0; i < M.size(); i++ ) {
        for(int j=0; j < M[i].size(); j++ ) {
            X(i,j)=M[i][j];
        }
    }
    return X;
}


#include "TestFunctions.h"

#include "stdafx.h"
#include <vector>
#include <iostream>
#include <iomanip>
#include <sstream>
#include "cc_GraphicsFunctions.h"
#include "cc_Nastran.h"
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

/////////////////////////////////////////////////////////////////////////////////
void Tests::DotProduct_2D_test()
{

	// Dot Product  2D
	std::vector<double>  vector_1(2);
	std::vector<double>  vector_2(2);

	vector_1[0] =	2.0;
	vector_1[1] =	-3.0;
	vector_2[0] =	-.5;
	vector_2[1] =	 4.0;

	double dp = DotProduct_2D(vector_1, vector_2);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(-13, dp, 0.0001);

}
/////////////////////////////////////////////////////////////////////////////////
void Tests::DotProduct_3D_test()
{
	/// Dot Product  3D
	std::vector<double>  vector_1(3);
	std::vector<double>  vector_2(3);

	vector_1[0] =	 4.0;
	vector_1[1] =	 2.0;
	vector_1[2] =	-6.0;
	vector_2[0] =	-5.0;
	vector_2[1] =	 3.0;
	vector_2[2] =	-2.0;

	double dp = DotProduct_3D(vector_1, vector_2);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(-2, dp, 0.0001);

}
/////////////////////////////////////////////////////////////////////////////////
void Tests::CrossProduct_3D_test()
{

	std::vector<double>  vector_1(3);
	std::vector<double>  vector_2(3);

	vector_1[0] =	 2.0;
	vector_1[1] =	 1.0;
	vector_1[2] =	-3.0;
	vector_2[0] =	 3.0;
	vector_2[1] =	-1.0;
	vector_2[2] =	 4.0;

	std::vector<double>  vector_out = CrossProduct_3D(vector_1, vector_2);

	//std::cout << std::endl << "Cross Product: " <<  vector_out[0] << "  " <<  vector_out[1] << "  " <<  vector_out[2];
	CPPUNIT_ASSERT_DOUBLES_EQUAL(1, vector_out[0], 0.0001);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(-17, vector_out[1], 0.0001);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(-5, vector_out[2], 0.0001);
}

void Tests::Magnitude_3D_test()
{
	std::vector<double>  vector_1(3);

	vector_1[0] =	 3.0;
	vector_1[1] =	 2.0;
	vector_1[2] =	-6.0;

	// Answer should be 7.0
	double m = Magnitude_3D(vector_1);
	//std::cout << std::endl << "Magnitude 3D: " <<   m;
	CPPUNIT_ASSERT_DOUBLES_EQUAL(7, m, 0.0001);
}

/////////////////////////////////////////////////////////////////////////////////
void Tests::UnitVector_3D_test()
{

	///** Magnitude 3D
	std::vector<double>  vector_1(3);

	vector_1[0] =	 1.0;
	vector_1[1] =	 5.0;
	vector_1[2] =	 3.0;

	std::vector<double>  vector_out = UnitVector_3D(vector_1);

	// Answer should be 0.169030851	0.845154255	0.507092553
	//std::cout << std::endl << "Unit Vector: " <<  vector_out[0] << "  " <<  vector_out[1] << "  " <<  vector_out[2];
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.16903, vector_out[0], 0.0001);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.84515, vector_out[1], 0.0001);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0.50709, vector_out[2], 0.0001);

}
/////////////////////////////////////////////////////////////////////////////////

void Tests::PointOnLine_2D_test()
{

	bool b = PointOnLine_2D( Point_2D(-.0001, 10),Line_2D( Point_2D(0,10),   Point_2D(10,10)), .001 );

	CPPUNIT_ASSERT(b);

}
/////////////////////////////////////////////////////////////////////////////////
#if 0
	////////////////////////////////
	//	struct BoostPolygon_struct
	///////////////////////////////
	struct BoostPolygon_struct
	{
		typedef boost::geometry::model::d2::point_xy<double> boostpoint_type;
		typedef boost::geometry::model::polygon<boostpoint_type> boostpolygon_type;

		boostpolygon_type boostPolygon;

	};
static void Point2D_Within_Polygon_2D_test(bool in_IncludePointsOnBoundary)
{
	// WARNING - Before running this function, make sure that BoostPolygon_struct has not changed
	// the above version should agree verbatim with the GraphicsFunctions.cpp version

	std::vector<Point_3D> points;
	points.push_back( Point_3D(0,0,0) );
	points.push_back( Point_3D(10,0,0) );
	points.push_back( Point_3D(10,10,0) );
	points.push_back( Point_3D(0,10,0) );

	std::vector<Line_2D> PolygonLines;
	for ( std::vector<Point_3D>::const_iterator i(points.begin()); i != points.end(); ++i )
	{
		std::vector<Point_3D>::const_iterator  j = i;
		++j;
		if ( j != points.end() )
			PolygonLines.push_back( Line_2D( Point_2D(i->x,i->y),   Point_2D(j->x,j->y) ));
		else
			PolygonLines.push_back( Line_2D( Point_2D(i->x,i->y), Point_2D( points.begin()->x,points.begin()->y)) );
	}

	::BoostPolygon_struct   boostPolygon_struct;
	// Populate boost polygon
	for ( std::vector<Point_3D>::const_iterator i(points.begin()); i != points.end(); ++i )
	{
		boost::geometry::exterior_ring(boostPolygon_struct.boostPolygon).push_back(boost::geometry::model::d2::point_xy<double>(i->x,i->y));
	}
	// Correct the plolygon - clockwise orientiatin, closed loop (i.e. last point repeated to close the loop
	boost::geometry::correct(boostPolygon_struct.boostPolygon);

	if (   boost::geometry::intersects(boostPolygon_struct.boostPolygon) )
		std::cout << std::endl << "ERROR -->Intersects";
	else
		std::cout << std::endl << "Does Not Intersect";

	std::vector<Line_2D> PolygonLinesSorted = SortPolygonLines (PolygonLines, .001 );

	if ( Point2D_Within_Polygon_2D( Point_2D(-.001, 10.001), PolygonLinesSorted, &boostPolygon_struct, true, .001) )
		std::cout << std::endl << "Inside/On Polygon = TRUE";
	else
		std::cout << std::endl << "Inside/On Polygon = FALSE";

	if ( Point2D_Within_Polygon_2D( Point_2D(-.001, 10.001), PolygonLinesSorted,&boostPolygon_struct, false , .001) )
		std::cout << std::endl << "Inside (but not on) Polygon = TRUE";
	else
		std::cout << std::endl << "Inside (but not on) = FALSE";

	if ( Point2D_Within_Polygon_2D( Point_2D(1.0, 6), PolygonLinesSorted, &boostPolygon_struct,true, .001) )
		std::cout << std::endl << "Inside/On Polygon = TRUE";
	else
		std::cout << std::endl << "Inside/On Polygon = FALSE";

	if ( Point2D_Within_Polygon_2D( Point_2D(1.0, 6), PolygonLinesSorted, &boostPolygon_struct,false, .001) )
		std::cout << std::endl << "Inside (but not on) = TRUE";
	else
		std::cout << std::endl << "Inside (but not on) = FALSE";

	std::vector<double> vector_x_axis;
	std::vector<double> vector_y_axis;

	VectorsFormingIntersection_of_FirstAndLastLine_2D(PolygonLinesSorted, vector_x_axis, vector_y_axis);

	std::cout << std::endl << "vector_x_axis: " <<  std::setw(10) <<  vector_x_axis[0] <<
													std::setw(10) <<  vector_x_axis[1];

	std::cout << std::endl << "vector_y_axis: " <<  std::setw(10) <<  vector_y_axis[0] <<
													std::setw(10) <<  vector_y_axis[1];
}

void Test::Point2D_Within_Polygon_2D_test()
{
	Point2D_Within_Polygon_2D_test(true);
}
#endif
/////////////////////////////////////////////////////////////////////////////////
void Tests::AngleBetweenVectors_test()
{

	std::vector<double>  vector_1(3);
	vector_1[0] =	 0.0;
	vector_1[1] =	 1.0;
	vector_1[2] =	 0.0;

	std::vector<double>  vector_2(3);
	vector_2[0] =	 0;
	vector_2[1] =	 -5.0;    //-1.41421;
	vector_2[2] =	 5.0;    //1.41421;

	/*
	std::vector<double>  vector_1(3);
	vector_1[0] =	 2.0;
	vector_1[1] =	 2.0;
	vector_1[2] =	 12.0;

	std::vector<double>  vector_2(3);
	vector_2[0] =	 1.0;
	vector_2[1] =	 1.0;
	vector_2[2] =	 6.0;
	*/

	double a = AngleBetweenVectors_3D(vector_1, vector_2);
	//std::cout << std::endl << "AngleBetweenVectors: " << a;

	CPPUNIT_ASSERT_DOUBLES_EQUAL(2.35619, a, 0.0001);

}
/////////////////////////////////////////////////////////////////////////////////
void Tests::TransformationMatrix_test()
{
	std::vector<double>  vector_1(3);
	// xy plane
	//vector_1[0] =	 1.0;
	//vector_1[1] =	 1.0;
	//vector_1[2] =	 0.0;
	// yz plane
	//vector_1[0] =	 0.0;
	//vector_1[1] =	 1.0;
	//vector_1[2] =	 1.0;
	// xz plane
	vector_1[0] =	 1.0;
	vector_1[1] =	 0.0;
	vector_1[2] =	 1.0;

	std::vector<double>  vector_2(3);
	// xy plane
	//vector_2[0] =	 -1;
	//vector_2[1] =	 1;
	//vector_2[2] =	 0;
	// yz plane
	//vector_2[0] =	 0.0;
	//vector_2[1] =	 -1.0;
	//vector_2[2] =	 1.0;
	// xz plane
	vector_2[0] =	 -1.0;
	vector_2[1] =	 0.0;
	vector_2[2] =	 1.0;

	std::vector<double>  offset(3);
	offset[0] =	 0;
	offset[1] =	 0;
	offset[2] =	 0;

	//std::cout << std::endl << "Vector 1: " <<  std::setw(10) << vector_1[0] <<  std::setw(10) << vector_1[1] << std::setw(10) << vector_1[2];
	//std::cout << std::endl << "Vector 2: " <<  std::setw(10) << vector_2[0] << std::setw(10)  << vector_2[1] << std::setw(10) << vector_2[2];
	//std::cout << std::endl << "offset  : " <<  std::setw(10) << offset[0]   << std::setw(10)  << offset[1]   << std::setw(10) << offset[2];

	TransformationMatrix  transformationMatrix_1;

	transformationMatrix_1.setTransformationMatrix( vector_1, vector_2, offset );

	//std::cout << std::endl << transformationMatrix_1;
	//std::cout << std::endl;

	Point_3D  TestCoords;
	// xy plane
	//TestCoords.x = 03;
	//TestCoords.y = 3;
	//TestCoords.z = 0;
	// yz plane
	//TestCoords.x = 0;
	//TestCoords.y = 3;
	//TestCoords.z = 3;
	// xz plane
	TestCoords.x = 0;
	TestCoords.y = 0;
	TestCoords.z = 4;

	Point_3D  TransformedCoords;

	TransformedCoords = transformationMatrix_1.getTransformedCoordinates(  TestCoords  );

	//std::cout << std::endl << "TestCoords x, y, z: " <<  TestCoords ;
	//std::cout << std::endl << "Transposed x, y, z: " <<  TransformedCoords;

	CPPUNIT_ASSERT_DOUBLES_EQUAL(2.82843, TransformedCoords.x, 0.0001);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(2.82843, TransformedCoords.y, 0.0001);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0, TransformedCoords.z, 0.0001);
}
/////////////////////////////////////////////////////////////////////////////////
void Tests::ComputeRotationAngles_test()
{
	std::vector<double>  vector_1(3);
	std::vector<double>  vector_2(3);

	//////////////////
	// No Rotation
	///////////////////
	/*
	// r[2,0] = 0   correct
	vector_1[0] =	 1.0;
	vector_1[1] =	 0.0;
	vector_1[2] =	 0.0;

	vector_2[0] =	 0.0;
	vector_2[1] =	 1.0;
	vector_2[2] =	 0.0;
	*/

	//////////////////////////////////////////////////
	// Rotation  45 degrees counter clockwise about z
	/////////////////////////////////////////////////
	// r[2,0] = 0      correct
	/*
	vector_1[0] =	 1.0;
	vector_1[1] =	 1.0;
	vector_1[2] =	 0.0;

	vector_2[0] =	 -1.0;
	vector_2[1] =	 1.0;
	vector_2[2] =	 0.0;
	*/
	//////////////////////////////////////////////////
	// Rotation  45 degrees clockwise about z
	/////////////////////////////////////////////////
	/*
	// r[2,0] = 0      correct
	vector_1[0] =	 1.0;
	vector_1[1] =	 -1.0;
	vector_1[2] =	 0.0;

	vector_2[0] =	 1.0;
	vector_2[1] =	 1.0;
	vector_2[2] =	 0.0;
	*/

	//////////////////////////////////////////////////
	// Rotation  45 about Y
	/////////////////////////////////////////////////
	/*
	// r[2,0] = 0     correct
	vector_1[0] =	 1.0;
	vector_1[1] =	 0.0;
	vector_1[2] =	 1.0;

	vector_2[0] =	 -1.0;
	vector_2[1] =	 0.0;
	vector_2[2] =	 1.0;
	*/

	//////////////////////////////////////////////////
	// Rotation  -45 about Y
	/////////////////////////////////////////////////
	/*
	// r[2,0] = 0     correct
	vector_1[0] =	 1.0;
	vector_1[1] =	 0.0;
	vector_1[2] =	 -1.0;

	vector_2[0] =	 1.0;
	vector_2[1] =	 0.0;
	vector_2[2] =	 1.0;
	*/

	//////////////////////////////////////////////////
	// Rotation  ??
	/////////////////////////////////////////////////
	// r[2,0] = 1   Correct
	/*
	vector_1[0] =	 0.0;
	vector_1[1] =	 1.0;
	vector_1[2] =	 1.0;

	vector_2[0] =	 0.0;
	vector_2[1] =	 -1.0;
	vector_2[2] =	 1.0;
	*/
	//////////////////////////////////////////////////
	// Rotation  -????
	/////////////////////////////////////////////////
	// r[2,0] = 1   correct
	/*
	vector_1[0] =	 0.0;
	vector_1[1] =	 -1.0;
	vector_1[2] =	 -1.0;

	vector_2[0] =	 0.0;
	vector_2[1] =	 1.0;
	vector_2[2] =	 -1.0;
	*/

	//////////////////////////////////////////////////
	// Rotation  -????
	/////////////////////////////////////////////////
	// r[2,0] = -1   correct
	/*
	vector_1[0] =	 0.0;
	vector_1[1] =	 -1.0;
	vector_1[2] =	 1.0;

	vector_2[0] =	 0.0;
	vector_2[1] =	 1.0;
	vector_2[2] =	 1.0;
	*/

	//////////////////////////////////////////////////
	// Rotation  small angle counter clockwise about z
	/////////////////////////////////////////////////
	// r[2,0] = 0  correct
	/*
	vector_1[0] =	 1.0;
	vector_1[1] =	 .00001;
	vector_1[2] =	 0;

	vector_2[0] =	 -.00001;;
	vector_2[1] =	 1.0;
	vector_2[2] =	 0.0;
	*/

	//////////////////////////////////////////////////
	// Rotation  small angle counter clockwise about z
	/////////////////////////////////////////////////
	// r[2,0] = 0
	/*
	double  value_1 = .00000001;
	double  value_2 = .99999999;

	vector_1[0] =	 value_1;
	vector_1[1] =	 value_2;
	vector_1[2] =	 0;

	vector_2[0] =	 -value_2;
	vector_2[1] =	 value_1;
	vector_2[2] =	 0.0;
	*/

	//////////////////////////////////////////////////
	// Rotation  small angle counter clockwise about x
	/////////////////////////////////////////////////
	// r[2,0] = 0
	///*
	double  value_1 = .00000001;
	double  value_2 = .99999999;

	vector_1[0] =	 value_1;
	vector_1[1] =	 value_2;
	vector_1[2] =	 value_1;

	vector_2[0] =	 value_1;
	vector_2[1] =	 -value_1;
	vector_2[2] =	 value_2;
	//*/

	std::vector<double>  offset(3);
	offset[0] =	 0;
	offset[1] =	 0;
	offset[2] =	 0;

	//std::cout << std::endl << "Vector 1: " <<  std::setw(10) << vector_1[0] <<  std::setw(10) << vector_1[1] << std::setw(10) << vector_1[2];
	//std::cout << std::endl << "Vector 2: " <<  std::setw(10) << vector_2[0] << std::setw(10)  << vector_2[1] << std::setw(10) << vector_2[2];
	//std::cout << std::endl << "offset  : " <<  std::setw(10) << offset[0]   << std::setw(10)  << offset[1]   << std::setw(10) << offset[2];

	TransformationMatrix  transformationMatrix_1;

	transformationMatrix_1.setTransformationMatrix( vector_1, vector_2, offset );

	//std::cout << std::endl << transformationMatrix_1;
	//std::cout << std::endl;

	double rot_x, rot_y, rot_z;

	transformationMatrix_1.getRotationAngles( rot_x, rot_y, rot_z );

	//std::cout << std::endl << "Rotation Angles x, y. z: " <<  rot_x << "  " << rot_y << "  " << rot_z;
	CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.5708, rot_x, 0.0001);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(-1.5708, rot_y, 0.0001);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0, rot_z, 0.0001);
}

/////////////////////////////////////////////////////////////////////////////////
void Tests::DuplicatePointInList_test()
{
	////////////////////////////////
	// DuplicatePointInList
	////////////////////////////////
	double precision = .001;
	std::list< Point_3D> Points_set;
	Points_set.push_back(Point_3D(0, 0, 0));
	Points_set.push_back(Point_3D(-5, 1, 4));
	Points_set.push_back(Point_3D(.01, 0, 0));
	Points_set.push_back(Point_3D(6, 7, 1));

	CPPUNIT_ASSERT(!DuplicatePointInList( Points_set, precision ));
}
/////////////////////////////////////////////////////////////////////////////////
void Tests::PointsWithinPolygon_test()
{

	////////////////////////////////
	// Points Within Polygon
	////////////////////////////////

	std::map<int,GridPoint> gridPoints_map;
	std::vector<Point_3D>   polygon;
	double precision = .0001;
	std::vector<int>       gridPointIds_WithinPolygon;

	/*
	gridPoints_map[1] = GridPoint(1, 2, Point_3D(0, 0, 0), 2);
	gridPoints_map[2] = GridPoint(2, 2, Point_3D(5, 0, 0), 2);
	gridPoints_map[3] = GridPoint(3, 2, Point_3D(5, 5, 0), 2);
	gridPoints_map[4] = GridPoint(4, 2, Point_3D(0, 5, 0), 2);

	polygon.push_back( Point_3D( -1,-1, 0));   // should yield 1 2 3 4
	polygon.push_back( Point_3D( 6,-1, 0));
	polygon.push_back( Point_3D( 6, 6, 0));
	polygon.push_back( Point_3D(-1, 6, 0));
	*/

	/*
	gridPoints_map[1] = GridPoint(1, 2, Point_3D(2.1, 3.5, 0), 2); // should yield 1
	gridPoints_map[2] = GridPoint(2, 2, Point_3D(0, 6, 0), 2);
	//gridPoints_map[3] = GridPoint(3, 2, Point_3D(5, 5, 0), 2);
	//gridPoints_map[4] = GridPoint(4, 2, Point_3D(0, 5, 0), 2);

	polygon.push_back( Point_3D( 0, 5, 0));
	polygon.push_back( Point_3D( 0, 0, 0));
	polygon.push_back( Point_3D( 5, 0, 0));
	polygon.push_back( Point_3D( 5, 5, 0));
	*/

	/*
	polygon.push_back( Point_3D( 1, 0, 0));
	polygon.push_back( Point_3D( 5, 0, 0));
	polygon.push_back( Point_3D( 5, 5, 0));
	polygon.push_back( Point_3D( 1, 5, 0));
	*/

	/*
	gridPoints_map[1] = GridPoint(1, 2, Point_3D(6, 5, 1), 2);
	gridPoints_map[2] = GridPoint(2, 2, Point_3D(6, 5.5, 8), 2);
	gridPoints_map[3] = GridPoint(3, 2, Point_3D(10, 5, 20), 2);
	gridPoints_map[4] = GridPoint(4, 2, Point_3D(5, 5, 20), 2);

	polygon.push_back( Point_3D( 6, 5, 0));  // should be 1 3 4
	polygon.push_back( Point_3D( 10, 5, 0));
	polygon.push_back( Point_3D( 10, 5, 20));
	polygon.push_back( Point_3D( 5, 5, 20));
	*/

	// Slanted Plane

	gridPoints_map[1] = GridPoint(1, 2, Point_3D(3, 3, 18), 2);
	gridPoints_map[2] = GridPoint(2, 2, Point_3D(6, 5.1, 8), 2);
	gridPoints_map[3] = GridPoint(3, 2, Point_3D(10, 5, 20), 2);
	gridPoints_map[4] = GridPoint(4, 2, Point_3D(5, 5, 20), 2);

	polygon.push_back( Point_3D( 1, 1, 0));  // should be 1 4
	polygon.push_back( Point_3D( 6, 6, 0));
	polygon.push_back( Point_3D( 6, 6, 20));
	polygon.push_back( Point_3D( 1, 1, 20));

	// Slanted negative Plane

/*
	gridPoints_map[1] = GridPoint(1, 2, Point_3D(-3, 3, 18), 2);
	gridPoints_map[2] = GridPoint(2, 2, Point_3D(6, 5.1, 8), 2);
	gridPoints_map[3] = GridPoint(3, 2, Point_3D(10, 5, 20), 2);
	gridPoints_map[4] = GridPoint(4, 2, Point_3D(5, 5, 20), 2);

	polygon.push_back( Point_3D( -1, 1, 0));  // should be 1
	polygon.push_back( Point_3D( -6, 6, 0));
	polygon.push_back( Point_3D( -6, 6, 20));
	polygon.push_back( Point_3D( -1, 1, 20));

	//GetGridPointsWithinPolygon( gridPoints_map,polygon, precision, gridPointIds_WithinPolygon);

*/

	Polygon_3D polygon_obj(polygon, precision );

	//polygon_obj.pointWithinGeometry( gridPoints_map[2].point, true, precision );

	GetGridPointsWithinGeometry (	gridPoints_map,
										polygon_obj,
										GEOMETRY_INTERIOR_AND_BOUNDARY,
										precision,
										gridPointIds_WithinPolygon );

	CPPUNIT_ASSERT_EQUAL(2, (int)gridPointIds_WithinPolygon.size());
	CPPUNIT_ASSERT_EQUAL(1, gridPointIds_WithinPolygon[0]);
	CPPUNIT_ASSERT_EQUAL(4, gridPointIds_WithinPolygon[1]);

	/*std::cout << std::endl << "IDs Within Polygon";
	for ( std::vector<int>::const_iterator i( gridPointIds_WithinPolygon.begin()); i != gridPointIds_WithinPolygon.end(); ++i)
	{
		std::cout << std::endl << *i;
	}*/

}

/////////////////////////////////////////////////////////////////////////////////
static void GetGridPointsWithinCircle_test(bool in_IncludePointsOnBoundary)
{

	/////////////////////////////
	// GetGridPointsWithinCircle
	/////////////////////////////

	std::map<int,GridPoint> gridPoints_map;
	double precision = .0001;
	std::vector<int>       gridPointIds_WithinCircle;

	gridPoints_map[1] = GridPoint(1, 2, Point_3D(0, 0, 10), 2);
	gridPoints_map[2] = GridPoint(2, 2, Point_3D(25, 21 - .0001, 10), 2);
	gridPoints_map[3] = GridPoint(3, 2, Point_3D(25.5, 20.5, 10.0001), 2);
	gridPoints_map[4] = GridPoint(4, 2, Point_3D(25.5, 20, 10), 2);
	gridPoints_map[5] = GridPoint(5, 2, Point_3D(25.00, 19, 10), 2);

	Circle_3D   circle(  Point_3D( 25,20, 10),    // should get back 2 3 4 5
							Point_3D( 26,20, 10),
							Point_3D( 25,22, 10));

	//Circle_3D   circle(  Point_3D( 25,20, 10),  // second point identical to first, should throw exception
	//					 Point_3D( 25,20, 10),
	//					 Point_3D( 25,22, 10));

	//Circle_3D   circle(  Point_3D( 25,20, 10),  // third point identical to first, should throw exception
	//					 Point_3D( 26,20, 10),
	//					 Point_3D( 25,20, 10));

	//Circle_3D   circle(  Point_3D( 25,20, 10),  // 3rd point colinear with line from 1st and 2nd points, should throw exception
	//					 Point_3D( 26,20, 10),
	//					 Point_3D( 27,20, 10));

	GetGridPointsWithinGeometry (	gridPoints_map,
										circle,
									 	GEOMETRY_INTERIOR_AND_BOUNDARY,
										precision,
										gridPointIds_WithinCircle );

	CPPUNIT_ASSERT_EQUAL(4, (int)gridPointIds_WithinCircle.size());
	CPPUNIT_ASSERT_EQUAL(2, gridPointIds_WithinCircle[0]);
	CPPUNIT_ASSERT_EQUAL(3, gridPointIds_WithinCircle[1]);
	CPPUNIT_ASSERT_EQUAL(4, gridPointIds_WithinCircle[2]);
	CPPUNIT_ASSERT_EQUAL(5, gridPointIds_WithinCircle[3]);
	/*std::cout << std::endl << "IDs Within Circle";
	for ( std::vector<int>::const_iterator i( gridPointIds_WithinCircle.begin()); i != gridPointIds_WithinCircle.end(); ++i)
	{
		std::cout << std::endl << *i;
	}*/

}

void Tests::GetGridPointsWithinCircle_test()
{
	::GetGridPointsWithinCircle_test(true);
}
/////////////////////////////////////////////////////////////////////////////////
static void GetGridPointsBetweenOrOnConcentricCircles_test(bool in_IncludePointsOnBoundary)
{

	////////////////////////////////////////////
	// GetGridPointsBetweenOrOnConcentricCircles
	////////////////////////////////////////////

	std::map<int,GridPoint> gridPoints_map;
	double precision = .0001;
	std::vector<int>       gridPointIds_BetweenOrOnConcentricCircles;

	gridPoints_map[1] = GridPoint(1, 2, Point_3D(0, 0, 10), 2);
	gridPoints_map[2] = GridPoint(2, 2, Point_3D(25, 21 - .0001, 10), 2);
	gridPoints_map[3] = GridPoint(3, 2, Point_3D(25.5, 20.5, 10.0001), 2);
	gridPoints_map[4] = GridPoint(4, 2, Point_3D(25.5, 20, 10), 2);
	gridPoints_map[5] = GridPoint(5, 2, Point_3D(25.00, 19, 10), 2);

	ConcentricCircles_3D concentricCircles(
							Point_3D( 25,20, 10),    // should get back 2 3 4 5
							Point_3D( 26,20, 10),
							Point_3D( 25,20.5, 10));

	GetGridPointsWithinGeometry (	gridPoints_map,
										concentricCircles,
										GEOMETRY_INTERIOR_AND_BOUNDARY,
										precision,
										gridPointIds_BetweenOrOnConcentricCircles );

	CPPUNIT_ASSERT_EQUAL(4, (int)gridPointIds_BetweenOrOnConcentricCircles.size());
	CPPUNIT_ASSERT_EQUAL(2, gridPointIds_BetweenOrOnConcentricCircles[0]);
	CPPUNIT_ASSERT_EQUAL(3, gridPointIds_BetweenOrOnConcentricCircles[1]);
	CPPUNIT_ASSERT_EQUAL(4, gridPointIds_BetweenOrOnConcentricCircles[2]);
	CPPUNIT_ASSERT_EQUAL(5, gridPointIds_BetweenOrOnConcentricCircles[3]);
/*	std::cout << std::endl << "IDs Between/Within Circle";
	for ( std::vector<int>::const_iterator i( gridPointIds_BetweenOrOnConcentricCircles.begin()); i != gridPointIds_BetweenOrOnConcentricCircles.end(); ++i)
	{
		std::cout << std::endl << *i;
	}*/

}

void Tests::GetGridPointsBetweenOrOnConcentricCircles_test()
{
	::GetGridPointsBetweenOrOnConcentricCircles_test(true);
}
/////////////////////////////////////////////////////////////////////////////////
void Tests::ShortestDistanceBetweenPointAndLine_test()
{

	////////////////////////////////////////////
	// ShortestDistanceBetweenPointAndLine
	////////////////////////////////////////////

	std::vector<Point_3D> line;
	double precision = .001;

	line.push_back( Point_3D(-5,5,0));
	line.push_back( Point_3D(-10,10,0));

	Point_3D  point(-10.001,10,0);  // on second point
	//Point_3D  point(-10,5,0);
	//Point_3D  point(-6,6,0);  // on the line

	double distance = ShortestDistanceBetweenPointAndLine(line, point, precision );
	/*std::cout << std::endl << "Line, start point, end point: " << line[0] << "  " << line[1];
	std::cout << std::endl << "Point: " << point;
	std::cout << std::endl << "Distance: " << distance;*/

	CPPUNIT_ASSERT_DOUBLES_EQUAL(0, distance, 0.001);

}
/////////////////////////////////////////////////////////////////////////////////
static void OnOrWithinCylinder_test(e_GeneralGeometryInclusionSpecifier	in_GeneralGeometryInclusionSpecifier)
{

	////////////////////////////////////////////
	// On or Within Cylinder
	////////////////////////////////////////////

	std::map<int,GridPoint> gridPoints_map;

	double precision = .001;
	std::vector<int>       gridPointIds_OnCylinderSurface;

	gridPoints_map[1] = GridPoint(1, 2, Point_3D(0, 0, 10), 2);
	gridPoints_map[2] = GridPoint(2, 2, Point_3D(358.9941, 104.2714, 165.0611),  2);
	gridPoints_map[3] = GridPoint(3, 2, Point_3D(307.16, 75 + 30, 267.98), 2);
	gridPoints_map[4] = GridPoint(4, 2, Point_3D(338.0405, 95.0519, 169.8683), 2);
	gridPoints_map[5] = GridPoint(5, 2, Point_3D(350, 80, 170), 2);	// interior of cylinder

	Cylinder_3D  cyclinder(
							Point_3D( 307.16, 75, 267.98),    // should get back 2 3 4 5
							Point_3D( 385.66, 75, 132.02),
							Point_3D( 358.9941, 104.2714, 165.0611),
							GEOMETRY_INCLUDE_END_CAP);

	//cylinder.push_back( Point_3D( 307.16, 75, 267.98));  // 1st point, first point at center of cylinder
	//cylinder.push_back( Point_3D( 385.66, 75, 132.02));	// 2nd point, second point at center of cylinder
	////cylinder.push_back( Point_3D( 25,20, 10));  // 2nd pointCheck identical points
	//cylinder.push_back( Point_3D( 358.9941, 104.2714, 165.0611));  // 3rd point, on cylinder surface
 	////cylinder.push_back( Point_3D( 25,20, 10));  // 3rd Check identical points
	////cylinder.push_back( Point_3D( 27,20, 10));   // 3rd Check colinear with line from 1st and 2nd points

	GetGridPointsWithinGeometry (	gridPoints_map,
									cyclinder,
									in_GeneralGeometryInclusionSpecifier,
									precision,
									gridPointIds_OnCylinderSurface );

	CPPUNIT_ASSERT_EQUAL(3, (int)gridPointIds_OnCylinderSurface.size());
	CPPUNIT_ASSERT_EQUAL(2, gridPointIds_OnCylinderSurface[0]);
	CPPUNIT_ASSERT_EQUAL(3, gridPointIds_OnCylinderSurface[1]);
	CPPUNIT_ASSERT_EQUAL(4, gridPointIds_OnCylinderSurface[2]);
	/*std::cout << std::endl << "IDs on/within cylinder";
	for ( std::vector<int>::const_iterator i( gridPointIds_OnCylinderSurface.begin()); i != gridPointIds_OnCylinderSurface.end(); ++i)
	{
		std::cout << std::endl << *i;
	}*/
}

void Tests::OnOrWithinCylinder_test()
{
	::OnOrWithinCylinder_test(isis_CADCommon::GEOMETRY_BOUNDARY_ONLY);
}
/////////////////////////////////////////////////////////////////////////////////
static void OnCylinderSurface_test(e_GeneralGeometryInclusionSpecifier	in_GeneralGeometryInclusionSpecifier)
{

	////////////////////////////////////////////
	// On Cylinder Surface
	////////////////////////////////////////////

	std::map<int,GridPoint> gridPoints_map;

	double precision = .001;
	std::vector<int>       gridPointIds_OnCylinderSurface;

	gridPoints_map[1] = GridPoint(1, 2, Point_3D(0, 0, 10), 2);
	gridPoints_map[2] = GridPoint(2, 2, Point_3D(358.9941, 104.2714, 165.0611),  2);
	gridPoints_map[3] = GridPoint(3, 2, Point_3D(307.16, 75 + 30, 267.98), 2);
	gridPoints_map[4] = GridPoint(4, 2, Point_3D(338.0405, 95.0519, 169.8683), 2);

	Cylinder_3D  cyclinderSurface(
							Point_3D( 307.16, 75, 267.98),		// should get back 2 3 4 for GEOMETRY_INCLUDE_END_CAP
							Point_3D( 385.66, 75, 132.02),		// should get back 2 4 for GEOMETRY_EXCLUDE_END_CAP
							Point_3D( 358.9941, 104.2714, 165.0611),
							GEOMETRY_INCLUDE_END_CAP);

	//cylinder.push_back( Point_3D( 307.16, 75, 267.98));  // 1st point, first point at center of cylinder
	//cylinder.push_back( Point_3D( 385.66, 75, 132.02));	// 2nd point, second point at center of cylinder
	////cylinder.push_back( Point_3D( 25,20, 10));  // 2nd pointCheck identical points
	//cylinder.push_back( Point_3D( 358.9941, 104.2714, 165.0611));  // 3rd point, on cylinder surface
 	////cylinder.push_back( Point_3D( 25,20, 10));  // 3rd Check identical points
	////cylinder.push_back( Point_3D( 27,20, 10));   // 3rd Check colinear with line from 1st and 2nd points

	GetGridPointsWithinGeometry (	gridPoints_map,
										cyclinderSurface,
										GEOMETRY_BOUNDARY_ONLY,
										precision,
										gridPointIds_OnCylinderSurface );

	CPPUNIT_ASSERT_EQUAL(3, (int)gridPointIds_OnCylinderSurface.size());
	CPPUNIT_ASSERT_EQUAL(2, gridPointIds_OnCylinderSurface[0]);
	CPPUNIT_ASSERT_EQUAL(3, gridPointIds_OnCylinderSurface[1]);
	CPPUNIT_ASSERT_EQUAL(4, gridPointIds_OnCylinderSurface[2]);
/*	std::cout << std::endl << "IDs On cylinder";
	for ( std::vector<int>::const_iterator i( gridPointIds_OnCylinderSurface.begin()); i != gridPointIds_OnCylinderSurface.end(); ++i)
	{
		std::cout << std::endl << *i;
	}	*/
}

void Tests::OnCylinderSurface_test()
{
	::OnCylinderSurface_test(GEOMETRY_BOUNDARY_ONLY);
}
/////////////////////////////////////////////////////////////////////////////////
static void WithinSphere_test(bool in_IncludePointsOnBoundary)
{

	////////////////////////////////////////////
	// Within Sphere
	////////////////////////////////////////////

	std::map<int,GridPoint> gridPoints_map;

	double precision = .001;
	std::vector<int>       gridPointIds_WithinSphere;

	gridPoints_map[1] = GridPoint(1, 2, Point_3D(5, 6, 1), 2);
	gridPoints_map[2] = GridPoint(2, 2, Point_3D(5.1, 6.1, .1 ),  2);
	gridPoints_map[3] = GridPoint(3, 2, Point_3D(6, 6, 0 ), 2);
	gridPoints_map[4] = GridPoint(4, 2, Point_3D(5, 7.00099999, 0), 2);

	Sphere_3D  sphere(
							Point_3D(  5, 6, 0 ),    // should get back ???
							Point_3D(  5, 7, 0 ));

	//sphere.pointWithinGeometry( Point_3D(5, 7.001, 0),true, precision );

	GetGridPointsWithinGeometry (	gridPoints_map,
										sphere,
										GEOMETRY_INTERIOR_AND_BOUNDARY,
										precision,
										gridPointIds_WithinSphere );

	CPPUNIT_ASSERT_EQUAL(4, (int)gridPointIds_WithinSphere.size());
	CPPUNIT_ASSERT_EQUAL(1, gridPointIds_WithinSphere[0]);
	CPPUNIT_ASSERT_EQUAL(2, gridPointIds_WithinSphere[1]);
	CPPUNIT_ASSERT_EQUAL(3, gridPointIds_WithinSphere[2]);
	CPPUNIT_ASSERT_EQUAL(4, gridPointIds_WithinSphere[3]);
/*	std::cout << std::endl << "IDs within Sphere";
	for ( std::vector<int>::const_iterator i( gridPointIds_WithinSphere.begin()); i != gridPointIds_WithinSphere.end(); ++i)
	{
		std::cout << std::endl << *i;
	}	*/

}

void Tests::WithinSphere_test()
{
	::WithinSphere_test(isis_CADCommon::GEOMETRY_BOUNDARY_ONLY);
}
/////////////////////////////////////////////////////////////////////////////////

void Tests::OnSphereSurface_test()
{

	////////////////////////////////////////////
	// On Sphere Surface
	////////////////////////////////////////////

	std::map<int,GridPoint> gridPoints_map;

	double precision = .001;
	std::vector<int>       gridPointIds_WithinsphereSurface;

	gridPoints_map[1] = GridPoint(1, 2, Point_3D(5, 6.001, 1.00), 2);
	gridPoints_map[2] = GridPoint(2, 2, Point_3D(5.1, 6.1, .1 ),  2);
	gridPoints_map[3] = GridPoint(3, 2, Point_3D(6, 6, 0 ), 2);
	gridPoints_map[4] = GridPoint(4, 2, Point_3D(338.0405, 95.0519, 169.8683), 2);

	SphereSurface_3D  sphereSurface(
							Point_3D( 5, 6, 0 ),    // should get back ???
							Point_3D(  5, 7, 0 ));

	sphereSurface.pointWithinGeometry(Point_3D(5, 6.01, 1.00), true, precision);

	GetGridPointsWithinGeometry (	gridPoints_map,
										sphereSurface,
										GEOMETRY_INTERIOR_AND_BOUNDARY,
										precision,
										gridPointIds_WithinsphereSurface );

	CPPUNIT_ASSERT_EQUAL(2, (int)gridPointIds_WithinsphereSurface.size());
	CPPUNIT_ASSERT_EQUAL(1, gridPointIds_WithinsphereSurface[0]);
	CPPUNIT_ASSERT_EQUAL(3, gridPointIds_WithinsphereSurface[1]);
	/*std::cout << std::endl << "IDs within sphereSurface";
	for ( std::vector<int>::const_iterator i( gridPointIds_WithinsphereSurface.begin()); i != gridPointIds_WithinsphereSurface.end(); ++i)
	{
		std::cout << std::endl << *i;
	}*/

}
/////////////////////////////////////////////////////////////////////////////////

static void WithinExtrusion_test(bool in_IncludePointsOnBoundary)
{
	////////////////////////////////////////////
	// Within Extrusion
	////////////////////////////////////////////

	std::map<int,GridPoint> gridPoints_map;

	double precision = .001;
	std::vector<int>       gridPointIds_WithinExtrusion;

	gridPoints_map[1] = GridPoint(1, 2, Point_3D(5, 0, 0 ), 2);
	gridPoints_map[2] = GridPoint(2, 2, Point_3D(1, -1, 1), 2);
	gridPoints_map[3] = GridPoint(3, 2, Point_3D(3, 3, 4.99), 2);
	gridPoints_map[4] = GridPoint(4, 2, Point_3D(5, 5, 20), 2);

	std::vector<Point_3D>   polygon;

	polygon.push_back( Point_3D( 5, 0, 0));
	polygon.push_back( Point_3D( 5, 5, 0));
	polygon.push_back( Point_3D( 0, 5, 0));
	polygon.push_back( Point_3D( 0, 0, 0));

	Point_3D  offsetPoint (0, 0, 5 );

	Extrusion_3D  extrusion( polygon,     // should get back ???
								offsetPoint,
								precision);

	//extrusion.pointWithinGeometry(Point_3D(4, 3, 1.00), true, precision);

	GetGridPointsWithinGeometry (	gridPoints_map,
										extrusion,
										GEOMETRY_INTERIOR_AND_BOUNDARY,
										precision,
										gridPointIds_WithinExtrusion );

	CPPUNIT_ASSERT_EQUAL(2, (int)gridPointIds_WithinExtrusion.size());
	CPPUNIT_ASSERT_EQUAL(1, gridPointIds_WithinExtrusion[0]);
	CPPUNIT_ASSERT_EQUAL(3, gridPointIds_WithinExtrusion[1]);

	/*std::cout << std::endl << "IDs within extrusionSurface";
	for ( std::vector<int>::const_iterator i( gridPointIds_WithinExtrusion.begin()); i != gridPointIds_WithinExtrusion.end(); ++i)
	{
		std::cout << std::endl << *i;
	}*/
}

void Tests::WithinExtrusion_test()
{
	::WithinExtrusion_test(true);
}
#if 0
/////////////////////////////////////////////////////////////////////////////////
//	void ComputeStressValues( double  in_StressTensor[3][3],
//							  double &out_VonMises,		// always postive
//							  double &out_MaxShear,		// either 0 ( no shear stress ) or positive
//							  double &out_MaxBearing	// either 0 ( no bearing stress ) or positive
//							 ) throw (isis::application_exception);
void ComputeStressValues_test()
{
	//		S00	S01	S02
	//		S10	S11	S12
	//		S20	S21	S22

	double stressTensor[3][3];

	// From Abaqus
	stressTensor[0][0] = -0.2267;
	stressTensor[1][1] = .0133441;
	stressTensor[2][2] = .0311546;

	stressTensor[0][1] = -.0137862;
	stressTensor[0][2] = -.107133;
	stressTensor[1][2] = -.0170857;

	// Result From Abaqus
	// Principal  .0721027   .0123583  -.266663
	// Von Mises  .313196
	// Tresca = .338765
	// Pressure = .0607339

	// From Abaqus
	stressTensor[0][0] = 100;
	stressTensor[1][1] = 100;
	stressTensor[2][2] = 100;

	stressTensor[0][1] = 0;
	stressTensor[0][2] = 0;
	stressTensor[1][2] = 0;

	//stressTensor[0][0] = 100;
	//stressTensor[1][1] = 200;
	//stressTensor[2][2] = 0;

	//stressTensor[0][1] = 88;
	//stressTensor[0][2] = 0;
	//stressTensor[1][2] = 0;

	double VonMises;
	double MaxShear;
	double MaxBearing;

	isis_CADCommon::ComputeStressValues( stressTensor, VonMises, MaxShear, MaxBearing );

	std::cout << std::endl << "Stress Tensor";
	std::cout << std::endl << stressTensor[0][0] << "  " << stressTensor[0][1] << "  " << stressTensor[0][2];
	std::cout << std::endl << stressTensor[1][0] << "  " << stressTensor[1][1] << "  " << stressTensor[1][2];
	std::cout << std::endl << stressTensor[2][0] << "  " << stressTensor[2][1] << "  " << stressTensor[2][2];
	std::cout << std::endl;
	std::cout << std::endl << "VonMises:   " << VonMises;
	std::cout << std::endl << "MaxShear:   " << MaxShear;
	std::cout << std::endl << "MaxBearing: " << MaxBearing;

	std::cout << std::endl << "******************************************";
	std::cout << std::endl << "************** CalculiX ******************";
	std::cout << std::endl << "******************************************";

	double stressTensor_CalculiX[6];
	stressTensor_CalculiX[0] = stressTensor[0][0];
	stressTensor_CalculiX[1] = stressTensor[1][1];
	stressTensor_CalculiX[2] = stressTensor[2][2];
	stressTensor_CalculiX[3] = stressTensor[0][1];
	stressTensor_CalculiX[5] = stressTensor[0][2];
	stressTensor_CalculiX[4] = stressTensor[1][2];

	double stressPrincipal_CalculiX[12];
	double alpha_0[3];
	double alpha_1[3];
	double alpha_2[3];
#define  TEST1
	//calcPrinc( double *s, double *p, double *a0, double *a1, double *a2, int flag )
	isis_CADCommon::calcPrinc(  stressTensor_CalculiX,  stressPrincipal_CalculiX, alpha_0, alpha_1, alpha_2, 0 );
	std::cout << std::endl << "CalculiX Principal 0: " << stressPrincipal_CalculiX[0];
	std::cout << std::endl << "CalculiX Principal 1: " << stressPrincipal_CalculiX[1];
	std::cout << std::endl << "CalculiX Principal 2: " << stressPrincipal_CalculiX[2];
/*
	double stressTensor[3][3];

	stressTensor[0][0] = -2;
	stressTensor[1][1] = 10;
	stressTensor[2][2] = -5;

	stressTensor[0][1] = 7;
	stressTensor[0][2] = -3;
	stressTensor[1][2] = 5;

	double VonMises;
	double MaxShear;
	double MaxBearing;

	isis_CADCommon::ComputeStressValues( stressTensor, VonMises, MaxShear, MaxBearing );

	std::cout << std::endl << "Stress Tensor";
	std::cout << std::endl << stressTensor[0][0] << "  " << stressTensor[0][1] << "  " << stressTensor[0][2];
	std::cout << std::endl << stressTensor[1][0] << "  " << stressTensor[1][1] << "  " << stressTensor[1][2];
	std::cout << std::endl << stressTensor[2][0] << "  " << stressTensor[2][1] << "  " << stressTensor[2][2];
	std::cout << std::endl;
	std::cout << std::endl << "VonMises:   " << VonMises;
	std::cout << std::endl << "MaxShear:   " << MaxShear;
	std::cout << std::endl << "MaxBearing: " << MaxBearing;
*/
}
#endif

template<typename T> std::vector<T> make_vec(T *values, int N) {
    std::vector<T> v;
	for (int i = 0; i < N; i++)
		v.push_back(values[i]);
	return v;
}

void Tests::NastranDeck_test()
{
	try
		{
			NastranDeck deck;
			std::cout << std::endl << "Reading Deck: " << "..\\..\\Edited_Thermal_Mesh.nas";
			deck.ReadNastranDeck("src\\CADAssembler\\CADCommonTest\\Edited_Thermal_Mesh.nas");
			//deck.ReadNastranDeck("..\\Edited_Thermal_Mesh.nas");

			//std::cout << std::endl << "Display Deck: ";
			//std::cout << deck;

			NastranDeckHelper nastranDeckHelper(deck);

			bool	DefaultGridPointTemperature_set;
			double  DefaultGridPointTemperature;

			//std::cout << std::endl;
			//std::cout << std::endl << "*****************  nastranDeckHelper ***********************";

			///////////////////////////////
			// DefaultGridPointTemperature
			///////////////////////////////
			nastranDeckHelper.getDefaultGridPointTemperature( DefaultGridPointTemperature_set, DefaultGridPointTemperature );

			//std::cout << std::endl;
			//std::cout << std::endl << "*****************  DefaultGridPointTemperature ***********************";

			CPPUNIT_ASSERT(DefaultGridPointTemperature_set);
			CPPUNIT_ASSERT_DOUBLES_EQUAL(293, DefaultGridPointTemperature, 0.0001);

			///////////////////////////////
			// GridPointTemperatures
			///////////////////////////////
			 std::map<int,double> gridPointToTemperature_map;

			//std::cout << std::endl;
			//std::cout << std::endl << "*****************  GridPointTemperatures ***********************";

			nastranDeckHelper.getSpecifiedGridPointTemperatures( gridPointToTemperature_map );

			CPPUNIT_ASSERT_EQUAL(9, (int)gridPointToTemperature_map.size());
			for each ( const std::pair<int,double> &i in gridPointToTemperature_map)
			{
				//std::cout << std::endl << "Grid Point: " << i.first << "  Temperature: " << i.second;
				CPPUNIT_ASSERT_DOUBLES_EQUAL(401.123, i.second, 0.0001);
			}

			//std::cout << std::endl;
			//std::cout << std::endl << "*****************  findElementsContainingSurface ***********************";

			std::vector<int> surfaceCornerGridIDs;

			surfaceCornerGridIDs.push_back(20);
			surfaceCornerGridIDs.push_back(14);
			surfaceCornerGridIDs.push_back(32);

			bool elementFound;
			std::set<int> elementIDs;

			nastranDeckHelper.findElementsContainingSurface( surfaceCornerGridIDs,elementFound, elementIDs );

			//std::cout <<std::endl << "Element Key: ";
			//for each ( const int &j in surfaceCornerGridIDs) std::cout << " " << j;

			CPPUNIT_ASSERT(elementFound);

			// In the deck Edited_Thermal_Mesh.nas, there are two tetra with the element combination
			// CTETRA,20,1,20,14,32,13,93,94,
			//	,117,87,84,92
			// CTETRA,10,1,14,20,32,5,93,117,       // We want this one
			//	,94,50,51,52

			int elementID = 0;
			for each ( const int &i_target in elementIDs )
			{
				if ( i_target == 10 ) elementID = 10;
			}

			CPPUNIT_ASSERT_EQUAL(10, elementID);
			CPPUNIT_ASSERT_EQUAL(10, deck.getElementData().find(elementID)->second.EID);
			CPPUNIT_ASSERT_EQUAL(CTETRA, deck.getElementData().find(elementID)->second.Type);

			//std::cout << std::endl;
			//std::cout << std::endl << "*****************  getHeatFluxLoadsForBoundarySurfaces ***********************";

			std::vector<HeatFluxLoad> heatFluxLoads;
			nastranDeckHelper.getHeatFluxLoadsForBoundarySurfaces( heatFluxLoads ) ;
			CPPUNIT_ASSERT_EQUAL(2, (int)heatFluxLoads.size());
			//const std::map<int, FEAElement> &elementData_map = deck.getElementData();

			CPPUNIT_ASSERT_EQUAL(std::string("QBDY3"), heatFluxLoads[0].name);
			CPPUNIT_ASSERT_EQUAL(39, heatFluxLoads[0].elementIDThatContainsSurface);
			CPPUNIT_ASSERT_DOUBLES_EQUAL(310, heatFluxLoads[0].Q0, 0.0001);
			CPPUNIT_ASSERT_EQUAL(59, heatFluxLoads[0].SID);
			int vals[] = {12,30,29,83,141,82};
			CPPUNIT_ASSERT_EQUAL(make_vec<int>(vals, 6), heatFluxLoads[0].surfaceGridPointIDs);

			CPPUNIT_ASSERT_EQUAL(std::string("QBDY3"), heatFluxLoads[1].name);
			CPPUNIT_ASSERT_EQUAL(49, heatFluxLoads[1].elementIDThatContainsSurface);
			CPPUNIT_ASSERT_DOUBLES_EQUAL(310, heatFluxLoads[1].Q0, 0.0001);
			CPPUNIT_ASSERT_EQUAL(59, heatFluxLoads[1].SID);
			int vals2[] = {29,12,11,82,73,77};
			CPPUNIT_ASSERT_EQUAL(make_vec<int>(vals2, 6), heatFluxLoads[1].surfaceGridPointIDs);

			//std::cout << std::endl;
			//std::cout << std::endl << "*****************  getSurfaceConvectionConstraints ***********************";

			std::vector<SurfaceConvection> surfaceConvections;
			nastranDeckHelper.getSurfaceConvectionConstraints ( surfaceConvections);
			CPPUNIT_ASSERT_EQUAL(2, (int)surfaceConvections.size());
			CPPUNIT_ASSERT_DOUBLES_EQUAL(222.34, surfaceConvections[0].convectionCoefficient, 0.01);
			CPPUNIT_ASSERT_DOUBLES_EQUAL(243.99, surfaceConvections[0].ambientTemperature, 0.01);
			CPPUNIT_ASSERT_EQUAL(24, surfaceConvections[0].elementIDThatContainsSurface);
			CPPUNIT_ASSERT_EQUAL(100005, surfaceConvections[0].convID);
			CPPUNIT_ASSERT_EQUAL(100004, surfaceConvections[0].pconvID);
			CPPUNIT_ASSERT_EQUAL(100003, surfaceConvections[0].mat4ID);
			CPPUNIT_ASSERT_EQUAL(145, surfaceConvections[0].spointID);
			int vals3[] = {3,1,4,35,36,44};
			CPPUNIT_ASSERT_EQUAL(make_vec<int>(vals3, 6), surfaceConvections[0].surfaceGridPointIDs);

			CPPUNIT_ASSERT_DOUBLES_EQUAL(222.34, surfaceConvections[1].convectionCoefficient, 0.01);
			CPPUNIT_ASSERT_DOUBLES_EQUAL(243.99, surfaceConvections[1].ambientTemperature, 0.01);
			CPPUNIT_ASSERT_EQUAL(43, surfaceConvections[1].elementIDThatContainsSurface);
			CPPUNIT_ASSERT_EQUAL(100006, surfaceConvections[1].convID);
			CPPUNIT_ASSERT_EQUAL(100004, surfaceConvections[1].pconvID);
			CPPUNIT_ASSERT_EQUAL(100003, surfaceConvections[1].mat4ID);
			CPPUNIT_ASSERT_EQUAL(145, surfaceConvections[1].spointID);
			int vals4[] = {2,1,3,34,35,41};
			CPPUNIT_ASSERT_EQUAL(make_vec<int>(vals4, 6), surfaceConvections[1].surfaceGridPointIDs);

			//std::cout << std::endl;
			//std::cout << std::endl << "*****************  getSurfaceElementsContainingGridPoints ***********************";

			std::set<int>  targetGridPoints;
			// Element 1
			// CTETRA,1,1,17,22,33,23,102,124,
			// ,105,103,121,128
			targetGridPoints.insert(17);
			targetGridPoints.insert(22);
			targetGridPoints.insert(33);
			//targetGridPoints.insert(23);

			// Element 9
			// CTETRA,9,1,9,15,21,10,63,96,
			// ,64,62,67,69

			targetGridPoints.insert(9);
			targetGridPoints.insert(15);
			targetGridPoints.insert(21);

			std::multimap< int, std::vector<int>> foundElements_map;

			nastranDeckHelper.getSurfaceElementsContainingGridPoints (	targetGridPoints, foundElements_map);

			CPPUNIT_ASSERT_EQUAL(3, (int)foundElements_map.size());

			for each ( const std::pair< int, std::vector<int>> &i in foundElements_map )
			{
				//std::cout << std::endl <<  "Element ID: " << i.first  << " Grid Points:" ;
				//for each ( const int &j in i.second ) std::cout << " " << j;
				if (i.first == 1)
				{
					int vals4[] = {17, 22, 33, 102, 124, 105};
					CPPUNIT_ASSERT_EQUAL(make_vec<int>(vals4, 6), i.second);
				} else if (i.first == 7)
				{
					int vals4[] = {21, 22, 33, 118, 124, 120};
					CPPUNIT_ASSERT_EQUAL(make_vec<int>(vals4, 6), i.second);
				} else if (i.first == 9)
				{
					int vals4[] = {9, 15, 21, 63, 96, 64};
					CPPUNIT_ASSERT_EQUAL(make_vec<int>(vals4, 6), i.second);
				} else {
					CPPUNIT_ASSERT(false);
				}

			}

			//std::cout << std::endl;
			//std::cout << std::endl << "*****************  getVolumetricHeatGenerations ***********************";

			std::vector<VolumetricHeatGeneration> volumetricHeatGenerations;

			nastranDeckHelper.getVolumetricHeatGenerations( volumetricHeatGenerations );

			CPPUNIT_ASSERT_EQUAL(1, (int)volumetricHeatGenerations.size());
			CPPUNIT_ASSERT_DOUBLES_EQUAL(.003114, volumetricHeatGenerations[0].powerInputPerVolume, 0.0001);
			int i=1;
			for each ( const int j in volumetricHeatGenerations[0].elementIDs )
			{
				CPPUNIT_ASSERT_EQUAL(j, i);
				i++;
			}

		}

		//catch ( isis::application_exception &ex )
		catch ( isis::application_exception &)
		{
			CPPUNIT_ASSERT(false);

		}

}

/////////////////////////////////////////////////////////////////////////////////

void Tests::ComputeTransformationToAlignVectors_test()
{
	// Warning, This function needs work
	// See GraphicsFunctions ComputeRotationMatrixToAlignVectorsy.cpp

	std::vector<double>	in_Vector_1;
	std::vector<double>	in_Vector_2;

	in_Vector_1.push_back(1);
	in_Vector_1.push_back(1);
	in_Vector_1.push_back(3);

	in_Vector_2.push_back(-1);
	in_Vector_2.push_back(10);
	in_Vector_2.push_back(5);

	double	rotationMatrix[3][3] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};

	//isis_CADCommon::ComputeRotationMatrixToAlignVectors( in_Vector_1, in_Vector_2,rotationMatrix);

	TransformationMatrix transformationMatrix;

	std::vector<double>	offset(3, 0.0);

	transformationMatrix.setTransformationMatrix(rotationMatrix, offset);

	isis_CADCommon::Point_3D  point_start(1,1,3);

	isis_CADCommon::Point_3D  point_transformed = transformationMatrix.getTransformedCoordinates( point_start);

	/*std::cout << std::endl << "Vector 1";
	for each ( double i in in_Vector_1 ) std::cout << std::endl << i;

	std::cout << std::endl << "Vector 2";
	for each ( double i in in_Vector_2 ) std::cout << std::endl << i;

	std::cout << std::endl;
	std::cout << std::endl << "Point Start:       " << point_start;
	std::cout << std::endl << "Point Transformed: " << point_transformed;*/

	CPPUNIT_ASSERT_DOUBLES_EQUAL(0, point_transformed.x, 0.0001);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0, point_transformed.y, 0.0001);
	CPPUNIT_ASSERT_DOUBLES_EQUAL(0, point_transformed.z, 0.0001);
}

/////////////////////////////////////////////////////////////////////////////////
	void Tests::Determinant_4_x_4_test ( )
	{
		// Example from http://nebula.deanza.edu/~bloom/math43/Determinant4x4Matrix.pdf
		double matrix[4][4];

		matrix[0][0] = 3.0;
		matrix[0][1] = 0.0;
		matrix[0][2] = 2.0;
		matrix[0][3] = -1.0;

		matrix[1][0] = 1.0;
		matrix[1][1] = 2.0;
		matrix[1][2] = 0.0;
		matrix[1][3] = -2.0;

		matrix[2][0] = 4.0;
		matrix[2][1] = 0.0;
		matrix[2][2] = 6.0;
		matrix[2][3] = -3.0;

		matrix[3][0] = 5.0;
		matrix[3][1] = 0.0;
		matrix[3][2] = 2.0;
		matrix[3][3] = 0.0;

		double determinant = Determinant_4_x_4(matrix);

		CPPUNIT_ASSERT_DOUBLES_EQUAL(20, determinant, 0.001);
		//std::cout << std::endl << "Determinant, Answer should be 20.";
		//std::cout << std::endl << "Determinant Answer: " << determinant;

		matrix[0][0] = 1.0;
		matrix[0][1] = 2.0;
		matrix[0][2] = 3.0;
		matrix[0][3] = 4.0;

		matrix[1][0] = -1.0;
		matrix[1][1] = -2.0;
		matrix[1][2] = -3.0;
		matrix[1][3] = -4.0;

		matrix[2][0] = 5.0;
		matrix[2][1] = 6.0;
		matrix[2][2] = 7.0;
		matrix[2][3] = 8.0;

		matrix[3][0] = -8.0;
		matrix[3][1] = -7.0;
		matrix[3][2] = -6.0;
		matrix[3][3] = -5.0;

		determinant = Determinant_4_x_4(matrix);

		CPPUNIT_ASSERT_DOUBLES_EQUAL(0, determinant, 0.001);
		//std::cout << std::endl << "Determinant, Answer should be 0.";
		//std::cout << std::endl << "Determinant Answer: " << determinant;

		matrix[0][0] = 5.0;
		matrix[0][1] = 23.0;
		matrix[0][2] = 14.0;
		matrix[0][3] = 17.0;

		matrix[1][0] = 4.0;
		matrix[1][1] = 5.0;
		matrix[1][2] = 6.0;
		matrix[1][3] = 7.0;

		matrix[2][0] = 8.0;
		matrix[2][1] = 9.0;
		matrix[2][2] = 10.0;
		matrix[2][3] = 11.0;

		matrix[3][0] = 24.0;
		matrix[3][1] = 18.0;
		matrix[3][2] = -2.0;
		matrix[3][3] = 4.0;

		determinant = Determinant_4_x_4(matrix);

		CPPUNIT_ASSERT_DOUBLES_EQUAL(2136, determinant, 0.001);
		//std::cout << std::endl << "Determinant, Answer should be 2136.";
		//std::cout << std::endl << "Determinant Answer: " << determinant;

	}
#if 0
	void PointWithinOrOnTetrahedron_test()
	{

		/* All positive coordinates
		isis_CADCommon::Point_3D  point_1(  0,  0, 0 );
		isis_CADCommon::Point_3D  point_2(  0,  1, 0 );
		isis_CADCommon::Point_3D  point_3(  1,  1, 0 );
		isis_CADCommon::Point_3D  point_4( .5, .5, 1.0 );

		std::vector<isis_CADCommon::Point_3D> Tetrahedron_points;

		Tetrahedron_points.push_back(point_1);
		Tetrahedron_points.push_back(point_2);
		Tetrahedron_points.push_back(point_3);
		Tetrahedron_points.push_back(point_4);

		//Point_3D point(.5,.5, 1.0005);  // Not Within/On
		//Point_3D point(.5,.5, 1.000);  // Within/On

		//Point_3D point(.25,.25, .500001);  // Not Within/On
		//Point_3D point(.25,.25, .5);  // Within/On

		//Point_3D point(.75,.75, .500001);  // Not Within/On
		//Point_3D point(.75,.75, .5);  // Within/On

		//Point_3D point(0.0, 1.0, 0);  // Within/On
		//Point_3D point(0.0, 1.000006, 0);  // Not Within/On

		//Point_3D point(1.0, 1.0, 0);  // Within/On
		//Point_3D point(1.0, 1.000006, 0);  // Not Within/On

		Point_3D point(.2, .2, .2);  //  Within/On
		bool pointWithin/On =  PointWithin/OnTetrahedron ( Tetrahedron_points, point );

		*/

		/* All negative coordinates
		isis_CADCommon::Point_3D  point_1(  0,  0, 0 );
		isis_CADCommon::Point_3D  point_2(  0,  -1, 0 );
		isis_CADCommon::Point_3D  point_3(  -1,  -1, 0 );
		isis_CADCommon::Point_3D  point_4( -.5, -.5, -1.0 );

		std::vector<isis_CADCommon::Point_3D> Tetrahedron_points;

		Tetrahedron_points.push_back(point_1);
		Tetrahedron_points.push_back(point_2);
		Tetrahedron_points.push_back(point_3);
		Tetrahedron_points.push_back(point_4);

		//Point_3D point(-.5,-.5, -1.0005);  // Not Within/On
		//Point_3D point(-.5,-.5, -1.000);  // Within/On

		//Point_3D point(-.25,-.25, -.500001);  // Not Within/On
		//Point_3D point(-.25,-.25, -.5);  // Within/On

		//Point_3D point(-.75,-.75, -.500001);  // Not Within/On
		//Point_3D point(-.75,-.75, -.5);  // Within/On

		//Point_3D point(0.0, -1.0, 0);  // Within/On
		//Point_3D point(0.0, -1.000006, 0);  // Not Within/On

		//Point_3D point(-1.0, -1.0, 0);  // Within/On
		//Point_3D point(-1.0, -1.000006, 0);  // Not Within/On

		Point_3D point(-.2, -.2, -.2);  //  Within/On

		bool pointWithin/On =  PointWithin/OnTetrahedron ( Tetrahedron_points, point );
		*/

		//* Mix negative and positive
		isis_CADCommon::Point_3D  point_1( -.5,   0, 0 );
		isis_CADCommon::Point_3D  point_2(   .5,  .5, 0 );
		isis_CADCommon::Point_3D  point_3(  .5,  0, 0 );

		isis_CADCommon::Point_3D  point_4(   0,   .5, 1.0 );

		std::vector<isis_CADCommon::Point_3D> Tetrahedron_points;

		Tetrahedron_points.push_back(point_1);
		Tetrahedron_points.push_back(point_2);
		Tetrahedron_points.push_back(point_3);
		Tetrahedron_points.push_back(point_4);

		//Point_3D point( 0, 0.5, -1.0005);  // Not Within/On
		//Point_3D point( 0, 0.5, 1.000);  // Within/On

		//Point_3D point(.25, .25, .500001);  // Not Within/On
		//Point_3D point(.25, .25, .5);  // Within/On

		//Point_3D point( .25, .25, .500001);  // Not Within/On
		//Point_3D point( -.25, .25, .5);  // Within/On

		//Point_3D point( .5, .5, 0);  // Within/On
		//Point_3D point(.5, .5000001, 0);  // Not Within/On

		//Point_3D point(-.5, 0 , 0);  // Within/On
		//Point_3D point( -.50000001, 0 , 0);  // Not Within/On

		Point_3D point(.2, .2, .2);  //  Within/On

		bool pointWithin =  PointWithinOrOnTetrahedron ( Tetrahedron_points, point );
		//*/

		std::cout << std::endl << "pointWithin: " << pointWithin;
	}

	void CentroidOfTetrahedron_test()
	{
		//isis_CADCommon::Point_3D  point_1(  0,  0, 0 );
		//isis_CADCommon::Point_3D  point_2(  1,  0, 0 );
		//isis_CADCommon::Point_3D  point_3(  0,  1, 0 );
		//isis_CADCommon::Point_3D  point_4( .5, .5, 1.0 );

		isis_CADCommon::Point_3D  point_1(  1,  0, 0 );
		isis_CADCommon::Point_3D  point_2(  0,  2, 0 );
		isis_CADCommon::Point_3D  point_3(  0,  4, 1 );
		isis_CADCommon::Point_3D  point_4( 0, 0, 3.0 );
		// Answer Centroid: 0.25  1.5  1

		std::vector<isis_CADCommon::Point_3D> Tetrahedron_points;

		Tetrahedron_points.push_back(point_1);
		Tetrahedron_points.push_back(point_2);
		Tetrahedron_points.push_back(point_3);
		Tetrahedron_points.push_back(point_4);

		Point_3D centroid;

		CentroidOfTetrahedron( Tetrahedron_points, centroid);

		std::cout << std::endl << "Centroid: " << centroid.x << "  " << centroid.y << "  " << centroid.z;
	}
#endif

	void Tests::VolumeOfTetrahedron_test()
	{
		// https://www.easycalculation.com/analytical/parellelepiped-tetrahedron-volume.php

		std::vector<isis_CADCommon::Point_3D> Tetrahedron_points;

		isis_CADCommon::Point_3D  point_1(  0,  0, 0 );
		isis_CADCommon::Point_3D  point_2(  2,  0, 0 );
		isis_CADCommon::Point_3D  point_3(  1,  2, 0 );
		isis_CADCommon::Point_3D  point_4( 1,  1.0, 2.0 );

		Tetrahedron_points.clear();
		Tetrahedron_points.push_back(point_1);
		Tetrahedron_points.push_back(point_2);
		Tetrahedron_points.push_back(point_3);
		Tetrahedron_points.push_back(point_4);

		double volume =  VolumeOfTetrahedron( Tetrahedron_points);
		//std::cout << std::endl << "Answer should be: 1.33";
		//std::cout << std::endl << "Volume: " << volume;
		CPPUNIT_ASSERT_DOUBLES_EQUAL(1.33333, volume, 0.001);

		isis_CADCommon::Point_3D  point_1_2(  0,  0, 0 );
		isis_CADCommon::Point_3D  point_2_2(  1,  0, 0 );
		isis_CADCommon::Point_3D  point_3_2(  0,  1, 0 );
		isis_CADCommon::Point_3D  point_4_2( .5, .5, 1.0 );

		Tetrahedron_points.clear();
		Tetrahedron_points.push_back(point_1_2);
		Tetrahedron_points.push_back(point_2_2);
		Tetrahedron_points.push_back(point_3_2);
		Tetrahedron_points.push_back(point_4_2);

		volume =  VolumeOfTetrahedron( Tetrahedron_points);
		//std::cout << std::endl << "Answer should be: 1.1666";
		//std::cout << std::endl << "Volume: " << volume;
		CPPUNIT_ASSERT_DOUBLES_EQUAL(0.16666666, volume, 0.001);

		isis_CADCommon::Point_3D  point_1_3(  -5,  0, 0 );
		isis_CADCommon::Point_3D  point_2_3(  1,  0, 0 );
		isis_CADCommon::Point_3D  point_3_3(  2,  -1, 0 );
		isis_CADCommon::Point_3D  point_4_3( .5, .5, 3.0 );

		Tetrahedron_points.clear();
		Tetrahedron_points.push_back(point_1_3);
		Tetrahedron_points.push_back(point_2_3);
		Tetrahedron_points.push_back(point_3_3);
		Tetrahedron_points.push_back(point_4_3);

		volume =  VolumeOfTetrahedron( Tetrahedron_points);
		//std::cout << std::endl << "Answer should be: 3.0";
		//std::cout << std::endl << "Volume: " << volume;
		CPPUNIT_ASSERT_DOUBLES_EQUAL(3.0, volume, 0.001);
	}


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

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

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

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

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

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

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

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

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

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

stiff.SetParameters(k_0, k_L, mpa);

stiff.BuildGeneralStiffnessMatrix ( mpsmesh );

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

stiff.MatrixSolver( br, mpx );
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

}

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

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

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

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

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

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

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

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

mpPreviousSolution = mpx;

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

}

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

    int upperindex = relevantMesh.IndexAbove( x );

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

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

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

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

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

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

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

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

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

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

    gradvec.push_back(m);
    }
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

}

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

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

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

    //PrintVector(ErrorEstimate);
}

Solver class generalised for all boundary conditions/GeneralizedHeat.cpp


#include <algorithm>
#include <iostream>
#include <vector>
#include <functional>
#include <stack>
#include <utility>

#include <tr1/array>

#include <boost/lambda/lambda.hpp>

#include <cassert>
#include <cstdlib>
#include <sys/time.h>
#include <ctime>

////////////////////////////////////////////////////////////////////////////////////
std::string tab(int);

std::string
tab(int size)
{
  std::string res;
  for( std::size_t i = 0; i < size; ++i)
  {
    res += " ";
  }
  return res;
}

////////////////////////////////////////////////////////////////////////////////////
typedef std::vector<unsigned int> vec_type;

std::ostream& operator<<(std::ostream&, const std::vector<unsigned int>&);

std::ostream&
operator<<(std::ostream& os, const std::vector<unsigned int>& c)
{
  std::cout << "{";
  std::vector<unsigned int>::const_iterator cit = c.begin();
  if (cit != c.end())
  {
    os << *cit;
    for( ++cit; cit != c.end(); ++cit)
    {
      os << "," << *cit;
    }
  }
  return os << "}";
}

////////////////////////////////////////////////////////////////////////////////////
template<typename Iterator>
void
quicksort(const Iterator first, const Iterator last)
{
  const std::size_t distance = std::distance(first, last);
  if (distance > 1)
  {
    Iterator pivot = first + distance/2;
    std::swap(*pivot, *(last - 1)); // backup pivot in last position
    pivot = last - 1;
    const Iterator middle = std::partition(first, pivot, boost::lambda::_1 < *pivot); // partition of [first,pivot)
    std::swap(*middle, *pivot);
    quicksort(first, middle);
    quicksort(middle + 1, last);
  }
}

////////////////////////////////////////////////////////////////////////////////////
template<typename Iterator>
void
quicksort_stack(const Iterator first, const Iterator last)
{
  typedef typename Iterator::value_type value_type;
  typedef std::pair<Iterator, Iterator> work_type;
  typedef std::stack< work_type > stack_type;
  stack_type stack;

  stack.push(make_pair(first, last));

  while (not stack.empty())
  {
    const work_type w = stack.top();
    stack.pop();

    const std::size_t distance = std::distance(w.first, w.second);
    if (distance > 1)
    {
      Iterator pivot = w.first + distance/2;
      std::swap(*pivot, *(w.second - 1)); // backup pivot in last position
      pivot = w.second - 1;
      const Iterator middle = std::partition(w.first, pivot, boost::lambda::_1 < *pivot); // partition of [first,pivot)
      std::swap(*middle, *pivot);
      stack.push(make_pair(w.first, middle));
      stack.push(make_pair(middle + 1, w.second));
    }
  }
}

////////////////////////////////////////////////////////////////////////////////////
template<typename Iterator>
void
quicksort2(Iterator first, Iterator last, int depth = 0)
{
  while (last - first > 1)
  {
    const std::size_t distance = std::distance(first, last);
    Iterator pivot = first + distance/2;
    std::swap(*pivot, *(last - 1)); // backup pivot in last position
    pivot = last - 1;
    const Iterator middle = std::partition(first, pivot, boost::lambda::_1 < *pivot); // partition of [first,pivot)
    std::swap(*middle, *pivot);
    if (middle - first < last - middle-1)
    {
      quicksort2(first, middle, depth + 4);
      first = middle + 1;
    }
    else
    {
      quicksort2(middle + 1, last, depth + 4);
      last = middle;
    }
  }
}

////////////////////////////////////////////////////////////////////////////////////
int
main ()
{
  std::tr1::array<unsigned int, 4> a = {19,16,1,13};
  std::vector<unsigned int> d(a.begin(), a.end());
  quicksort(d.begin(), d.end());

  std::cout << std::endl << d << std::endl;

  srandom(static_cast<int>(time(NULL)));
  struct timeval begin;
  struct timeval end;
  ::gettimeofday(&begin, NULL);
  for(std::size_t i = 0; i < 10; ++i)
  {
    std::vector<unsigned int> data;
    for( unsigned int i = 0; i < 10000; ++i)
    {
      data.push_back(static_cast<unsigned int>(random() % 20));
    }
//    std::cout << std::endl << data << std::endl;
    std::vector<unsigned int> oracle(data.begin(), data.end());
    quicksort(data.begin(), data.end());
    std::sort(oracle.begin(), oracle.end());
//    std::cout << data << std::endl;
    assert(data == oracle);
  }
  ::gettimeofday(&end, NULL);
  std::cout << (end.tv_sec - begin.tv_sec)*1000 + (end.tv_usec - begin.tv_usec)/1000 << "ms " << std::endl;

  return 0;
}