// Header
#include <polyvec/geometry/path.hpp>

// polyvec
#include <polyvec/api.hpp>
#include <polyvec/core/constants.hpp>
#include <polyvec/misc.hpp>
#include <polyvec/geometry/line.hpp>
#include <polyvec/debug.hpp>
#include <polyvec/utils/matrix.hpp>
#include <polyvec/utils/directions.hpp>
#include <polyvec/geometry/winding_number.hpp>
#include <polyvec/core/log.hpp>
#include <polyvec/geometry/raster.hpp>
#include <polyvec/geometry/angle.hpp>
#include <polyvec/geometry/line.hpp>
#include <polyvec/geometry/winding_number.hpp>
#include <polyvec/polygon-tracer/error-metrics.hpp>

// c++ stl
#include <algorithm>
#include <limits>

//
#include <Eigen/Geometry>

using namespace Eigen;
using namespace polyvec;
using namespace std;

NAMESPACE_BEGIN(polyfit)
NAMESPACE_BEGIN(PathUtils)

void order_clockwise(mat2x& P) {
	bool ccw;
	WindingNumber::compute_orientation(P, ccw);
	PF_LOGF("orientation %s", ccw ? "counter-clockwise" : "clockwise");

	if (ccw) {
		Reverse(P);
	}
}

bool DegenerateClassifierAxisAligned1Px::should_skip_edge (const vec2& p0, const vec2& p1) {
    const vec2 dabs = (p1 - p0).cwiseAbs();
    return dabs.maxCoeff() < 1. + PF_EPS && dabs.minCoeff() < PF_EPS;
    //return dabs.minCoeff() < PF_EPS;
}

bool DegenerateClassifier2Px::should_skip_edge(const vec2& p0, const vec2& p1) {
	const vec2 dabs = (p1 - p0).cwiseAbs();
	return dabs.maxCoeff() < 2. + PF_EPS;
}

template <typename TDegenerateClassifier>
int find_next_non_degenerate_corner(
    const Eigen::Matrix2Xd& P,
    const int vertex,          // Current vertex
    const int vertex_next,     // Next expected vertex
    const bool circular
) {
    if (!circular && vertex_next == P.cols() - 1) {
        return vertex;
    }

    const int direction = CircularDist(P, vertex, vertex_next) < CircularDist(P, vertex_next, vertex) ? +1 : -1;

    const vec2 p0 = P.col(vertex);
    const vec2 p1 = P.col(vertex_next);
    const vec2 p2 = CircularAt(P, vertex_next + direction);

    // If this actually happens, the conditionals below should be made into a for loop statement maybe?
    const bool is_next_degenerate = TDegenerateClassifier::should_skip_edge(p0, p1);

    const bool is_next_flat = AngleUtils::spanned_shortest(p0, p1, p2) > PV_FLAT_ANGLE_MIN;

    if (is_next_degenerate || is_next_flat) {
        return Circular(P, vertex_next + direction);
    } else {
        return vertex_next;
    }
}

//explicit instantiations
template int find_next_non_degenerate_corner<DegenerateClassifierAxisAligned1Px>(const Eigen::Matrix2Xd& P, const int vertex, const int vertex_next, const bool circular);
template int find_next_non_degenerate_corner<DegenerateClassifier2Px>(const Eigen::Matrix2Xd& P, const int vertex, const int vertex_next, const bool circular);

void compute_hull_with_overlaps(const Eigen::Matrix2Xd& points, Eigen::Matrix2Xd& boundary, const int direction) {
	std::vector<int> convexities;
	PathUtils::compute_convexities(points, convexities);

	boundary.resize(Eigen::NoChange, 0);

	for (Index i = 0; i < (Index)convexities.size(); ++i) {
		if (convexities[i] == 0) {
			continue;
		}

		const Vector2d p = points.col(i);
		const Vector2d p_next = CircularAt(points, i + 1);
		const Vector2d p_prev = CircularAt(points, i - 1);

		const Vector2d cp = (p - p_next) * .5 + (p - p_prev) * .5;
		MatrixUtils::append(boundary, p + cp * convexities[i] * direction);
	}
}

double bounding_box_diagonal(const Eigen::Matrix2Xd& points) {
    Vector2d min(INFINITY, INFINITY), max(-INFINITY, -INFINITY);
    for (Index i = 0; i < points.cols(); ++i) {
        min = min.cwiseMin(points.col(i));
        max = max.cwiseMax(points.col(i));
    }
    return (max - min).norm();
}

void compute_convexities(const Eigen::Matrix2Xd& points, std::vector<int>& convexities) {
	convexities.resize(points.cols(), 0);

	bool is_ccw;
	WindingNumber::compute_orientation(points, is_ccw);
	const int orientation = is_ccw ? -1 : +1;

	for (Index i = 0; i < points.cols(); ++i) {
		int j = (i + 1) % (int)points.cols();
		int k = (i - 1 + (int)points.cols()) % (int)points.cols();

		Vector2d tangent_i = (points.col(j) - points.col(i)).normalized();
		Vector2d tangent_k = (points.col(i) - points.col(k)).normalized();
		Vector2d normal_k = orientation * Vector2d(-tangent_k[1], tangent_k[0]);

		double dconvexity = -tangent_i.dot(normal_k);
		int iconvexity;

		if (abs(dconvexity) < 1e-5) {
			iconvexity = 0;
		}
		else if (dconvexity > 0) {
			iconvexity = 1;
		}
		else {
			iconvexity = -1;
		}

		convexities[i] = iconvexity;
	}
}

// Starting from a corner, it counts the number of flat vertices before reaching another corner
int count_flat_vertices_before_transition(const std::vector<int>& convexities, const Eigen::Index from, const Eigen::Index direction) {
	assert_break(convexities[from] != 0);
	assert_break(direction == +1 || direction == -1);

	int distance = 0;
	while (++distance < convexities.size()) {
		const int next_index = (from + (distance * direction) + (int)convexities.size() * 2) % (int)convexities.size();

		if (convexities[next_index] != 0) {
			break;
		}
	}

	return distance;
}

int count_transitions_between_vertices(const std::vector<int>& convexities, const Eigen::Index src, const Eigen::Index dst) {
	int vertex = Circular(convexities, src + 1);
	int transitions = 0;

	while (vertex != dst) {
		transitions += convexities[vertex] != 0 ? 1 : 0;
		vertex = Circular(convexities, vertex + 1);
	}

	return transitions;
}

bool is_edge_inflected(const Eigen::Vector2d& v0, const Eigen::Vector2d& v1, const Eigen::Vector2d& v2, const Eigen::Vector2d& v3) {
	const Vector2d vfrom2d = v1 - v0;
	const Vector2d vto2d = v3 - v2;
	const Vector2d sep2d = v2 - v1;

	const Vector3d vfrom(vfrom2d.x(), vfrom2d.y(), 0.);
	const Vector3d vto(vto2d.x(), vto2d.y(), 0.);
	const Vector3d sep(sep2d.x(), sep2d.y(), 0.);

	const Vector3d cross_from = sep.cross(vfrom);
	const Vector3d cross_to = sep.cross(vto);

	int sfrom = misc::sign(cross_from.z());
	int sto = misc::sign(cross_to.z());
	return sfrom * sto == 0 ? false : sfrom == sto;
}

Vertex end_of_convex_region(const std::vector<int>& C, const Vertex v0, const int d, bool circular) {
	PF_ASSERT(v0 >= 0 && v0 < (Vertex)C.size());

	if (!circular && v0 == C.size() - 1) {
		return v0;
	}

	int c_last = C[v0];
	Vertex v = Circular(C, v0 + d);

	while (true) {
		if (!circular && v == C.size() - 1) {
			return v;
		}

		if (C[v] != 0) {
			if (c_last == C[v]) {
				break;
			}
			c_last = C[v];
		}

		v = Circular(C, v + d);
	}

	return v;
}

bool contains_closed(const int size, Vertex v0, Vertex v1, Vertex v, bool circular) {
	Vertex d01 = circular ? CircularDist(size, v0, v1) : (v1 - v0);
	Vertex d1 = circular ? CircularDist(size, v, v1) : (v1 - v);
	return d1 <= d01;
}

bool contains_closed(const mat2x& P, Vertex v0, Vertex v1, Vertex v, bool circular) {
	return contains_closed(P.cols(), v0, v1, v, circular);
}

bool contains_open(const mat2x& P, Vertex v0, Vertex v1, Vertex v, bool circular) {
	Vertex d01 = circular ? CircularDist(P, v0, v1) : (v1 - v0);
	Vertex d1 = circular ? CircularDist(P, v, v1) : (v1 - v);
	return d1 > 0 && d1 < d01;
}

bool has_equal_distance(const mat2x& P, Vertex v0, Vertex v, Vertex v1, bool circular) {
	if (!circular && (v < v0 || v > v1)) {
		return false;
	}

	return CircularDist(P, v0, v) == CircularDist(P, v, v1);
}

Eigen::Index next_transition_vertex(const std::vector<int>& convexities, const Eigen::Index from, const int direction, const bool circular) {
	Index vertex = Circular(convexities, from + direction);

	while (convexities[vertex] == 0) {
		vertex = Circular(convexities, vertex + direction);
	}

	return vertex;
}

Vertex distance_to_next_corner (const std::vector<int>& convexities, const Eigen::Index from, const int direction) {
    return direction > 0 ?
        CircularDist(convexities, from, next_transition_vertex(convexities, from, direction)) :
        CircularDist(convexities, next_transition_vertex(convexities, from, direction), from);
}

bool are_axis_aligned(const Eigen::Vector2d& p0, const Eigen::Vector2d& p1) {
	return abs((p0.x() - p1.x())) < constants::Eps ||
		abs((p0.y() - p1.y())) < constants::Eps;
}

double shortest_angle_spanned(const vec2& p0, const vec2& p1, const vec2& p2) {
	return acos((p0 - p1).normalized().dot((p2 - p1).normalized()));
}

// todo: is a cross product really needed?
bool are_consecutive_angles_opposite(const vec2& p0, const vec2& p1, const vec2& p2, const vec2& p3) {
	vec3 v01 = vec3::Zero(), v23 = vec3::Zero(), v12 = vec3::Zero();
	v01.segment(0, 2) = p1 - p0;
	v23.segment(0, 2) = p3 - p2;
	v12.segment(0, 2) = p2 - p1;

	const int sign_xprev = misc::sign(v12.cross(v01).z());
	const int sign_xnext = misc::sign(v12.cross(v23).z());
	return sign_xprev * sign_xnext == 0 ? false : sign_xprev == sign_xnext;
}

/* OLD VERSION USING WYKOBY primitives
vec2 distance_bounds_from_points(const mat2x& points, const mat2& p, vec2i vp) {
	Vertex voxel_src = vp(0);
	Vertex voxel_dst = vp(1);
	vec2 from = p.col(0);
	vec2 to = p.col(1);

	// This happens when the raster region has 1-pixel wide diagonals
	if (voxel_src != voxel_dst && GeomRaster::are_overlapping(from, to)) {
		return vec2(FLT_MAX, FLT_MAX);
	}

	const double WEAK_THR = .5;

	// This is perfectly valid. The reason is that generated nodes store the respective voxel
	// with the `-` sign in front.
	voxel_src = abs(voxel_src);
	voxel_dst = abs(voxel_dst);

	//
	// Finding how voxel midpoints to check for accuracy
	geom::segment edge_segment(from, to);
	const double edge_segment_len = edge_segment.length() + constants::Eps;
	::polyvec::index n_voxels = points.cols();
	::polyvec::index num_nodes = -1;

	if (voxel_dst < voxel_src) {
		num_nodes = voxel_dst + n_voxels - voxel_src;
	} else {
		num_nodes = voxel_dst - voxel_src;
	}

	//
	// Keeping track of the maximum violation
	double max_in_accuracy_violation = -1.;
	double max_out_accuracy_violation = -1.;

	for (int i = 0; i < num_nodes; ++i) {
		::polyvec::index v = (voxel_src + i) % n_voxels;
		::polyvec::index next_v = (v + 1) % n_voxels;

		// only considering pixel points, not raster points,
		// todo: this shouldn't really be here
		if ((points.col(v) - points.col(next_v)).norm() < 1. - PF_EPS) {
			next_v = (v + 2) % n_voxels;
			PF_ASSERT((points.col(v) - points.col(next_v)).norm() >= 1. - PF_EPS);
			++i;
		}

		Vector2d cur = points.col(v);
		Vector2d next = points.col(next_v);
		Vector2d midpoint = 0.5 * (cur + next);
		Vector2d normal = next - cur;

		normal = polyvec::util::normal_dir(normal).normalized();

		// WEAK ACCURACY (<0.5) - FIRST TEST
		// We first check along the midpoint normal
		double d = std::numeric_limits<double>::quiet_NaN();

		const vec2 n_abs = normal.cwiseAbs();

		if (geom::ray_intersect(midpoint, normal, edge_segment, d)) {
			//PF_ASSERT(abs(d - d2) < 1e-4 ||abs(d + d2) < 1e-4);

			if (std::abs(d) >= WEAK_THR) {
				d = std::numeric_limits<double>::quiet_NaN();
			}
		}

		if (std::isnan(d))
		{
			// STRONG ACCURACY (>=0.5 to 0.5+slack) - SECOND TEST
			// Accuracy is checked as distance from the offset midpoint and its projection on the segment
			real2 off_midpoint_out = midpoint + 0.5 * normal;
			real2 off_midpoint_in = midpoint - 0.5 * normal;

			double dist_out = -1;
			double dist_in = -1;

			real2 out_proj = geom::project(off_midpoint_out, edge_segment);
			real2 in_proj = geom::project(off_midpoint_in, edge_segment);

			// Is the point on the line ?
			if (!geom::point_on_line(edge_segment, out_proj) && !geom::point_on_line(edge_segment, in_proj)) {
				//printf("edge    (%f %f) (%f %f)\n", edge_segment.src(0), edge_segment.src(1), edge_segment.dst(0), edge_segment.dst(1));
				//printf("out proj %f %f\n", out_proj(0), out_proj(1));
				//printf("in  proj %f %f\n", in_proj(0), in_proj(1));
				return vec2(FLT_MAX, FLT_MAX);
			}

			real2 d_out = (out_proj - off_midpoint_out).cwiseAbs();
			real2 d_in = (in_proj - off_midpoint_in).cwiseAbs();

			dist_out = ::std::max(d_out.x(), d_out.y());
			dist_in = ::std::max(d_in.x(), d_in.y());

			assert_break(dist_out >= 0.);
			assert_break(dist_in >= 0.);

			if (dist_out < dist_in) {
				d = abs(.5 + dist_out);
			}
			else {
				d = -abs(.5 + dist_in);
			}
		}

		if (d > 0) {
			max_out_accuracy_violation = max(max_out_accuracy_violation, abs(d));
		}
		else {
			max_in_accuracy_violation = max(max_in_accuracy_violation, abs(d));
		}
	}

	double accuracy_in = 0.;
	double accuracy_out = 0.;

	if (max_out_accuracy_violation > 0) {
		accuracy_out = max_out_accuracy_violation;
	}

	if (max_in_accuracy_violation > 0) {
		accuracy_in = max_in_accuracy_violation;
	}

	return vec2(std::min(accuracy_in, accuracy_out), std::max(accuracy_in, accuracy_out));
}

bool edge_is_within_relaxed_distance_bounds(const mat2x& P, const int vsrc, const int vdst) {
	mat2 e;
	e.col(0) = P.col(vsrc);
	e.col(1) = P.col(vdst);
	const vec2 d_error = PathUtils::distance_bounds_from_points(P, e, {vsrc, vdst});
	return ErrorMetrics::accuracy_within_bounds_relaxed(d_error.minCoeff(), d_error.maxCoeff());

}
*/

