#include "drake/systems/primitives/affine_system.h"

#include <set>
#include <utility>

#include <Eigen/Eigenvalues>

#include "drake/common/default_scalars.h"
#include "drake/common/drake_assert.h"
#include "drake/common/drake_throw.h"
#include "drake/common/eigen_types.h"
#include "drake/common/symbolic.h"
#include "drake/common/symbolic_decompose.h"
#include "drake/systems/framework/basic_vector.h"

namespace drake {
namespace systems {

using std::make_unique;
using std::unique_ptr;

template <typename T>
TimeVaryingAffineSystem<T>::TimeVaryingAffineSystem(
    SystemScalarConverter converter, int num_states, int num_inputs,
    int num_outputs, double time_period)
    : LeafSystem<T>(std::move(converter)),
      num_states_(num_states),
      num_inputs_(num_inputs),
      num_outputs_(num_outputs),
      time_period_(time_period),
      x0_(VectorX<T>::Zero(num_states)),
      Sqrt_Sigma_x0_(Eigen::MatrixXd::Zero(num_states, num_states)) {
  DRAKE_DEMAND(num_states_ >= 0);
  DRAKE_DEMAND(num_inputs_ >= 0);
  DRAKE_DEMAND(num_outputs_ >= 0);
  DRAKE_DEMAND(time_period_ >= 0.0);

  // Declare state and input/output ports.
  // Declares the state variables and (potentially) the discrete-time update.
  if (num_states > 0) {
    if (time_period_ == 0.0) {
      this->DeclareContinuousState(num_states_);
    } else {
      this->DeclareDiscreteState(num_states_);
      this->DeclarePeriodicDiscreteUpdate(time_period_, 0.0);
    }
  }

  if (num_inputs_ > 0)
    this->DeclareInputPort(kVectorValued, num_inputs_);
  if (num_outputs_ > 0) {
    // N.B. Subclasses that override CalcOutputY may want to fine-tune the
    // output port's prerequisites; see AffineSystem's ctor for an example.
    // By default, the output port will depend on everything (time, input,
    // state, accuracy, etc.).
    this->DeclareVectorOutputPort(BasicVector<T>(num_outputs_),
                                  &TimeVaryingAffineSystem::CalcOutputY);
  }
}

template <typename T>
const InputPort<T>& TimeVaryingAffineSystem<T>::get_input_port()
    const {
  DRAKE_DEMAND(num_inputs_ > 0);
  return System<T>::get_input_port(0);
}

template <typename T>
const OutputPort<T>& TimeVaryingAffineSystem<T>::get_output_port()
    const {
  DRAKE_DEMAND(num_outputs_ > 0);
  return System<T>::get_output_port(0);
}

template <typename T>
void TimeVaryingAffineSystem<T>::configure_default_state(
    const Eigen::Ref<const VectorX<T>>& x0) {
  DRAKE_DEMAND(x0.rows() == num_states_);
  if (num_states_ == 0) return;
  x0_ = x0;
}

template <typename T>
void TimeVaryingAffineSystem<T>::configure_random_state(
    const Eigen::Ref<const Eigen::MatrixXd>& covariance) {
  DRAKE_DEMAND(covariance.rows() == num_states_);
  DRAKE_DEMAND(covariance.cols() == num_states_);
  if (num_states_ == 0) return;
  Sqrt_Sigma_x0_ = Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd>(covariance)
                        .operatorSqrt();
}

// This is the default implementation for this virtual method.
template <typename T>
void TimeVaryingAffineSystem<T>::CalcOutputY(
    const Context<T>& context, BasicVector<T>* output_vector) const {
  const T t = context.get_time();

  VectorX<T> y = y0(t);
  DRAKE_DEMAND(y.rows() == num_outputs_);

  if (num_states_ > 0) {
    const MatrixX<T> Ct = C(t);
    DRAKE_DEMAND(Ct.rows() == num_outputs_ && Ct.cols() == num_states_);
    const VectorX<T>& x = (this->time_period() == 0.)
        ? dynamic_cast<const BasicVector<T>&>(
            context.get_continuous_state_vector()).get_value()
        : context.get_discrete_state().get_vector().get_value();
    y += Ct * x;
  }

  if (num_inputs_ > 0) {
    const auto& u = get_input_port().Eval(context);
    const MatrixX<T> Dt = D(t);
    DRAKE_DEMAND(Dt.rows() == num_outputs_ && Dt.cols() == num_inputs_);
    y += Dt * u;
  }

  output_vector->SetFromVector(y);
}

template <typename T>
void TimeVaryingAffineSystem<T>::DoCalcTimeDerivatives(
    const Context<T>& context, ContinuousState<T>* derivatives) const {
  if (num_states_ == 0 || time_period_ > 0) return;

  const T t = context.get_time();

  VectorX<T> xdot = f0(t);
  DRAKE_THROW_UNLESS(xdot.rows() == num_states_);

  const auto& x =
      dynamic_cast<const BasicVector<T>&>(context.get_continuous_state_vector())
          .get_value();
  const MatrixX<T> At = A(t);
  DRAKE_THROW_UNLESS(At.rows() == num_states_ && At.cols() == num_states_);
  xdot += At * x;

  if (num_inputs_ > 0) {
    const auto& u = get_input_port().Eval(context);

    const MatrixX<T> Bt = B(t);
    DRAKE_THROW_UNLESS(Bt.rows() == num_states_ && Bt.cols() == num_inputs_);
    xdot += Bt * u;
  }
  derivatives->SetFromVector(xdot);
}

template <typename T>
void TimeVaryingAffineSystem<T>::DoCalcDiscreteVariableUpdates(
    const drake::systems::Context<T>& context,
    const std::vector<const drake::systems::DiscreteUpdateEvent<T>*>&,
    drake::systems::DiscreteValues<T>* updates) const {
  if (num_states_ == 0 || time_period_ == 0.0) return;

  const T t = context.get_time();

  // TODO(russt): consider demanding that t is a multiple of time_period_.
  // But this could be non-trivial for non-double T.

  VectorX<T> xn = f0(t);
  DRAKE_DEMAND(xn.rows() == num_states_);

  const auto& x = context.get_discrete_state(0).get_value();

  const MatrixX<T> At = A(t);
  DRAKE_DEMAND(At.rows() == num_states_ && At.cols() == num_states_);
  xn += At * x;

  if (num_inputs_ > 0) {
    const auto& u = get_input_port().Eval(context);

    const MatrixX<T> Bt = B(t);
    DRAKE_DEMAND(Bt.rows() == num_states_ && Bt.cols() == num_inputs_);
    xn += Bt * u;
  }
  updates->get_mutable_vector().SetFromVector(xn);
}

template <typename T>
void TimeVaryingAffineSystem<T>::SetDefaultState(const Context<T>& context,
                                                 State<T>* state) const {
  unused(context);
  if (num_states_ == 0) return;

  if (time_period_ == 0.0) {
    state->get_mutable_continuous_state().SetFromVector(x0_);
  } else {
    state->get_mutable_discrete_state(0).SetFromVector(x0_);
  }
}

template <typename T>
void TimeVaryingAffineSystem<T>::SetRandomState(
    const Context<T>& context, State<T>* state,
    RandomGenerator* generator) const {
  unused(context);
  if (num_states_ == 0) return;

  Eigen::VectorXd w(num_states_);
  std::normal_distribution<double> normal;
  for (int i = 0; i < num_states_; i++) {
    w[i] = normal(*generator);
  }
  const auto x0 = x0_ + Sqrt_Sigma_x0_ * w;
  if (time_period_ == 0.0) {
    state->get_mutable_continuous_state().SetFromVector(x0);
  } else {
    state->get_mutable_discrete_state(0).SetFromVector(x0);
  }
}

// Our public constructor declares that our most specific subclass is
// AffineSystem, and then delegates to our protected constructor.
template <typename T>
AffineSystem<T>::AffineSystem(const Eigen::Ref<const Eigen::MatrixXd>& A,
                              const Eigen::Ref<const Eigen::MatrixXd>& B,
                              const Eigen::Ref<const Eigen::VectorXd>& f0,
                              const Eigen::Ref<const Eigen::MatrixXd>& C,
                              const Eigen::Ref<const Eigen::MatrixXd>& D,
                              const Eigen::Ref<const Eigen::VectorXd>& y0,
                              double time_period)
    : AffineSystem<T>(
          SystemTypeTag<AffineSystem>{},
          A, B, f0, C, D, y0, time_period) {}

namespace {

// Returns whether a matrix is "meaningful" when pre-multiplying a vector.
bool IsMeaningful(const Eigen::MatrixXd& m) {
  return m.size() > 0 && (m.array() != 0).any();
}

}  // namespace

// Our protected constructor does all of the real work -- everything else
// delegates to here.
template <typename T>
AffineSystem<T>::AffineSystem(SystemScalarConverter converter,
                              const Eigen::Ref<const Eigen::MatrixXd>& A,
                              const Eigen::Ref<const Eigen::MatrixXd>& B,
                              const Eigen::Ref<const Eigen::VectorXd>& f0,
                              const Eigen::Ref<const Eigen::MatrixXd>& C,
                              const Eigen::Ref<const Eigen::MatrixXd>& D,
                              const Eigen::Ref<const Eigen::VectorXd>& y0,
                              double time_period)
    : TimeVaryingAffineSystem<T>(
          std::move(converter), f0.size(), D.cols(), D.rows(), time_period),
      A_(A),
      B_(B),
      f0_(f0),
      C_(C),
      D_(D),
      y0_(y0),
      has_meaningful_C_(IsMeaningful(C)),
      has_meaningful_D_(IsMeaningful(D)) {
  DRAKE_DEMAND(this->num_states() == A.rows());
  DRAKE_DEMAND(this->num_states() == A.cols());
  DRAKE_DEMAND(this->num_states() == B.rows());
  DRAKE_DEMAND(this->num_states() == C.cols());
  DRAKE_DEMAND(this->num_inputs() == B.cols());
  DRAKE_DEMAND(this->num_inputs() == D.cols());
  DRAKE_DEMAND(this->num_outputs() == C.rows());
  DRAKE_DEMAND(this->num_outputs() == D.rows());

  // Specify our output port's dependencies more precisely than our base class
  // is able to.  We know that output never depends on time nor parameters,
  // only on state (iff C if non-zero) and input (iff D is non-zero).
  if (this->num_outputs() > 0) {
    const OutputPort<T>& output_port = this->get_output_port();
    const auto& leaf_port = dynamic_cast<const LeafOutputPort<T>&>(output_port);
    const CacheIndex cache_index = leaf_port.cache_entry().cache_index();
    CacheEntry& cache_entry = this->get_mutable_cache_entry(cache_index);
    std::set<DependencyTicket>& prereqs = cache_entry.mutable_prerequisites();
    prereqs.clear();
    if (has_meaningful_C_) {
      prereqs.insert(this->all_state_ticket());
    }
    if (has_meaningful_D_) {
      prereqs.insert(this->all_input_ports_ticket());
    }
  }
}

// Our copy constructor delegates to the public constructor; this used only by
// SystemScalarConverter as known to our public constructor, not by subclasses.
template <typename T>
template <typename U>
AffineSystem<T>::AffineSystem(const AffineSystem<U>& other)
    : AffineSystem(other.A(), other.B(), other.f0(), other.C(), other.D(),
                   other.y0(), other.time_period()) {
  this->ConfigureDefaultAndRandomStateFrom(other);
}

template <typename T>
unique_ptr<AffineSystem<T>> AffineSystem<T>::MakeAffineSystem(
    const Eigen::Ref<const VectorX<symbolic::Expression>>& dynamics,
    const Eigen::Ref<const VectorX<symbolic::Expression>>& output,
    const Eigen::Ref<const VectorX<symbolic::Variable>>& state_vars,
    const Eigen::Ref<const VectorX<symbolic::Variable>>& input_vars,
    const double time_period) {
  // Need to extract, A, B, f₀, C, D, y₀ such that,
  //
  //     dynamics = Ax + Bu + f₀
  //     output   = Cx + Du + y₀
  //
  // where x = state_vars and u = input_vars.
  const int num_states = state_vars.size();
  DRAKE_DEMAND(num_states == dynamics.size());
  const int num_inputs = input_vars.size();
  const int num_outputs = output.size();

  Eigen::MatrixXd AB(num_states, num_states + num_inputs);
  Eigen::VectorXd f0(num_states);
  VectorX<symbolic::Variable> vars(num_states + num_inputs);
  vars << state_vars, input_vars;
  DecomposeAffineExpressions(dynamics, vars, &AB, &f0);
  const auto A = AB.leftCols(num_states);
  const auto B = AB.rightCols(num_inputs);

  Eigen::MatrixXd CD(num_outputs, num_states + num_inputs);
  Eigen::VectorXd y0(num_outputs);
  DecomposeAffineExpressions(output, vars, &CD, &y0);
  const auto C = CD.leftCols(num_states);
  const auto D = CD.rightCols(num_inputs);

  return make_unique<AffineSystem<T>>(A, B, f0, C, D, y0, time_period);
}

template <typename T>
void AffineSystem<T>::CalcOutputY(const Context<T>& context,
                                  BasicVector<T>* output_vector) const {
  auto y = output_vector->get_mutable_value();
  y = y0_;

  if (has_meaningful_C_) {
    const VectorX<T>& x = (this->time_period() == 0.)
        ? dynamic_cast<const BasicVector<T>&>(
            context.get_continuous_state_vector()).get_value()
        : context.get_discrete_state().get_vector().get_value();
    y += C_ * x;
  }

  if (has_meaningful_D_) {
    const auto& u = this->get_input_port().Eval(context);
    y += D_ * u;
  }
}

template <typename T>
void AffineSystem<T>::DoCalcTimeDerivatives(
    const Context<T>& context, ContinuousState<T>* derivatives) const {
  if (this->num_states() == 0 || this->time_period() > 0.0) return;

  const auto& x =
      dynamic_cast<const BasicVector<T>&>(context.get_continuous_state_vector())
          .get_value();

  VectorX<T> xdot = A_ * x + f0_;

  if (this->num_inputs() > 0) {
    const auto& u = this->get_input_port().Eval(context);

    xdot += B_ * u;
  }
  derivatives->SetFromVector(xdot);
}

template <typename T>
void AffineSystem<T>::DoCalcDiscreteVariableUpdates(
    const drake::systems::Context<T>& context,
    const std::vector<const drake::systems::DiscreteUpdateEvent<T>*>&,
    drake::systems::DiscreteValues<T>* updates) const {
  if (this->num_states() == 0 || this->time_period() == 0.0) return;

  const auto& x = context.get_discrete_state(0).get_value();

  VectorX<T> xnext = A_ * x + f0_;

  if (this->num_inputs() > 0) {
    const auto& u = this->get_input_port().Eval(context);

    xnext += B_ * u;
  }
  updates->get_mutable_vector().SetFromVector(xnext);
}

}  // namespace systems
}  // namespace drake

DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
    class ::drake::systems::TimeVaryingAffineSystem)

DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
    class ::drake::systems::AffineSystem)


#include "drake/solvers/fbstab/components/dense_feasibility.h"

#include <cmath>
#include <stdexcept>

#include <Eigen/Dense>

#include "drake/solvers/fbstab/components/dense_data.h"
#include "drake/solvers/fbstab/components/dense_variable.h"

namespace drake {
namespace solvers {
namespace fbstab {

DenseFeasibility::DenseFeasibility(int nz, int nv) {
  if (nz <= 0 || nv <= 0) {
    throw std::runtime_error(
        "Inputs to DenseFeasibility::DenseFeasibility must be positive.");
  }
  nz_ = nz;
  nv_ = nv;
  z1_.resize(nz_);
  v1_.resize(nv_);
}

void DenseFeasibility::ComputeFeasibility(const DenseVariable& x, double tol) {
  if (tol <= 0) {
    throw std::runtime_error(
        "In DenseFeasibility::ComputeFeasibility: tol must be positive.");
  }
  const DenseData* const data = x.data();
  const Eigen::MatrixXd& H = data->H();
  const Eigen::MatrixXd& A = data->A();
  const Eigen::VectorXd& f = data->f();
  const Eigen::VectorXd& b = data->b();

  // The conditions for dual-infeasibility are:
  // max(Az) <= 0 and f'*z < 0 and ||Hz|| <= tol * ||z||
  v1_.noalias() = A * x.z();
  const double d1 = v1_.maxCoeff();

  const double d2 = f.dot(x.z());

  z1_.noalias() = H * x.z();
  const double d3 = z1_.lpNorm<Eigen::Infinity>();
  const double w = x.z().lpNorm<Eigen::Infinity>();

  if ((d1 <= tol * w) && (d2 < 0) && (d3 <= tol * w)) {
    dual_feasible_ = false;
  } else {
    dual_feasible_ = true;
  }

  // The conditions for primal infeasibility are:
  // v'*b < 0 and ||A'*v|| \leq tol * ||v||
  const double p1 = b.dot(x.v());

  z1_.noalias() = A.transpose() * x.v();
  const double p2 = z1_.lpNorm<Eigen::Infinity>();
  const double u = x.v().lpNorm<Eigen::Infinity>();

  if ((p1 < 0) && (p2 <= tol * u)) {
    primal_feasible_ = false;
  } else {
    primal_feasible_ = true;
  }
}

}  // namespace fbstab
}  // namespace solvers
}  // namespace drake


#include <shared_mutex>
#include <dlib/dnn.h>
#include <dlib/image_loader/image_loader.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include "facerec.h"
#include "jpeg_mem_loader.h"
#include "classify.h"

using namespace dlib;

template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual = add_prev1<block<N,BN,1,tag1<SUBNET>>>;

template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
using residual_down = add_prev2<avg_pool<2,2,2,2,skip1<tag2<block<N,BN,2,tag1<SUBNET>>>>>>;

template <int N, template <typename> class BN, int stride, typename SUBNET>
using block  = BN<con<N,3,3,1,1,relu<BN<con<N,3,3,stride,stride,SUBNET>>>>>;

template <int N, typename SUBNET> using ares      = relu<residual<block,N,affine,SUBNET>>;
template <int N, typename SUBNET> using ares_down = relu<residual_down<block,N,affine,SUBNET>>;

template <typename SUBNET> using alevel0 = ares_down<256,SUBNET>;
template <typename SUBNET> using alevel1 = ares<256,ares<256,ares_down<256,SUBNET>>>;
template <typename SUBNET> using alevel2 = ares<128,ares<128,ares_down<128,SUBNET>>>;
template <typename SUBNET> using alevel3 = ares<64,ares<64,ares<64,ares_down<64,SUBNET>>>>;
template <typename SUBNET> using alevel4 = ares<32,ares<32,ares<32,SUBNET>>>;

using anet_type = loss_metric<fc_no_bias<128,avg_pool_everything<
                            alevel0<
                            alevel1<
                            alevel2<
                            alevel3<
                            alevel4<
                            max_pool<3,3,2,2,relu<affine<con<32,7,7,2,2,
                            input_rgb_image_sized<150>
                            >>>>>>>>>>>>;

static const size_t RECT_LEN = 4;
static const size_t DESCR_LEN = 128;
static const size_t RECT_SIZE = RECT_LEN * sizeof(long);
static const size_t DESCR_SIZE = DESCR_LEN * sizeof(float);

class FaceRec {
public:
	FaceRec(const char* model_dir) {
		detector_ = get_frontal_face_detector();

		std::string dir = model_dir;
		std::string shape_predictor_path = dir + "/shape_predictor_5_face_landmarks.dat";
		std::string resnet_path = dir + "/dlib_face_recognition_resnet_model_v1.dat";

		deserialize(shape_predictor_path) >> sp_;
		deserialize(resnet_path) >> net_;
	}

	// TODO(Kagami): Jittering?
	std::pair<std::vector<rectangle>, std::vector<descriptor>>
	Recognize(const matrix<rgb_pixel>& img, int max_faces) {
		std::vector<rectangle> rects;
		std::vector<descriptor> descrs;

		{
			std::lock_guard<std::mutex> lock(detector_mutex_);
			rects = detector_(img);
		}

		// Short circuit.
		if (rects.size() == 0 || (max_faces > 0 && rects.size() > (size_t)max_faces))
			return {std::move(rects), std::move(descrs)};

		std::sort(rects.begin(), rects.end());

		std::vector<matrix<rgb_pixel>> face_imgs;
		for (const auto& rect : rects) {
			auto shape = sp_(img, rect);
			matrix<rgb_pixel> face_chip;
			extract_image_chip(img, get_face_chip_details(shape, 150, 0.25), face_chip);
			face_imgs.push_back(std::move(face_chip));
		}

		{
			std::lock_guard<std::mutex> lock(net_mutex_);
			descrs = net_(face_imgs);
		}

		return {std::move(rects), std::move(descrs)};
	}

	void SetSamples(std::vector<descriptor>&& samples, std::unordered_map<int, int>&& cats) {
		std::unique_lock<std::shared_mutex> lock(samples_mutex_);
		samples_ = std::move(samples);
		cats_ = std::move(cats);
	}

	int Classify(const descriptor& test_sample, float tolerance) {
		std::shared_lock<std::shared_mutex> lock(samples_mutex_);
		return classify(samples_, cats_, test_sample, tolerance);
	}

	float Distanace(const descriptor& sample1, const descriptor& sample2) {
		return distance(sample1, sample2);
	}
private:
	std::mutex detector_mutex_;
	std::mutex net_mutex_;
	std::shared_mutex samples_mutex_;
	frontal_face_detector detector_;
	shape_predictor sp_;
	anet_type net_;
	std::vector<descriptor> samples_;
	std::unordered_map<int, int> cats_;
};

// Plain C interface for Go.

facerec* facerec_init(const char* model_dir) {
	facerec* rec = (facerec*)calloc(1, sizeof(facerec));
	try {
		FaceRec* cls = new FaceRec(model_dir);
		rec->cls = (void*)cls;
	} catch(serialization_error& e) {
		rec->err_str = strdup(e.what());
		rec->err_code = SERIALIZATION_ERROR;
	} catch (std::exception& e) {
		rec->err_str = strdup(e.what());
		rec->err_code = UNKNOWN_ERROR;
	}
	return rec;
}

faceret* facerec_recognize(facerec* rec, const uint8_t* img_data, int len, int max_faces) {
	faceret* ret = (faceret*)calloc(1, sizeof(faceret));
	FaceRec* cls = (FaceRec*)(rec->cls);
	matrix<rgb_pixel> img;
	std::vector<rectangle> rects;
	std::vector<descriptor> descrs;
	try {
		// TODO(Kagami): Support more file types?
		load_mem_jpeg(img, img_data, len);
		std::tie(rects, descrs) = cls->Recognize(img, max_faces);
	} catch(image_load_error& e) {
		ret->err_str = strdup(e.what());
		ret->err_code = IMAGE_LOAD_ERROR;
		return ret;
	} catch (std::exception& e) {
		ret->err_str = strdup(e.what());
		ret->err_code = UNKNOWN_ERROR;
		return ret;
	}
	ret->num_faces = descrs.size();
	if (ret->num_faces == 0)
		return ret;
	ret->rectangles = (long*)malloc(ret->num_faces * RECT_SIZE);
	for (int i = 0; i < ret->num_faces; i++) {
		long* dst = ret->rectangles + i * 4;
		dst[0] = rects[i].left();
		dst[1] = rects[i].top();
		dst[2] = rects[i].right();
		dst[3] = rects[i].bottom();
	}
	ret->descriptors = (float*)malloc(ret->num_faces * DESCR_SIZE);
	for (int i = 0; i < ret->num_faces; i++) {
		void* dst = (uint8_t*)(ret->descriptors) + i * DESCR_SIZE;
		void* src = (void*)&descrs[i](0,0);
		memcpy(dst, src, DESCR_SIZE);
	}
	return ret;
}

void facerec_set_samples(
	facerec* rec,
	const float* c_samples,
	const int32_t* c_cats,
	int len
) {
	FaceRec* cls = (FaceRec*)(rec->cls);
	std::vector<descriptor> samples;
	samples.reserve(len);
	for (int i = 0; i < len; i++) {
		descriptor sample = mat(c_samples + i*DESCR_LEN, DESCR_LEN, 1);
		samples.push_back(std::move(sample));
	}
	std::unordered_map<int, int> cats;
	cats.reserve(len);
	for (int i = 0; i < len; i++) {
		cats[i] = c_cats[i];
	}
	cls->SetSamples(std::move(samples), std::move(cats));
}

int facerec_classify(facerec* rec, const float* c_test_sample, float tolerance) {
	FaceRec* cls = (FaceRec*)(rec->cls);
	descriptor test_sample = mat(c_test_sample, DESCR_LEN, 1);
	return cls->Classify(test_sample, tolerance);
}

float facerec_distance(facerec* rec, const float* c_sample1, const float* c_sample2) {
	FaceRec* cls = (FaceRec*)(rec->cls);
	descriptor sample1 = mat(c_sample1, DESCR_LEN, 1);
	descriptor sample2 = mat(c_sample2, DESCR_LEN, 1);
	return cls->Distanace(sample1, sample2);
}

void facerec_free(facerec* rec) {
	if (rec) {
		if (rec->cls) {
			FaceRec* cls = (FaceRec*)(rec->cls);
			delete cls;
			rec->cls = NULL;
		}
		free(rec);
	}
}


#include <boost/math/special_functions/next.hpp>
#include <boost/random.hpp>

#include <limits>

#include "caffe/common.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"