// returns true if exp is the first vertex encountered traversing the polygon P from i in direction d
bool points_in_same_partition(const mat2x& P, Vertex i, Vertex exp, Vertex fail, int d) {
	Vertex v = i + d;
	while (v != fail && v != i) {
		if (v == exp) {
			return true;
		}

		v = Circular(P, v + d);
	}

	return false;
}

Vertex opposite(const Eigen::Index& size, const Eigen::Index& v, const Eigen::Index& vsym) {
	if (CircularDist(size, vsym, v) < CircularDist(size, v, vsym )) {
		return Circular(size, vsym - 1);
	}
	else {
		return Circular(size, vsym + 1);
	}
}

void order(const mat2x& P, std::vector<Vertex>& V, Vertex vstart, bool circular) {
	// todo: test
	PF_ASSERT(circular);

	std::vector<Vertex> Vord;

	Vertex prev = vstart;
	while (Vord.size() < V.size()) {
		Vertex dmin = numeric_limits<Vertex>::max();
		Vord.emplace_back(-1);

		// finding the next closest vertex in the path
		for (int i = 0; i < V.size(); ++i) {
			const Vertex d = CircularDist(P, vstart, V[i]);

			const bool exists = find(Vord.begin(), Vord.end(), V[i]) != Vord.end();

			if (!exists && d < dmin) {
				dmin = d;
				Vord.back() = V[i];
			}
		}

		PF_ASSERT(Vord.back() != -1);
	}

	V = std::move(Vord);
}

double angle_at_corner(
	const mat2x& B,
	const vecXi& P,
	const int v,
	const bool circular
) {
	if (!circular && (v == 0 || v == P.size() - 1)) {
		return 0.;
	}

	return AngleUtils::spanned_shortest(
		B.col(CircularAt(P, v - 1)),
		B.col(CircularAt(P, v)),
		B.col(CircularAt(P, v + 1))
	);
}

int count_degenerate_edges_exclusive(
	const mat2x& P, // raster boundary
	const mat2xi& E, // exceptions
	const bool circular
) {
	Vertex off = circular ? 0 : 1; (void)off;
	int count = 0;

	for (int i = 0; i < P.cols(); ++i) {
		if (!circular && (i == 0 || i >= P.cols() - 1)) {
			continue;
		}

		// testing if any of the two edges is contained in the intervals to exclude
		bool exclude = false;
		for (int j = 0; j < E.cols(); ++j) {
			if (PathUtils::contains_open(P, E(0, j), E(1, j), i, circular)) {
				exclude = true;
				break;
			}
		}

		if (exclude) {
			continue;
		}

		// testing inflection + length 1
		const vec2 p0p = CircularAt(P, i - 1);
		const vec2 p0 = P.col(i);
		const vec2 p1 = CircularAt(P, i + 1);
		const vec2 p1n = CircularAt(P, i + 2);

		const bool is_short_aa = PathUtils::are_axis_aligned(p0, p1) && (p1 - p0).norm() < 1. + 1e-4;
		const bool is_inflection = AngleUtils::have_opposite_convexity(p0p, p0, p1, p1n);
		if (is_short_aa && is_inflection) {
			++count;
		}
	}

	return count;
}

Eigen::Index find_closest_corner_before_strict(
	const mat2x& B,      // list of points
	const vecXi& P,      // polygon vertices
	const Eigen::Index q // corner limit (index to B)
) {
	Index min_d = numeric_limits<Index>::max();
	Index min_v = -1;

	for (Index i = 0; i < P.size(); ++i) {
		const Index d = CircularDist(B, P(i), q);

		if (d > 0 && d < min_d) {
			min_d = d;
			min_v = i;
		}
	}

	return min_v;
}

// Given a list of points and a list of vertices with the same orientation, it finds the first
// vertex of P which lies after the queried point, excluding the point itself.
Eigen::Index find_closest_corner_after_strict(
	const mat2x& B,      // list of points
	const vecXi& P,      // polygon vertices
	const Eigen::Index q // corner limit (index to B)
) {
	Index min_d = numeric_limits<Index>::max();
	Index min_v = -1;

	for (Index i = 0; i < P.size(); ++i) {
		const Index d = CircularDist(B, q, P(i));
		if (d > 0 && d < min_d) {
			min_d = d;
			min_v = i;
		}
	}

	return min_v;
}

NAMESPACE_END(PathUtils)
NAMESPACE_END(polyvec)


#include <chrono>
#include <random>

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <vicon/Subject.h>
#include <vicon_odom/filter.h>
#include <Eigen/Geometry>
#include <tf2_ros/transform_broadcaster.h>

static ros::Publisher odom_pub;
static ros::Publisher pose_cov_pub;
static ros::Publisher pose_pub;
static KalmanFilter kf;
static nav_msgs::Odometry odom_msg;
static geometry_msgs::PoseStamped pose_msg;
static geometry_msgs::PoseWithCovarianceStamped pose_cov_msg;
static tf2_ros::TransformBroadcaster* tfb;

static std::string fixed_frame_id;
static std::string base_frame_id;

static KalmanFilter::State_t proc_noise_diag;
static KalmanFilter::Measurement_t meas_noise_diag;
static unsigned int consecutive_occlusions;

// the minimum number of consecutive time steps at which less than 3 markers are visible before the KF can be re-initialized
static unsigned int min_consec_occ;

// the minimum number of markers that must be visible for vicon attitude to be published
static unsigned int min_visible_markers;

// Variance of the noise added to the position pose message.
static float pos_injected_noise_stddev = 0.0;
// Variance of the extra noise added to the position pose message in between
// the given bounds.
static float pos_injected_extra_noise_stddev = 0.0;
static float high_noise_x_min = -FLT_MAX;
static float high_noise_y_min = -FLT_MAX;
static float high_noise_z_min = -FLT_MAX;
static float high_noise_x_max = FLT_MAX;
static float high_noise_y_max = FLT_MAX;
static float high_noise_z_max = FLT_MAX;
static bool apply_position_noise = false;
static bool apply_extra_position_noise = false;

std::default_random_engine *generator;
std::normal_distribution<float> *normal_dist;

#define OPTIONAL_GET(param) \
  if (n.hasParam(#param)) \
  { \
    n.getParam(#param, param); \
  }

std::chrono::high_resolution_clock::time_point last_time;
float desired_dt; // in seconds.

static void vicon_callback(const vicon::Subject::ConstPtr &msg)
{
  static ros::Time t_last_proc = msg->header.stamp;

  double dt = (msg->header.stamp - t_last_proc).toSec();
  t_last_proc = msg->header.stamp;

  auto time_now = std::chrono::high_resolution_clock::now();
  auto dur = std::chrono::duration_cast<std::chrono::microseconds>(time_now - last_time).count();
  if (dur > 1.5 * 1e6 * desired_dt) {
    ROS_WARN("vicon_odom callback dt: %f ms", dur / 1000.0);
  }
  last_time = time_now;

  unsigned int num_visible_markers = 0;
  unsigned int total_num_markers = msg->markers.size();
  for (unsigned int i = 0; i < total_num_markers; i++)
  {
    if (!msg->markers[i].occluded)
      num_visible_markers++;
  }

  if (num_visible_markers < 3)
  {
    //ROS_WARN("vicon_odom: less than 3 visible markers, skipping KF update");
    consecutive_occlusions++;
    return;
  }

  if (consecutive_occlusions > min_consec_occ)
  {
    KalmanFilter::State_t newstate;
    newstate(0) = msg->position.x;
    newstate(1) = msg->position.y;
    newstate(2) = msg->position.z;
    newstate(3) = 0.0;
    newstate(4) = 0.0;
    newstate(5) = 0.0;

    kf.initialize(newstate,
                  0.01*KalmanFilter::ProcessCov_t::Identity(),
                  proc_noise_diag.asDiagonal(),
                  meas_noise_diag.asDiagonal());
    consecutive_occlusions = 0;
    //ROS_WARN("vicon_odom: re-initialize KF position to vicon marker centroid");
  }

  // Kalman filter for getting translational velocity from position measurements
  kf.processUpdate(dt);
  const KalmanFilter::Measurement_t meas(msg->position.x, msg->position.y, msg->position.z);
  if(!msg->occluded)
  {
    static ros::Time t_last_meas = msg->header.stamp;
    double meas_dt = (msg->header.stamp - t_last_meas).toSec();
    t_last_meas = msg->header.stamp;
    kf.measurementUpdate(meas, meas_dt);
  }

  const KalmanFilter::State_t state = kf.getState();
  const KalmanFilter::ProcessCov_t proc_noise = kf.getProcessNoise();

  odom_msg.header.seq = msg->header.seq;
  odom_msg.header.stamp = msg->header.stamp;
  odom_msg.header.frame_id = fixed_frame_id;
  odom_msg.child_frame_id = base_frame_id;
  odom_msg.pose.pose.position.x = state(0);
  odom_msg.pose.pose.position.y = state(1);
  odom_msg.pose.pose.position.z = state(2);
  odom_msg.twist.twist.linear.x = state(3);
  odom_msg.twist.twist.linear.y = state(4);
  odom_msg.twist.twist.linear.z = state(5);

  for(int i = 0; i < 3; i++)
  {
    for(int j = 0; j < 3; j++)
    {
      odom_msg.pose.covariance[6*i+j] = proc_noise(i,j);
      odom_msg.twist.covariance[6*i+j] = proc_noise(3+i, 3+j);
    }
  }

  pose_cov_msg.header = odom_msg.header;
  pose_cov_msg.pose.pose.position = odom_msg.pose.pose.position;
  pose_cov_msg.pose.pose.orientation = msg->orientation;

  // This should be the variance of the noise present in the raw vicon data.
  float variance = 0.01 * 0.01;

  if (apply_position_noise) {
    pose_cov_msg.pose.pose.position.x += pos_injected_noise_stddev *
                                     normal_dist->operator()(*generator);
    pose_cov_msg.pose.pose.position.y += pos_injected_noise_stddev *
                                     normal_dist->operator()(*generator);
    pose_cov_msg.pose.pose.position.z += pos_injected_noise_stddev *
                                     normal_dist->operator()(*generator);
    variance += std::pow(pos_injected_noise_stddev, 2);
  }

  if (apply_extra_position_noise)
  {
    if (high_noise_x_min < odom_msg.pose.pose.position.x &&
        odom_msg.pose.pose.position.x < high_noise_x_max &&
        high_noise_y_min < odom_msg.pose.pose.position.y &&
        odom_msg.pose.pose.position.y < high_noise_y_max &&
        high_noise_z_min < odom_msg.pose.pose.position.z &&
        odom_msg.pose.pose.position.z < high_noise_z_max)
    {
      pose_cov_msg.pose.pose.position.x += pos_injected_extra_noise_stddev *
                                       normal_dist->operator()(*generator);
      pose_cov_msg.pose.pose.position.y += pos_injected_extra_noise_stddev *
                                       normal_dist->operator()(*generator);
      pose_cov_msg.pose.pose.position.z += pos_injected_extra_noise_stddev *
                                       normal_dist->operator()(*generator);
      variance += std::pow(pos_injected_extra_noise_stddev, 2);
    }
  }

  pose_cov_msg.pose.covariance[0] = variance;
  pose_cov_msg.pose.covariance[7] = variance;
  pose_cov_msg.pose.covariance[14] = variance;
  pose_cov_msg.pose.covariance[35] = 0.02 * 0.02;

  odom_msg.pose.covariance[0] = variance;
  odom_msg.pose.covariance[7] = variance;
  odom_msg.pose.covariance[14] = variance;
  odom_msg.pose.covariance[35] = 0.02 * 0.02;

  pose_cov_pub.publish(pose_cov_msg);

  pose_msg.header = pose_cov_msg.header;
  pose_msg.pose = pose_cov_msg.pose.pose;
  pose_pub.publish(pose_msg);

  if (num_visible_markers >= min_visible_markers)
  {
    odom_msg.pose.pose.orientation.x = msg->orientation.x;
    odom_msg.pose.pose.orientation.y = msg->orientation.y;
    odom_msg.pose.pose.orientation.z = msg->orientation.z;
    odom_msg.pose.pose.orientation.w = msg->orientation.w;

    // Single step differentiatation for angular velocity
    static Eigen::Matrix3d R_prev(Eigen::Matrix3d::Identity());
    Eigen::Matrix3d R(Eigen::Quaterniond(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z));
    if(dt > 1e-6)
    {
      Eigen::Matrix3d R_dot = (R - R_prev)/dt;
      Eigen::Matrix3d w_hat = R_dot * R.transpose();

      odom_msg.twist.twist.angular.x = w_hat(2, 1);
      odom_msg.twist.twist.angular.y = w_hat(0, 2);
      odom_msg.twist.twist.angular.z = w_hat(1, 0);
    }
    R_prev = R;

    geometry_msgs::TransformStamped ts;
    ts.transform.translation.x = odom_msg.pose.pose.position.x;
    ts.transform.translation.y = odom_msg.pose.pose.position.y;
    ts.transform.translation.z = odom_msg.pose.pose.position.z;
    ts.transform.rotation.x = odom_msg.pose.pose.orientation.x;
    ts.transform.rotation.y = odom_msg.pose.pose.orientation.y;
    ts.transform.rotation.z = odom_msg.pose.pose.orientation.z;
    ts.transform.rotation.w = odom_msg.pose.pose.orientation.w;
    ts.header = odom_msg.header;
    ts.child_frame_id = odom_msg.child_frame_id;
    tfb->sendTransform(ts);
  }
  else
  {
    // use invalid quaternion to indicate that attitude is unreliable
    odom_msg.pose.pose.orientation.x = 0.0;
    odom_msg.pose.pose.orientation.y = 0.0;
    odom_msg.pose.pose.orientation.z = 0.0;
    odom_msg.pose.pose.orientation.w = 0.0;

    odom_msg.twist.twist.angular.x = 0.0;
    odom_msg.twist.twist.angular.y = 0.0;
    odom_msg.twist.twist.angular.z = 0.0;
  }

  odom_pub.publish(odom_msg);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "vicon_odom");

  ros::NodeHandle n("~");

  tfb = new tf2_ros::TransformBroadcaster();

  if (!n.hasParam("frame_id/fixed"))
  {
    ROS_ERROR("vicon_odom: failed to find param 'frame_id/fixed'");
    return EXIT_FAILURE;
  }
  n.getParam("frame_id/fixed", fixed_frame_id);

  if (!n.hasParam("frame_id/base"))
  {
    ROS_ERROR("vicon_odom: failed to find param 'frame_id/base'");
    return EXIT_FAILURE;
  }
  n.getParam("frame_id/base", base_frame_id);

  if (!n.hasParam("vicon_kf/min_consecutive_occlusions_for_restart"))
  {
    ROS_ERROR("vicon_odom: failed to find param 'vicon_kf/min_consecutive_occlusions_for_restart'");
    return EXIT_FAILURE;
  }
  int tmp;
  n.getParam("vicon_kf/min_consecutive_occlusions_for_restart", tmp);
  min_consec_occ = static_cast<unsigned int>(tmp);

  if (!n.hasParam("vicon_kf/min_visible_markers"))
  {
    ROS_ERROR("vicon_odom: failed to find param 'vicon_kf/min_visible_markers'");
    return EXIT_FAILURE;
  }
  n.getParam("vicon_kf/min_visible_markers", tmp);
  min_visible_markers = static_cast<unsigned int>(tmp);

  if (n.hasParam("pos_injected_noise_stddev"))
  {
    apply_position_noise = true;
    n.getParam("pos_injected_noise_stddev", pos_injected_noise_stddev);
  }

  if (n.hasParam("pos_injected_extra_noise_stddev"))
  {
    apply_extra_position_noise = true;
    n.getParam("pos_injected_extra_noise_stddev", pos_injected_extra_noise_stddev);
    OPTIONAL_GET(high_noise_x_min)
    OPTIONAL_GET(high_noise_y_min)
    OPTIONAL_GET(high_noise_z_min)
    OPTIONAL_GET(high_noise_x_max)
    OPTIONAL_GET(high_noise_y_max)
    OPTIONAL_GET(high_noise_z_max)
  }

  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
  std::default_random_engine gen(seed);
  std::normal_distribution<float> std_normal(0.0, 1.0);

  // There has to be a better way to do this...
  generator = &gen;
  normal_dist = &std_normal;

  if (min_visible_markers < 4)
  {
    ROS_ERROR("vicon_odom: 'vicon_kf/min_visible_markers' must be at least 4");
    return EXIT_FAILURE;
  }

  double max_accel;
  n.param("max_accel", max_accel, 5.0);

  double dt, vicon_fps;
  n.param("vicon_fps", vicon_fps, 100.0);
  ROS_ASSERT(vicon_fps > 0.0);
  dt = 1/vicon_fps;
  desired_dt = dt;

  proc_noise_diag(0) = 0.5*max_accel*dt*dt;
  proc_noise_diag(1) = 0.5*max_accel*dt*dt;
  proc_noise_diag(2) = 0.5*max_accel*dt*dt;
  proc_noise_diag(3) = max_accel*dt;
  proc_noise_diag(4) = max_accel*dt;
  proc_noise_diag(5) = max_accel*dt;
  proc_noise_diag = proc_noise_diag.array().square();
  meas_noise_diag(0) = 1e-4;
  meas_noise_diag(1) = 1e-4;
  meas_noise_diag(2) = 1e-4;
  meas_noise_diag = meas_noise_diag.array().square();
  kf.initialize(KalmanFilter::State_t::Zero(),
                0.01*KalmanFilter::ProcessCov_t::Identity(),
                proc_noise_diag.asDiagonal(),
                meas_noise_diag.asDiagonal());

  ros::Subscriber vicon_sub = n.subscribe("vicon", 10, &vicon_callback,
                                          ros::TransportHints().tcpNoDelay());

  odom_pub = n.advertise<nav_msgs::Odometry>("odom", 10);
  pose_pub = n.advertise<geometry_msgs::PoseStamped>("pose", 10);
  pose_cov_pub = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose_cov", 10);

  consecutive_occlusions = 0;

  ros::spin();

  return 0;
}


#include "MainSystem.h"
#include <fstream>
#include <boost/filesystem.hpp>
#include <vector>
#include <iostream>
#include "Geometry/Geometry.h"
#include "Global/settings.h"

namespace vslap
{

class VSLAPMainUI;

MainSystem::MainSystem()
{
    mFrames.clear();
    mImageName.clear();
    shouldExit = false;
}
MainSystem::~MainSystem()
{
}
int MainSystem::InitalCalib(std::string filename_)
{
    std::ifstream infile;
    infile.open(filename_);
    double fx, fy, cx, cy, gma;
    infile >> fx >> fy >> cx >> cy >> gma;
    K.setIdentity();

    K(0, 0) = fx;
    K(1, 1) = fy;
    K(0, 2) = cx;
    K(1, 2) = cy;
    invK = K.inverse();

    infile >> width >> height;
    infile.close();

    m_geoSolver = new GeometrySolver(K);
    return 1;
}
int MainSystem::LoadFrames(std::string folder_)
{

    std::vector<std::string> allFiles;
    allFrameId.clear();
    boost::filesystem::path pose_dir(folder_);
    boost::filesystem::directory_iterator end_itr;
    for (boost::filesystem::directory_iterator itr(pose_dir); itr != end_itr; ++itr)
    {
        if (boost::filesystem::is_regular_file(itr->status()) && itr->path().extension() == ".p2d")
        {
            std::string cur_file = itr->path().string();
            allFiles.push_back(cur_file);
        }
        if (boost::filesystem::is_regular_file(itr->status()) && itr->path().extension() == ".jpg")
        {
            std::string cur_file = itr->path().string();
            mImageName.push_back(cur_file);
        }
    }
    std::sort(allFiles.begin(), allFiles.end());
    std::sort(mImageName.begin(), mImageName.end());

    for (int i = 0; i < allFiles.size(); i++)
    {
        SkeletonFrame tmpFrame(i, width, height, K);
        tmpFrame.loadFile(allFiles[i]);
        mFrames.push_back(tmpFrame);
        allFrameId.push_back(i);
    }
    nFrames = allFiles.size();
    return allFiles.size();
}
void MainSystem::calcSimilarityMatrix()
{
    std::cout<<"Let go test"<<std::endl;
    int nFrames = mFrames.size();
    Eigen::MatrixXd simMatrix(nFrames, nFrames);
    for (int i = 0; i < nFrames; i++)
    {
        for (int j = 0; j < nFrames; j++)
        {
            std::cout<<"Frame: "<<j<<std::endl;
            HumanPose2D skel1 = mFrames[i].getSkeleton(0);
            HumanPose2D skel2 = mFrames[j].getSkeleton(0);
            simMatrix(i, j) = m_geoSolver->HumanPoseResidual(skel1, skel2,1);
        }

    }
    std::ofstream matOut("/home/xiul/databag/cTxt.txt");
    matOut<<simMatrix;
    matOut.close();
}
void MainSystem::letsTest()
{
    std::cout << "START Test Thread" << std::endl;
    mainThread_ = boost::thread(&MainSystem::testEssentialMatrixLoop, this);
}
void MainSystem::letsGo()
{
    std::cout << "START Main Thread" << std::endl;
    mainThread_ = boost::thread(&MainSystem::runningThread, this);
}
void MainSystem::setupUI()
{
    std::cout<<"Start UI"<<std::endl;
    m_ui = new VSLAPMainUI(width,height,nFrames);
    std::cout<<"UI started"<<std::endl;
}
void MainSystem::joinAll()
{
    mainThread_.join();
    m_ui ->join();
}

void MainSystem::runningThread()
{
    while (!shouldExit)
    {
        if (!allFrameId.empty())
        {
            int curFrameId = allFrameId.front();
            allFrameId.pop_front();
            std::string curImageName = mImageName[curFrameId];

            cv::Mat img = cv::imread(curImageName.c_str());
            m_ui->addImageDisplay(img);
        }
        else
        {
            //frame currently empty, waiting
            //m_uiSFM->imageLoaded = true;
        }
    }
}
void MainSystem::testEssentialMatrix(int id1, int id2)
{
    cv::Mat img1 = cv::imread(mImageName[id1].c_str());
    cv::Mat img2 = cv::imread(mImageName[id2].c_str());
    cv::imshow("img", img1);
    cv::imshow("img2", img2);
    cv::waitKey(-1);
    HumanPose2D skel1 = mFrames[id1].getSkeleton(0);
    HumanPose2D skel2 = mFrames[id2].getSkeleton(0);
    int nGoodPair = 0;
    Eigen::MatrixXd kps1;
    Eigen::MatrixXd kps2;
    for (int jId = 0; jId < BODY_JOINTS_NUM - 4; jId++)
    {

        if (skel1.joints[jId](2) * skel2.joints[jId](2) > GOOD_PAIR_TH)
        {
            kps1.conservativeResize(3, kps1.cols() + 1);
            kps1.col(kps1.cols() - 1) = skel1.joints[jId];

            kps2.conservativeResize(3, kps2.cols() + 1);
            kps2.col(kps2.cols() - 1) = skel2.joints[jId];
            nGoodPair++;
        }
    }
    Eigen::Matrix3d E = m_geoSolver->EssentialMatrix(kps1, kps2, nGoodPair);
    std::cout << E << std::endl;
    std::cout << m_geoSolver->m_R1 << std::endl;
    Eigen::Vector3d ea = m_geoSolver->m_R1.eulerAngles(2, 1, 0);
    std::cout << ea << std::endl;
    std::cout << m_geoSolver->m_t << std::endl;
}

void MainSystem::testEssentialMatrixLoop()
{
    while (!shouldExit)
    {
        int id1 = m_ui->testA;
        int id2 = m_ui->testB;
        cv::Mat img1 = cv::imread(mImageName[id1].c_str());
        cv::Mat img2 = cv::imread(mImageName[id2].c_str());
        m_ui->addImageADisplay(img1);
        m_ui->addImageBDisplay(img2);

        HumanPose2D skel1 = mFrames[id1].getSkeleton(0);
        HumanPose2D skel2 = mFrames[id2].getSkeleton(0);
        int nGoodPair = 0;
        Eigen::MatrixXd kps1;
        Eigen::MatrixXd kps2;
        HumanPose3D m_pose;
        m_geoSolver->DirectPoseLift(skel1,m_pose);
        m_ui->m_pose = m_pose;
        for (int jId = 0; jId < 5; jId++)
        {
            if (skel1.joints[TORSO_PART[jId]](2) > GOOD_PAIR_TH && skel2.joints[TORSO_PART[jId]](2) > GOOD_PAIR_TH)
            {
                kps1.conservativeResize(3, kps1.cols() + 1);
                kps1.col(kps1.cols() - 1) = skel1.joints[TORSO_PART[jId]];

                kps2.conservativeResize(3, kps2.cols() + 1);
                kps2.col(kps2.cols() - 1) = skel2.joints[TORSO_PART[jId]];
                nGoodPair++;
            }
        }
        int nearPair = 0;
        for (int jId = 0; jId < 4; jId++)
        {

            if (skel1.joints[NEAR_PART[jId]](2) > GOOD_PAIR_TH && skel2.joints[NEAR_PART[jId]](2) > GOOD_PAIR_TH)
            {
                kps1.conservativeResize(3, kps1.cols() + 1);
                kps1.col(kps1.cols() - 1) = skel1.joints[NEAR_PART[jId]];

                kps2.conservativeResize(3, kps2.cols() + 1);
                kps2.col(kps2.cols() - 1) = skel2.joints[NEAR_PART[jId]];
                nearPair++;
            }
        }
        Eigen::MatrixXd kps1All;
        Eigen::MatrixXd kps2All;
        for(int jId=0;jId<14;jId++){
            kps1All.conservativeResize(3, kps1All.cols() + 1);
            kps1All.col(kps1All.cols() - 1) = skel1.joints[jId];

            kps2All.conservativeResize(3, kps2All.cols() + 1);
            kps2All.col(kps2All.cols() - 1) = skel2.joints[jId];
        }

        if (nGoodPair == 5)
        {
            Eigen::Matrix3d E = m_geoSolver->EssentialMatrix(kps1, kps2, 5 + nearPair);
            double err = m_geoSolver->calcAlgebraicError(kps1All,kps2All,E);
            std::cout<<"Err:"<<err<<std::endl;
            m_ui->R1 = m_geoSolver->m_R1;
            m_ui->R2 = m_geoSolver->m_R2;
            m_ui->t =m_geoSolver->m_t;
        }
        else
        {
            m_ui->R1.setIdentity();
            m_ui->R2.setIdentity();
            m_ui->t.setZero();
        }
    }
}
}


#include <boost/test/unit_test.hpp>

#include "defines.hpp"

#include <scorum/protocol/asset.hpp>
#include <scorum/rewards_math/formulas.hpp>

using namespace scorum::rewards_math;
using namespace scorum::protocol;

using scorum::protocol::curve_id;

namespace database_fixture {
struct rewards_math_calculate_weight_fixture
{
    const share_type any_positive_rshares = 10000;
    const share_type recent_positive_rshares = any_positive_rshares / 2;
    const uint64_t any_max_vote_weight = 10000u;
    const fc::time_point_sec when_comment_created = fc::time_point_sec(TEST_GENESIS_TIMESTAMP);
    const fc::time_point_sec now = when_comment_created + SCORUM_BLOCK_INTERVAL * 10;
};
}

using namespace database_fixture;

BOOST_FIXTURE_TEST_SUITE(rewards_math_calculate_weight_tests, rewards_math_calculate_weight_fixture)

BOOST_AUTO_TEST_CASE(calculate_max_vote_weight_invalid_params)
{
    SCORUM_REQUIRE_THROW(
        calculate_max_vote_weight(recent_positive_rshares, any_positive_rshares, curve_id::square_root), fc::exception);
}

BOOST_AUTO_TEST_CASE(calculate_max_vote_weight_positive)
{
    BOOST_CHECK_GT(calculate_max_vote_weight(any_positive_rshares, recent_positive_rshares, curve_id::square_root), 0u);
}

BOOST_AUTO_TEST_CASE(calculate_vote_weight_invalid_params)
{
    SCORUM_REQUIRE_THROW(calculate_vote_weight(any_max_vote_weight, now, when_comment_created, fc::microseconds(0)),
                         fc::exception);

    SCORUM_REQUIRE_THROW(
        calculate_vote_weight(any_max_vote_weight, when_comment_created, now, SCORUM_REVERSE_AUCTION_WINDOW_SECONDS),
        fc::exception);
}

BOOST_AUTO_TEST_CASE(calculate_vote_weight_positive)
{
    BOOST_CHECK_GT(
        calculate_vote_weight(any_max_vote_weight, now, when_comment_created, SCORUM_REVERSE_AUCTION_WINDOW_SECONDS),
        0u);
}

BOOST_AUTO_TEST_SUITE_END()


/* -*- mode: c++; c-basic-offset: 2; indent-tabs-mode: nil; tab-width: 8 -*-
 *
 * $Id$
 */
#include <cctbx/boost_python/flex_fwd.h>
#include <boost/tokenizer.hpp>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/dict.hpp>
#include <boost/python/list.hpp>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/vec3.h>
#include <scitbx/vec2.h>
#include <scitbx/constants.h>
#include <scitbx/math/mean_and_variance.h>
#include <cctbx/miller.h>
#include <cctbx/uctbx.h>
#include <vector>
#include <map>
#include <set>
#include <boost/python/extract.hpp>

using namespace boost::python;

namespace xfel {

struct correction_vector_store {
  typedef scitbx::vec2<double> vec2;
  typedef scitbx::vec3<double> vec3;
  scitbx::af::flex_int tiles;
  scitbx::af::shared<int> tilecounts;
  scitbx::af::shared<vec3> tilecenters;

  scitbx::af::shared<double> radii;
  scitbx::af::shared<vec2> mean_cv;
  scitbx::af::shared<vec2> master_coords;
  scitbx::af::shared<vec2> master_cv;
  scitbx::af::shared<int> master_tiles;
  scitbx::af::shared<vec2> all_tile_obs_spo;
  vec2 overall_cv;
  double sum_sq_cv;

  void initialize_per_tile_sums(){
    tilecounts = scitbx::af::shared<int>(tiles.size()/4,0);
    radii = scitbx::af::shared<double>(tiles.size()/4,0.);
    mean_cv = scitbx::af::shared<vec2>(tiles.size()/4,vec2(0.,0.));
    master_tiles = scitbx::af::shared<int>();
    master_coords = scitbx::af::shared<vec2>();
    master_cv = scitbx::af::shared<vec2>();
    all_tile_obs_spo = scitbx::af::shared<vec2>();
    overall_cv = vec2();
    sum_sq_cv = 0.;
    tilecenters = scitbx::af::shared<vec3>();
    for (int x = 0; x < tiles.size()/4; ++x){
      tilecenters.push_back( vec3(
        (tiles[4*x+0] + tiles[4*x+2])/2.,
        (tiles[4*x+1] + tiles[4*x+3])/2.,
         0.) );
    }
  }

  int
  register_line(double const&a,double const&b,double const&c,double const&d,
                double const&e,double const&f,double const&g,double const&h){
    vec2 observed_center(a,b);
    vec2 refined_center(c,d);
    vec2 observed_spot(e,f);
    vec2 predicted_spot(g,h);
    vec2 prediction = predicted_spot - refined_center;

    vec2 correction_vector = predicted_spot - observed_spot;

    int itile = 0;
    for (int x = 0; x < tiles.size()/4; ++x){
      if (tiles[4*x+0]<predicted_spot[0] && predicted_spot[0]<tiles[4*x+2] &&
          tiles[4*x+1]<predicted_spot[1] && predicted_spot[1]<tiles[4*x+3]){
         itile = x;
         break;
      }
    }
    SCITBX_ASSERT(correction_vector.length() <= 10);
    tilecounts[itile]+=1;
    radii[itile]+=prediction.length();
    mean_cv[itile] = mean_cv[itile] + correction_vector;
    master_tiles.push_back(itile);
    master_cv.push_back(correction_vector);
    master_coords.push_back(prediction);
    all_tile_obs_spo.push_back(observed_spot -
                               vec2(tilecenters[itile][0],tilecenters[itile][1]));
    overall_cv += correction_vector;
    sum_sq_cv += correction_vector.length_sq();
    return itile;
  }

  double
  weighted_average_angle_deg_from_tile(int const& itile) const {

    scitbx::af::shared<vec2> selected_cv;
    scitbx::af::shared<vec2> selected_tile_obs_spo;
    scitbx::af::shared<vec2> translated_correction_vectors;
    scitbx::af::shared<vec2> all_tile_pred_spo;

    for (int x = 0; x < master_tiles.size(); ++x){
      if (master_tiles[x]==itile){
        selected_cv.push_back( master_cv[x] );
        selected_tile_obs_spo.push_back( all_tile_obs_spo[x] );
        translated_correction_vectors.push_back( master_cv[x] - mean_cv[itile] );
        all_tile_pred_spo.push_back( all_tile_obs_spo[x] + master_cv[x] );
      }
    }

    double numerator = 0.;
    double denominator = 0.;

    for (int x = 0; x < selected_cv.size(); ++x){
      vec2 co = selected_tile_obs_spo[x];
      vec2 cp = all_tile_pred_spo[x];
      double co_cp_norm = co.length()*cp.length();
      if (co_cp_norm==0) {continue;}
      double co_dot_cp = co*cp;
      double co_cross_cp_coeff = (co[0]*cp[1]-cp[0]*co[1]);
      double sin_theta = co_cross_cp_coeff / co_cp_norm;
      double cos_theta = co_dot_cp / co_cp_norm;
      double angle_deg = std::atan2(sin_theta,cos_theta)/scitbx::constants::pi_180;
      double weight = std::sqrt(co_cp_norm);
      numerator += weight*angle_deg;
      denominator += weight;
    }
    return numerator/denominator;
  }

  double
  weighted_average_angle_deg_from_tile(int const& itile, vec2 const& post_mean_cv_itile,
    scitbx::af::shared<double> correction_vector_x,
    scitbx::af::shared<double> correction_vector_y ) const {

    scitbx::af::shared<vec2> selected_cv;
    scitbx::af::shared<vec2> selected_tile_obs_spo;
    scitbx::af::shared<vec2> translated_correction_vectors;
    scitbx::af::shared<vec2> all_tile_pred_spo;

    for (int x = 0; x < master_tiles.size(); ++x){
      if (master_tiles[x]==itile){
        vec2 correction_vector (correction_vector_x[x],correction_vector_y[x]);
        selected_cv.push_back( correction_vector );
        selected_tile_obs_spo.push_back( all_tile_obs_spo[x] );
        translated_correction_vectors.push_back( correction_vector - post_mean_cv_itile );
        all_tile_pred_spo.push_back( all_tile_obs_spo[x] + correction_vector );
      }
    }

    double numerator = 0.;
    double denominator = 0.;

    for (int x = 0; x < selected_cv.size(); ++x){
      vec2 co = selected_tile_obs_spo[x];
      vec2 cp = all_tile_pred_spo[x];
      double co_cp_norm = co.length()*cp.length();
      double co_dot_cp = co*cp;
      double co_cross_cp_coeff = (co[0]*cp[1]-cp[0]*co[1]);
      double sin_theta = co_cross_cp_coeff / co_cp_norm;
      double cos_theta = co_dot_cp / co_cp_norm;
      double angle_deg = std::atan2(sin_theta,cos_theta)/scitbx::constants::pi_180;
      double weight = std::sqrt(co_cp_norm);
      numerator += weight*angle_deg;
      denominator += weight;
    }
    return numerator/denominator;
  }

};

static boost::python::tuple
get_radial_tangential_vectors(correction_vector_store const& L, int const& itile){

    scitbx::vec2<double> radial(0,0);
    scitbx::vec2<double> tangential;
    for (int x = 0; x < L.master_tiles.size(); ++x){
      if (L.master_tiles[x]==itile){
        radial += L.master_coords[x];
      }
    }
    radial = radial.normalize();
    tangential = scitbx::vec2<double>( -radial[1], radial[0] );

    // Now consider 2D Gaussian distribution of all the observations
    scitbx::af::shared<double> radi_projection;
    scitbx::af::shared<double> tang_projection;
    for (int x = 0; x < L.master_tiles.size(); ++x){
      if (L.master_tiles[x]==itile){
        scitbx::vec2<double> recentered_cv = L.master_cv[x] - L.mean_cv[itile];
        radi_projection.push_back( recentered_cv*radial );
        tang_projection.push_back( recentered_cv*tangential );
      }
    }
    scitbx::math::mean_and_variance<double> radistats(radi_projection.const_ref());
    scitbx::math::mean_and_variance<double> tangstats(tang_projection.const_ref());

    return make_tuple(radial,tangential,radistats.mean(),tangstats.mean(),
                      radistats.unweighted_sample_standard_deviation(),
                      tangstats.unweighted_sample_standard_deviation());
}

static boost::python::tuple
get_radial_tangential_vectors(correction_vector_store const& L, int const& itile,
    scitbx::vec2<double> const& post_mean_cv_itile,
    scitbx::af::shared<double> correction_vector_x,
    scitbx::af::shared<double> correction_vector_y,
    scitbx::af::shared<double> model_calc_minus_center_x,
    scitbx::af::shared<double> model_calc_minus_center_y
    ){

    scitbx::vec2<double> radial(0,0);
    scitbx::vec2<double> tangential;
    for (int x = 0; x < L.master_tiles.size(); ++x){
      if (L.master_tiles[x]==itile){
        radial += scitbx::vec2<double>(model_calc_minus_center_x[x],model_calc_minus_center_y[x]);
      }
    }
    radial = radial.normalize();
    tangential = scitbx::vec2<double>( -radial[1], radial[0] );

    // Now consider 2D Gaussian distribution of all the observations
    scitbx::af::shared<double> radi_projection;
    scitbx::af::shared<double> tang_projection;
    for (int x = 0; x < L.master_tiles.size(); ++x){
      if (L.master_tiles[x]==itile){
        scitbx::vec2<double> correction_vector (correction_vector_x[x],correction_vector_y[x]);
        scitbx::vec2<double> recentered_cv = correction_vector - post_mean_cv_itile;
        radi_projection.push_back( recentered_cv*radial );
        tang_projection.push_back( recentered_cv*tangential );
      }
    }
    scitbx::math::mean_and_variance<double> radistats(radi_projection.const_ref());
    scitbx::math::mean_and_variance<double> tangstats(tang_projection.const_ref());

    return make_tuple(radial,tangential,radistats.mean(),tangstats.mean(),
                      radistats.unweighted_sample_standard_deviation(),
                      tangstats.unweighted_sample_standard_deviation());
}

static boost::python::tuple
get_correction_vector_xy(correction_vector_store const& L, int const& itile){

    scitbx::af::shared<double> xcv;
    scitbx::af::shared<double> ycv;
    for (int x = 0; x < L.master_tiles.size(); ++x){
      if (L.master_tiles[x]==itile){
        xcv.push_back( L.master_cv[x][0] );
        ycv.push_back( L.master_cv[x][1] );
      }
    }
    return make_tuple(xcv,ycv);
}

struct column_parser {
  std::vector<scitbx::af::shared<int> > int_columns;
  std::vector<scitbx::af::shared<double> > double_columns;
  std::map<std::string,int> int_column_lookup;
  std::map<std::string,int> double_column_lookup;
  std::vector<int> int_token_addresses;
  std::vector<int> double_token_addresses;
  column_parser(){}

  void set_int(std::string const& key, int const& optional_column){
    int_columns.push_back(scitbx::af::shared<int>());
    int_token_addresses.push_back(optional_column);
    int_column_lookup[key]=int_columns.size()-1;
  }
  void set_int(std::string const& key, scitbx::af::shared<int> values){
    int_columns.push_back(values);
    int_column_lookup[key]=int_columns.size()-1;
  }
  void set_double(std::string const& key, int const& optional_column){
    double_columns.push_back(scitbx::af::shared<double>());
    double_token_addresses.push_back(optional_column);
    double_column_lookup[key]=double_columns.size()-1;
  }
  void set_double(std::string const& key, scitbx::af::shared<double> values){
    double_columns.push_back(values);
    double_column_lookup[key]=double_columns.size()-1;
  }
  scitbx::af::shared<int> get_int(std::string const& key) {
    return int_columns[int_column_lookup[key]];
  }
  scitbx::af::shared<double> get_double(std::string const& key) {
    return double_columns[double_column_lookup[key]];
  }
  void parse_from_line(std::string const& line){
    typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
    boost::char_separator<char> sep(" ");
    tokenizer tok(line,sep);
    tokenizer::iterator tok_iter = tok.begin();
    std::vector<std::string> tokens;
    for (; tok_iter!=tok.end(); ++tok_iter){
      tokens.push_back( (*tok_iter) );
    }

    //parse the integers
    for (int i = 0; i<int_columns.size(); ++i){
      int_columns[i].push_back( atoi( tokens[int_token_addresses[i]].c_str()) );
    }
    //parse the doubles
    for (int i = 0; i<double_columns.size(); ++i){
      double_columns[i].push_back( atof(tokens[double_token_addresses[i]].c_str()) );
    }
  }
};

struct scaling_results {
private:
  /*
   * For each unique reflection, the set of accepted frame ID:s on
   * which it was observed.
   */
  std::vector<std::set<int> > reflection_frame;

  /*
   * Lower limit on correlation coefficient.
   */
  double reflection_frame_min_corr;

  /*
   * Populate each element of reflection_frame with the union of
   * frames with correlation coefficient greater than
   * reflection_frame_min_corr.  This function is intended for lazy
   * evaluation.
   */
  void
  update_frame_count()
  {
    shared_double cc = frames.get_double("cc");
    shared_int frame_id = observations.get_int("frame_id");
    shared_int hkl_id = observations.get_int("hkl_id");

    reflection_frame.resize(merged_asu_hkl.size());
    for (std::size_t i = 0; i < hkl_id.size(); i++) {
      const int this_frame_id = frame_id[i];
      if (cc[this_frame_id] > reflection_frame_min_corr)
        reflection_frame[hkl_id[i]].insert(this_frame_id);
    }
  }

public:
  int frame_id_dwell;
  typedef scitbx::af::shared<int> shared_int;
  typedef scitbx::af::shared<double> shared_double;
  typedef scitbx::af::versa<bool, scitbx::af::flex_grid<> > shared_bool;
  typedef
   scitbx::af::versa<cctbx::miller::index<>, scitbx::af::flex_grid<> > shared_miller;
  typedef scitbx::vec3<double> vec3;
  typedef scitbx::af::shared<vec3> shared_vec3;
  column_parser& observations, frames;
  shared_miller& merged_asu_hkl;
  shared_bool& selected_frames;

  shared_double sum_I,sum_I_SIGI,summed_wt_I,summed_weight;
  shared_double n_rejected, n_obs, d_min_values;
  shared_int completeness, summed_N, hkl_ids;
  shared_vec3 i_isig_list;
  int Nhkl;
  bool include_negatives_;

  scaling_results (column_parser &observations, column_parser &frames,
                   shared_miller& hkls, shared_bool& data_subset, bool include_negatives):
    observations(observations),frames(frames),merged_asu_hkl(hkls),include_negatives_(include_negatives),
    selected_frames(data_subset){}
  void mark0 (double const& params_min_corr,
              cctbx::uctbx::unit_cell const& params_unit_cell) {
    shared_int hkl_id = observations.get_int("hkl_id");
    shared_int frame_id = observations.get_int("frame_id");
    shared_double intensity = observations.get_double("i");
    shared_double sigi = observations.get_double("sigi");
    shared_double cc = frames.get_double("cc");
    shared_double slope = frames.get_double("slope");
    int Nframes = frame_id.size();
    int Nhkl = merged_asu_hkl.accessor().focus()[0];
    initialize_results(Nframes, Nhkl);

    for (int iobs = 0; iobs < hkl_id.size(); ++iobs){
      int this_frame_id = frame_id[iobs];
      if (!selected_frames[this_frame_id]) {continue;}
      int this_hkl_id = hkl_id[iobs];
      double this_cc, this_slope;
      if (this_frame_id != frame_id_dwell){
        frame_id_dwell = this_frame_id;
        this_cc = cc[this_frame_id];
        this_slope = slope[this_frame_id];
      }
      if (this_cc <= params_min_corr){
        continue;
      }
      completeness[this_hkl_id] += 1;
      double this_i = intensity[iobs];
      double this_sig = sigi[iobs];
      n_obs[this_frame_id] += 1;
      if (!include_negatives_ && this_i <=0.){
        n_rejected[this_frame_id] += 1;
        continue;
      }
      summed_N[this_hkl_id] += 1;
      double Intensity = this_i / this_slope;
      double isigi = this_i/this_sig;
      sum_I[this_hkl_id] += Intensity;
      sum_I_SIGI[this_hkl_id] += isigi;
      cctbx::miller::index<> this_index( merged_asu_hkl[this_hkl_id] );
      hkl_ids.push_back(this_hkl_id);
      i_isig_list.push_back( vec3(
        Intensity, isigi, this_slope));
      double this_d_spacing = params_unit_cell.d(this_index);
      double this_frame_d_min = d_min_values[this_frame_id];
      if (this_frame_d_min==0.){
        d_min_values[this_frame_id] = this_d_spacing;
      } else if (this_d_spacing < this_frame_d_min) {
        d_min_values[this_frame_id] = this_d_spacing;
      }

      double sigma = this_sig / this_slope;
      double variance = sigma * sigma;
      summed_wt_I[this_hkl_id] += Intensity / variance;
      summed_weight[this_hkl_id] += 1. / variance;

    }
  }

  /*
   * For each resolution bin, find the union of accepted frames
   * contributing at least one observation of a reflection.
   */
  std::size_t
  count_frames(
    double params_min_corr, const shared_bool& reflection_selection)
  {
    std::set<int> s;

    if (reflection_frame.size() != merged_asu_hkl.size() ||
        reflection_frame_min_corr != params_min_corr) {
      reflection_frame_min_corr = params_min_corr;
      update_frame_count();
    }

    SCITBX_ASSERT(reflection_frame.size() == reflection_selection.size());
    for (std::size_t i = 0; i < reflection_frame.size(); i++) {
      if (reflection_selection[i])
        s.insert(reflection_frame[i].begin(), reflection_frame[i].end());
    }

    return s.size();
  }

  void mark1 (double const& params_min_corr,
              cctbx::uctbx::unit_cell const& params_unit_cell) {
    // this eliminates the filter based on correlation with isomorphous structure
    // so more reflections are included than in mark0
    shared_int hkl_id = observations.get_int("hkl_id");
    shared_int frame_id = observations.get_int("frame_id");
    shared_double intensity = observations.get_double("i");
    shared_double sigi = observations.get_double("sigi");
    int Nframes = frame_id.size();
    int Nhkl = merged_asu_hkl.accessor().focus()[0];
    initialize_results(Nframes, Nhkl);

    for (int iobs = 0; iobs < hkl_id.size(); ++iobs){
      int this_frame_id = frame_id[iobs];
      if (!selected_frames[this_frame_id]) {continue;}
      int this_hkl_id = hkl_id[iobs];
      if (this_frame_id != frame_id_dwell){
        frame_id_dwell = this_frame_id;
      }
      completeness[this_hkl_id] += 1;
      double this_i = intensity[iobs];
      double this_sig = sigi[iobs];
      n_obs[this_frame_id] += 1;
      if (!include_negatives_ && this_i <=0.){
        n_rejected[this_frame_id] += 1;
        continue;
      }
      summed_N[this_hkl_id] += 1;
      double Intensity = this_i;
      double isigi = this_i/this_sig;
      sum_I[this_hkl_id] += Intensity;
      sum_I_SIGI[this_hkl_id] += isigi;
      cctbx::miller::index<> this_index( merged_asu_hkl[this_hkl_id] );
      hkl_ids.push_back(this_hkl_id);
      i_isig_list.push_back( vec3(
        Intensity, isigi, 1.0));
      double this_d_spacing = params_unit_cell.d(this_index);
      double this_frame_d_min = d_min_values[this_frame_id];
      if (this_frame_d_min==0.){
        d_min_values[this_frame_id] = this_d_spacing;
      } else if (this_d_spacing < this_frame_d_min) {
        d_min_values[this_frame_id] = this_d_spacing;
      }

      double sigma = this_sig;
      double variance = sigma * sigma;
      summed_wt_I[this_hkl_id] += Intensity / variance;
      summed_weight[this_hkl_id] += 1. / variance;
    }
  }

  private:
  void initialize_results(const int& Nframes, const int& Nhkl){
    frame_id_dwell=-1;
    sum_I = shared_double(Nhkl, 0.);
    sum_I_SIGI = shared_double(Nhkl, 0.);
    completeness = shared_int(Nhkl, 0.);
    summed_N = shared_int(Nhkl, 0.);
    summed_wt_I = shared_double(Nhkl, 0.);
    summed_weight = shared_double(Nhkl, 0.);
    n_rejected = shared_double(Nframes, 0.);
    n_obs = shared_double(Nframes, 0.);
    d_min_values = shared_double(Nframes, 0.);
    i_isig_list = shared_vec3();
  }
};

static boost::python::tuple
get_scaling_results(scaling_results const& L){
  return make_tuple(    L.sum_I, L.sum_I_SIGI,
    L.completeness, L.summed_N,
    L.summed_wt_I, L.summed_weight,
    L.n_rejected, L.n_obs,
    L.d_min_values, L.hkl_ids, L.i_isig_list );
}

static boost::python::dict
get_isigi_dict(scaling_results const& L){
  boost::python::dict ISIGI;
  std::map<int, boost::python::list> cpp_mapping;
  for (int ditem=0; ditem<L.i_isig_list.size(); ++ditem){
    int hkl_id = L.hkl_ids[ditem];
    scaling_results::vec3 dataitem = L.i_isig_list[ditem];
    if (cpp_mapping.find(hkl_id) == cpp_mapping.end()) {
      cpp_mapping[hkl_id]=boost::python::list();
    }
    boost::python::tuple i_isigi = make_tuple( dataitem[0], dataitem[1], dataitem[2] );
    cpp_mapping[hkl_id].append( i_isigi );
  }
  for (std::map<int, boost::python::list>::const_iterator item = cpp_mapping.begin();
       item != cpp_mapping.end(); ++item) {
    cctbx::miller::index<> this_index( L.merged_asu_hkl[item->first] );
    boost::python::tuple miller_index = make_tuple( this_index[0],this_index[1],this_index[2] );
    ISIGI[miller_index] = item->second;
  }
  return ISIGI;
}

static scitbx::af::shared<double>
compute_normalized_deviations(boost::python::dict const& ISIGI, scaling_results::shared_miller hkl_list) {
  /*
   * This formulation of the normalized deviations of a set of intensities and sigmas is similar to that
   * described in Evans 2011, but includes the nn term as currently implmented by aimless
   *
   */
  using namespace boost::python;
  scitbx::af::shared<double> result;
  for (std::size_t hkl_id = 0; hkl_id < hkl_list.size(); hkl_id++) {
    cctbx::miller::index<> this_index = hkl_list[hkl_id];
    tuple miller_index = make_tuple( this_index[0],this_index[1],this_index[2] );
    if (!ISIGI.has_key(miller_index))
      continue;

    list data = extract<list>(ISIGI[miller_index]);
    std::size_t n = len(data);
    std::size_t n_accept = 0;

    scitbx::af::shared<double> intensities;
    scitbx::af::shared<double> sigmas;
    scitbx::af::shared<double> meanIprimes;
    scitbx::af::shared<bool> accepted;
    double intensity = 0;
    double sigma = 0;
    double sumI = 0;

    // compute meanIprime, which for each observation, is the mean of all other observations of this hkl
    for (std::size_t i = 0; i < n; i++) {
      // tuple of scaled intensity I (obs/slope), Isigi (obs/sigma), slope
      tuple dataitem = extract<tuple>(data[i]);
      // scaled intensity
      intensity = extract<double>(dataitem[0]);

      // corrected sigma (original sigma/slope)
      sigma = intensity / extract<double>(dataitem[1]);

      accepted.push_back(sigma > 0);
      if (sigma <= 0)
        continue;
      ++n_accept;

      intensities.push_back(intensity);
      sigmas.push_back(sigma);
      sumI += intensity;
    }
    double nn = 0;
    if (n_accept > 0) {
      nn = std::sqrt((n_accept-1.0)/n_accept);
    }

    // compute the normalized deviations
    std::size_t counter = 0;
    for (std::size_t i = 0; i < n; i++) {
      if (accepted[i]) {
        double meanIprime = (sumI-intensities[counter]) / (n_accept>1 ? (n_accept-1) : 1);
        result.push_back(nn * (intensities[counter] - meanIprime) / sigmas[counter]);
        counter++;
      }
      else {
        result.push_back(0.0);
      }
    }
  }
  return result;
}

boost::python::dict
apply_sd_error_params(boost::python::dict const& ISIGI, const double sdfac, const double sdb, const double sdadd) {
  /*
   * Apply a set of sd params (sdfac, sdb and sdd) to an ISIGI dict
   */
  using namespace boost::python;
  dict result;
  list keys = ISIGI.keys();
  for (std::size_t hkl_id = 0; hkl_id < len(keys); hkl_id++) {
    list data = extract<list>(ISIGI[keys[hkl_id]]);
    std::size_t n = len(data);

    scitbx::af::shared<double> intensities(n);
    scitbx::af::shared<double> sigmas(n);
    scitbx::af::shared<double> scales(n);
    double sumI = 0;

    for (std::size_t i = 0; i < n; i++) {
      tuple dataitem = extract<tuple>(data[i]);
      // tuple of scaled intensity I (obs/slope), Isigi (obs/sigma), slope
      intensities[i] = extract<double>(dataitem[0]);
      double isigi = extract<double>(dataitem[1]);
      SCITBX_ASSERT(isigi != 0.0);
      sigmas[i] = intensities[i] / isigi;
      scales[i] = extract<double>(dataitem[2]);
      sumI += intensities[i];
    }

    list corrected_data = list();

    // set up the entry for this hkl in the returned ISIGI dict
    for (std::size_t i = 0; i < n; i++) {
      // compute meanIprime, which for each observation, is the mean of all other observations of this hkl
      double meanIprime = (sumI-intensities[i]) / (n>1 ? (n-1) : 1);

      // apply correction parameters
      double tmp = std::pow(sigmas[i],2) + sdb * meanIprime + std::pow(sdadd*meanIprime,2);

      // avoid rare negatives
      double minimum = 0.1 * std::pow(sigmas[i],2);
      if (tmp < minimum)
        tmp = minimum;

      double sigma_corrected = sdfac * std::sqrt(tmp);
      SCITBX_ASSERT(sigma_corrected != 0.0);

      tuple i_isigi = make_tuple(intensities[i],intensities[i]*scales[i]/sigma_corrected,scales[i]);

      corrected_data.append(i_isigi);
    }

    result[keys[hkl_id]] = corrected_data;
  }
  return result;
}

double distance_between_points(scitbx::vec2<int> const& a, scitbx::vec2<int> const& b) {
  return std::sqrt((std::pow(double(b[0]-a[0]),2)+std::pow(double(b[1]-a[1]),2)));
}

void radial_average(scitbx::af::versa<double, scitbx::af::flex_grid<> > & data,
scitbx::af::versa<bool, scitbx::af::flex_grid<> > & mask,
scitbx::vec2<int> const& beam_center,
scitbx::af::shared<double> sums,
scitbx::af::shared<double> sums_sq,
scitbx::af::shared<int> counts,
double pixel_size, double distance,
scitbx::vec2<int> const& upper_left,
scitbx::vec2<int> const& lower_right) {
  std::size_t extent = sums.size();
  double extent_in_mm = extent * pixel_size;
  double extent_two_theta = std::atan(extent_in_mm/distance)*180/scitbx::constants::pi;

  for(std::size_t y = upper_left[1]; y < lower_right[1]; y++) {
    for(std::size_t x = upper_left[0]; x < lower_right[0]; x++) {
      double val = data(x,y);
      if(val > 0 && mask(x,y)) {
        scitbx::vec2<int> point((int)x,(int)y);
        double d_in_mm = distance_between_points(point,beam_center) * pixel_size;
        double twotheta = std::atan(d_in_mm/distance)*180/scitbx::constants::pi;
        std::size_t bin = (std::size_t)std::floor(twotheta*extent/extent_two_theta);
        if (bin >= extent)
          continue;
        sums[bin] += val;
        sums_sq[bin] += val*val;
        counts[bin]++;
      }
    }
  }
}

void radial_histogram(scitbx::af::versa<double, scitbx::af::flex_grid<> > & data,
scitbx::af::versa<double, scitbx::af::flex_grid<> > & histogram,
scitbx::vec2<int> const& intensity_extent,
scitbx::vec2<int> const& beam_center,
double pixel_size, double distance,
scitbx::vec2<int> const& upper_left,
scitbx::vec2<int> const& lower_right) {
  /* Create a raidal histogram plot for online analysis
   * @param data Image data (pixels)
   * @param histogram Radial histogram plot to be worked on
   * @param intensity_extent Pixel intensities in this range will be binned on the
   * y axis of the histogram.
   * @param beam_center Beam center in pixels
   * @param pixel_size Pixel size in mm
   * @param distance Detector distance in mm
   * @param upper_left Upper left corner of data to histogram (pixels)
   * @param lower_right Lower right corner of data to histogram (pixels)
   */
  std::size_t num_radial_bins = histogram.accessor().focus()[1];
  std::size_t num_intensity_bins = histogram.accessor().focus()[0];

  double extent_in_mm = num_radial_bins * pixel_size;
  double extent_two_theta = std::atan(extent_in_mm/distance)*180/scitbx::constants::pi;

  // iterate over every pixel in data within the given bounds
  for(std::size_t y = upper_left[1]; y < lower_right[1]; y++) {
    for(std::size_t x = upper_left[0]; x < lower_right[0]; x++) {
      double val = data(x,y);

      if(val < intensity_extent[0]) val = intensity_extent[0];
      else if(val > intensity_extent[1]-1) val = intensity_extent[1]-1;

      // find the intensity bin
      std::size_t valbin = (std::size_t)std::floor(
        num_intensity_bins * (val - intensity_extent[0]) / (intensity_extent[1] - intensity_extent[0]));

      if (valbin == 0) continue;

      // find the radial bin
      scitbx::vec2<int> point((int)x,(int)y);
      double d_in_mm = distance_between_points(point,beam_center) * pixel_size;
      double twotheta = std::atan(d_in_mm/distance)*180/scitbx::constants::pi;
      std::size_t bin = (std::size_t)std::floor(twotheta*num_radial_bins/extent_two_theta);

      std::size_t hg_index = valbin*num_radial_bins+bin;
      SCITBX_ASSERT(hg_index >= 0 && hg_index < histogram.size());
      histogram[hg_index]++;
    }
  }
}

/* Included here, because it depends on structures declared only in
 * this file.
 */
#include "merging.cpp"

namespace boost_python { namespace {

  void
  init_module() {
    using namespace boost::python;

    typedef return_value_policy<return_by_value> rbv;
    typedef default_call_policies dcp;

    def("get_correction_vector_xy", &get_correction_vector_xy);
    def("get_radial_tangential_vectors",
        (boost::python::tuple(*)(correction_vector_store const&, int const&))
        &get_radial_tangential_vectors);
    def("get_radial_tangential_vectors",
        (boost::python::tuple(*)(correction_vector_store const&, int const&,
         scitbx::vec2<double> const&,
         scitbx::af::shared<double>,
         scitbx::af::shared<double>,
         scitbx::af::shared<double>,
         scitbx::af::shared<double>))
        &get_radial_tangential_vectors);

    class_<scaling_results>("scaling_results",no_init)
      .def(init<column_parser&, column_parser&, scaling_results::shared_miller&,
                scaling_results::shared_bool&, bool>())
      .def("count_frames",&scaling_results::count_frames)
      .def("mark0",&scaling_results::mark0)
      .def("mark1",&scaling_results::mark1)
    ;
    def("compute_functional_and_gradients", &compute_functional_and_gradients);
    def("curvatures", &curvatures);
    def("get_scaling_results_mark2", &get_scaling_results_mark2);
    def("get_scaling_results", &get_scaling_results);
    def("get_isigi_dict", &get_isigi_dict);
    def("compute_normalized_deviations", &compute_normalized_deviations);
    def("apply_sd_error_params", &apply_sd_error_params);
    class_<correction_vector_store>("correction_vector_store",init<>())
      .add_property("tiles",
        make_getter(&correction_vector_store::tiles, rbv()),
        make_setter(&correction_vector_store::tiles, dcp()))
      .def("register_line",&correction_vector_store::register_line)
      .def("initialize_per_tile_sums",&correction_vector_store::initialize_per_tile_sums)
      .add_property("tilecounts",
        make_getter(&correction_vector_store::tilecounts, rbv()))
      .add_property("radii",
        make_getter(&correction_vector_store::radii, rbv()),
        make_setter(&correction_vector_store::radii, dcp()))
      .add_property("mean_cv",
        make_getter(&correction_vector_store::mean_cv, rbv()),
        make_setter(&correction_vector_store::mean_cv, dcp()))
      .add_property("master_tiles",
        make_getter(&correction_vector_store::master_tiles, rbv()))
      .add_property("master_cv",
        make_getter(&correction_vector_store::master_cv, rbv()))
      .add_property("overall_cv",
        make_getter(&correction_vector_store::overall_cv, rbv()),
        make_setter(&correction_vector_store::overall_cv, dcp()))
      .add_property("sum_sq_cv",
        make_getter(&correction_vector_store::sum_sq_cv, rbv()))
      .add_property("master_coords",
        make_getter(&correction_vector_store::master_coords, rbv()))
      .add_property("all_tile_obs_spo",
        make_getter(&correction_vector_store::all_tile_obs_spo, rbv()))
      .def("weighted_average_angle_deg_from_tile",
           (double(correction_vector_store::*)(int const&)const)
           &correction_vector_store::weighted_average_angle_deg_from_tile)
      .def("weighted_average_angle_deg_from_tile",
           (double(correction_vector_store::*)(int const&,correction_vector_store::vec2 const&,
           scitbx::af::shared<double>,
           scitbx::af::shared<double>)const)
           &correction_vector_store::weighted_average_angle_deg_from_tile)
    ;

    class_<column_parser>("column_parser",init<>())
      .def("set_int",(void(column_parser::*)(std::string const&, int const&))&column_parser::set_int)
      .def("set_int",(void(column_parser::*)(std::string const&, scitbx::af::shared<int>))&column_parser::set_int)
      .def("set_double",(void(column_parser::*)(std::string const&, int const&))&column_parser::set_double)
      .def("set_double",(void(column_parser::*)(std::string const&, scitbx::af::shared<double>))&column_parser::set_double)
      .def("get_int",&column_parser::get_int)
      .def("get_double",&column_parser::get_double)
      .def("parse_from_line",&column_parser::parse_from_line)
    ;

    def("radial_average", &radial_average,
      (arg("data"), arg("beam_center"), arg("sums"), arg("sums_sq"), arg("counts"),
       arg("pixel_size"), arg("distance"),
       arg("upper_left"), arg("lower_right")))
    ;

    def("radial_histogram", &radial_histogram,
      (arg("data"), arg("histogram"), arg("intensity_extent"), arg("beam_center"),
       arg("pixel_size"), arg("distance"),
       arg("upper_left"), arg("lower_right")))
    ;
}
}}} // namespace xfel::boost_python::<anonymous>

BOOST_PYTHON_MODULE(xfel_ext)
{
  xfel::boost_python::init_module();

}


/*
 * Copyright (c) 2015-2016, Luca Fulchir<luca@fulchir.it>, All rights reserved.
 *
 * This file is part of "libRaptorQ".
 *
 * libRaptorQ is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as
 * published by the Free Software Foundation, either version 3
 * of the License, or (at your option) any later version.
 *
 * libRaptorQ is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * and a copy of the GNU Lesser General Public License
 * along with libRaptorQ.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once

#include "RaptorQ/v1/common.hpp"
#include "RaptorQ/v1/multiplication.hpp"
#include <cmath>
#include <Eigen/Core>
#include <vector>

namespace RaptorQ__v1 {
namespace Impl {

class RAPTORQ_LOCAL Octet
{
public:
    Octet () {}
    Octet (const uint8_t val) noexcept : data(val) {}
    Octet (const Octet&) noexcept = default;
    Octet& operator= (const Octet&) noexcept = default;
    Octet (Octet&&) noexcept = default;
    Octet& operator= (Octet&&) noexcept = default;

    explicit operator uint8_t() const { return data; }

    Octet& operator-= (const Octet a)
    {
        data ^= a.data;
        return *this;
    }
    friend Octet operator- (Octet lhs, const Octet rhs)
    {
        lhs.data ^= rhs.data;
        return lhs;
    }
    Octet& operator+= (const Octet a)
    {
        data ^= a.data;
        return *this;
    }
    friend Octet operator+ (Octet lhs, const Octet rhs)
    {
        lhs.data ^= rhs.data;
        return lhs;
    }
    // xor, addition, subtraction... they're the same to me...
    Octet& operator^= (const Octet &a)
    {
        data ^= a.data;
        return *this;
    }
    friend Octet operator^ (Octet lhs, const Octet rhs)
    {
        lhs.data ^= rhs.data;
        return lhs;
    }
    Octet& operator*= (const Octet a)
    {
        if (data != 0 && a.data != 0) {
            data = RaptorQ__v1::Impl::oct_exp[oct_log[data - 1] +
                                            oct_log[a.data - 1]];
        } else {
            data = 0;
        }
        return *this;
    }
    friend Octet operator* (Octet lhs, const Octet rhs)
    {
        if (lhs.data != 0 && rhs.data != 0) {
            lhs.data = RaptorQ__v1::Impl::oct_exp[oct_log[lhs.data - 1] +
                                                        oct_log[rhs.data - 1]];
        } else {
            lhs.data = 0;
        }
        return lhs;
    }
    Octet& operator/= (const Octet a)
    {
        if (a.data != 0 && data != 0) {
            data = RaptorQ__v1::Impl::oct_exp[oct_log[data - 1] -
                                                    oct_log[a.data - 1] + 255];
        }
        return *this;
    }

    friend Octet operator/ (Octet lhs, const Octet rhs)
    {
        lhs /= rhs;
        return lhs;
    }

    Octet inverse() const
    {
        return Octet (RaptorQ__v1::Impl::oct_exp[255 - oct_log[data - 1]]);
    }

    bool operator== (const Octet a) const
    { return data == a.data; }
    bool operator!= (const Octet a) const
    { return data != a.data; }

    friend std::ostream &operator<< (std::ostream &os, const Octet m) {
        // used to print
        os << static_cast<uint32_t> (m.data);
        return os;
    }
private:
    uint8_t data;
};

inline uint8_t abs (Octet x) { return static_cast<uint8_t> (x); }

}   // namespace Impl
}   // namespace RaptorQ

namespace Eigen {
template<>
struct NumTraits<RaptorQ__v1::Impl::Octet> : NumTraits<uint8_t> {};
}


// All content Copyright (C) 2018 Genomics plc
#ifndef MATRIX_HPP
#define MATRIX_HPP

#include <numeric>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

namespace wecall
{
namespace utils
{

    using matrix_t = boost::numeric::ublas::matrix< double >;
    using matrixRow_t = boost::numeric::ublas::matrix_row< const utils::matrix_t >;
    using matrixColumn_t = boost::numeric::ublas::matrix_column< const utils::matrix_t >;

    template < typename MatrixRow >
    double sumMatrixRowOverAllIndices( const MatrixRow & matrixRow )
    {
        return std::accumulate( matrixRow.begin(), matrixRow.end(), 0.0 );
    }

    template < typename MatrixRow, typename IndexSet >
    double sumMatrixRowOverIndexSubset( const MatrixRow & matrixRow, const IndexSet & indices )
    {
        auto theSum = 0.0;

        for ( const auto index : indices )
        {
            theSum += matrixRow( index );
        }
        return theSum;
    }

    void smoothLowOutliers( utils::matrix_t & matrix_t, const double maxDifference );

}  // namespace utils
}  // namespace wecall

#endif


#ifndef DETECTNET_TRANSFORMATION_HPP
#define DETECTNET_TRANSFORMATION_HPP

#include <boost/array.hpp>

#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>

#include <vector>

#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/layer.hpp"
#include "caffe/proto/caffe.pb.h"

namespace caffe {

template<typename Dtype>
class CoverageGenerator;

template<typename Dtype>
struct BboxLabel_;

struct AugmentSelection;

/**
 * @brief Applies common transformations to the input data, such as
 * scaling, mirroring, substracting the image mean...
 */
template <typename Dtype>
class DetectNetTransformationLayer : public Layer<Dtype> {
 public:
  typedef cv::Size2i Size2i;
  typedef cv::Size_<Dtype> Size2v;
  typedef cv::Point2i Point2i;
  typedef cv::Point_<Dtype> Point2v;
  typedef cv::Rect Rect;
  typedef cv::Rect_<Dtype> Rectv;
  typedef cv::Vec<Dtype, 3> Vec3v;
  typedef cv::Mat_<cv::Vec<Dtype, 1> > Mat1v;
  typedef cv::Mat_<Vec3v> Mat3v;
  typedef BboxLabel_<Dtype> BboxLabel;

  explicit DetectNetTransformationLayer(const LayerParameter& param);

  virtual ~DetectNetTransformationLayer() {}

  virtual void LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);

  virtual inline const char* type() const { return "DetectNetTransformation"; }
  virtual inline int ExactNumBottomBlobs() const { return 2; }
  virtual inline int ExactNumTopBlobs() const { return 2; }

 protected:
  virtual void Forward_cpu(
      const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);

  virtual void Backward_cpu(
      const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down,
      const vector<Blob<Dtype>*>& bottom) {}

  void transform(
      const Mat3v& inputImage,
      const vector<BboxLabel>& inputBboxes,
      Mat3v* outputImage,
      Dtype* outputLabel);

  /**
   * @return a Dtype from [0..1].
   */
  Dtype randDouble();

  bool augmentation_flip(
      const Mat3v& img,
      Mat3v* img_aug,
      const vector<BboxLabel>& bboxlist,
      vector<BboxLabel>*);
  float augmentation_rotate(
      const Mat3v& img_src,
      Mat3v* img_aug,
      const vector<BboxLabel>& bboxlist,
      vector<BboxLabel>*);
  float augmentation_scale(
      const Mat3v& img,
      Mat3v* img_temp,
      const vector<BboxLabel>& bboxlist,
      vector<BboxLabel>*);
  void transform_scale(
      const Mat3v& img,
      Mat3v* img_temp,
      const vector<BboxLabel>& bboxList,
      vector<BboxLabel>* bboxList_aug,
      const Size2i& size);
  Point2i augmentation_crop(
      const Mat3v& img_temp,
      Mat3v* img_aug,
      const vector<BboxLabel>& bboxlist,
      vector<BboxLabel>*);

  void transform_crop(
      const Mat3v& img_temp,
      Mat3v* img_aug,
      const vector<BboxLabel>& bboxlist,
      vector<BboxLabel>* bboxlist_aug,
      Rect inner,
      Size2i outer_area,
      Point2i outer_offset) const;

  float augmentation_hueRotation(
      const Mat3v& img,
      Mat3v* result);

  float augmentation_desaturation(
      const Mat3v& img,
      Mat3v* result);

  Mat1v getTransformationMatrix(Rect region, Dtype rotation) const;
  Rect getBoundingRect(Rect region, Dtype rotation) const;
  void matToBlob(const Mat3v& source, Dtype* destination) const;
  void matsToBlob(const vector<Mat3v>& source, Blob<Dtype>* destination) const;
  vector<Mat3v> blobToMats(const Blob<Dtype>& image) const;
  vector<vector<BboxLabel> > blobToLabels(const Blob<Dtype>& labels) const;
  Mat3v dataToMat(
      const Dtype* _data,
      Size2i dimensions) const;
  void retrieveMeanImage(Size2i dimensions = Size2i());
  void retrieveMeanChannels();

  void meanSubtract(Mat3v* source) const;
  void pixelMeanSubtraction(Mat3v* source) const;
  void channelMeanSubtraction(Mat3v* source) const;

  DetectNetAugmentationParameter a_param_;
  DetectNetGroundTruthParameter g_param_;
  TransformationParameter t_param_;

  shared_ptr<CoverageGenerator<Dtype> > coverage_;

  Phase phase_;

  Mat3v data_mean_;
  boost::array<Dtype, 3> mean_values_;
  shared_ptr<Caffe::RNG> rng_;
};

}  // namespace caffe