namespace caffe {

template<>
void caffe_rng_uniform<half_fp>(const int_tp n,
                             const half_fp a, const half_fp b,
                             half_fp* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_LE(a, b);
  boost::uniform_real<float> random_distribution(static_cast<float>(a),
    caffe_nextafter<float>(static_cast<float>(b)));

  boost::variate_generator<caffe::rng_t*,
    boost::uniform_real<float>> variate_generator(
    caffe_rng(), random_distribution);
  for (int_tp i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template<>
void caffe_rng_gaussian<half_fp>(const int_tp n,
                         const half_fp a, const half_fp sigma,
                         half_fp* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  float fsigma = sigma;
  float fa = a;
  boost::normal_distribution<float> random_distribution(fa, fsigma);
  boost::variate_generator<caffe::rng_t*,
    boost::normal_distribution<float> > variate_generator(
        caffe_rng(), random_distribution);
  for (int_tp i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template<>
void caffe_rng_gaussian<uint8_t>(const int_tp n, const uint8_t a,
                                 const uint8_t sigma, uint8_t* r) {
  // FIXME
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  float fsigma = sigma;
  float fa = a;
  boost::normal_distribution<float> random_distribution(fa, fsigma);
  boost::variate_generator<caffe::rng_t*,
    boost::normal_distribution<float> > variate_generator(
        caffe_rng(), random_distribution);
  for (int_tp i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template<>
void caffe_rng_gaussian<uint16_t>(const int_tp n, const uint16_t a,
                                  const uint16_t sigma, uint16_t* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  float fsigma = sigma;
  float fa = a;
  boost::normal_distribution<float> random_distribution(fa, fsigma);
  boost::variate_generator<caffe::rng_t*,
    boost::normal_distribution<float>> variate_generator(
        caffe_rng(), random_distribution);
  for (int_tp i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template<>
void caffe_rng_gaussian<uint32_t>(const int_tp n, const uint32_t a,
                                  const uint32_t sigma, uint32_t* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  float fsigma = sigma;
  float fa = a;
  boost::normal_distribution<float> random_distribution(fa, fsigma);
  boost::variate_generator<caffe::rng_t*,
    boost::normal_distribution<float>> variate_generator(
        caffe_rng(), random_distribution);
  for (int_tp i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template<>
void caffe_rng_gaussian<uint64_t>(const int_tp n, const uint64_t a,
                                  const uint64_t sigma, uint64_t* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  float fsigma = sigma;
  float fa = a;
  boost::normal_distribution<float> random_distribution(fa, fsigma);
  boost::variate_generator<caffe::rng_t*,
    boost::normal_distribution<float>> variate_generator(
        caffe_rng(), random_distribution);
  for (int_tp i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template<>
void caffe_rng_bernoulli<half_fp, int32_t>(const int_tp n,
                                    const half_fp p, int32_t* r) {
  // FIXME
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  float f_p = p;
  boost::bernoulli_distribution<float> random_distribution(f_p);
  boost::variate_generator<caffe::rng_t*,
  boost::bernoulli_distribution<float> > variate_generator(
      caffe_rng(), random_distribution);
  for (int_tp i = 0; i < n; ++i) {
    r[i] = static_cast<int32_t>(variate_generator());
  }
}

template<>
void caffe_rng_bernoulli<half_fp, uint32_t>(const int_tp n,
                                             const half_fp p, uint32_t* r) {
  // FIXME
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  float f_p = p;
  boost::bernoulli_distribution<float> random_distribution(f_p);
  boost::variate_generator<caffe::rng_t*,
  boost::bernoulli_distribution<float>> variate_generator(
      caffe_rng(), random_distribution);
  for (int_tp i = 0; i < n; ++i) {
    r[i] = static_cast<uint32_t>(variate_generator());
  }
}

uint_tp caffe_rng_rand() {
  return (*caffe_rng())();
}

template<typename Dtype>
Dtype caffe_nextafter(const Dtype b) {
  return boost::math::nextafter<Dtype>(b, std::numeric_limits<Dtype>::max());
}

template
float caffe_nextafter(const float b);

template
double caffe_nextafter(const double b);

template<>
void caffe_rng_uniform<uint8_t>(const uint_tp n, uint8_t* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  boost::uniform_int<uint8_t> random_distribution(
      std::numeric_limits<uint8_t>::min(),
      std::numeric_limits<uint8_t>::max());
  boost::variate_generator<caffe::rng_t*, boost::uniform_int<uint8_t> >
                            variate_generator(caffe_rng(), random_distribution);
  for (uint_tp i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template<>
void caffe_rng_uniform<uint16_t>(const uint_tp n, uint16_t* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  boost::uniform_int<uint16_t> random_distribution(
      std::numeric_limits<uint16_t>::min(),
      std::numeric_limits<uint16_t>::max());
  boost::variate_generator<caffe::rng_t*, boost::uniform_int<uint16_t> >
                            variate_generator(caffe_rng(), random_distribution);
  for (uint_tp i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template<>
void caffe_rng_uniform<uint32_t>(const uint_tp n, uint32_t* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  boost::uniform_int<uint32_t> random_distribution(
      std::numeric_limits<uint32_t>::min(),
      std::numeric_limits<uint32_t>::max());
  boost::variate_generator<caffe::rng_t*, boost::uniform_int<uint32_t> >
                            variate_generator(caffe_rng(), random_distribution);
  for (uint_tp i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template<>
void caffe_rng_uniform<uint64_t>(const uint_tp n, uint64_t* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  boost::uniform_int<uint64_t> random_distribution(
      std::numeric_limits<uint64_t>::min(),
      std::numeric_limits<uint64_t>::max());
  boost::variate_generator<caffe::rng_t*, boost::uniform_int<uint64_t> >
                            variate_generator(caffe_rng(), random_distribution);
  for (uint_tp i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template<typename Dtype>
void caffe_rng_uniform(const int_tp n, const Dtype a, const Dtype b, Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_LE(a, b);
  boost::uniform_real<Dtype> random_distribution(a, caffe_nextafter<Dtype>(b));
  boost::variate_generator<caffe::rng_t*,
  boost::uniform_real<Dtype> > variate_generator(
      caffe_rng(), random_distribution);
  for (int_tp i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template void caffe_rng_uniform<float>(const int_tp n, const float a,
                                       const float b, float* r);
template void caffe_rng_uniform<double>(const int_tp n, const double a,
                                        const double b, double* r);
template void caffe_rng_uniform<uint8_t>(const int_tp n, const uint8_t a,
                                         const uint8_t b, uint8_t* r);
template void caffe_rng_uniform<uint16_t>(const int_tp n, const uint16_t a,
                                          const uint16_t b, uint16_t* r);
template void caffe_rng_uniform<uint32_t>(const int_tp n, const uint32_t a,
                                          const uint32_t b, uint32_t* r);
template void caffe_rng_uniform<uint64_t>(const int_tp n, const uint64_t a,
                                          const uint64_t b, uint64_t* r);

template<typename Dtype>
void caffe_rng_gaussian(const int_tp n, const Dtype a, const Dtype sigma,
                        Dtype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GT(sigma, 0);
  boost::normal_distribution<Dtype> random_distribution(a, sigma);
  boost::variate_generator<caffe::rng_t*,
  boost::normal_distribution<Dtype> > variate_generator(
      caffe_rng(), random_distribution);
  for (int_tp i = 0; i < n; ++i) {
    r[i] = variate_generator();
  }
}

template
void caffe_rng_gaussian<float>(const int_tp n, const float mu,
                               const float sigma, float* r);

template
void caffe_rng_gaussian<double>(const int_tp n, const double mu,
                                const double sigma, double* r);

template<typename Dtype, typename Itype>
void caffe_rng_bernoulli(const int_tp n, const Dtype p, Itype* r) {
  CHECK_GE(n, 0);
  CHECK(r);
  CHECK_GE(p, 0);
  CHECK_LE(p, 1);
  boost::bernoulli_distribution<Dtype> random_distribution(p);
  boost::variate_generator<caffe::rng_t*,
  boost::bernoulli_distribution<Dtype> > variate_generator(
      caffe_rng(), random_distribution);
  for (int_tp i = 0; i < n; ++i) {
    r[i] = static_cast<Itype>(variate_generator());
  }
}

template void caffe_rng_bernoulli<half_fp, int8_t>(const int_tp n,
                                  const half_fp p, int8_t* r);
template void caffe_rng_bernoulli<half_fp, int16_t>(const int_tp n,
                                  const half_fp p, int16_t* r);
template void caffe_rng_bernoulli<half_fp, int32_t>(const int_tp n,
                                  const half_fp p, int32_t* r);
template void caffe_rng_bernoulli<half_fp, int64_t>(const int_tp n,
                                  const half_fp p, int64_t* r);
template void caffe_rng_bernoulli<half_fp, uint8_t>(const int_tp n,
                                  const half_fp p, uint8_t* r);
template void caffe_rng_bernoulli<half_fp, uint16_t>(const int_tp n,
                                  const half_fp p, uint16_t* r);
template void caffe_rng_bernoulli<half_fp, uint32_t>(const int_tp n,
                                  const half_fp p, uint32_t* r);
template void caffe_rng_bernoulli<half_fp, uint64_t>(const int_tp n,
                                  const half_fp p, uint64_t* r);

template void caffe_rng_bernoulli<float, int8_t>(const int_tp n,
                                  const float p, int8_t* r);
template void caffe_rng_bernoulli<float, int16_t>(const int_tp n,
                                  const float p, int16_t* r);
template void caffe_rng_bernoulli<float, int32_t>(const int_tp n,
                                  const float p, int32_t* r);
template void caffe_rng_bernoulli<float, int64_t>(const int_tp n,
                                  const float p, int64_t* r);
template void caffe_rng_bernoulli<float, uint8_t>(const int_tp n,
                                  const float p, uint8_t* r);
template void caffe_rng_bernoulli<float, uint16_t>(const int_tp n,
                                  const float p, uint16_t* r);
template void caffe_rng_bernoulli<float, uint32_t>(const int_tp n,
                                  const float p, uint32_t* r);
template void caffe_rng_bernoulli<float, uint64_t>(const int_tp n,
                                  const float p, uint64_t* r);

template void caffe_rng_bernoulli<double, int8_t>(const int_tp n,
                                  const double p, int8_t* r);
template void caffe_rng_bernoulli<double, int16_t>(const int_tp n,
                                  const double p, int16_t* r);
template void caffe_rng_bernoulli<double, int32_t>(const int_tp n,
                                  const double p, int32_t* r);
template void caffe_rng_bernoulli<double, int64_t>(const int_tp n,
                                  const double p, int64_t* r);
template void caffe_rng_bernoulli<double, uint8_t>(const int_tp n,
                                  const double p, uint8_t* r);
template void caffe_rng_bernoulli<double, uint16_t>(const int_tp n,
                                  const double p, uint16_t* r);
template void caffe_rng_bernoulli<double, uint32_t>(const int_tp n,
                                  const double p, uint32_t* r);
template void caffe_rng_bernoulli<double, uint64_t>(const int_tp n,
                                  const double p, uint64_t* r);

template void caffe_rng_bernoulli<uint8_t, int8_t>(const int_tp n,
                                  const uint8_t p, int8_t* r);
template void caffe_rng_bernoulli<uint8_t, int16_t>(const int_tp n,
                                  const uint8_t p, int16_t* r);
template void caffe_rng_bernoulli<uint8_t, int32_t>(const int_tp n,
                                  const uint8_t p, int32_t* r);
template void caffe_rng_bernoulli<uint8_t, int64_t>(const int_tp n,
                                  const uint8_t p, int64_t* r);
template void caffe_rng_bernoulli<uint8_t, uint8_t>(const int_tp n,
                                  const uint8_t p, uint8_t* r);
template void caffe_rng_bernoulli<uint8_t, uint16_t>(const int_tp n,
                                  const uint8_t p, uint16_t* r);
template void caffe_rng_bernoulli<uint8_t, uint32_t>(const int_tp n,
                                  const uint8_t p, uint32_t* r);
template void caffe_rng_bernoulli<uint8_t, uint64_t>(const int_tp n,
                                  const uint8_t p, uint64_t* r);

template void caffe_rng_bernoulli<uint16_t, int8_t>(const int_tp n,
                                  const uint16_t p, int8_t* r);
template void caffe_rng_bernoulli<uint16_t, int16_t>(const int_tp n,
                                  const uint16_t p, int16_t* r);
template void caffe_rng_bernoulli<uint16_t, int32_t>(const int_tp n,
                                  const uint16_t p, int32_t* r);
template void caffe_rng_bernoulli<uint16_t, int64_t>(const int_tp n,
                                  const uint16_t p, int64_t* r);
template void caffe_rng_bernoulli<uint16_t, uint8_t>(const int_tp n,
                                  const uint16_t p, uint8_t* r);
template void caffe_rng_bernoulli<uint16_t, uint16_t>(const int_tp n,
                                  const uint16_t p, uint16_t* r);
template void caffe_rng_bernoulli<uint16_t, uint32_t>(const int_tp n,
                                  const uint16_t p, uint32_t* r);
template void caffe_rng_bernoulli<uint16_t, uint64_t>(const int_tp n,
                                  const uint16_t p, uint64_t* r);

template void caffe_rng_bernoulli<uint32_t, int8_t>(const int_tp n,
                                  const uint32_t p, int8_t* r);
template void caffe_rng_bernoulli<uint32_t, int16_t>(const int_tp n,
                                  const uint32_t p, int16_t* r);
template void caffe_rng_bernoulli<uint32_t, int32_t>(const int_tp n,
                                  const uint32_t p, int32_t* r);
template void caffe_rng_bernoulli<uint32_t, int64_t>(const int_tp n,
                                  const uint32_t p, int64_t* r);
template void caffe_rng_bernoulli<uint32_t, uint8_t>(const int_tp n,
                                  const uint32_t p, uint8_t* r);
template void caffe_rng_bernoulli<uint32_t, uint16_t>(const int_tp n,
                                  const uint32_t p, uint16_t* r);
template void caffe_rng_bernoulli<uint32_t, uint32_t>(const int_tp n,
                                  const uint32_t p, uint32_t* r);
template void caffe_rng_bernoulli<uint32_t, uint64_t>(const int_tp n,
                                  const uint32_t p, uint64_t* r);

template void caffe_rng_bernoulli<uint64_t, int8_t>(const int_tp n,
                                  const uint64_t p, int8_t* r);
template void caffe_rng_bernoulli<uint64_t, int16_t>(const int_tp n,
                                  const uint64_t p, int16_t* r);
template void caffe_rng_bernoulli<uint64_t, int32_t>(const int_tp n,
                                  const uint64_t p, int32_t* r);
template void caffe_rng_bernoulli<uint64_t, int64_t>(const int_tp n,
                                  const uint64_t p, int64_t* r);
template void caffe_rng_bernoulli<uint64_t, uint8_t>(const int_tp n,
                                  const uint64_t p, uint8_t* r);
template void caffe_rng_bernoulli<uint64_t, uint16_t>(const int_tp n,
                                  const uint64_t p, uint16_t* r);
template void caffe_rng_bernoulli<uint64_t, uint32_t>(const int_tp n,
                                  const uint64_t p, uint32_t* r);
template void caffe_rng_bernoulli<uint64_t, uint64_t>(const int_tp n,
                                  const uint64_t p, uint64_t* r);

}  // namespace caffe


#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <emscripten/fetch.h>

#include <iostream>
#include <istream>
#include <streambuf>

#include <dlib/image_processing.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include "opencv2/core/core.hpp"
#include "opencv2/calib3d/calib3d.hpp"

#include "sxbrowser.h"
#include "img_dlib.h"

using namespace dlib;

const char *facefile="shape_predictor_68_face_landmarks.dat";
static shape_predictor sp;
static int loaded = -1;

struct membuf : std::streambuf {
  membuf(const char *begin, const char *end) {
    this->setg((char *)begin, (char *)begin, (char *)end);
  }
};

static cv::Mat get_camera_matrix(float focal_length, cv::Point2d center) {
  cv::Mat camera_matrix = (cv::Mat_<double>(3,3) << focal_length, 0, center.x, 0 , focal_length, center.y, 0, 0, 1);
  return camera_matrix;
}

static std::vector<float> rotationMatrixToEulerAngles(cv::Mat &R) {
  float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) +  R.at<double>(1,0) * R.at<double>(1,0) );
  bool singular = sy < 1e-6; // If

  float x, y, z;
  if (!singular) {
    x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
    y = atan2(-R.at<double>(2,0), sy);
    z = atan2(R.at<double>(1,0), R.at<double>(0,0));
  } else {
    x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
    y = atan2(-R.at<double>(2,0), sy);
    z = 0;
  }
  std::vector<float> res;
  res.push_back(x);
  res.push_back(y);
  res.push_back(z);
  return res;
}

static std::vector<cv::Point3d> get_3d_model_points() {
  std::vector<cv::Point3d> object_pts;

  object_pts.push_back(cv::Point3d(6.825897, 6.760612, 4.402142));     //#33 left brow left corner
  object_pts.push_back(cv::Point3d(1.330353, 7.122144, 6.903745));     //#29 left brow right corner
  object_pts.push_back(cv::Point3d(-1.330353, 7.122144, 6.903745));    //#34 right brow left corner
  object_pts.push_back(cv::Point3d(-6.825897, 6.760612, 4.402142));    //#38 right brow right corner
  object_pts.push_back(cv::Point3d(5.311432, 5.485328, 3.987654));     //#13 left eye left corner
  object_pts.push_back(cv::Point3d(1.789930, 5.393625, 4.413414));     //#17 left eye right corner
  object_pts.push_back(cv::Point3d(-1.789930, 5.393625, 4.413414));    //#25 right eye left corner
  object_pts.push_back(cv::Point3d(-5.311432, 5.485328, 3.987654));    //#21 right eye right corner
  object_pts.push_back(cv::Point3d(2.005628, 1.409845, 6.165652));     //#55 nose left corner
  object_pts.push_back(cv::Point3d(-2.005628, 1.409845, 6.165652));    //#49 nose right corner
  object_pts.push_back(cv::Point3d(2.774015, -2.080775, 5.048531));    //#43 mouth left corner
  object_pts.push_back(cv::Point3d(-2.774015, -2.080775, 5.048531));   //#39 mouth right corner
  object_pts.push_back(cv::Point3d(0.000000, -3.116408, 6.097667));    //#45 mouth central bottom corner
  object_pts.push_back(cv::Point3d(0.000000, -7.415691, 4.070434));    //#6 chin corner
  return object_pts;
}

static void get_2d_image_points(full_object_detection &shape,
				std::vector<cv::Point2d>& pts) {
  pts.push_back(cv::Point2d(shape.part(17).x(), shape.part(17).y())); //#17 left brow left corner
  pts.push_back(cv::Point2d(shape.part(21).x(), shape.part(21).y())); //#21 left brow right corner
  pts.push_back(cv::Point2d(shape.part(22).x(), shape.part(22).y())); //#22 right brow left corner
  pts.push_back(cv::Point2d(shape.part(26).x(), shape.part(26).y())); //#26 right brow right corner
  pts.push_back(cv::Point2d(shape.part(36).x(), shape.part(36).y())); //#36 left eye left corner
  pts.push_back(cv::Point2d(shape.part(39).x(), shape.part(39).y())); //#39 left eye right corner
  pts.push_back(cv::Point2d(shape.part(42).x(), shape.part(42).y())); //#42 right eye left corner
  pts.push_back(cv::Point2d(shape.part(45).x(), shape.part(45).y())); //#45 right eye right corner
  pts.push_back(cv::Point2d(shape.part(31).x(), shape.part(31).y())); //#31 nose left corner
  pts.push_back(cv::Point2d(shape.part(35).x(), shape.part(35).y())); //#35 nose right corner
  pts.push_back(cv::Point2d(shape.part(48).x(), shape.part(48).y())); //#48 mouth left corner
  pts.push_back(cv::Point2d(shape.part(54).x(), shape.part(54).y())); //#54 mouth right corner
  pts.push_back(cv::Point2d(shape.part(57).x(), shape.part(57).y())); //#57 mouth central bottom corner
  pts.push_back(cv::Point2d(shape.part(8).x(), shape.part(8).y()));   //#8 chin corner
}

static void get_head_orientation(Coord width, Coord height,
				 std::vector<cv::Point2d>& image_points, std::vector<float>& euler) {
  static auto model_points = get_3d_model_points();
  double focal_length = width;
  cv::Mat camera_matrix = get_camera_matrix(focal_length, cv::Point2d(width/2.0, width/2.0));
  cv::Mat tv,rv,rm;
  cv::Mat dist_coeffs = cv::Mat::zeros(4,1,cv::DataType<double>::type);

  cv::solvePnP(model_points, image_points, camera_matrix, dist_coeffs,
	       rv, tv);
  cv::Rodrigues(rv, rm);
  euler = rotationMatrixToEulerAngles(rm);
}

static void downloadSucceeded(emscripten_fetch_t *fetch) {
  membuf buf(fetch->data, fetch->data+fetch->numBytes);
  std::istream ins(&buf);
  deserialize(sp, ins);
  loaded = 1;
  emscripten_fetch_close(fetch);
}

static void downloadFailed(emscripten_fetch_t *fetch) {
  emscripten_fetch_close(fetch);
  loaded = 0;
}

void init_pose() {
  emscripten_fetch_attr_t attr;
  emscripten_fetch_attr_init(&attr);
  strcpy(attr.requestMethod, "GET");
  attr.attributes = EMSCRIPTEN_FETCH_LOAD_TO_MEMORY;
  attr.onsuccess = downloadSucceeded;
  attr.onerror = downloadFailed;
  emscripten_fetch(&attr, facefile);
}

int pose_status() {
  return loaded;
}

static frontal_face_detector face_det = get_frontal_face_detector();

void find_faces(const Img<Rgb8>& img, std::vector<Box>& boxes) {
  std::vector<rectangle> dets = face_det(img);
  for (std::vector<rectangle>::iterator it=dets.begin(); it!=dets.end(); ++it)
    boxes.push_back(Box(it->left(), it->top(), it->right(), it->bottom()));
}

int get_pose(const Img<Rgb8>& img,
	     std::vector<Box>& dets, int scaleDets,
	     std::vector<Point2f>& points, std::vector<float>& euler, Box& bbox) {
  int closest = -1;
  long maxArea = 0;
  rectangle facert;
  for (size_t i = 0; i < dets.size(); i++) {
    rectangle rt((long)(dets[i].left * scaleDets),
		(long)(dets[i].top * scaleDets),
		 (long)(dets[i].right * scaleDets),
		 (long)(dets[i].bottom * scaleDets));
    if (rt.area() > maxArea) {
      closest = i;
      facert = rt;
      maxArea = rt.area();
    }
  }
  if (closest >= 0) {
    std::vector<cv::Point2d> image_points;
    full_object_detection shape = sp(img, facert);
    get_2d_image_points(shape, image_points);
    get_head_orientation(img.Width(), img.Height(), image_points, euler);
    for (auto pt: image_points)
      points.push_back(Point2f(pt.x, pt.y));
    bbox = Box(facert.left(), facert.top(), facert.right(), facert.bottom());
  }
  return closest;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SIMD_CORRECT_FMA_INCLUDED
#define BOOST_SIMD_FUNCTION_SIMD_CORRECT_FMA_INCLUDED

#include <boost/simd/function/scalar/correct_fma.hpp>
#include <boost/simd/arch/common/generic/function/autodispatcher.hpp>
#include <boost/simd/arch/common/simd/function/correct_fma.hpp>

#if defined(BOOST_HW_SIMD_X86)

#  if BOOST_HW_SIMD_X86 >= BOOST_HW_SIMD_X86_FMA3_VERSION
// #    include <boost/simd/arch/x86/fma3/simd/function/correct_fma.hpp>
#  endif
#  if BOOST_HW_SIMD_X86 >= BOOST_HW_SIMD_X86_FMA4_VERSION
// #    include <boost/simd/arch/x86/fma4/simd/function/correct_fma.hpp>
#  endif

#endif

#if defined(BOOST_HW_SIMD_PPC)

#  if BOOST_HW_SIMD_PPC >= BOOST_HW_SIMD_PPC_VMX_VERSION
// #    include <boost/simd/arch/power/vmx/simd/function/correct_fma.hpp>
#  endif

#endif

#endif


#include <ros/ros.h>

#include <string>
#include <fstream>

// ros includes
#include <ros/ros.h>
#include <ros/package.h>
#include <ros/console.h>
#include <std_srvs/Trigger.h>
#include <dynamic_reconfigure/server.h>
#include <intrinsic_cal/observeConfig.h>

// ros actionlib includes
#include <actionlib/server/simple_action_server.h>
#include <actionlib/client/simple_action_client.h>

// intrinsic_cal includes
#include <industrial_extrinsic_cal/cal_srv_solve.h>

// industrial_extrinsic_cal includes
#include <industrial_extrinsic_cal/basic_types.h>
#include <industrial_extrinsic_cal/camera_observer.hpp>
#include <industrial_extrinsic_cal/camera_definition.h>
#include <industrial_extrinsic_cal/observation_scene.h>
#include <industrial_extrinsic_cal/observation_data_point.h>
#include <industrial_extrinsic_cal/ros_camera_observer.h>
#include <industrial_extrinsic_cal/ceres_costs_utils.hpp>
#include <industrial_extrinsic_cal/ceres_costs_utils.h>
#include <industrial_extrinsic_cal/circle_cost_utils.hpp>
#include <industrial_extrinsic_cal/calibration_job_definition.h>
#include <industrial_extrinsic_cal/calibrationAction.h>
#include <industrial_extrinsic_cal/calibrate.h>
#include <industrial_extrinsic_cal/covariance.h>
#include <industrial_extrinsic_cal/ros_target_display.hpp>
#include <industrial_extrinsic_cal/camera_yaml_parser.h>
#include <industrial_extrinsic_cal/targets_yaml_parser.h>
#include <industrial_extrinsic_cal/camera_definition.h>
#include <robo_cylinder/HomeCmd.h>
#include <robo_cylinder/MoveMeters.h>
#include <robo_cylinder/MovePulses.h>
#include <robo_cylinder/PowerIO.h>
#include <robo_cylinder/StatusUpdate.h>
#include <robo_cylinder/VelAcc.h>

// boost includes
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>

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

// other includes
#include <sys/stat.h>

using boost::shared_ptr;
using ceres::Solver;
using ceres::CostFunction;
using industrial_extrinsic_cal::Camera;
using industrial_extrinsic_cal::Target;
using industrial_extrinsic_cal::Roi;
using industrial_extrinsic_cal::CameraObservations;
using industrial_extrinsic_cal::Point3d;
using industrial_extrinsic_cal::Pose6d;
using industrial_extrinsic_cal::P_BLOCK;
using industrial_extrinsic_cal::CameraParameters;
using industrial_extrinsic_cal::NoWaitTrigger;
using industrial_extrinsic_cal::ROSCameraObserver;
using std::string;

class robocyl_ical_8D
{
public:
  void dynReConfCallBack(intrinsic_cal::observeConfig& config, uint32_t level)
  {
    // get ready for taking images
    camera_->camera_observer_->clearTargets();
    camera_->camera_observer_->clearObservations();

    Roi roi;
    roi.x_min = 0;
    roi.y_min = 0;
    roi.x_max = image_width_;
    roi.y_max = image_height_;

    industrial_extrinsic_cal::Cost_function cost_type = industrial_extrinsic_cal::cost_functions::RailICal5;

    camera_->camera_observer_->addTarget(target_, roi, cost_type);
    // collect images/observations
    camera_->camera_observer_->triggerCamera();
    while (!camera_->camera_observer_->observationsDone());
    CameraObservations camera_observations;
    camera_->camera_observer_->getObservations(camera_observations);
    ROS_ERROR("done");
  }

  robocyl_ical_8D(const  ros::NodeHandle& nh)
    :nh_(nh), P_(NULL), problem_initialized_(false), total_observations_(0), scene_(0)
  {
    ros::NodeHandle priv_nh("~");

    // load parameters pertaining to camera and target
    if(!priv_nh.getParam( "target_rows", target_rows_)){
      ROS_ERROR("Must set param:  target_rows");
    }
    if(!priv_nh.getParam( "target_cols", target_cols_)){
      ROS_ERROR("Must set param:  target_cols");
    }
    if(!priv_nh.getParam( "target_circle_dia", circle_diameter_)){
      ROS_ERROR("Must set param:  target_circle_dia");
    }
    if(!priv_nh.getParam( "target_spacing", circle_spacing_)){
      ROS_ERROR("Must set param:  target_spacing");
    }
    if(!priv_nh.getParam( "camera_name", camera_name_)){
      ROS_ERROR("Must set param:  camera_name_");
    }
    if(!priv_nh.getParam( "image_topic", image_topic_)){
      ROS_ERROR("Must set param:  image_topic_");
    }
    if(!priv_nh.getParam( "image_height", image_height_)){
      ROS_ERROR("Must set param:  image_height_");
    }
    if(!priv_nh.getParam( "image_width", image_width_)){
      ROS_ERROR("Must set param:  image_width_");
    }
    save_data_ = false;
    priv_nh.getParam( "save_data", save_data_);
    if(save_data_){
      if(!priv_nh.getParam("image_directory",image_directory_)){
	ROS_ERROR("save_data set true, but no image_directory defined");
      }
      else{
	ROS_ERROR("robocyl_ical_8D is saving image and rail position data in image_directory %s", image_directory_.c_str());
      }
    }
    bool is_moving = true;
    camera_ =  shared_ptr<industrial_extrinsic_cal::Camera>(new industrial_extrinsic_cal::Camera("my_camera", camera_parameters_, is_moving));
    camera_->trigger_ = shared_ptr<NoWaitTrigger>(new NoWaitTrigger());
    camera_->camera_observer_ = shared_ptr<ROSCameraObserver>(new ROSCameraObserver(image_topic_, camera_name_));
    if(save_data_) camera_->camera_observer_->set_image_directory(image_directory_);
    sleep(5); // wait for camera to come up or else this will fail
    if(!camera_->camera_observer_->pullCameraInfo(camera_->camera_parameters_.focal_length_x,
						  camera_->camera_parameters_.focal_length_y,
						  camera_->camera_parameters_.center_x,
						  camera_->camera_parameters_.center_y,
						  camera_->camera_parameters_.distortion_k1,
						  camera_->camera_parameters_.distortion_k2,
						  camera_->camera_parameters_.distortion_k3,
						  camera_->camera_parameters_.distortion_p1,
						  camera_->camera_parameters_.distortion_p2,
						  image_width_, image_height_))  {
      ROS_FATAL("Could not get camera information for %s from topic %s. Shutting down node.", camera_name_.c_str(), image_topic_.c_str());
      ros::shutdown();
    }
    ROS_INFO("initial camera info focal:%f %f center:%f %f  radial:%f %f %f tang: %f %f",
	     camera_->camera_parameters_.focal_length_x,
	     camera_->camera_parameters_.focal_length_y,
	     camera_->camera_parameters_.center_x,
	     camera_->camera_parameters_.center_y,
	     camera_->camera_parameters_.distortion_k1,
	     camera_->camera_parameters_.distortion_k2,
	     camera_->camera_parameters_.distortion_k3,
	     camera_->camera_parameters_.distortion_p1,
	     camera_->camera_parameters_.distortion_p2);

    initMCircleTarget(target_rows_, target_cols_, circle_diameter_, circle_spacing_);

    // get rail specific parameters
    if(!priv_nh.getParam( "num_camera_locations", num_camera_locations_)){
      ROS_ERROR("Must set param:  num_camera_locations");
    }
    if(!priv_nh.getParam( "camera_spacing", camera_spacing_)){
      ROS_ERROR("Must set param:  camera_spacing");
    }
    if(!priv_nh.getParam( "rail_start_position", rail_start_)){
      ROS_ERROR("Must set param:  target_to_rail_distance");
    }
    string move_client_name("/move_meters");
    if(!priv_nh.getParam( "move_client", move_client_name)){
      ROS_WARN("move_client = %s", move_client_name.c_str());
    }
    string power_client_name("/power_io");
    if(!priv_nh.getParam( "power_client", power_client_name)){
      ROS_WARN("power_client = %s", power_client_name.c_str());
    }
    string home_client_name("/home");
    if(!priv_nh.getParam( "home_client", home_client_name)){
      ROS_WARN("home_client = %s", home_client_name.c_str());
    }

    // initialize stage power it on, and send it home
    move_client_   = nh_.serviceClient<robo_cylinder::MoveMeters>(move_client_name);
    power_client_  = nh_.serviceClient<robo_cylinder::PowerIO>(power_client_name);
    home_client_   = nh_.serviceClient<robo_cylinder::HomeCmd>(home_client_name);
    pio_request_.io = 1;
    power_client_.call(pio_request_, pio_response_);
    home_client_.call(hc_request_, hc_response_);

    // advertise services
    start_server_       = nh_.advertiseService( "ICalSrvStart", &robocyl_ical_8D::startCallBack, this);
    load_server_        = nh_.advertiseService( "ICalSrvLoad",  &robocyl_ical_8D::loadCallBack, this);
    observation_server_ = nh_.advertiseService( "ICalSrvObs",   &robocyl_ical_8D::observationCallBack, this);
    run_server_         = nh_.advertiseService( "ICalSrvRun",   &robocyl_ical_8D::runCallBack, this);
    save_server_        = nh_.advertiseService( "ICalSrvSave",  &robocyl_ical_8D::saveCallBack, this);
    covariance_server_  = nh_.advertiseService( "ICalSrvCov",   &robocyl_ical_8D::covCallBack, this);

    std::string recon_node_name = "~/observe";
    pnh_ = ros::NodeHandle(recon_node_name.c_str());
    server_.reset(new dynamic_reconfigure::Server<intrinsic_cal::observeConfig>(pnh_));

    dynamic_reconfigure::Server<intrinsic_cal::observeConfig>::CallbackType f;

    f = boost::bind(&robocyl_ical_8D::dynReConfCallBack, this, _1, _2);
    server_->setCallback(f);
  };// end of constructor

  // initialize all the points in the target
  void initMCircleTarget(int rows, int cols, double circle_dia, double spacing)
  {
    target_ =  shared_ptr<industrial_extrinsic_cal::Target>(new industrial_extrinsic_cal::Target());
    target_->is_moving_ = true;
    target_->target_name_ = "modified_circle_target";
    target_->target_frame_ = "target_frame";
    target_->target_type_ =  pattern_options::ModifiedCircleGrid;
    target_->circle_grid_parameters_.pattern_rows = rows;
    target_->circle_grid_parameters_.pattern_cols = cols;
    target_->circle_grid_parameters_.spacing      = spacing;
    target_->circle_grid_parameters_.circle_diameter = circle_dia;
    target_->circle_grid_parameters_.is_symmetric = true;
    // create a grid of points
    target_->pose_.setOrigin(0,0,.8);
    tf::Matrix3x3 R(-1, 0, 0, 0, 1, 0, 0, 0, -1);
    target_->pose_.setBasis(R);
    target_->num_points_ = rows*cols;
    target_->generatePoints();
  };// end initMCircleTarget

    // callback functions
  bool startCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res)
  {
    if(problem_initialized_)	delete(P_);
    P_ = new ceres::Problem;
    target_to_camera_poses.clear();
    target_to_camera_poses.reserve(100);
    problem_initialized_ = true;
    total_observations_  = 0;
    scene_=0;
    ax_ay_[0] = 0.0;// ideally the motion of rail is along z axis of camera
    ax_ay_[1] = 0.0;
    res.message = string("Intrinsic calibration service started");
    res.success = true;
    return(true);
  }

  // called to load previously collected observations
  bool loadCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res)
  {
    char msg[100];
    industrial_extrinsic_cal::Cost_function cost_type = industrial_extrinsic_cal::cost_functions::RailICal5;

    char current_image_scene_chars[255];
    sprintf(current_image_scene_chars,"_%03d_%03d.jpg",scene_,0); // add rail distance index to image, scene_ will automatically be added by camera_observer.save_current_image()
    std::string image_file_currently = camera_->camera_name_ + std::string(current_image_scene_chars);
    std::string image_file_now = current_image_file(scene_,image_file_currently);

    while(camera_->camera_observer_->exists_test(image_file_now)){
      //reset everything for each individual scene
      if(problem_initialized_ != true ){
        ROS_INFO("calling start");
        std_srvs::TriggerRequest  sreq;
        std_srvs::TriggerResponse sres;
        startCallBack(sreq,sres);
      }
      // add a new target pose for each scene_, initialized to something reasonable for our standard setup
      Pose6d P;
      P.setOrigin(0,0,.8);
      tf::Matrix3x3 R(-1, 0, 0, 0, 1, 0, 0, 0, -1);
      P.setBasis(R);
      target_to_camera_poses.push_back(P);

      // iether creates a place to hold all usefull information about images or groups all of the valuable information into p_blocks
      P_BLOCK intrinsics = camera_->camera_parameters_.pb_intrinsics;
      P_BLOCK extrinsics = target_to_camera_poses[scene_].pb_pose;
      for(int i = 0; i<(num_camera_locations_);i++){
        // todo load files before you preform observations
        char image_scene_chars[255];
        sprintf(image_scene_chars,"_%03d_%03d.jpg",scene_,i); // add rail distance index to image, scene_ will automatically be added by camera_observer.save_current_image()
        std::string image_file = camera_->camera_name_ + std::string(image_scene_chars);

        camera_->camera_observer_->load_current_image(scene_,image_file);

        double Dist = i*camera_spacing_;
        // set the roi to the whole image
        Roi roi;
        roi.x_min = 0;
        roi.y_min = 0;
        roi.x_max = image_width_;
        roi.y_max = image_height_;

        // get ready for taking Observations
        camera_->camera_observer_->clearTargets();
        camera_->camera_observer_->clearObservations();

        //creates a target in region of interest and adds up points in target
        int total_pts=0;
        camera_->camera_observer_->addTarget(target_, roi, cost_type);
        total_pts += target_->num_points_;

        // preform observations
        CameraObservations camera_observations;

        camera_->camera_observer_->getObservations(camera_observations);
        int num_observations = (int)camera_observations.size();
        ROS_INFO("Found %d observations", (int)camera_observations.size());

        // add observations to problem
        num_observations = (int)camera_observations.size();
        if (num_observations != total_pts)
        {
          ROS_ERROR("Target Locator could not find all targets found %d out of %d", num_observations, total_pts);
        }
        else if(!camera_->camera_observer_->checkObservationProclivity(camera_observations))
        {
          ROS_ERROR("Proclivities Check not successful");
        }
        else
        {
          // add a new cost to the problem for each observation
          total_observations_ += num_observations;
          for (int k = 0; k < num_observations; k++)
          {
            double image_x = camera_observations[k].image_loc_x;
            double image_y = camera_observations[k].image_loc_y;
            Point3d point  = camera_observations[k].target->pts_[camera_observations[k].point_id];
            CostFunction *cost_function = industrial_extrinsic_cal::RailICal5::Create(image_x, image_y, Dist, point);
            P_->AddResidualBlock(cost_function, NULL, intrinsics, extrinsics, ax_ay_);
          }  // for each observation at this camera_location
        } // end of else (there are some observations to add)

      }
      ROS_INFO("now have %d observations after scene %d",total_observations_, scene_);
      scene_++;
      sprintf(current_image_scene_chars,"_%03d_%03d.jpg",scene_,0); // add rail distance index to image, scene_ will automatically be added by camera_observer.save_current_image()
      image_file_currently = camera_->camera_name_ + std::string(current_image_scene_chars);
      image_file_now = current_image_file(scene_,image_file_currently);
    }
    sprintf(msg, "Ical_srv now has %d observations after scene %d",total_observations_, scene_);

    res.message = string(msg);
    res.success = true;

    return(true);
  }; // end of load service};

  // called to collect observations for the current pose of the scene
  bool observationCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res)
  {
    char msg[100];
    industrial_extrinsic_cal::Cost_function cost_type = industrial_extrinsic_cal::cost_functions::RailICal5;

    if(problem_initialized_ != true ){
      ROS_INFO("calling start");
      std_srvs::TriggerRequest  sreq;
      std_srvs::TriggerResponse sres;
      startCallBack(sreq,sres);
    }

    // add a new target pose for each scene_, initialized to something reasonable for our standard setup
    Pose6d P;
    P.setOrigin(0,0,.8);
    tf::Matrix3x3 R(-1, 0, 0, 0, 1, 0, 0, 0, -1);
    P.setBasis(R);
    target_to_camera_poses.push_back(P);

    P_BLOCK intrinsics = camera_->camera_parameters_.pb_intrinsics;
    P_BLOCK extrinsics = target_to_camera_poses[scene_].pb_pose;

    // This is a bit abnormal,
    // a single scene has a sequence of camera locations
    // it is expected that the initial pose at the first pose is changed each scene
    // therefore, the target   , new_image_collected_(false)must be defined to be "moving"
    // We get a new set of extrinsic parameters for the target with each call to observationCallBack()
    for(int i=0; i<num_camera_locations_; i++){

      // move the rail into position, and wait a bit for any vibrations to settle
      double Dist = i*camera_spacing_;
      ROS_INFO("moving to %lf",0.8 - Dist);
      mm_request_.meters = 0.8 - Dist;// note the length of stage is 0.8, so this moves from front to back

      move_client_.call(mm_request_, mm_response_); // this call blocks until camera is moved
      sleep(2.0); // wait for oscillations to die out

      // set the roi to the whole image
      Roi roi;
      roi.x_min = 0;
      roi.y_min = 0;
      roi.x_max = image_width_;
      roi.y_max = image_height_;

      // get ready for taking images
      camera_->camera_observer_->clearTargets();
      camera_->camera_observer_->clearObservations();

      int total_pts=0;
      camera_->camera_observer_->addTarget(target_, roi, cost_type);
      total_pts += target_->num_points_;

      // collect images/observations
      camera_->camera_observer_->triggerCamera();
      while (!camera_->camera_observer_->observationsDone());
      CameraObservations camera_observations;
      camera_->camera_observer_->getObservations(camera_observations);
      int num_observations = (int)camera_observations.size();
      ROS_INFO("Found %d observations", (int)camera_observations.size());

      // add observations to problem
      num_observations = (int)camera_observations.size();
      if (save_data_){
        char image_scene_chars[255];
        sprintf(image_scene_chars,"_%03d_%03d.jpg",scene_,i); // add rail distance index to image, scene_ will automatically be added by camera_observer.save_current_image()
        std::string image_file = camera_->camera_name_ + std::string(image_scene_chars);
        camera_->camera_observer_->save_current_image(scene_,image_file);
      }// end if save_data_
      if (num_observations != total_pts)
      {
        ROS_ERROR("Target Locator could not find all targets found %d out of %d", num_observations, total_pts);
      }
      else if(!camera_->camera_observer_->checkObservationProclivity(camera_observations))
      {
        ROS_ERROR("Proclivities Check not successful");
      }
      else
      {
        // add a new cost to the problem for each observation
        total_observations_ += num_observations;
        for (int k = 0; k < num_observations; k++)
        {
          double image_x = camera_observations[k].image_loc_x;
          double image_y = camera_observations[k].image_loc_y;
          Point3d point  = camera_observations[k].target->pts_[camera_observations[k].point_id];
          if(k==10) ROS_ERROR("target point %d = %8.3lf %8.3lf %8.3lf observed at %8.3lf %8.3lf",camera_observations[k].point_id, point.x, point.y, point.z, image_x, image_y);
          CostFunction *cost_function = industrial_extrinsic_cal::RailICal5::Create(image_x, image_y, Dist, point);
          P_->AddResidualBlock(cost_function, NULL, intrinsics, extrinsics, ax_ay_);
        }  // for each observation at this camera_location
      } // end of else (there are some observations to add)
    }// for each linear rail position

    ROS_INFO("now have %d observations after scene %d",total_observations_, scene_);
    scene_++;

    // move back to start so that camera may be adjusted where it is most critial
    mm_request_.meters = 0.8;// note the length of stage is 0.8, so this moves from front to back
    move_client_.call(mm_request_, mm_response_); // this call blocks until camera is moved

    sprintf(msg, "Ical_srv now has %d observations after scene %d",total_observations_, scene_);

    res.message = string(msg);
    res.success = true;

    return(true);

  }; // end observation service};

  bool runCallBack( industrial_extrinsic_cal::cal_srv_solveRequest &req, industrial_extrinsic_cal::cal_srv_solveResponse &res)
  {
    char msg[100];

    // check for obvious errors
    if(problem_initialized_==false){
      ROS_ERROR("must call start service");
      sprintf(msg, "must call start service to initialized ical service");
      res.message = string(msg);
      res.success = false;
      return(true);
    }
    if(total_observations_ == 0){
      ROS_ERROR("must call observations service at least once");
      sprintf(msg, "must call observations service at least once");
      res.message = string(msg);
      res.success = false;
      return(true);
    }
    for(int i=0;i<target_to_camera_poses.size();i++){
      sprintf(msg,"initial target_pose %d",i);
      target_to_camera_poses[i].show(msg);
    }

    ROS_INFO("intial camera_matrix data: [ %lf, 0.0, %lf, 0.0, %lf, %lf, 0.0, 0.0, 1.0]",
	     camera_->camera_parameters_.focal_length_x, camera_->camera_parameters_.center_x,
	     camera_->camera_parameters_.focal_length_y, camera_->camera_parameters_.center_y);
    ROS_INFO("initial distortion data: [ %lf,  %lf,  %lf,  %lf,  %lf]", camera_->camera_parameters_.distortion_k1,
	     camera_->camera_parameters_.distortion_k2, camera_->camera_parameters_.distortion_p1,
	     camera_->camera_parameters_.distortion_p2, camera_->camera_parameters_.distortion_k3);
    ROS_INFO("initial rotation of axis of motion %lf %lf", ax_ay_[0], ax_ay_[1]);

    Solver::Options options;
    Solver::Summary summary;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = true;
    options.max_num_iterations = 1000;
    ceres::Solve(options, P_, &summary);
    if (summary.termination_type != ceres::NO_CONVERGENCE)
      {
	double initial_cost = sqrt( 2*summary.initial_cost / total_observations_);
	double final_cost   = sqrt( 2*summary.final_cost   / total_observations_);

	ROS_INFO("Problem solved, initial cost = %lf, final cost = %lf", initial_cost, final_cost);
	for(int i=0;i<target_to_camera_poses.size();i++){
	  sprintf(msg,"final target_pose %d",i);
	  target_to_camera_poses[i].show(msg);
	}

	ROS_INFO("camera_matrix data: [ %lf, 0.0, %lf, 0.0, %lf, %lf, 0.0, 0.0, 1.0]",
		 camera_->camera_parameters_.focal_length_x, camera_->camera_parameters_.center_x,
		 camera_->camera_parameters_.focal_length_y, camera_->camera_parameters_.center_y);
	ROS_INFO("distortion data: [ %lf,  %lf,  %lf,  %lf,  %lf]", camera_->camera_parameters_.distortion_k1,
		 camera_->camera_parameters_.distortion_k2, camera_->camera_parameters_.distortion_p1,
		 camera_->camera_parameters_.distortion_p2, camera_->camera_parameters_.distortion_k3);
	ROS_INFO("projection_matrix data: [ %lf, 0.0, %lf, 0.0, 0.0, %lf, %lf, 0.0, 0.0, 0.0, 1.0, 0.0]",
		 camera_->camera_parameters_.focal_length_x, camera_->camera_parameters_.center_x,
		 camera_->camera_parameters_.focal_length_y, camera_->camera_parameters_.center_y);
	ROS_INFO("final rotation of axis of motion %lf %lf", ax_ay_[0], ax_ay_[1]);
	if (final_cost <= req.allowable_cost_per_observation)
	  {
	    ROS_INFO("calibration was successful");
	  }
	else
	  {
	    res.final_cost_per_observation = final_cost;
	    ROS_ERROR("allowable cost exceeded %f > %f", final_cost, req.allowable_cost_per_observation);
	    sprintf(msg, "allowable cost exceeded %f > %f", final_cost, req.allowable_cost_per_observation);
	    res.message = string(msg);
	    res.success = false;
	    return(true);
	  }
	sprintf(msg, "final cost %f", final_cost);
	res.final_cost_per_observation = final_cost;
	res.message = string(msg);
	res.success = true;
	return(true);
      }
    ROS_ERROR("NO CONVERGENCE");
    sprintf(msg, "NO CONVERGENCE");
    res.message = string(msg);
    res.success = false;
    return(true);
  }; // end runCallBack()

  bool saveCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res)
  {
    char msg[100];
    camera_->camera_observer_->pushCameraInfo(
					      camera_->camera_parameters_.focal_length_x, camera_->camera_parameters_.focal_length_y,
					      camera_->camera_parameters_.center_x, camera_->camera_parameters_.center_y,
					      camera_->camera_parameters_.distortion_k1, camera_->camera_parameters_.distortion_k2,
					      camera_->camera_parameters_.distortion_k3, camera_->camera_parameters_.distortion_p1,
					      camera_->camera_parameters_.distortion_p2);
    sprintf(msg, "intrinsics: %8.2lf %8.2lf %8.2lf %8.2lf, %5.3lf %5.3lf %5.3lf, %5.3lf %5.3lf",
	    camera_->camera_parameters_.focal_length_x, camera_->camera_parameters_.focal_length_y,
	    camera_->camera_parameters_.center_x, camera_->camera_parameters_.center_y,
	    camera_->camera_parameters_.distortion_k1, camera_->camera_parameters_.distortion_k2,
	    camera_->camera_parameters_.distortion_k3, camera_->camera_parameters_.distortion_p1,
	    camera_->camera_parameters_.distortion_p2);

    res.message = string(msg);
    res.success = true;
    return(true);

  }// end saveCallback()
  bool covCallBack( std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res)
  {
    std::string covariance_file_name("/home/clewis/junk.txt");
    if(!computeCovariance(covariance_file_name)){
      ROS_ERROR("could not compute covariance");
      res.success = false;
      res.message = "failure";
    }
    else{
      res.message = "good job";
      res.success = true;
    }
    return(true);
  };

  bool computeCovariance(string& covariance_file_name)
  {
    FILE* fp;
    if ((fp = fopen(covariance_file_name.c_str(), "w")) != NULL)
    {
      ceres::Covariance::Options covariance_options;
      covariance_options.algorithm_type = ceres::DENSE_SVD;
      ceres::Covariance covariance(covariance_options);

      std::vector<std::pair<const double*, const double*> > covariance_pairs;
      covariance_pairs.push_back(std::make_pair(camera_->camera_parameters_.pb_intrinsics, camera_->camera_parameters_.pb_intrinsics));
      covariance_pairs.push_back(std::make_pair(ax_ay_,ax_ay_));
      covariance_pairs.push_back(std::make_pair(camera_->camera_parameters_.pb_intrinsics, ax_ay_));

      if(covariance.Compute(covariance_pairs, P_))
      {
        fprintf(fp, "covariance blocks:\n");
        double cov_in_in[9*9];
        double cov_axis_axis[2*2];
        double cov_in_axis[9*2];
        if(covariance.GetCovarianceBlock(camera_->camera_parameters_.pb_intrinsics, camera_->camera_parameters_.pb_intrinsics, cov_in_in) &&
           covariance.GetCovarianceBlock(ax_ay_, ax_ay_, cov_axis_axis) &&
           covariance.GetCovarianceBlock(camera_->camera_parameters_.pb_intrinsics, ax_ay_, cov_in_axis)){
          fprintf(fp, "cov_in_in is 9x9\n");
          for(int i=0;i<9;i++){
            double sigma_i = sqrt(fabs(cov_in_in[i*9+i]));
            for(int j=0;j<9;j++){
              double sigma_j = sqrt(fabs(cov_in_in[j*9+j]));
              double value;
              if(i==j){
                value = sigma_i;
              }
              else{
                if(sigma_i==0) sigma_i = 1;
                if(sigma_j==0) sigma_j = 1;
                value = cov_in_in[i * 9 + j]/(sigma_i*sigma_j);
              }
              fprintf(fp, "%16.5f ", value);
            }  // end of j loop
            fprintf(fp, "\n");
          }  // end of i loop
          fprintf(fp, "cov_axis_axis is 2x2\n");
          for(int i=0;i<2;i++){
            double sigma_i = sqrt(fabs(cov_axis_axis[i*2+i]));
            for(int j=0;j<2;j++){
              double sigma_j = sqrt(fabs(cov_axis_axis[j*2+j]));
              double value;
              if(i==j){
                value = sigma_i;
              }
              else{
                if(sigma_i==0) sigma_i = 1;
                if(sigma_j==0) sigma_j = 1;
                value = cov_axis_axis[i * 2 + j]/(sigma_i*sigma_j);
              }
              fprintf(fp, "%16.5f ", value);
            }  // end of j loop
            fprintf(fp, "\n");
          }  // end of i loop
          fprintf(fp, "cov_in_axis is 9x2\n");
          for(int i=0;i<9;i++){
            double sigma_i = sqrt(fabs(cov_in_in[i*9+i]));
            for(int j=0;j<2;j++){
              double sigma_j = sqrt(fabs(cov_axis_axis[j*2+j]));
              double value;
              if(sigma_i==0) sigma_i = 1;
              if(sigma_j==0) sigma_j = 1;
              value = cov_in_axis[i * 2 + j]/(sigma_i*sigma_j);
              fprintf(fp, "%16.5f ", value);
            }  // end of j loop
            fprintf(fp, "\n");
          }  // end of i loop
        }// end if success getting covariance
        fclose(fp);
        return(true);
      }// end if covariances could be computed
      else{
        ROS_ERROR("could not compute covariance");
      }
    }// end if file opens
    ROS_ERROR("could not open covariance file %s", covariance_file_name.c_str());
    return (false);
  };  // end computeCovariance()

  //construct image file from scene name
  std::string current_image_file (int scene, std::string& filename){
    std::string present_image_file_path_name;
    char scene_chars[8];
    sprintf(scene_chars,"_%03d.jpg",scene);
    if(filename == ""){ // build file name from image_directory_,
      present_image_file_path_name  = image_directory_ + std::string("/") +  camera_name_ + std::string(scene_chars);
    }
    else{
      present_image_file_path_name  = image_directory_ + "/" +  filename;
    }
    return present_image_file_path_name;
  }; // end of current_image_file()

private:
  boost::shared_ptr<dynamic_reconfigure::Server<intrinsic_cal::observeConfig> > server_;
  ros::NodeHandle pnh_;
  ros::NodeHandle nh_;
  double ax_ay_[2];                           /*!< The ax and ay parameters defining the axis of motion relative to optical axis */
  ros::ServiceServer start_server_;
  ros::ServiceServer load_server_;
  ros::ServiceServer observation_server_;
  ros::ServiceServer run_server_;
  ros::ServiceServer save_server_;
  ros::ServiceServer covariance_server_;
  ceres::Problem *P_;
  bool problem_initialized_;
  int total_observations_;
  int scene_;
  int num_camera_locations_;
  double camera_spacing_;
  double rail_start_;
  bool save_data_;
  string image_directory_;
  robo_cylinder::MoveMeters::Request mm_request_;   /**< request when transform is part of a mutable set */
  robo_cylinder::MoveMeters::Response mm_response_; /**< request when transform is part of a mutable set */
  robo_cylinder::PowerIO::Request pio_request_;     /**< request when transform is part of a mutable set */
  robo_cylinder::PowerIO::Response pio_response_;   /**< request when transform is part of a mutable set */
  robo_cylinder::HomeCmd::Request hc_request_;      /**< request when transform is part of a mutable set */
  robo_cylinder::HomeCmd::Response hc_response_;    /**< request when transform is part of a mutable set */
  // robo-cylinder clients
  ros::ServiceClient move_client_; /**< a client for calling the service to move the robo-cylinder to a new location */
  ros::ServiceClient power_client_; /**< a client for calling the service to turn on the robo-cylinder */
  ros::ServiceClient home_client_; /**< a client for calling the service to move robo-cylinder to its home position */
  shared_ptr<Target> target_;
  shared_ptr<Camera> camera_;
  std::vector<Pose6d> target_to_camera_poses;
  int target_rows_;
  int target_cols_;
  double circle_spacing_;
  double circle_diameter_;
  std::string camera_name_;
  std::string image_topic_;
  int image_height_;
  int image_width_;
  CameraParameters camera_parameters_;
};// end of class robocyl_ical_8D

int main(int argc, char** argv)
{
  ros::init(argc, argv, "ical_cal_service");
  ros::NodeHandle node_handle;
  robocyl_ical_8D cal_object(node_handle);
  ros::spin();
  ros::waitForShutdown();
  return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_SSSE3_HMSB_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_SSSE3_HMSB_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSSE3_SUPPORT

#include <boost/simd/reduction/functions/hmsb.hpp>
#include <boost/dispatch/attributes.hpp>
#include <cstddef>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( hmsb_
                                    , boost::simd::tag::ssse3_
                                    , (A0)
                                    , ((simd_ < type16_<A0>
                                              , boost::simd::tag::sse_
                                              >
                                      ))
                                    )
  {
    typedef std::size_t result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      typedef typename A0::template rebind<char>::type s8type;

      __m128i mask =  _mm_setr_epi8 ( 0x01,0x03,0x05,0x07,0x09,0x0B,0x0D,0x0F
                                    , -128,-128,-128,-128,-128,-128,-128,-128
                                    );

      return _mm_movemask_epi8(_mm_shuffle_epi8(bitwise_cast<s8type>(a0),mask));
    }
  };

} } }

#endif
#endif


#include <numeric>

#include <mve/image_color.h>
#include <acc/bvh_tree.h>
#include <Eigen/Core>
#include <Eigen/LU>

#include "texturing.h"
#include "sparse_table.h"
#include "progress_counter.h"
#include "util.h"
#include "histogram.h"

typedef acc::BVHTree<unsigned int, math::Vec3f> BVHTree;

TEX_NAMESPACE_BEGIN

/*
* texture_views: 图片
* face_projection_infos: 保存view能看到的face(s)的组合
* */
void calculate_face_projection_infos(mve::TriangleMesh::ConstPtr mesh, TextureViews * texture_views, Settings const & settings, FaceProjectionInfos * face_projection_infos){
    std::vector<unsigned int> const & faces = mesh->get_faces();  // 三角形顶点索引
    std::vector<math::Vec3f> const & vertices = mesh->get_vertices();
    mve::TriangleMesh::NormalList const & face_normals = mesh->get_face_normals();

    std::size_t const num_views = texture_views->size();

    std::cout<<"\t 使用"<<faces.size()<<"个面构建BVH树..."<<std::flush;
    BVHTree bvh_tree(faces, vertices);
    std::cout<<"完成. "<<std::endl;

    ProgressCounter view_counter("\t 计算每个面片的代价", num_views);
    #pragma omp parallel
    {
        std::vector<std::pair<size_t, FaceProjectionInfo>> projected_face_view_infos;

        #pragma omp for schedule(dynamic)
        for(std::uint16_t j=0; j<static_cast<std::uint16_t>(num_views); ++j){  // 遍历每个view
            view_counter.progress<SIMPLE>();

            TextureView * texture_view = &texture_views->at(j);
            texture_view->load_image();
            texture_view->generate_validity_mask();

            if(settings.data_term == DATA_TERM_GMI){
                texture_view->generate_gradient_magnitude();  // 每个view计算梯度图
                texture_view->erode_validity_mask();
            }

            math::Vec3f const & view_pos = texture_view->get_pos();
            math::Vec3f const & viewing_direction = texture_view->get_viewing_direction();

            for (std::size_t i = 0; i < faces.size(); i += 3) {  // 遍历每个三角形
                std::size_t face_id = i / 3;

                math::Vec3f const & v1 = vertices[faces[i]];
                math::Vec3f const & v2 = vertices[faces[i + 1]];
                math::Vec3f const & v3 = vertices[faces[i + 2]];
                math::Vec3f const & face_normal = face_normals[face_id];
                math::Vec3f const face_center = (v1 + v2 + v3) / 3.0f;

                // 检查可见性 和 计算quality
                // 1. 检查可见性
                math::Vec3f view_to_face_vec = (face_center - view_pos).normalized();
                math::Vec3f face_to_view_vec = (view_pos - face_center).normalized();

                // 背面剔除
                float viewing_angle = face_to_view_vec.dot(face_normal);
                if (viewing_angle < 0.0f || viewing_direction.dot(view_to_face_vec) < 0.0f)  // 面背对视角 或者 视角背对面 不管这个面
                    continue;

                if (!texture_view->inside(v1, v2, v3))  // 将点投到view上看是否有效, 以及看mask
                    continue;

                // 视域剔除
                if (settings.geometric_visibility_test) {
                    /* Viewing rays do not collide? */
                    bool visible = true;
                    math::Vec3f const * samples[] = {&v1, &v2, &v3};

                    for (std::size_t k=0; k<sizeof(samples) / sizeof(samples[0]); ++k) {  // 从三角形的三个顶点连线到view_pos看有没有被挡住, 如果其中一个被挡住, 则三角形设为不可见
                        BVHTree::Ray ray;
                        ray.origin = *samples[k];
                        ray.dir = view_pos - ray.origin;
                        ray.tmax = ray.dir.norm();  // BVH最大盒子的最大值 https://zhuanlan.zhihu.com/p/114307697
                        ray.tmin = ray.tmax * 0.0001f;  // BVH最大盒子的最小值
                        ray.dir.normalize();

                        BVHTree::Hit hit;
                        if (bvh_tree.intersect(ray, &hit)) {
                            visible = false;
                            break;
                        }
                    }
                    if (!visible) continue;
                }
                FaceProjectionInfo info = {j, 0.0f, math::Vec3f(0.0f, 0.0f, 0.0f)};  // {view_id, quality, mean_color}

                // 计算quality
                texture_view->get_face_info(v1, v2, v3, &info, settings);

                if(info.quality == 0.0) continue;

                // 转ycbcr
                mve::image::color_rgb_to_ycbcr(*(info.mean_color));  //

                std::pair<std::size_t, FaceProjectionInfo> pair(face_id, info);  // view和face的信息保存到一起
                projected_face_view_infos.push_back(pair);
            }

            texture_view->release_image();
            texture_view->release_validity_mask();
            if (settings.data_term == DATA_TERM_GMI) {
                texture_view->release_gradient_magnitude();
            }

            view_counter.inc();
        }

        #pragma omp critical
        {
            for (std::size_t i = projected_face_view_infos.size(); 0 < i; --i){
                std::size_t face_id = projected_face_view_infos[i-1].first;  // 面
                FaceProjectionInfo const & info = projected_face_view_infos[i-1].second;  // 视角信息
                face_projection_infos->at(face_id).push_back(info);
            }
            projected_face_view_infos.clear();
        }
    }

}

/*
* 消除面片投影在的所有视角的中质量与其他大多数视角中的颜色有很大不同的面片
* param infos 包含从多个视角看到的一张脸的信息
* param settings 运行时配置。
* return 异常值删除是否成功。
* */
bool photometric_outlier_detection(std::vector<FaceProjectionInfo> * infos, Settings const & settings) {
    if (infos->size() == 0) return true;  // 这个面没有被任何view看到

    // 参数配置
    double const gauss_rejection_threshold = 6e-3;
    double const minimal_covariance = 5e-4;  // 如果所有协方差都低于这个值，就停止异常检测

    int const outlier_detection_iterations = 10;  // 迭代次数
    int const minimal_num_inliers = 4;

    float outlier_removal_factor = std::numeric_limits<float>::signaling_NaN();
    switch(settings.outlier_removal){
        case OUTLIER_REMOVAL_NONE: return true;
        case OUTLIER_REMOVAL_GAUSS_CLAMPING: outlier_removal_factor = 1.0f; break;
        case OUTLIER_REMOVAL_GAUSS_DAMPING:  outlier_removal_factor = 0.2f; break;
    }

    Eigen::MatrixX3d inliers(infos->size(), 3);  // shape=view的个数 * 3, 存这个面在每个view的mean color
    for(std::size_t row=0; row < infos->size(); ++row) {
        inliers.row(row) = mve_to_eigen(infos->at(row).mean_color).cast<double>();
    }

    std::vector<std::uint32_t> is_inlier(infos->size(), 1);  // 把所有的view都先当成inlier, 1为inlier, 0不是

    Eigen::RowVector3d var_mean;
    Eigen::Matrix3d covariance;
    Eigen::Matrix3d covariance_inv;

    for(int i = 0; i < outlier_detection_iterations; ++i) {  // mean shift 迭代一定次数
        if(inliers.rows() < minimal_num_inliers)  // inlier小于一定数目就不剔除了
            return false;

        // 计算inliers(每个view的平均颜色)的平均颜色和协方差
        var_mean = inliers.colwise().mean();  // 计算三个通道的mean值
        Eigen::MatrixX3d centered = inliers.rowwise() - var_mean;  // 和mean作差
        covariance = (centered.adjoint() * centered) / double(inliers.rows() - 1);

        // 如果所有协方差都很小，就停止，保留已有inlier, 并将outlier的质量设置为零
        if(covariance.array().abs().maxCoeff() < minimal_covariance) {  // 给矩阵的每个元素加绝对值
            for(std::size_t row = 0; row < infos->size(); ++row) {  // 遍历每个view
                if(!is_inlier[row]){
                    infos->at(row).quality = 0.0f;
                }
            }
            return true;
        }

        Eigen::FullPivLU<Eigen::Matrix3d> lu(covariance);
        if (!lu.isInvertible()) return false;  // 如果协方差变得不可导, 退出

        covariance_inv = lu.inverse();

        // 计算新的内联线数（高斯值高于阈值的所有视图）
        for (std::size_t row = 0; row < infos->size(); ++row) {
            Eigen::RowVector3d color = mve_to_eigen(infos->at(row).mean_color).cast<double>();
            double gauss_value = multi_gauss_unnormalized(color, var_mean, covariance_inv);
            is_inlier[row] = (gauss_value >= gauss_rejection_threshold ? 1 : 0);  // 越大越好
        }

        // 根据还有多少linlier调整矩阵
        inliers.resize(std::accumulate(is_inlier.begin(), is_inlier.end(), 0), Eigen::NoChange);  // 缩小inliers列表, 有多少个is_inlier, inlier列表就多大
        // 把筛剩下的inlier放入inliers列表中
        for (std::size_t row = 0, inlier_row = 0; row < infos->size(); ++row) {
            if (is_inlier[row]) {
                inliers.row(inlier_row++) = mve_to_eigen(infos->at(row).mean_color).cast<double>();
            }
        }
    }

    covariance_inv *= outlier_removal_factor;
    for (FaceProjectionInfo & info : *infos) {
        Eigen::RowVector3d color = mve_to_eigen(info.mean_color).cast<double>();
        double gauss_value = multi_gauss_unnormalized(color, var_mean, covariance_inv);
        assert(0.0 <= gauss_value && gauss_value <= 1.0);
        switch(settings.outlier_removal) {
            case OUTLIER_REMOVAL_NONE:
                return true;
            case OUTLIER_REMOVAL_GAUSS_DAMPING:
                info.quality *= gauss_value; break;
            case OUTLIER_REMOVAL_GAUSS_CLAMPING:
                if (gauss_value < gauss_rejection_threshold) {info.quality = 0.0f; break;}
                else break;  // ???是吗
        }
    }
    return true;
}

void postprocess_face_infos(Settings const & settings, FaceProjectionInfos * face_projection_infos, DataCosts * data_costs){
    ProgressCounter face_counter("\t 处理面片信息", face_projection_infos->size());

    #pragma omp parallel for schedule(dynamic)
    for (std::size_t i = 0; i < face_projection_infos->size(); ++i) {  // 遍历每个面
        face_counter.progress<SIMPLE>();

        std::vector<FaceProjectionInfo> & infos = face_projection_infos->at(i);
        if (settings.outlier_removal != OUTLIER_REMOVAL_NONE) {
            photometric_outlier_detection(&infos, settings);

            infos.erase(std::remove_if(infos.begin(), infos.end(),
                [](FaceProjectionInfo const & info) -> bool {return info.quality == 0.0f;}),
                infos.end());  // 将infos(当前面)里quality是0的视角移除
        }
        std::sort(infos.begin(), infos.end());

        face_counter.inc();
    }

    // 确定标准化函数
    float max_quality = 0.0f;  // 所有面的所有view中quality最大的
    for (std::size_t i = 0; i < face_projection_infos->size(); ++i) {  // 遍历每个面的信息(把outlier给剔除了)
        for (FaceProjectionInfo const & info : face_projection_infos->at(i))
            max_quality = std::max(max_quality, info.quality);
    }

    Histogram hist_qualities(0.0f, max_quality, 10000);  // 创建一个10000个bin的直方图, 最小值是0, 最大值是max_quality
    for (std::size_t i = 0; i < face_projection_infos->size(); ++i) {  // 将所有面的所有view的quality加到直方图中, 从小到大排
        for (FaceProjectionInfo const & info : face_projection_infos->at(i))
            hist_qualities.add_value(info.quality);
    }

    float percentile = hist_qualities.get_approx_percentile(0.995f);

    /* 计算 costs. */
    for (std::uint32_t i = 0; i < face_projection_infos->size(); ++i) {
        for (FaceProjectionInfo const & info : face_projection_infos->at(i)) {

            /* Clamp to percentile and normalize. */
            float normalized_quality = std::min(1.0f, info.quality / percentile);  // quality过高的都限制在1及以内
            float data_cost = (1.0f - normalized_quality);  // quality越低, cost越高
            data_costs->set_value(i, info.view_id, data_cost);
        }

        /* Ensure that all memory is freeed. */
        face_projection_infos->at(i) = std::vector<FaceProjectionInfo>();
    }

    std::cout << "\t 图片中质量最大的面片: " << max_quality << std::endl;
    std::cout << "\t 把质量范围控制在" << percentile << "以内" << std::endl;
}

void calculate_data_costs(mve::TriangleMesh::ConstPtr mesh, TextureViews * texture_views, Settings const & settings, DataCosts * data_costs){
    std::size_t const num_faces = mesh->get_faces().size()/3;  // 三角形面片的个数
    std::size_t const num_views = texture_views->size();  // 视角的个数

    if (num_faces > std::numeric_limits<std::uint32_t>::max())
        throw std::runtime_error("超出最多面片数");
    if (num_views > std::numeric_limits<std::uint16_t>::max())
        throw std::runtime_error("低于最少面片数");

    FaceProjectionInfos face_projection_infos(num_faces);  // 默认值初始化num_faces个vector 一个面片一个vector, 这个vector里再存能看到这个面的所有views, 以及这个面在当前view的颜色质量信息. 如果面片质量为0, 则是空的.
    calculate_face_projection_infos(mesh, texture_views, settings, &face_projection_infos);  // 计算face_projection_infos

    postprocess_face_infos(settings, &face_projection_infos, data_costs);  // 预先计算data_cost, 后面会用到
}

TEX_NAMESPACE_END


#include "DisplayableMesh.h"

#include <osg/Geode>
#include <osg/Geometry>
#include <osg/Material>
#include <osgUtil/SmoothingVisitor>

#include "qvm_eigen.h"
#include "qvm_osg.h"

#include "enumerate.h"
#include "osg_utils.h"
#include <boost/qvm/quat_operations.hpp>

osg::ref_ptr<osg::Geometry> createPolygon(const Face & f, const osg::Vec4 & color)
{
	osg::ref_ptr<osg::Vec3Array> points = new osg::Vec3Array();
	osg::ref_ptr<osg::DrawElementsUInt> polygonFace = new osg::DrawElementsUInt(osg::PrimitiveSet::TRIANGLES, 0);
	osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array();

	for(const auto & [faceIdx, vertex] : enumerate(f.getVertices()))
	{
		points->push_back(osg::Vec3(vertex(0), vertex(1), vertex(2)));
		polygonFace->push_back(faceIdx);
		colors->push_back(color);
	}

	osg::ref_ptr<osg::Geometry> osg_polygon = new osg::Geometry();
	osg_polygon->setVertexArray(points.get());
	osg_polygon->addPrimitiveSet(polygonFace);
	osg_polygon->setColorArray(colors.get());
	osg_polygon->setColorBinding(osg::Geometry::BIND_PER_VERTEX);

	return osg_polygon;
}

osg::ref_ptr<osg::Geometry> createLines(const Face & f, const osg::Vec4 & color)
{
	osg::ref_ptr<osg::Vec3Array> points = new osg::Vec3Array();
	osg::ref_ptr<osg::DrawElementsUInt> lineFace = new osg::DrawElementsUInt(osg::PrimitiveSet::LINE_LOOP, 0);
	osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array();

	for (const auto &[faceIdx, vertex] : enumerate(f.getVertices()))
	{
		points->push_back(osg::Vec3(vertex(0), vertex(1), vertex(2)));
		lineFace->push_back(faceIdx);
		colors->push_back(color);
	}

	osg::ref_ptr<osg::Geometry> osg_polygon = new osg::Geometry();
	osg_polygon->setVertexArray(points.get());
	osg_polygon->addPrimitiveSet(lineFace);
	osg_polygon->setColorArray(colors.get());
	osg_polygon->setColorBinding(osg::Geometry::BIND_PER_VERTEX);

	return osg_polygon;
}

DisplayableMesh::DisplayableMesh(const Mesh & navMesh, const osg::Vec4 & color, bool showWireFrame) :
	m_navMesh(navMesh),
	m_color(color),
	m_showWireFrame(showWireFrame)
{
	// empty
}

osg::ref_ptr<osg::Group> DisplayableMesh::getGeometry() const
{
	osg::ref_ptr<osg::Group> ret = new osg::Group();

	osg::ref_ptr<osg::Geode> faceGeode = new osg::Geode();
	ret->addChild(faceGeode);

	for (const auto & v : m_navMesh.getFaces())
	{
		osg::ref_ptr<osg::Geometry> face = createPolygon(v, m_color);
		faceGeode->addChild(face);

		osg::ref_ptr<osg::Material> material = new osg::Material();
		material->setDiffuse(osg::Material::FRONT_AND_BACK, m_color);
		//material->setSpecular(osg::Material::FRONT, osg::Vec4(0.0f, 0.1f, 0.0f, 1.0f));
		material->setAmbient(osg::Material::FRONT, m_color);
		//material->setEmission(osg::Material::FRONT, osg::Vec4(0.0f, 0.0f, 0.0f, 1.0f));
		material->setShininess(osg::Material::FRONT, 25.0);

		face->getOrCreateStateSet()->setAttribute(material);
		face->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::ON);
	}

	osg::ref_ptr<osgUtil::SmoothingVisitor> visitor = new osgUtil::SmoothingVisitor;
	visitor->setCreaseAngle(0);
	faceGeode->accept(*visitor);

	if (m_showWireFrame)
	{
		osg::ref_ptr<osg::Geode> lineMeshGeode = new osg::Geode();
		ret->addChild(lineMeshGeode);

		for (const auto & v : m_navMesh.getFaces())
		{
			osg::ref_ptr<osg::Geometry> lines = createLines(v, m_color * 0.5);
			lineMeshGeode->addChild(lines);

			setAttributesNonLightingBlendable(lineMeshGeode);
			lineMeshGeode->getOrCreateStateSet()->setMode(GL_DEPTH_TEST, osg::StateAttribute::OFF);
		}
	}

	return ret;
}


#pragma once
#include <Eigen/Core>
#include <trajopt/common.hpp>
#include "sco/modeling_utils.hpp"

namespace trajopt {
using Eigen::MatrixX2d;

struct ZMPConstraint : public IneqConstraint, public Plotter {
  RobotAndDOFPtr m_rad;
  VarVector m_vars;
  MatrixX2d m_ab, m_pts;
  VectorXd m_c;
  ZMPConstraint(RobotAndDOFPtr rad, const MatrixX2d& hullpts, const VarVector& vars);
  DblVec value(const DblVec&);
  ConvexConstraintsPtr convex(const DblVec&, Model* model);
  void Plot(const DblVec& x, OR::EnvironmentBase& env, std::vector<OR::GraphHandlePtr>& handles);

};

struct StaticTorqueCost : public CostFromErrFunc {
  RobotAndDOFPtr m_rad;
  VarVector m_vars;
  StaticTorqueCost(RobotAndDOFPtr rad, const VarVector& vars, double coeff);
};

struct PECost : public CostFromErrFunc {
  RobotAndDOFPtr m_rad;
  VarVector m_vars;
  PECost(RobotAndDOFPtr rad, const VarVector& vars, double coeff);
};

struct FootHeightConstraint : public ConstraintFromFunc {
  FootHeightConstraint(RobotAndDOFPtr rad, OpenRAVE::KinBody::LinkPtr link, double height, const VarVector& vars);
};

}