#endif /* DETECTNET_TRANSFORMATION_HPP */


/*
 * Copyright [2015] [Ke Sun <sunke.polyu@gmail.com>]
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <eigen_conversions/eigen_msg.h>
#include <mocap_base/MoCapDriverBase.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

namespace mocap {

Subject::Subject(ros::NodeHandle* nptr, const string& sub_name,
    const std::string& p_frame):
  name         (sub_name),
  status       (LOST),
  nh_ptr       (nptr),
  parent_frame (p_frame){

  pub_filter = nh_ptr->advertise<nav_msgs::Odometry>(name+"/odom", 1);
  pub_raw = nh_ptr->advertise<geometry_msgs::PoseStamped>(name+"/pose", 1);
  pub_vel = nh_ptr->advertise<geometry_msgs::TwistStamped>(name+"/velocity", 1);
  return;
}

// Get and set name of the subject
const string& Subject::getName() {
  boost::shared_lock<boost::shared_mutex> read_lock(mtx);
  return name;
}
void Subject::setName(const string& sub_name) {
  boost::unique_lock<boost::shared_mutex> write_lock(mtx);
  name = sub_name;
}

// Enable or diable the subject
const Subject::Status& Subject::getStatus() {
  boost::shared_lock<boost::shared_mutex> read_lock(mtx);
  return status;
}
void Subject::enable() {
  boost::unique_lock<boost::shared_mutex> write_lock(mtx);
  status = INITIALIZING;
  ROS_INFO("Initializing subject %s", name.c_str());
}
void Subject::disable() {
  boost::unique_lock<boost::shared_mutex> write_lock(mtx);
  kFilter.reset();
  status = LOST;
  ROS_WARN("Lost track of subject %s", name.c_str());
}

// Get the state of the subject
const Quaterniond& Subject::getAttitude() {
  boost::shared_lock<boost::shared_mutex> read_lock(mtx);
  return kFilter.attitude;
}
const Vector3d& Subject::getPosition() {
  boost::shared_lock<boost::shared_mutex> read_lock(mtx);
  return kFilter.position;
}
const Vector3d& Subject::getAngularVel() {
  boost::shared_lock<boost::shared_mutex> read_lock(mtx);
  return kFilter.angular_vel;
}
const Vector3d& Subject::getLinearVel() {
  boost::shared_lock<boost::shared_mutex> read_lock(mtx);
  return kFilter.linear_vel;
}

// Set the noise parameter for the kalman filter
bool Subject::setParameters(
    const Matrix<double, 12, 12>& u_cov,
    const Matrix<double, 6, 6>& m_cov,
    const int& freq) {
  boost::unique_lock<boost::shared_mutex> write_lock(mtx);
  return kFilter.init(u_cov, m_cov, freq);
}

// Process the new measurement
void Subject::processNewMeasurement(
    const double& time,
    const Quaterniond& m_attitude,
    const Vector3d& m_position) {
  //ros::Time tbefore_filter = ros::Time::now();

  boost::unique_lock<boost::shared_mutex> write_lock(mtx);

  // Publish raw data from mocap system
  geometry_msgs::PoseStamped pose_raw;
  pose_raw.header.stamp = ros::Time(time);
  pose_raw.header.frame_id = parent_frame;

  tf::quaternionEigenToMsg(m_attitude, pose_raw.pose.orientation);
  tf::pointEigenToMsg(m_position, pose_raw.pose.position);

  pub_raw.publish(pose_raw);
  if (!kFilter.isReady()) {
    status = INITIALIZING;
    kFilter.prepareInitialCondition(time, m_attitude, m_position);
    return;
  }

  // Transform matrix from parent to child frame
  Isometry3d tf_parent2child = Isometry3d(Translation3d(m_position) * m_attitude);

  status = TRACKED;
  // Perfrom the kalman filter

  kFilter.prediction(time);
  kFilter.update(m_attitude, m_position);
  // Publish the new state
  nav_msgs::Odometry odom_filter;
  odom_filter.header.stamp = ros::Time(time);
  odom_filter.header.frame_id = parent_frame;
  odom_filter.child_frame_id = name + "/base_link";
  tf::quaternionEigenToMsg(kFilter.attitude, odom_filter.pose.pose.orientation);
  tf::pointEigenToMsg(kFilter.position, odom_filter.pose.pose.position);
  // Transform twist to child frame
  tf::vectorEigenToMsg(tf_parent2child.linear() * kFilter.angular_vel, odom_filter.twist.twist.angular);
  tf::vectorEigenToMsg(tf_parent2child.linear() * kFilter.linear_vel, odom_filter.twist.twist.linear);
  // To be compatible with the covariance in ROS, we have to do some shifting
  Map<Matrix<double, 6, 6, RowMajor> > pose_cov(odom_filter.pose.covariance.begin());
  Map<Matrix<double, 6, 6, RowMajor> > vel_cov(odom_filter.twist.covariance.begin());
  pose_cov.topLeftCorner<3, 3>() = kFilter.state_cov.block<3, 3>(3, 3);
  pose_cov.topRightCorner<3, 3>() = kFilter.state_cov.block<3, 3>(3, 0);
  pose_cov.bottomLeftCorner<3, 3>() = kFilter.state_cov.block<3, 3>(0, 3);
  pose_cov.bottomRightCorner<3, 3>() = kFilter.state_cov.block<3, 3>(0, 0);
  vel_cov.topLeftCorner<3, 3>() = kFilter.state_cov.block<3, 3>(9, 9);
  vel_cov.topRightCorner<3, 3>() = kFilter.state_cov.block<3, 3>(9, 6);
  vel_cov.bottomLeftCorner<3, 3>() = kFilter.state_cov.block<3, 3>(6, 9);
  vel_cov.bottomRightCorner<3, 3>() = kFilter.state_cov.block<3, 3>(6, 6);
  // Transform the velocity cov to child frame
  Matrix<double, 6, 6, RowMajor> r_vel = Matrix<double, 6, 6, RowMajor>::Zero();	//Zero initialized velocity cov matrix
  r_vel.block<3, 3>(0, 0) = r_vel.block<3, 3>(3, 3) = tf_parent2child.linear();
  vel_cov = r_vel * vel_cov * r_vel.transpose();

  pub_filter.publish(odom_filter);
  //ROS_INFO("filter time: %f", (ros::Time::now() - tbefore_filter).toSec());

  // Publish velocity in parent frame
  geometry_msgs::TwistStamped vel;
  vel.header.stamp = ros::Time(time);
  vel.header.frame_id = parent_frame;

  tf::vectorEigenToMsg(kFilter.angular_vel, vel.twist.angular);
  tf::vectorEigenToMsg(kFilter.linear_vel, vel.twist.linear);

  pub_vel.publish(vel);

  return;
}
}


#include "hoCuNDArray_utils.h"
#include "radial_utilities.h"
#include "hoNDArray_fileio.h"
#include "cuNDArray.h"
#include "imageOperator.h"
#include "identityOperator.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuConebeamProjectionOperator.h"
#include "cuConvolutionOperator.h"
#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_elemwise.h"
#include "hoCuNDArray_blas.h"
#include "cgSolver.h"
#include "CBCT_acquisition.h"
#include "complext.h"
#include "encodingOperatorContainer.h"
#include "vector_td_io.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuGPBBSolver.h"
#include "hoCuTvOperator.h"
#include "hoCuTvPicsOperator.h"
#include "hoCuNCGSolver.h"
#include "hoCuCgDescentSolver.h"
#include "hoNDArray_utils.h"
#include "hoCuPartialDerivativeOperator.h"

#include <iostream>
#include <algorithm>
#include <sstream>
#include <math_constants.h>
#include <boost/program_options.hpp>

using namespace std;
using namespace Gadgetron;

namespace po = boost::program_options;

boost::shared_ptr<hoCuNDArray<float> > calculate_prior(boost::shared_ptr<CBCT_binning>  binning,boost::shared_ptr<CBCT_acquisition> ps, hoCuNDArray<float>& projections, std::vector<size_t> is_dims, floatd3 imageDimensions){
  std::cout << "Calculating FDK prior" << std::endl;
	boost::shared_ptr<CBCT_binning> binning_pics =binning->get_3d_binning();
	    std::vector<size_t> is_dims3d = is_dims;
	    is_dims3d.pop_back();
	    boost::shared_ptr< hoCuConebeamProjectionOperator >
	      Ep( new hoCuConebeamProjectionOperator() );
	    Ep->setup(ps,binning_pics,imageDimensions);
	    Ep->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());
	    Ep->set_domain_dimensions(&is_dims3d);
	      Ep->set_use_filtered_backprojection(true);
	    boost::shared_ptr<hoCuNDArray<float> > prior3d(new hoCuNDArray<float>(&is_dims3d));
	    Ep->mult_MH(&projections,prior3d.get());

	    hoCuNDArray<float> tmp_proj(*ps->get_projections());
	    Ep->mult_M(prior3d.get(),&tmp_proj);
	    float s = dot(ps->get_projections().get(),&tmp_proj)/dot(&tmp_proj,&tmp_proj);
	    *prior3d *= s;
	    boost::shared_ptr<hoCuNDArray<float> > prior(new hoCuNDArray<float>(*expand( prior3d.get(), is_dims.back() )));
	    std::cout << "Prior complete" << std::endl;
	    return prior;
}

int main(int argc, char** argv)
{
  string acquisition_filename;
  string outputFile;
  uintd3 imageSize;
  floatd3 voxelSize;
  int device;
  unsigned int downsamples;
  unsigned int iterations;
  float rho;
  po::options_description desc("Allowed options");

  desc.add_options()
    ("help", "produce help message")
    ("acquisition,a", po::value<string>(&acquisition_filename)->default_value("acquisition.hdf5"), "Acquisition data")
    ("samples,n",po::value<unsigned int>(),"Number of samples per ray")
    ("output,f", po::value<string>(&outputFile)->default_value("reconstruction.real"), "Output filename")
    ("size,s",po::value<uintd3>(&imageSize)->default_value(uintd3(512,512,1)),"Image size in pixels")
    ("binning,b",po::value<string>(),"Binning file for 4d reconstruction")
    ("SAG","Use exact SAG correction if present")
    ("voxelSize,v",po::value<floatd3>(&voxelSize)->default_value(floatd3(0.488f,0.488f,1.0f)),"Voxel size in mm")
    ("dimensions,d",po::value<floatd3>(),"Image dimensions in mm. Overwrites voxelSize.")
    ("iterations,i",po::value<unsigned int>(&iterations)->default_value(10),"Number of iterations")
    ("TV,T",po::value<float>(),"TV Weight ")
    ("PICS",po::value<float>(),"TV Weight of the prior image (Prior image compressed sensing)")
    ("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
    ("downsample,D",po::value<unsigned int>(&downsamples)->default_value(0),"Downsample projections this factor")
    ("rho",po::value<float>(&rho)->default_value(0.5f),"Rho-value for line search. Must be between 0 and 1. Smaller value means faster runtime, but less stable algorithm.")
    ("use_prior","Use an FDK prior")
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << desc << "\n";
    return 1;
  }

  std::cout << "Command line options:" << std::endl;
  for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
    boost::any a = it->second.value();
    std::cout << it->first << ": ";
    if (a.type() == typeid(std::string)) std::cout << it->second.as<std::string>();
    else if (a.type() == typeid(int)) std::cout << it->second.as<int>();
    else if (a.type() == typeid(unsigned int)) std::cout << it->second.as<unsigned int>();
    else if (a.type() == typeid(float)) std::cout << it->second.as<float>();
    else if (a.type() == typeid(vector_td<float,3>)) std::cout << it->second.as<vector_td<float,3> >();
    else if (a.type() == typeid(vector_td<int,3>)) std::cout << it->second.as<vector_td<int,3> >();
    else if (a.type() == typeid(vector_td<unsigned int,3>)) std::cout << it->second.as<vector_td<unsigned int,3> >();
    else std::cout << "Unknown type" << std::endl;
    std::cout << std::endl;
  }

  cudaSetDevice(device);
  cudaDeviceReset();

  //Really weird stuff. Needed to initialize the device?? Should find real bug.
  cudaDeviceManager::Instance()->lockHandle();
  cudaDeviceManager::Instance()->unlockHandle();

  boost::shared_ptr<CBCT_acquisition> ps(new CBCT_acquisition());
  ps->load(acquisition_filename);
  ps->get_geometry()->print(std::cout);
	ps->downsample(downsamples);

  boost::shared_ptr<CBCT_binning> binning(new CBCT_binning());
  if (vm.count("binning")){
    std::cout << "Loading binning data" << std::endl;
    binning->load(vm["binning"].as<string>());
  } else binning->set_as_default_3d_bin(ps->get_projections()->get_size(2));
  binning->print(std::cout);

  floatd3 imageDimensions;
  if (vm.count("dimensions")){
    imageDimensions = vm["dimensions"].as<floatd3>();
    voxelSize = imageDimensions/imageSize;
  }
  else imageDimensions = voxelSize*imageSize;

  float lengthOfRay_in_mm = norm(imageDimensions);
  unsigned int numSamplesPerPixel = 3;
  float minSpacing = min(voxelSize)/numSamplesPerPixel;

  unsigned int numSamplesPerRay;
  if (vm.count("samples")) numSamplesPerRay = vm["samples"].as<unsigned int>();
  else numSamplesPerRay = ceil( lengthOfRay_in_mm / minSpacing );

  float step_size_in_mm = lengthOfRay_in_mm / numSamplesPerRay;
  size_t numProjs = ps->get_projections()->get_size(2);
  size_t needed_bytes = 2 * prod(imageSize) * sizeof(float);
  std::vector<size_t> is_dims = to_std_vector((uint64d3)imageSize);

  std::cout << "IS dimensions " << is_dims[0] << " " << is_dims[1] << " " << is_dims[2] << std::endl;
  std::cout << "Image size " << imageDimensions << std::endl;

  is_dims.push_back(binning->get_number_of_bins());

  // Define encoding matrix
  boost::shared_ptr< hoCuConebeamProjectionOperator >
    E( new hoCuConebeamProjectionOperator() );

  E->setup(ps,binning,imageDimensions);
  E->set_domain_dimensions(&is_dims);
  E->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());

  hoCuGPBBSolver<float> solver;
  //hoCuNCGSolver<float> solver;
  //hoCuCgDescentSolver<float> solver;

  solver.set_encoding_operator(E);
  solver.set_domain_dimensions(&is_dims);
  solver.set_max_iterations(iterations);
  solver.set_output_mode(hoCuGPBBSolver<float>::OUTPUT_VERBOSE);
  solver.set_non_negativity_constraint(true);
  //solver.set_rho(rho);

  hoCuNDArray<float> projections = *ps->get_projections();
  std::cout << "Projection nrm" << nrm2(&projections) << std::endl;
  if (E->get_use_offset_correction())
    	E->offset_correct(&projections);

  std::cout << "Projection nrm" << nrm2(&projections) << std::endl;
  boost::shared_ptr<hoCuNDArray<float> > prior;

  if (vm.count("use_prior")) {
  	prior = calculate_prior(binning,ps,projections,is_dims,imageDimensions);
  	solver.set_x0(prior);
  }

  if (vm.count("TV")){
    std::cout << "Total variation regularization in use" << std::endl;
    boost::shared_ptr<hoCuTvOperator<float,4> > tv(new hoCuTvOperator<float,4>);
    tv->set_weight(vm["TV"].as<float>());
    solver.add_nonlinear_operator(tv);
    /*
    boost::shared_ptr<hoCuTvOperator<float,4> > tv2(new hoCuTvOperator<float,4>);
    tv2->set_step(2);
    tv2->set_weight(vm["TV"].as<float>());
    solver.add_nonlinear_operator(tv2);
    boost::shared_ptr<hoCuTvOperator<float,4> > tv3(new hoCuTvOperator<float,4>);
    tv3->set_step(3);
    tv3->set_weight(vm["TV"].as<float>());
    solver.add_nonlinear_operator(tv3);
*/

  }

  if (vm.count("PICS")){
    std::cout << "PICS in use" << std::endl;
    if (!prior.get()) prior = calculate_prior(binning,ps,projections,is_dims,imageDimensions);
    boost::shared_ptr<hoCuTvPicsOperator<float,3> > pics (new hoCuTvPicsOperator<float,3>);
    pics->set_prior(prior);
    pics->set_weight(vm["PICS"].as<float>());
    solver.add_nonlinear_operator(pics);
    solver.set_x0(prior);
  }

  boost::shared_ptr< hoCuNDArray<float> > result = solver.solve(&projections);

  write_nd_array<float>( result.get(), outputFile.c_str());
}