/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *
 * FGraphPy.cpp
 *
 *  Created on: Apr 5, 2019
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>

#include "mrob/factor_graph_solve.hpp"
#include "mrob/factors/nodePose2d.hpp"
#include "mrob/factors/factor1Pose2d.hpp"
#include "mrob/factors/factor2Poses2d.hpp"
#include "mrob/factors/nodePose3d.hpp"
#include "mrob/factors/factor1Pose3d.hpp"
#include "mrob/factors/factor2Poses3d.hpp"
#include "mrob/factors/nodeLandmark3d.hpp"
#include "mrob/factors/factor1Pose1Landmark3d.hpp"
#include "mrob/factors/nodeLandmark2d.hpp"
#include "mrob/factors/factor1Pose1Landmark2d.hpp"
#include "mrob/factors/nodePlane4d.hpp"
#include "mrob/factors/factor1Pose1Plane4d.hpp"

#include "mrob/factors/factor1PosePoint2Plane.hpp"

#include <Eigen/Geometry>

namespace py = pybind11;
using namespace mrob;

/**
 * Create auxiliary class to include all functions:
 *    - creates specific factors and nodes (while the cpp maintains a polymorphic data structure)
 *
 */

class FGraphPy : public FGraphSolve
{
public:
    /**
     * Constructor for the python binding. By default uses the Cholesky adjoint solving type.
     * For the robust factor type, it indicates one (default - Quadratic)
     * and all factors will automatically be this kind of robust factors
     *
     * NOTE: in cpp each new factor needs to specify the robust type, which give more freedom, but
     * since the .py is a more "structured" class, it is included in the constructor and that's
     * all the user will ever see
     *
     * TODO: change type of robust? maybe some apps would need that feature...
     */
    FGraphPy(mrob::Factor::robustFactorType robust_type = mrob::Factor::robustFactorType::QUADRATIC) :
        FGraphSolve(FGraphSolve::matrixMethod::ADJ), robust_type_(robust_type) {}
    factor_id_t add_node_pose_2d(const py::EigenDRef<const Mat31> x)
    {
        std::shared_ptr<mrob::Node> n(new mrob::NodePose2d(x));
        this->add_node(n);
        return n->get_id();
    }
    void add_factor_1pose_2d(const py::EigenDRef<const Mat31> obs, uint_t nodeId, const py::EigenDRef<const Mat3> obsInvCov)
    {
        auto n1 = this->get_node(nodeId);
        std::shared_ptr<mrob::Factor> f(new mrob::Factor1Pose2d(obs,n1,obsInvCov,robust_type_));
        this->add_factor(f);
    }
    void add_factor_2poses_2d(const py::EigenDRef<const Mat31> obs, uint_t nodeOriginId, uint_t nodeTargetId,
            const py::EigenDRef<const Mat3> obsInvCov, bool updateNodeTarget)
    {
        auto nO = this->get_node(nodeOriginId);
        auto nT = this->get_node(nodeTargetId);
        std::shared_ptr<mrob::Factor> f(new mrob::Factor2Poses2d(obs,nO,nT,obsInvCov, updateNodeTarget,robust_type_));
        this->add_factor(f);
    }
    void add_factor_2poses_2d_odom(const py::EigenDRef<const Mat31> obs, uint_t nodeOriginId, uint_t nodeTargetId, const py::EigenDRef<const Mat3> obsInvCov)
    {
        auto nO = this->get_node(nodeOriginId);
        auto nT = this->get_node(nodeTargetId);
        std::shared_ptr<mrob::Factor> f(new mrob::Factor2Poses2dOdom(obs,nO,nT,obsInvCov,true,robust_type_));//true is to update the node value according to obs
        this->add_factor(f);
    }

    // 3D factor graph
    // ------------------------------------------------------------------------------------
    /*factor_id_t add_node_pose_3d(const py::EigenDRef<const Mat4> x)
    {
        std::shared_ptr<mrob::Node> n(new mrob::NodePose3d(x));
        this->add_node(n);
        return n->get_id();
    }*/
    factor_id_t add_node_pose_3d(const SE3 &x)
    {
        std::shared_ptr<mrob::Node> n(new mrob::NodePose3d(x));
        this->add_node(n);
        return n->get_id();
    }
    factor_id_t add_factor_1pose_3d(const SE3 &obs, uint_t nodeId, const py::EigenDRef<const Mat6> obsInvCov)
    {
        auto n1 = this->get_node(nodeId);
        std::shared_ptr<mrob::Factor> f(new mrob::Factor1Pose3d(obs,n1,obsInvCov,robust_type_));
        this->add_factor(f);
        return f->get_id();
    }
    factor_id_t add_factor_2poses_3d(const SE3 &obs, uint_t nodeOriginId, uint_t nodeTargetId,
            const py::EigenDRef<const Mat6> obsInvCov, bool updateNodeTarget)
    {
        auto nO = this->get_node(nodeOriginId);
        auto nT = this->get_node(nodeTargetId);
        std::shared_ptr<mrob::Factor> f(new mrob::Factor2Poses3d(obs,nO,nT,obsInvCov, updateNodeTarget, robust_type_));
        this->add_factor(f);
        return f->get_id();
    }

    // 3D Landmarks
    // ------------------------------------------------------------------------------------
    factor_id_t add_node_landmark_3d(const py::EigenDRef<const Mat31> x)
    {
        std::shared_ptr<mrob::Node> n(new mrob::NodeLandmark3d(x));
        this->add_node(n);
        return n->get_id();
    }
    factor_id_t add_factor_1pose_1landmark_3d(const py::EigenDRef<const Mat31> obs, uint_t nodePoseId,
                uint_t nodeLandmarkId, const py::EigenDRef<const Mat3> obsInvCov, bool initializeLandmark)
    {
        auto n1 = this->get_node(nodePoseId);
        auto n2 = this->get_node(nodeLandmarkId);
        std::shared_ptr<mrob::Factor> f(new mrob::Factor1Pose1Landmark3d(obs,n1,n2,obsInvCov,initializeLandmark, robust_type_));
        this->add_factor(f);
        return f->get_id();
    }

    // 2D Landmarks
    // ------------------------------------------------------------------------------------
    factor_id_t add_node_landmark_2d(const py::EigenDRef<const Mat21> x)
    {
        std::shared_ptr<mrob::Node> n(new mrob::NodeLandmark2d(x));
        this->add_node(n);
        return n->get_id();
    }
    factor_id_t add_factor_1pose_1landmark_2d(const py::EigenDRef<const Mat21> obs, uint_t nodePoseId,
                uint_t nodeLandmarkId, const py::EigenDRef<const Mat2> obsInvCov, bool initializeLandmark)
    {
        auto n1 = this->get_node(nodePoseId);
        auto n2 = this->get_node(nodeLandmarkId);
        std::shared_ptr<mrob::Factor> f(new mrob::Factor1Pose1Landmark2d(obs,n1,n2,obsInvCov,initializeLandmark, robust_type_));
        this->add_factor(f);
        return f->get_id();
    }

    // Planes in 4d, i.e. pi = [nx.ny,nz,d] \in P^3
    // There is a problem with scaling of long distances (d->inf n->0), but well, this
    // is not a minimal representation. For finite distances should be fine.
    // ------------------------------------------------------------------------------------
    factor_id_t add_node_plane_4d(const py::EigenDRef<const Mat41> x)
    {
        std::shared_ptr<mrob::Node> n(new mrob::NodePlane4d(x));
        this->add_node(n);
        return n->get_id();
    }
    factor_id_t add_factor_1pose_1plane_4d(const py::EigenDRef<const Mat41> obs, uint_t nodePoseId,
                uint_t nodeLandmarkId, const py::EigenDRef<const Mat4> obsInvCov)
    {
        auto n1 = this->get_node(nodePoseId);
        auto n2 = this->get_node(nodeLandmarkId);
        std::shared_ptr<mrob::Factor> f(new mrob::Factor1Pose1Plane4d(obs,n1,n2,obsInvCov, robust_type_));
        this->add_factor(f);
        return f->get_id();
    }

    // point to plane and p2p optimizations. Variants of weighted ICP
    // ----------------------------------------------------
    // point to Plane factor
    factor_id_t add_factor_1pose_point2plane(const py::EigenDRef<const Mat31> z_point_x, const py::EigenDRef<const Mat31> z_point_y,
            const py::EigenDRef<const Mat31> z_normal_y, factor_id_t nodePoseId, const py::EigenDRef<const Mat1> obsInf)
    {
        auto n1 = this->get_node(nodePoseId);
        std::shared_ptr<mrob::Factor> f(new mrob::Factor1PosePoint2Plane(z_point_x,z_point_y, z_normal_y,n1,obsInf, robust_type_));
        this->add_factor(f);
        return f->get_id();
    }
private:
    mrob::Factor::robustFactorType robust_type_;
};

void init_FGraph(py::module &m)
{
    py::enum_<FGraphSolve::optimMethod>(m, "FGraph.optimMethod")
        .value("GN", FGraphSolve::optimMethod::GN)
        .value("LM", FGraphSolve::optimMethod::LM)
        .export_values()
        ;
    py::enum_<Factor::robustFactorType>(m, "FGraph.robutsFactorType")
        .value("QUADRATIC", Factor::robustFactorType::QUADRATIC)
        .value("CAUCHY", Factor::robustFactorType::CAUCHY)
        .value("HUBER", Factor::robustFactorType::HUBER)
        .value("MCCLURE", Factor::robustFactorType::MCCLURE)
        .value("RANSAC", Factor::robustFactorType::RANSAC)
        .export_values()
        ;
    // Fgraph class adding factors and providing method to solve the inference problem.
    py::class_<FGraphPy> (m,"FGraph")
            .def(py::init<Factor::robustFactorType>(),
                    "Constructor, solveType default is ADJ and robust factor is quadratic.",
                    py::arg("robust_type") =  Factor::robustFactorType::QUADRATIC)
            .def("solve", &FGraphSolve::solve,
                    "Solves the corresponding FG.\n"
                    "Options:\n method = mrob.GN (Gauss Newton), by default option. It carries out a SINGLE iteration.\n"
                    "                  = mrob.LM (Levenberg-Marquard), it has several parameters:\n"
                    " - marIters = 30 (by default). Only for LM\n"
                    " - lambda = 1-6, LM paramter for the size of the update\n"
                    " - solutionTolerance: convergence criteria.",
                    py::arg("method") =  FGraphSolve::optimMethod::GN,
                    py::arg("maxIters") = 30,
                    py::arg("lambda") = 1e-6,
                    py::arg("solutionTolerance") = 1e-2)
            .def("chi2", &FGraphSolve::chi2,
                    "Calculated the chi2 of the problem.\n"
                    "By default re-evaluates residuals, \n"
                    "if set to false if doesn't:    evaluateResidualsFlag = False",
                    py::arg("evaluateResidualsFlag") = true)
            .def("get_estimated_state", &FGraphSolve::get_estimated_state,
                    "returns the list of states ordered according to ids.\n"
                    "Each state can be of different size and some of these elements might be matrices if the are 3D poses")
            .def("get_information_matrix", &FGraphSolve::get_information_matrix,
                    "Returns the information matrix (sparse matrix). It requires to be calculated -> solved the problem",
                    py::return_value_policy::copy)
            .def("get_adjacency_matrix", &FGraphSolve::get_adjacency_matrix,
                    "Returns the adjacency matrix (sparse matrix). It requires to be calculated -> solved the problem",
                    py::return_value_policy::copy)
            .def("get_W_matrix", &FGraphSolve::get_W_matrix,
                    "Returns the W matrix of observation noises(sparse matrix). It requires to be calculated -> solved the problem",
                    py::return_value_policy::copy)
            .def("get_vector_b", &FGraphSolve::get_vector_b,
                    "Returns the vector  b = A'Wr, from residuals. It requires to be calculated -> solved the problem",
                    py::return_value_policy::copy)
            .def("get_chi2_array", &FGraphSolve::get_chi2_array,
                    "Returns the vector of chi2 values for each factor. It requires to be calculated -> solved the problem",
                    py::return_value_policy::copy)
            .def("number_nodes", &FGraphSolve::number_nodes, "Returns the number of nodes")
            .def("number_factors", &FGraphSolve::number_factors, "Returns the number of factors")
            .def("print", &FGraph::print, "By default False: does not print all the information on the Fgraph", py::arg("completePrint") = false)
            // Robust factors GUI
            // TODO, we want to set a default robust function? maybe at ini?
            // TODO we want a way to change the robust factor for each node, maybe accesing by id? This could be away to inactivate factors...
            // -----------------------------------------------------------------------------
            // Specific call to 2D
            .def("add_node_pose_2d", &FGraphPy::add_node_pose_2d,
                    " - arguments, initial estimate (np.zeros(3)\n"
                    "output, node id, for later usage")
            .def("add_factor_1pose_2d", &FGraphPy::add_factor_1pose_2d)
            .def("add_factor_2poses_2d", &FGraphPy::add_factor_2poses_2d,
                    "Factors connecting 2 poses. If last input set to true (by default false), also updates "
                    "the value of the target Node according to the new obs + origin node",
                    py::arg("obs"),
                    py::arg("nodeOriginId"),
                    py::arg("nodeTargetId"),
                    py::arg("obsInvCov"),
                    py::arg("updateNodeTarget") = false)
            .def("add_factor_2poses_2d_odom", &FGraphPy::add_factor_2poses_2d_odom,
                    "add_factor_2poses_2d_odom(obs, nodeOriginId, nodeTargetId, W)"
                    "\nFactor connecting 2 poses, following an odometry model."
                    "\nArguments are obs, nodeOriginId, nodeTargetId and obsInvCov",
                    py::arg("obs"),
                    py::arg("nodeOriginId"),
                    py::arg("nodeTargetId"),
                    py::arg("obsInvCov"))
            // 2d Landmkarks
            .def("add_node_landmark_2d", &FGraphPy::add_node_landmark_2d,
                    "Landmarks are 2D points, in [x,y]. It requries initialization, "
                    "although factor 1pose 1land 2d can initialize with the inverse observation function")
            .def("add_factor_1pose_1landmark_2d", &FGraphPy::add_factor_1pose_1landmark_2d,
                    "Factor connecting 1 pose and 1 point (landmark).",
                    py::arg("obs"),
                    py::arg("nodePoseId"),
                    py::arg("nodeLandmarkId"),
                    py::arg("obsInvCov"),
                    py::arg("initializeLandmark") = false)
            // -----------------------------------------------------------------------------
            // Specific call to 3D
            .def("add_node_pose_3d", &FGraphPy::add_node_pose_3d,
                    "Input are 3D poses, as Lie Algebra of RBT around the Identity")
            .def("add_factor_1pose_3d", &FGraphPy::add_factor_1pose_3d)
            .def("add_factor_2poses_3d", &FGraphPy::add_factor_2poses_3d,
                            "Factors connecting 2 poses. If last input set to true (by default false), also updates the value of the target Node according to the new obs + origin node",
                            py::arg("obs"),
                            py::arg("nodeOridingId"),
                            py::arg("nodeTargetId"),
                            py::arg("obsInvCov"),
                            py::arg("updateNodeTarget") = false)
            // -----------------------------------------------------------------------------
            // Landmark or Point 3D
            .def("add_node_landmark_3d", &FGraphPy::add_node_landmark_3d,
                    "Ladmarks are 3D points, in [x,y,z]")
            .def("add_factor_1pose_1landmark_3d", &FGraphPy::add_factor_1pose_1landmark_3d,
                            "Factor connecting 1 pose and 1 point (landmark).",
                            py::arg("obs"),
                            py::arg("nodePoseId"),
                            py::arg("nodeLandmarkId"),
                            py::arg("obsInvCov"),
                            py::arg("initializeLandmark") = false)
            // -----------------------------------------------------------------------------
            // Plane 4d Landmark to Pose 3D
            .def("add_node_plane_4d", &FGraphPy::add_node_plane_4d,
                    "Panes are points in P^3, in [nx,ny,nz, d]")
            .def("add_factor_1pose_1plane_4d", &FGraphPy::add_factor_1pose_1plane_4d,
                            "Factor observing a plane(landmark) from the current pose.",
                            py::arg("obs"),
                            py::arg("nodePoseId"),
                            py::arg("nodeLandmarkId"),
                            py::arg("obsInvCov"))
            // ------------------------------------------------------------------------------
            // point to plane registration
            .def("add_factor_1pose_point2plane", &FGraphPy::add_factor_1pose_point2plane,
                     "Factor measuring point to plane distance in a registration problem",
                            py::arg("z_point_x"),
                            py::arg("z_point_y"),
                            py::arg("z_normal_y"),
                            py::arg("nodePoseId"),
                            py::arg("obsInf"))
            ;

}


#pragma once
#include <Eigen/Dense>
#include "autd3.hpp"
#include "arfModel.hpp"

namespace dynaman {

	class MuxThrustSearcher {
	public:

		MuxThrustSearcher(
			autd::GeometryPtr geo,
			std::shared_ptr<arfModelLinearBase> arf_model,
			int num_time_slices,
			float duty_max = 1.0f
		);

		Eigen::VectorXf Search(
			const Eigen::Vector3f& pos,
			const Eigen::Vector3f& direction
			);

	private:
		float MaximizeThrust(
			const Eigen::Vector3f& pos,
			const Eigen::Vector3f& direction,
			const std::vector<size_t>& indexes_autd,
			Eigen::VectorXf& duty
		);
		int m_num_time_slices;
		Eigen::MatrixXf m_centers_autd;
		std::vector<Eigen::Matrix3f> m_rots_autd;
		std::shared_ptr<arfModelLinearBase> m_arf_model;
		float m_duty_max;
		std::vector<std::vector<size_t>> m_autd_comb;
	};

	Eigen::VectorXf MaximizeThrust(
		const Eigen::Vector3f& pos,
		const Eigen::Vector3f& direction,
		const float duty_max,
		autd::GeometryPtr geo,
		std::shared_ptr<arfModelLinearBase> arf_model
	);

	float MaximumThrust(
		const Eigen::Vector3f& pos,
		const Eigen::Vector3f& direction,
		const float duty_max,
		autd::GeometryPtr geo,
		std::shared_ptr<arfModelLinearBase> arfModel
	);

	float MaximumThrust(
		const Eigen::Vector3f& start,
		const Eigen::Vector3f& end,
		const float duty_max,
		const int num_points,
		autd::GeometryPtr geo,
		std::shared_ptr<arfModelLinearBase> arfModel
	);

}


#include <boost/ut.hpp>// single header
// import boost.ut;        // single module (C++20)
#include "tl/utility.hpp"
#include <string_view>
int
  main()
{
  using namespace boost::ut::literals;
  using namespace boost::ut::operators::terse;
  using namespace boost::ut;
  using namespace std::string_view_literals;
  using namespace std::string_literals;
  [[maybe_unused]] suite sequence = [] {
    "create random char array"_test = [] {
      auto s = tl::utility::sequence<0, 10>();
      s([]<auto I>() {
        expect(le(I, 10U));
        expect(ge(I, 0U));
      });
    };
  };
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    ScenarioRunner.h
 * @brief   Simple class to test navigation scenarios
 * @author  Frank Dellaert
 */

#include <gtsam/navigation/ScenarioRunner.h>
#include <gtsam/base/timing.h>

#include <boost/assign.hpp>
#include <cmath>

using namespace std;
using namespace boost::assign;

namespace gtsam {

static double intNoiseVar = 0.0000001;
static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;

PreintegratedImuMeasurements ScenarioRunner::integrate(
    double T, const Bias& estimatedBias, bool corrupted) const {
  PreintegratedImuMeasurements pim(p_, estimatedBias);

  const double dt = imuSampleTime();
  const size_t nrSteps = T / dt;
  double t = 0;
  for (size_t k = 0; k < nrSteps; k++, t += dt) {
    Vector3 measuredOmega =
        corrupted ? measuredAngularVelocity(t) : actualAngularVelocity(t);
    Vector3 measuredAcc =
        corrupted ? measuredSpecificForce(t) : actualSpecificForce(t);
    pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
  }

  return pim;
}

NavState ScenarioRunner::predict(const PreintegratedImuMeasurements& pim,
                                 const Bias& estimatedBias) const {
  const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0));
  return pim.predict(state_i, estimatedBias);
}

Matrix9 ScenarioRunner::estimateCovariance(double T, size_t N,
                                           const Bias& estimatedBias) const {
  gttic_(estimateCovariance);

  // Get predict prediction from ground truth measurements
  NavState prediction = predict(integrate(T));

  // Sample !
  Matrix samples(9, N);
  Vector9 sum = Vector9::Zero();
  for (size_t i = 0; i < N; i++) {
    auto pim = integrate(T, estimatedBias, true);
    NavState sampled = predict(pim);
    Vector9 xi = sampled.localCoordinates(prediction);
    samples.col(i) = xi;
    sum += xi;
  }

  // Compute MC covariance
  Vector9 sampleMean = sum / N;
  Matrix9 Q;
  Q.setZero();
  for (size_t i = 0; i < N; i++) {
    Vector9 xi = samples.col(i) - sampleMean;
    Q += xi * xi.transpose();
  }

  return Q / (N - 1);
}

Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const {
  Matrix samples(6, N);
  Vector6 sum = Vector6::Zero();
  for (size_t i = 0; i < N; i++) {
    samples.col(i) << accSampler_.sample() / sqrt_dt_,
        gyroSampler_.sample() / sqrt_dt_;
    sum += samples.col(i);
  }

  // Compute MC covariance
  Vector6 sampleMean = sum / N;
  Matrix6 Q;
  Q.setZero();
  for (size_t i = 0; i < N; i++) {
    Vector6 xi = samples.col(i) - sampleMean;
    Q += xi * xi.transpose();
  }

  return Q / (N - 1);
}

}  // namespace gtsam


#pragma once

#include <gtest/gtest.h>
#include <Eigen/Core>

#define ASSERT_MATRICES_EQ_WITH_NAN(first, second) assertMatrixesEqualWithNan((first), #first, (second), #second, __LINE__)
static void assertMatrixesEqualWithNan(Eigen::Ref<const Eigen::MatrixXf> first, std::string firstName,
                                       Eigen::Ref<const Eigen::MatrixXf> second, std::string secondName, int line) {
  ASSERT_EQ(first.rows(), second.rows());
  ASSERT_EQ(first.cols(), second.cols());

  bool matricesAreEqual = true;
  for (Eigen::Index row = 0; row < first.rows() && matricesAreEqual; ++row) {
    for (Eigen::Index col = 0; col < first.cols() && matricesAreEqual; ++col) {
      bool ifRealThenValid = first.block<1, 1>(row, col).isApprox(second.block<1, 1>(row, col));
      bool bothNaN = std::isnan(first(row, col)) && std::isnan(second(row, col));
      if (ifRealThenValid || bothNaN) {
        continue;
      } else {
        matricesAreEqual = false;
      }
    }
  }

  Eigen::IOFormat compactFormat(2, 0, ",", "\n", "[", "]");
  ASSERT_TRUE(matricesAreEqual)                                       // NO LINT
      << "L. " << std::to_string(line) << ": Matrices are not equal"  // NO LINT
      << "\n"                                                         // NO LINT
      << firstName << "\n"                                            // NO LINT
      << first.format(compactFormat) << "\n"                          // NO LINT
      << secondName << "\n"                                           // NO LINT
      << second.format(compactFormat) << "\n";                        // NO LINT
}


//
// Created by Dominik Krupke, http://krupke.cc on 10/7/17.
//

#include <gtest/gtest.h>
#include <boost/graph/adjacency_list.hpp>
#include "utils.h"

TEST(Utils, IsEdgeCovered)
{
  using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS>;
  Graph graph{6};
  auto e0 = boost::add_edge(0,3, graph).first;
  boost::add_edge(0,4, graph);
  boost::add_edge(0,5, graph);
  boost::add_edge(1,3, graph);
  boost::add_edge(1,4, graph);
  boost::add_edge(1,5, graph);
  boost::add_edge(2,3, graph);
  boost::add_edge(2,4, graph);
  auto e1 = boost::add_edge(2,5, graph).first;

  std::vector<boost::graph_traits<Graph>::vertex_descriptor> vertex_cover;
  vertex_cover.push_back(0);
  vertex_cover.push_back(1);

  ASSERT_TRUE(bipartvc::details::is_edge_covered(e0, graph, vertex_cover));
  ASSERT_FALSE(bipartvc::details::is_edge_covered(e1, graph, vertex_cover));
}


/*
 *   This file is part of SRS project.
 *
 *   SRS is free software: you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation, either version 3 of the License, or
 *   (at your option) any later version.
 *
 *   SRS is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with SRS. If not, see <http://www.gnu.org/licenses/>.
 *
 *   Created by: Yifang Sun, Jianbin Qin
 *   Last modified by: Yifang Sun, Jianbin Qin
 */

#include <stdio.h>
#include <math.h>
#include <cstdlib>
#include <fstream>
#include <getopt.h>
#include <boost/math/distributions/chi_squared.hpp>

void usage();
void cal_para_given_t(int, double, double, double);
void cal_para_given_m(int, int, double, double);

int main(int argc, char* argv[]) {
  const struct option longopts[] = {
      { "help", no_argument, 0, 'h' },
      { "page-size", required_argument, 0, 'b' },
      { "m", required_argument, 0, 'm' },
      { "cardinality", required_argument, 0, 'n' },
      { "approximation-ratio", required_argument, 0, 'c' },
      { "max-number-of-points", required_argument, 0, 't' },
      { "success-probability", required_argument, 0, 'p' },
      { 0, 0, 0, 0 },
  };

  int index;
  int iarg = 0;
  opterr = 1;    //getopt error message (off: 0)

  double c = 4.0;  //default value
  double success_prob = 0.132121;  // default value: 1/2 - 1/e
  double max_f = 0;
  int m = -1;
  int n = -1;

  bool is_valid_command = true;

  while (iarg != -1) {
    iarg = getopt_long(argc, argv, "m:n:c:t:p:h", longopts, &index);

    switch (iarg) {
      case 'c':
        if (optarg) {
          c = atof(optarg);
        }
        break;
      case 'h':
        usage();
        return 0;
      case 'm':
        if (optarg) {
          m = atoi(optarg);
        }
        break;
      case 'n':
        if (optarg) {
          n = atoi(optarg);
        }
        break;
      case 'p':
        if (optarg) {
          success_prob = atof(optarg);
        }
        break;
      case 't':
        if (optarg) {
          max_f = atof(optarg);
        }
        break;
    }
  }

  if (n <= 0 || success_prob < 0 || success_prob > 1 || c < 1 || max_f < 0
      || max_f > 1 || m < 0 || (max_f != 0 && m != -1)
      || (max_f == 0 && m == -1)) {
    is_valid_command = false;
  } else if (m == -1) {
    cal_para_given_t(n, max_f, c, success_prob);
  } else {
    cal_para_given_m(n, m, c, success_prob);
  }

  if (!is_valid_command) {
    usage();
  }

  return 0;
}

/*
 * Given: c, T/n, n, p_\tau
 * Output: minimum m, p_\tau', and T'
 * Algorithm 6 in the paper
 * */
void cal_para_given_t(int n, double max_f, double c, double sp) {
  int m = 0;
  bool find = false;
  while (!find) {
    m++;
    boost::math::chi_squared chi(m);
    double prev_t = 1.0;
    // check if current m valid
    for (double p = 0.00001; p < 1; p = p + 0.00001) {
      double t = boost::math::cdf(chi, boost::math::quantile(chi, p) / c / c)
          / (p - sp);  //quantile equals to inverse_cdf
      if (t > 0 && t < 1) {
        if (t < max_f) {
          find = true;
          break;
        }
        if (t > prev_t) {
          break;
        }
        prev_t = t;
      }
    }
  }
  if (m > 30) {  // if m is too large, warn user
    printf(
        "Warning: m is too large, please consider enlarge c or t, reduce p\n");
  }
  printf("A feasible setting is:\nm = %d\n", m);

  double prev_t = 1.0;
  boost::math::chi_squared chi(m);
  // find the best threshold for a given m (to minimize T)
  for (double p = 0.000001; p < 1; p += 0.000001) {
    double t = boost::math::cdf(chi, boost::math::quantile(chi, p) / c / c)
        / (p - sp);
    if (t > 0 && t < 1) {
      if (t > prev_t) {
        printf("prob_thres(-r) = %f\nT_max(-t) = %d\nt = %f\n", p, (int) (n * t) + 1,
               t);
        break;
      }
      prev_t = t;
    }
  }
}

/*
 * Given: c, m, n, p_\tau
 * Output: p_\tau' and T'
 * Different from Algorithm 6.
 */
void cal_para_given_m(int n, int m, double c, double sp) {
  double t = 0, p = 0;
  bool find = false;

  boost::math::chi_squared chi(m);
  double prev_t = 1.0;

  // find the best threshold for a given m (to minimize T)
  for (p = 0.000001; p < 1; p += 0.000001) {
    t = boost::math::cdf(chi, boost::math::quantile(chi, p) / c / c) / (p - sp);
    if (t > 0 && t < 1) {
      if (t > prev_t && t < 1) {
        find = true;
        break;
      }
      prev_t = t;
    }
  }

  if (!find) {  // it is possible that given m doesn't work
    printf(
        "Error: Cannot find valid setting. Please consider enlarge c or m, or reduce p\n");
    return;
  }

  printf("A feasible setting is:\nm = %d\n", m);
  printf("prob_thres(-r) = %f\nT_max(-t) = %d\nt = %f\n", p, (int) (n * t) + 1, t);
}

void usage() {
  printf("SRS-Mem (v1.0)\n");
  printf("Options\n");
  printf("-c {value}\tapproximation ratio (>= 1), default value: 4\n");
  printf("-p {value}\tsuccess probability, default value: 1/2 - 1/e\n");
  printf("-m {value}\tdimensionality of the projected space\n");
  printf("-n {value}\tcardinality\n");
  printf("-t {value}\tmaximum fraction of verify points\n");
  printf("\n");

  printf("Usage:\n");
  printf("Calculate a feasible setting of parameters (given m)\n");
  printf("[-c] -m -n [-p]\n");
  printf("Calculate a feasible setting of parameters (given t)\n");
  printf("[-c] -n -p [-t]\n");
}


/* CopyrighT (C) 2014 InfiniDB, Inc.
   Copyright (c) 2019 MariaDB Corporation

   This program is free software; you can redistribute it and/or
   modify it under the terms of the GNU General Public License
   as published by the Free Software Foundation; version 2 of
   the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
   MA 02110-1301, USA. */

//  $Id: wf_sum_avg.cpp 3932 2013-06-25 16:08:10Z xlou $

//#define NDEBUG
#include <cassert>
#include <cmath>
#include <sstream>
#include <iomanip>
using namespace std;

#include <boost/shared_ptr.hpp>
using namespace boost;

#include "loggingid.h"
#include "errorcodes.h"
#include "idberrorinfo.h"
using namespace logging;

#include "rowgroup.h"
using namespace rowgroup;

#include "idborderby.h"
using namespace ordering;

#include "joblisttypes.h"
#include "calpontsystemcatalog.h"
using namespace execplan;

#include "windowfunctionstep.h"
using namespace joblist;

#include "wf_sum_avg.h"

namespace windowfunction
{

template<typename T_IN, typename T_OUT>
inline void WF_sum_avg<T_IN,T_OUT>::checkSumLimit(const T_IN& val,
    const T_OUT& sum)
{ }

template<>
inline void WF_sum_avg<int128_t,int128_t>::checkSumLimit(const int128_t& val,
    const int128_t& sum)
{
    datatypes::AdditionOverflowCheck ofCheckOp;
    ofCheckOp(sum, val);
}

template<>
inline void WF_sum_avg<long double,long double>::checkSumLimit(const long double& val,
    const long double& sum)
{ }

template<>
inline void WF_sum_avg<float, long double>::checkSumLimit(const float&,
    const long double&)
{ }

template<>
inline void WF_sum_avg<long, long double>::checkSumLimit(const long&,
    const long double&)
{ }

template<>
inline void WF_sum_avg<unsigned long, long double>::checkSumLimit(const unsigned long&,
    const long double&)
{ }
template<>
inline void WF_sum_avg<double, long double>::checkSumLimit(const double&,
    const long double&)
{ }

template<>
void WF_sum_avg<int128_t,int128_t>::checkSumLimit(const int128_t&, const int128_t&);
template<>
void WF_sum_avg<long double,long double>::checkSumLimit(const long double& val, const long double&);
template<>
void WF_sum_avg<float, long double>::checkSumLimit(const float&, const long double&);
template<>
void WF_sum_avg<long, long double>::checkSumLimit(const long&, const long double&);
template<>
void WF_sum_avg<unsigned long, long double>::checkSumLimit(const unsigned long&,
    const long double&);
template<>
void WF_sum_avg<double, long double>::checkSumLimit(const double&, const long double&);

template<typename T_IN, typename T_OUT>
int128_t WF_sum_avg<T_IN, T_OUT>::calculateAvg(const int128_t& sum,
    const uint64_t count,
    const int scale)
{
    int128_t avg = 0;
    int128_t factor;
    datatypes::getScaleDivisor(factor, scale);
    if (scale > 0)
    {
        if ((sum * factor) / factor == sum)
        {
            avg = sum * factor;
            avg /= count;
        }
        else
        {
            // scale won't fit before divide, we're gonna lose precision.
            avg = sum / count;
            if ((avg * factor) / factor != avg) // Still won't fit
            {
                string errStr = string("AVG(int)");
                errStr = IDBErrorInfo::instance()->errorMsg(ERR_WF_OVERFLOW, errStr);
                cerr << errStr << endl;
                throw IDBExcept(errStr, ERR_WF_OVERFLOW);
            }
            avg *= factor;
        }
    }
    else
    {
        avg = sum / count;
    }

    avg += (avg < 0) ? (-0.5) : (0.5);
    return avg;
}

template<typename T_IN, typename T_OUT>
inline long double WF_sum_avg<T_IN, T_OUT>::calculateAvg(const long double& sum,
    const uint64_t count,
    const int scale)
{
    return sum / count;
}

// For the static function makeFunction, the template parameters are ignored
template<typename T_IN, typename T_OUT>
boost::shared_ptr<WindowFunctionType> WF_sum_avg<T_IN, T_OUT>::makeFunction(int id, const string& name, int ct, WindowFunctionColumn* wc)
{
    boost::shared_ptr<WindowFunctionType> func;
    switch (ct)
    {
        case CalpontSystemCatalog::TINYINT:
        case CalpontSystemCatalog::SMALLINT:
        case CalpontSystemCatalog::MEDINT:
        case CalpontSystemCatalog::INT:
        case CalpontSystemCatalog::BIGINT:
        {
            // Look into using int128_t instead of long double
            func.reset(new WF_sum_avg<int64_t, long double>(id, name));
            break;
        }

        case CalpontSystemCatalog::UTINYINT:
        case CalpontSystemCatalog::USMALLINT:
        case CalpontSystemCatalog::UMEDINT:
        case CalpontSystemCatalog::UINT:
        case CalpontSystemCatalog::UBIGINT:
        {
            func.reset(new WF_sum_avg<uint64_t, long double>(id, name));
            break;
        }

        case CalpontSystemCatalog::DECIMAL:
        case CalpontSystemCatalog::UDECIMAL:
        {
            decltype(datatypes::MAXDECIMALWIDTH) width =
                wc->functionParms()[0]->resultType().colWidth;

            if (width < datatypes::MAXDECIMALWIDTH)
            {
                if (ct == CalpontSystemCatalog::UDECIMAL)
                    func.reset(new WF_sum_avg<uint64_t, int128_t>(id, name));
                else
                    func.reset(new WF_sum_avg<int64_t, int128_t>(id, name));
            }
            else if (width == datatypes::MAXDECIMALWIDTH)
            {
                func.reset(new WF_sum_avg<int128_t, int128_t>(id, name));
            }
            break;
        }

        case CalpontSystemCatalog::DOUBLE:
        case CalpontSystemCatalog::UDOUBLE:
        {
            func.reset(new WF_sum_avg<double, long double>(id, name));
            break;
        }

        case CalpontSystemCatalog::FLOAT:
        case CalpontSystemCatalog::UFLOAT:
        {
            func.reset(new WF_sum_avg<float, long double>(id, name));
            break;
        }

        case CalpontSystemCatalog::LONGDOUBLE:
        {
            func.reset(new WF_sum_avg<long double, long double>(id, name));
            break;
        }
        default:
        {
            string errStr = name + "(" + colType2String[ct] + ")";
            errStr = IDBErrorInfo::instance()->errorMsg(ERR_WF_INVALID_PARM_TYPE, errStr);
            cerr << errStr << endl;
            throw IDBExcept(errStr, ERR_WF_INVALID_PARM_TYPE);

            break;
        }
    }

    return func;
}

template<typename T_IN, typename T_OUT>
WindowFunctionType* WF_sum_avg<T_IN, T_OUT>::clone() const
{
    return new WF_sum_avg<T_IN, T_OUT>(*this);
}

template<typename T_IN, typename T_OUT>
void WF_sum_avg<T_IN, T_OUT>::resetData()
{
    fAvg = 0;
    fSum = 0;
    fCount = 0;
    fSet.clear();

    WindowFunctionType::resetData();
}

template<typename T_IN, typename T_OUT>
void WF_sum_avg<T_IN, T_OUT>::operator()(int64_t b, int64_t e, int64_t c)
{
    uint64_t colOut = fFieldIndex[0];

    if ((fFrameUnit == WF__FRAME_ROWS) ||
            (fPrev == -1) ||
            (!fPeer->operator()(getPointer(fRowData->at(c)), getPointer(fRowData->at(fPrev)))))
    {
        // for unbounded - current row special handling
        if (fPrev >= b && fPrev < c)
            b = c;
        else if (fPrev <= e && fPrev > c)
            e = c;

        uint64_t colIn = fFieldIndex[1];
        int scale = fRow.getScale(colOut) - fRow.getScale(colIn);
        for (int64_t i = b; i <= e; i++)
        {
            if (i % 1000 == 0 && fStep->cancelled())
                break;

            fRow.setData(getPointer(fRowData->at(i)));

            if (fRow.isNullValue(colIn) == true)
                continue;

            CDT cdt;
            getValue(colIn, fVal, &cdt);

            if ((!fDistinct) || (fSet.find(fVal) == fSet.end()))
            {
                checkSumLimit(fVal, fSum);
                fSum += (T_OUT)fVal;
                fCount++;

                if (fDistinct)
                    fSet.insert(fVal);
            }
        }

        if ((fCount > 0) && (fFunctionId == WF__AVG || fFunctionId == WF__AVG_DISTINCT))
        {
            fAvg = calculateAvg(fSum, fCount, scale);
        }
    }

    T_OUT* v = NULL;

    if (fCount > 0)
    {
        if (fFunctionId == WF__AVG || fFunctionId == WF__AVG_DISTINCT)
            v = &fAvg;
        else
            v = &fSum;
    }

    setValue(fRow.getColType(colOut), b, e, c, v);

    fPrev = c;
}

template
boost::shared_ptr<WindowFunctionType> WF_sum_avg<int64_t, long double>::makeFunction(int, const string&, int, WindowFunctionColumn*);

}   //namespace
// vim:ts=4 sw=4:


#include <boost/geometry/formulas/thomas_direct.hpp>


//
//  dataset.hpp
//  gencoefficents
//
//  Created by Sam Mason on 04/02/2014.
//  Copyright (c) 2014 Sam Mason. All rights reserved.
//

#ifndef mdi_dataset_hpp
#define mdi_dataset_hpp

#include <iostream>
#include <vector>
#include <string>
#include <Eigen/Dense>

#include "csv.hpp"
#include "interdataset.hpp"
#include "cuda/cudasampler.hpp"

class shared {
  int _nclus;
  Eigen::MatrixXi _alloc;

public:
  shared(int K, int N, int n) : _nclus(N), _alloc(Eigen::MatrixXi::Zero(n, K)) { }

  int nitems() const { return _alloc.rows();  }
  int nfiles() const { return _alloc.cols();  }
  int nclus()  const { return _nclus; }

  // sample all state from prior --- good for initialisation!
  void sampleFromPrior();

  const Eigen::MatrixXi &allocs() const { return _alloc;  }
  typename Eigen::MatrixXi::ConstColXpr alloc(int k)  const {
    return _alloc.col(k);  }

  void setAlloc(const Eigen::MatrixXi &x);
  void setAlloc(int k, Eigen::VectorXi file);

  int alloc(int k, int i) const { return _alloc(i, k); }

  float	nuConditionalAlpha() const;
  float	phiConditionalAlpha(int k, int l) const;
  float	gammaConditionalAlpha(int k, int j) const;
};

#endif


#include "popstatem.h"
#include "seqem.h"
#include "meep_math.h"
#include "gt_matrix.h"
#include "tuple_print.h"
#include <cmath>
#include <functional>
#include <algorithm>
#include <iostream>
#include <stdexcept>
#include <cerrno>
#include <cstring>
#include <limits>
#include <boost/math/tools/roots.hpp>

typedef Popstatem::theta_t theta_t;

Popstatem::Popstatem(Pileupdata p, int ploidy) : plp(p), theta(std::make_tuple(0.1,Seqem::uniform_pi,1,0.1)),
	em(std::bind(&Popstatem::q_function, this, std::placeholders::_1), std::bind(&Popstatem::m_function,this,std::placeholders::_1), theta),
	ploidy(ploidy), m(ploidy), possible_gts(Genotype::enumerate_gts(ploidy)){
	// possible_gts = Genotype::enumerate_gts(ploidy);
}

Popstatem::Popstatem(Pileupdata p): Popstatem(p, 2){
}

Popstatem::Popstatem(std::string samfile, std::string refname, int ploidy) : plp(samfile, refname), theta(std::make_tuple(0.1,Seqem::uniform_pi,1,0.1)),
	em(std::bind(&Popstatem::q_function, this, std::placeholders::_1), std::bind(&Popstatem::m_function,this,std::placeholders::_1), theta),
	ploidy(ploidy), m(ploidy), possible_gts(Genotype::enumerate_gts(ploidy)){
	// possible_gts = Genotype::enumerate_gts(ploidy);
}

Popstatem::Popstatem(std::string samfile, std::string refname) : Popstatem(samfile, refname, 2) {
}

theta_t Popstatem::start(double stop){
	return em.start(stop);
}

double Popstatem::q_function(theta_t theta){
	double likelihood = 0.0;
	pileupdata_t plpdata = plp.get_data();

	for (pileupdata_t::iterator tid = plpdata.begin(); tid != plpdata.end(); ++tid){
		for(std::vector<pileuptuple_t>::iterator pos = tid->begin(); pos != tid->end(); ++pos){
			const std::vector<char> &x = std::get<0>(*pos);
			for (std::vector<Genotype>::iterator g = possible_gts.begin(); g != possible_gts.end(); ++g){
				likelihood += pg_x_given_theta(*g,x,theta);
			}
		}
	}
	return likelihood;
}

theta_t Popstatem::m_function(theta_t theta){
	//optimize epsilon and update gt matrix m
	pileupdata_t plpdata = plp.get_data();
	std::vector<double> s(3,0.0); //TODO:make this generic, depends on ploidy
	GT_Matrix n;
	for (auto tid : plpdata){
		for(auto pos : tid){
			std::vector<char> x = std::get<0>(pos);
			char ref = std::get<3>(pos);
			double eps = std::get<3>(theta);
			std::map<char,double> pi = std::get<1>(theta);
			Seqem::increment_s(s, x, possible_gts, std::make_tuple(eps),pi);
			load_matrix(n,x,ref);
		}
	}
	double epsilon = Seqem::calc_epsilon(s);

	m = n; //this must be set before theta, w, and pi can be optimized
	std::cout << m << std::endl;

	//optimize theta, w, and pi
	double th = boost::math::tools::newton_raphson_iterate([this](const double& x){return std::make_tuple(dq_dtheta(x),ddq_dtheta(x));},std::get<0>(theta),0.0,10000.0,5);
	double w = boost::math::tools::newton_raphson_iterate([this](const double& x){return std::make_tuple(dq_dw(x),ddq_dw(x));},std::get<2>(theta),-1000.0,100000.0,5);

	std::map<char,double> pi = std::get<1>(theta);
	std::map<char,double> new_pi(pi);
	double p = 0.0;
	for (size_t i = 0; i < Genotype::alleles.size()-1; ++i){
		char a = Genotype::alleles[i];
		double max_pi = 1.0;
		for (size_t j = 0; j < Genotype::alleles.size()-1; ++j){
			char p_allele = Genotype::alleles[j];
			max_pi -= (p_allele == a ? 0 : new_pi[p_allele]);
		}
		double optimum = boost::math::tools::newton_raphson_iterate([this,a](const double& x){return std::make_tuple(dq_dpi(a,x),ddq_dpi(a,x));},pi[a],0.0,max_pi,5);
		std::cout << "allele:" << a << "\tpi:" << pi[a] <<"\toptimum:" << optimum  << std::endl;
		new_pi[a] = optimum;
		p += optimum;
	}
	new_pi[Genotype::alleles.back()] = 1 - p;

	return std::make_tuple(th,new_pi,w,epsilon);
}

void Popstatem::load_matrix(GT_Matrix &m, std::vector<char> x, char ref){
	for (auto g : possible_gts){
		double pg_x = pg_x_given_theta(g,x,theta);
		double pdata = pdata_given_theta(x,theta,possible_gts);
		m(ref,g) += (pg_x / pdata);
		// m(ref,g) += pg_x;
	}
}

double Popstatem::dq_dtheta(double th){
	std::map<char,double> pi = std::get<1>(theta);
	double refweight = std::get<2>(theta);
	double dq = 0.0;
	int numalleles = Genotype::alleles.size();
	int numgts = possible_gts.size();
	for (int i = 0; i < numalleles; ++i){ //for each reference base
		for (int j = 0; j < numgts; ++j){ //for each genotype
			Genotype g = possible_gts[j];
			for (auto it = g.gt.begin(); it != g.gt.end(); ++it){ //for each base in genotype
				char allele = it -> first;
				for (int k = 0; k < it->second; ++k){ //add pi/(alpha + 0) twice for het; pi/(alpha + 1) for homozygote
					dq += m[i][j] * (pi[allele]/(allele_alpha(allele,Genotype::alleles[i],refweight,th,pi) + k));
				}
			}
			for (int l = 0; l < ploidy; ++l){ //subtract alpha, alpha + 1
				dq -= m[i][j] * (1.0 / ref_alpha(refweight, th) + l);
			}
		}
	}
	return dq;
}

double Popstatem::ddq_dtheta(double th){
	double ddq = 0.0;
	std::map<char,double> pi = std::get<1>(theta);
	double refweight = std::get<2>(theta);
	int numalleles = Genotype::alleles.size();
	int numgts = possible_gts.size();
	for (int i = 0; i < numalleles; ++i){ //for each reference base
		for (int j = 0; j < numgts; ++j){ //for each genotype
			Genotype g = possible_gts[j];
			for (int l = 0; l < ploidy; ++l){ //add  1/alpha, 1/(alpha + 1)
				ddq += m[i][j] * (1.0 / pow(ref_alpha(refweight, th) + l,2));
			}
			for (auto it = g.gt.begin(); it != g.gt.end(); ++it){ //for each base in genotype
				char allele = it -> first;
				for (int k = 0; k < it->second; ++k){ //add pi/(alpha + 0) twice for het; pi/(alpha + 1) for homozygote
					ddq -= m[i][j] * (pow(pi[allele],2)/pow(allele_alpha(allele,Genotype::alleles[i],refweight,th,pi) + k,2));
				}
			}
		}
	}
	return ddq;
}

double Popstatem::dq_dw(double w){
	double dq = 0.0;
	double th = std::get<0>(theta);
	std::map<char,double> pi = std::get<1>(theta);
	int numalleles = Genotype::alleles.size();
	int numgts = possible_gts.size();
	for (int i = 0; i < numalleles; ++i){ //for each reference base
		for (int j = 0; j < numgts; ++j){ //for each genotype
			Genotype g = possible_gts[j];
			for (auto it = g.gt.begin(); it != g.gt.end(); ++it){ //for each base in genotype
				char allele = it -> first;
				if (allele == Genotype::alleles[i]){
					for (int k = 0; k < it->second; ++k){ //add 1/(alpha + 0) or +0 and +1 for homozygote
						dq += m[i][j] * (1.0/(allele_alpha(allele,(char)Genotype::alleles[i],w,th,pi) + k));
					}
				}
			}
		}
	}
	return dq;
}

double Popstatem::ddq_dw(double w){
	double ddq = 0.0;
	double th = std::get<0>(theta);
	std::map<char,double> pi = std::get<1>(theta);
	int numalleles = Genotype::alleles.size();
	int numgts = possible_gts.size();
	for (int i = 0; i < numalleles; ++i){ //for each reference base
		for (int j = 0; j < numgts; ++j){ //for each genotype
			Genotype g = possible_gts[j];
			for (auto it = g.gt.begin(); it != g.gt.end(); ++it){ //for each base in genotype
				char allele = it -> first;
				if (allele == Genotype::alleles[i]){
					for (int k = 0; k < it->second; ++k){ //add 1/(alpha + 0) or +0 and +1 for homozygote
						ddq -= m[i][j] * (1.0/pow(allele_alpha(allele,(char)Genotype::alleles[i],w,th,pi) + k,2));
					}
				}
			}
		}
	}
	return ddq;
}

double Popstatem::dq_dpi(char a, double pi){
	double dq = 0.0;
	double th = std::get<0>(theta);
	std::map<char,double> p = std::get<1>(theta);
	double refweight = std::get<2>(theta);
	int numalleles = Genotype::alleles.size();
	int numgts = possible_gts.size();
	double pi_last = 1.0;
	for (int i = 0; i < numalleles - 1; ++i){
		char allele = Genotype::alleles[i];
		pi_last -= (allele == a ? pi : p[allele]);
	}
	// pi_last = (pi_last < 0 ? 0 : pi_last);
	for (int i = 0; i < numalleles; ++i){ //for each reference base
		for (int j = 0; j < numgts; ++j){ //for each genotype
			Genotype g = possible_gts[j];
			for (auto it = g.gt.begin(); it != g.gt.end(); ++it){ //for each base in genotype
				char allele = it -> first;
				if (allele == a){
					for (int k = 0; k < it->second; ++k){ //add 1/(alpha + 0) or +0 and +1 for homozygote
						dq += m[i][j] * th /(allele_alpha(allele,Genotype::alleles[i],refweight,th,pi) + k);
					}
				}
				else if(allele == Genotype::alleles.back()){
					for (int k = 0; k < it->second; ++k){ //add 1/(alpha + 0) or +0 and +1 for homozygote
						dq -= m[i][j] * th /(allele_alpha(allele,Genotype::alleles[i],refweight,th,pi_last) + k);
					}
				}
			}
		}
	}
	return dq;
}

double Popstatem::ddq_dpi(char a, double pi){
	double ddq = 0.0;
	double th = std::get<0>(theta);
	std::map<char,double> p = std::get<1>(theta);
	double refweight = std::get<2>(theta);
	int numalleles = Genotype::alleles.size();
	int numgts = possible_gts.size();
	double pi_last = 1.0;
	for (int i = 0; i < numalleles - 1; ++i){
		char allele = Genotype::alleles[i];
		pi_last -= (allele == a ? pi : p[allele]);
	}
	// pi_last = (pi_last < 0 ? 0 : pi_last);
	for (int i = 0; i < numalleles; ++i){ //for each reference base
		for (int j = 0; j < numgts; ++j){ //for each genotype
			Genotype g = possible_gts[j];
			for (auto it = g.gt.begin(); it != g.gt.end(); ++it){ //for each base in genotype
				char allele = it -> first;
				if (allele == a){
					for (int k = 0; k < it->second; ++k){ //add 1/(alpha + 0) or +0 and +1 for homozygote
						ddq -= m[i][j] * pow(th,2) / pow(allele_alpha(allele,Genotype::alleles[i],refweight,th,pi) + k,2);
					}
				}
				else if (allele == Genotype::alleles.back()){
					for (int k = 0; k < it->second; ++k){ //add 1/(alpha + 0) or +0 and +1 for homozygote
						ddq -= m[i][j] * pow(th,2) / pow(allele_alpha(allele,Genotype::alleles[i],refweight,th,pi_last) + k,2);
					}
				}
			}
		}
	}
	return ddq;
}

void Popstatem::apply_over_gt(std::function<void (int, int, std::vector<Genotype>::iterator)> f){
	int numalleles = Genotype::alleles.size();
	int numgts = possible_gts.size();
	for (int i = 0; i < numalleles; ++i){
		for(int j = 0; j < numgts; ++j){
			Genotype g = possible_gts[j];
			for(std::vector<Genotype>::iterator it = g.gt.begin(); it != g.gt.end(); ++it){
				f(i,j,it);
			}
		}
	}
}

double Popstatem::allele_alpha(char allele, char ref, double ref_weight, double theta, std::map<char,double> pi){
	double w = (ref == allele ? ref_weight : 0);
	return theta * pi[allele] + w;
}

double Popstatem::allele_alpha(char allele, char ref, double ref_weight, double theta, double pi){
	double w = (ref == allele ? ref_weight : 0);
	return theta * pi + w;
}
double Popstatem::ref_alpha(double ref_weight, double theta){
	return ref_weight + theta;
}

double Popstatem::pg_x_given_theta(Genotype g, std::vector<char> x, theta_t theta){
	double e = std::get<3>(theta);
	std::map<char,double> p = std::get<1>(theta);
	return Seqem::pg_x_given_theta(g,x,std::make_tuple(e),p);
}

double Popstatem::pdata_given_theta(std::vector<char> x, theta_t theta, std::vector<Genotype> gts){
	double p = 0.0;
	for (auto g : gts){
		p += pg_x_given_theta(g, x, theta);
	}
	return p;
}


/**
 * \ file UniversalFixedDelayLineFilter.cpp
 */

#include <ATK/Delay/UniversalFixedDelayLineFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <ATK/Mock/SimpleSinusGeneratorFilter.h>
#include <ATK/Mock/TriangleCheckerFilter.h>

#include <ATK/Tools/SumFilter.h>

#include <gtest/gtest.h>

#include <boost/math/constants/constants.hpp>

constexpr gsl::index PROCESSSIZE = (1024 * 64);

TEST(UniversalFixedDelayLineFilter, sinus_line100_delay50_test)
{
  std::vector<double> data(PROCESSSIZE);
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<double>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::UniversalFixedDelayLineFilter<double> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(50);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(gsl::index i = 0; i < 50; ++i)
  {
    ASSERT_EQ(0, outdata[i]);
  }

  for(gsl::index i = 50; i < PROCESSSIZE; ++i)
  {
    ASSERT_EQ(data[i - 50], outdata[i]);
  }
}

TEST(UniversalFixedDelayLineFilter, sinus_line100_delay50_progressive_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_frequency(480);

  ATK::UniversalFixedDelayLineFilter<double> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(50);
  filter.process(50);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_input_port(0, &filter, 0);
  sum.set_input_port(1, &generator, 0);

  ATK::TriangleCheckerFilter<double> output;
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &sum, 0);
  output.set_amplitude(0);

  for (std::size_t i = 1; i < 100; ++i)
  {
    output.process(i);
  }
}

TEST(UniversalFixedDelayLineFilter, sinus_line1000_delay50_progressive_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_frequency(480);

  ATK::UniversalFixedDelayLineFilter<double> filter(1000);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(50);
  filter.process(50);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_input_port(0, &filter, 0);
  sum.set_input_port(1, &generator, 0);

  ATK::TriangleCheckerFilter<double> output;
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &sum, 0);
  output.set_amplitude(0);

  for (std::size_t i = 50; i < 1000; ++i)
  {
    output.process(i);
  }
}
TEST(UniversalFixedDelayLineFilter, sinus_line25_delay24_blend_1_feedforward_1_feedback_0_test)
{
  std::vector<double> data(PROCESSSIZE);
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<double>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::UniversalFixedDelayLineFilter<double> filter(25);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(24);
  filter.set_blend(1);
  filter.set_feedback(0);
  filter.set_feedforward(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(gsl::index i = 24; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(outdata[i], 0, 0.0001);
  }
}

TEST(UniversalFixedDelayLineFilter, sinus_line25_delay24_blend_0_feedforward_0_feedback_1_test)
{
  std::vector<double> data(PROCESSSIZE);
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<double>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::UniversalFixedDelayLineFilter<double> filter(25);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(24);
  filter.set_blend(0);
  filter.set_feedback(.9999999);
  filter.set_feedforward(0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(gsl::index i = 24; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(outdata[i], 0, 0.0001);
  }
}

TEST(UniversalFixedDelayLineFilter, sinus_delay_test)
{
  ATK::UniversalFixedDelayLineFilter<double> filter(128);
  filter.set_delay(10);
  ASSERT_EQ(filter.get_delay(), 10);
}

TEST(UniversalFixedDelayLineFilter, sinus_delay_range_test)
{
  ATK::UniversalFixedDelayLineFilter<double> filter(128);
  ASSERT_THROW(filter.set_delay(0), ATK::RuntimeError);
}

TEST(UniversalFixedDelayLineFilter, sinus_delay_range2_test)
{
  ATK::UniversalFixedDelayLineFilter<double> filter(128);
  ASSERT_THROW(filter.set_delay(128), ATK::RuntimeError);
}

TEST(UniversalFixedDelayLineFilter, sinus_blend_test)
{
  ATK::UniversalFixedDelayLineFilter<double> filter(128);
  filter.set_blend(0.5);
  ASSERT_EQ(filter.get_blend(), 0.5);
}

TEST(UniversalFixedDelayLineFilter, sinus_feedforward_test)
{
  ATK::UniversalFixedDelayLineFilter<double> filter(128);
  filter.set_feedforward(0.5);
  ASSERT_EQ(filter.get_feedforward(), 0.5);
}

TEST(UniversalFixedDelayLineFilter, sinus_feedback_test)
{
  ATK::UniversalFixedDelayLineFilter<double> filter(128);
  filter.set_feedback(0.5);
  ASSERT_EQ(filter.get_feedback(), 0.5);
}

TEST(UniversalFixedDelayLineFilter, sinus_feedback_range_test)
{
  ATK::UniversalFixedDelayLineFilter<double> filter(128);
  ASSERT_THROW(filter.set_feedback(1), ATK::RuntimeError);
}

TEST(UniversalFixedDelayLineFilter, sinus_feedback_range2_test)
{
  ATK::UniversalFixedDelayLineFilter<double> filter(128);
  ASSERT_THROW(filter.set_feedback(-1), ATK::RuntimeError);
}


#include "apch.h"
#include "Cam.h"
	//#include "Apsis/Utility/Maths.h"
	//	#include <Eigen/Core>
	//	#include <Eigen/Geometry>
	//	#include <Eigen/Dense>

namespace A {

	Cam::Cam()
	{
		AP_PROFILE_FN();
	}

	Cam::Cam(const Eigen::Matrix4f& proj)
		: m_Proj(proj)
	{
		AP_PROFILE_FN();
		RefreshProj();
	}

	void Cam::RefreshTransform()
	{
		AP_PROFILE_FN();
		m_Transform = Eigen::Affine3f::Identity();
		//m_Transform.scale(m_Scale);
		m_Transform.rotate(m_Orientation);
		m_Transform.translate(m_Position);
		RefreshView();
	}

	void Cam::RefreshView()
	{
		AP_PROFILE_FN();
		m_View = m_Transform.inverse();
		RefreshVP();
	}

	void Cam::RefreshProj()
	{
		AP_PROFILE_FN();
		m_Zoom = std::max(m_Zoom, m_MinZoom);	// Clamp zoom
		m_Zoom = std::min(m_Zoom, m_MaxZoom);
		m_Proj = CreateOrthographicProjection(m_Left * 1.0f / m_Zoom, m_Right * 1.0f / m_Zoom, m_Bottom * 1.0f / m_Zoom, m_Top * 1.0f / m_Zoom, m_Near, m_Far);
		RefreshVP();
	}

	void Cam::RefreshVP()
	{
		AP_PROFILE_FN();
		m_VP = m_Proj * m_View;
	}

	OrthoCam::OrthoCam(float left, float right, float bottom, float top, float z_near, float z_far)
	{
		AP_PROFILE_FN();
		m_Left = left;
		m_Right = right;
		m_Bottom = bottom;
		m_Top = top;
		m_Near = z_near;
		m_Far = z_far;
		RefreshProj();
	}

	void OrthoCam::SetProj(float left, float right, float bottom, float top, float z_near, float z_far)
	{
		AP_PROFILE_FN();
		m_Left = left;
		m_Right = right;
		m_Bottom = bottom;
		m_Top = top;
		m_Near = z_near;
		m_Far = z_far;
		RefreshProj();
	}

}


#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>
#include <scitbx/math/r3_rotation.h>

namespace scitbx { namespace math { namespace r3_rotation {
namespace {

  struct axis_and_angle_from_matrix_wrappers
  {
    typedef axis_and_angle_from_matrix<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<return_by_value> rbv;
      class_<w_t>("r3_rotation_axis_and_angle_from_matrix", no_init)
        .def(init<mat3<double> const&>((arg("r"))))
        .add_property("axis", make_getter(&w_t::axis, rbv()))
        .def("angle", &w_t::angle, (arg("deg")=false))
        .def("as_matrix", &w_t::as_matrix)
        .def("as_unit_quaternion", &w_t::as_unit_quaternion)
      ;
    }
  };

}} // namespace <anonymous>::r3_rotation

namespace boost_python {

  void wrap_r3_rotation()
  {
    using namespace boost::python;
    def("r3_rotation_axis_and_angle_as_matrix",
      (mat3<double>(*)(
        vec3<double> const&, double, bool deg, double const&))
      r3_rotation::axis_and_angle_as_matrix, (
        arg("axis"),
        arg("angle"),
        arg("deg")=false,
        arg("min_axis_length")=1e-15));
    def("r3_rotation_axis_and_angle_as_matrix",
      (af::shared<mat3<double> >(*)(
        af::shared<vec3<double> > const&,
        af::shared<double> const&,
        bool,
        double const&)
      ) r3_rotation::axis_and_angle_as_matrix, (
        arg("axes"),
        arg("angle"),
        arg("deg")=false,
        arg("min_axis_length")=1e-15));

    def("r3_rotation_axis_and_angle_as_unit_quaternion",
      (af::tiny<double, 4>(*)(
        vec3<double> const&, double, bool deg, double const&))
      r3_rotation::axis_and_angle_as_unit_quaternion, (
        arg("axis"),
        arg("angle"),
        arg("deg")=false,
        arg("min_axis_length")=1e-15));

    r3_rotation::axis_and_angle_from_matrix_wrappers::wrap();

    def("r3_rotation_vector_to_vector",
      (mat3<double>(*)(
        vec3<double> const&,
        vec3<double> const&,
        double const&))
      r3_rotation::vector_to_vector, (
        arg("given_unit_vector"),
        arg("target_unit_vector"),
        arg("sin_angle_is_zero_threshold")=1e-10));

    def("r3_rotation_vector_to_001",
      (mat3<double>(*)(
        vec3<double> const&,
        double const&))
      r3_rotation::vector_to_001, (
        arg("given_unit_vector"),
        arg("sin_angle_is_zero_threshold")=1e-10));

    def("r3_rotation_vector_to_010",
      (mat3<double>(*)(
        vec3<double> const&,
        double const&))
      r3_rotation::vector_to_010, (
        arg("given_unit_vector"),
        arg("sin_angle_is_zero_threshold")=1e-10));

    def("r3_rotation_vector_to_100",
      (mat3<double>(*)(
        vec3<double> const&,
        double const&))
      r3_rotation::vector_to_100, (
        arg("given_unit_vector"),
        arg("sin_angle_is_zero_threshold")=1e-10));

    def("r3_rotation_unit_quaternion_as_matrix",
      (mat3<double>(*)(af::tiny<double, 4> const&))
        r3_rotation::unit_quaternion_as_matrix, (arg("q")));
    def("r3_rotation_matrix_as_unit_quaternion",
      (af::tiny<double, 4>(*)(mat3<double> const&))
        r3_rotation::matrix_as_unit_quaternion, (arg("r")));
  }

}}} // namespace scitbx::math::boost_python


#ifndef _RYSQ_META_HPP_
#define _RYSQ_META_HPP_

#include <boost/mpl/min_max.hpp>
#include <boost/mpl/if.hpp>
#include <rysq/core.hpp>
#include "util.h"
#include "transpose.hpp"

namespace rysq {
    namespace meta {

	namespace mpl = boost::mpl;

	template<int x, int y> struct max {
	    static const int value = (x >= y) ? x : y;
	};

	template<int x, int y> struct min {
	    static const int value = (x < y) ? x : y;
	};

	template<rysq::type S> class shell {
	public:
	    static const rysq::type type = S;
	    static const int L = (S < 0) ? -S : S;

	    static const int nc = (S < 0) ? -S + 1 : 1;
	    static const int size = util::binomial2<L+2>::value + (nc-1);
	    static const int index = (S < 0) ? 0 : (S*(S + 1)*(S + 2))/6;

	    typedef mpl::integral_c_tag tag;
	    static const int value = ((1 << L) | ((S < 0) ? ((1 << L) - 1) : 0)) - 1;
	};

	template<rysq::type A_, rysq::type B_>
	class state {
	public:
	    typedef shell<A_> A;
	    typedef shell<B_> B;
	    static const int L = A::L + B::L;
	    static const int nc0 = A::nc;
	    static const int nc1 = B::nc;
	    static const int nc = nc0*nc1;
	    static const int size = A::size*B::size;

	    typedef mpl::integral_c_tag tag;
	    static const int value = A::value + B::value;
	    static const rysq::type max = (A::value >= B::value) ? A_ : B_;
	    static const rysq::type min = (A::value < B::value) ? A_ : B_;
	};

	template<rysq::type A_, rysq::type B_, rysq::type C_, rysq::type D_>
	class braket {
	public:
	    typedef shell<A_> A;
	    typedef shell<B_> B;
	    typedef shell<C_> C;
	    typedef shell<D_> D;

	    typedef state<A_,B_> bra;
	    typedef state<C_,D_> ket;

	    static const int L = bra::L + ket::L;
	    static const int nci = bra::nc0;
	    static const int ncj = bra::nc1;
	    static const int nck = ket::nc0;
	    static const int ncl = ket::nc1;
	    static const int nc = nci*ncj*nck*ncl;
	    static const int size = bra::size*ket::size;

	    typedef mpl::integral_c_tag tag;
	    static const int value = bra::value + ket::value;
	    typedef typename mpl::max<bra,ket>::type max;
	    typedef typename mpl::min<bra,ket>::type min;

	    // 	    typedef util::byteset::type value_type;
	    // 	    static const value_type value = util::byteset::set<A_,B_,C_,D_>::value;
 	};

	template<class A_, class B_, bool apply = true>
	struct transpose {
	    typedef typename mpl::if_<mpl::bool_<apply>, B_, A_>::type A;
	    typedef typename mpl::if_<mpl::bool_<apply>, A_, B_>::type B;
	    static const bool value = apply;
	};

	template<rysq::type A, rysq::type B, bool apply>
	struct transpose <shell<A>, shell<B>, apply> {
	    typedef typename
	    mpl::if_<mpl::bool_<apply>, meta::state<B,A>, meta::state<A,B> >::type type;
	    static const bool value = apply;
	};

	template<rysq::type A_, rysq::type B_, rysq::type C_, rysq::type D_>
	struct sorted {

	    typedef state<A_,B_> bra_;
	    typedef state<C_,D_> ket_;

	    typedef shell<bra_::max> A;
	    typedef shell<bra_::min> B;
	    typedef shell<ket_::max> C;
	    typedef shell<ket_::min> D;

	    typedef meta::braket<A::type, B::type, C::type, D::type> ABCD;
	    typedef meta::braket<C::type, D::type, A::type, B::type> CDAB;

	    static const bool t0_ = shell<A_>::value < shell<B_>::value;
	    static const bool t1_ = shell<C_>::value < shell<D_>::value;
	    typedef mpl::bool_<((A::value == C::value) && (B::value < D::value)) ||
				(A::value < C::value)> t_;

	    typedef typename mpl::if_<t_, CDAB, ABCD>::type braket;

	    static const int value = rysq::mpl::transpose<t0_, t1_, t_::value>::value;

	};

	template<class bra, class ket>
	struct sort;

	template<type A, type B, type C, type D>//bra>//, class ket>
	struct sort<state<A,B>, state<C,D> > {
	    typedef typename sorted<A,B,C,D>::braket braket;
	};

    }
}

#endif /* _RYSQ_META_HPP_ */


//
// Libor Novak
// 03/21/2017
//
// Tests object detection of 3D bounding boxes of a single scale detector on an image pyramid
//
// Optionaly it can apply non-maxima suppression (merging) on bounding boxes.
//

#include <caffe/caffe.hpp>
#include "caffe/util/benchmark.hpp"
#include "caffe/util/pgp.hpp"

// This code only works with OpenCV!
#ifdef USE_OPENCV

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <algorithm>
#include <iosfwd>
#include <memory>
#include <string>
#include <utility>
#include <vector>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

// Maximum 2D intersection over union of two boxes that will be both kept after NMS
#define IOU_2D_THRESHOLD 0.5

namespace {

    /**
     * @brief Checks if the Z coordinate of all points in X_3x8 is in front of C_3x1
     * @param X_3x8
     * @param C_3x1
     * @return True if z of all points in larger than z of C
     */
    bool checkZ (const cv::Mat &X_3x8, const cv::Mat &C_3x1)
    {
        for (int p = 0; p < 8; ++p)
        {
            if (X_3x8.at<double>(2, p) < C_3x1.at<double>(2,0))
            {
                return false;
            }
        }

        return true;
    }

    /**
     * @brief Check if the bottom trapezoid has some reasonable size (area)
     * @param X_3x8 Coordinates of 3D bounding box corners ordered FBL FBR RBR RBL FTL FTR RTR RTL
     * @return True if it has good size
     */
    bool checkBBSize (const cv::Mat &X_3x8)
    {
        // We will compute the area of the bottom trapezoid of the bounding box - we need to find the
        // "height" h of the trapezoid in order to use the A = (a+b)*h / 2 equation for computing its area

        // Get the plane defined by the RBL-FBL normal vector and FBL and intersect it with the RBR-FBR line
        // The coordinates are ordered FBL FBR RBR RBL FTL FTR RTR RTL
        cv::Mat n_3x1 = X_3x8(cv::Rect(0, 0, 1, 3)) - X_3x8(cv::Rect(3, 0, 1, 3));  // RBL - FBL
        double d = -n_3x1.dot(X_3x8(cv::Rect(3, 0, 1, 3)));  // d from ax+by+cz+d=0

        // Intersect the plane with RBR-FBR line (it has the same direction as RBL-FBL)
        double lambda = -(n_3x1.dot(X_3x8(cv::Rect(1, 0, 1, 3))) + d) / n_3x1.dot(n_3x1);
        cv::Mat ip_3x1 = X_3x8(cv::Rect(1, 0, 1, 3)) + lambda*n_3x1;  // Intersection point

        // Now compute the distance of the intersected point and FBL to get the h
        cv::Mat temp = X_3x8(cv::Rect(3, 0, 1, 3)) - ip_3x1;
        double h = std::sqrt(temp.dot(temp));
        // Distance of FBL and RBL - another side of the trapezoid
        double a = std::sqrt(n_3x1.dot(n_3x1));

        // Check the area - if it is too small or too big throw it away
        double area = (a*a*h) / 2.0;
        if (area < 1.5 || area > 7500) return false;
        return true;
    }

}

/**
 * @brief Wraps the input layer into a vector of cv::Mat so we could assign data to it more easily
 * @param input_layer Pointer to the net input layer blob
 * @param input_channels Vector of cv::Mat, which will be assigned
 */
void wrapInputLayer (caffe::Blob<float>* input_layer, std::vector<cv::Mat> &out_input_channels)
{
    out_input_channels.clear();

    int height = input_layer->shape(2);
    int width  = input_layer->shape(3);

    float* input_data = input_layer->mutable_cpu_data();

    for (int i = 0; i < input_layer->shape(1); ++i)
    {
        cv::Mat channel(height, width, CV_32FC1, input_data);
        out_input_channels.push_back(channel);
        input_data += width * height;
    }
}

std::vector<BB3D> extract3DBoundingBoxes (caffe::Blob<float> *output, const std::string &path_image,
                                          double scale, const PGP *pgp_p, bool size_filter)
{
    std::vector<BB3D> bounding_boxes;

    float *data_output = output->mutable_cpu_data();

    // 3D bounding box
    cv::Mat acc_prob(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 0));
    cv::Mat acc_fblx(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 1));
    cv::Mat acc_fbly(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 2));
    cv::Mat acc_fbrx(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 3));
    cv::Mat acc_fbry(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 4));
    cv::Mat acc_rblx(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 5));
    cv::Mat acc_rbly(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 6));
    cv::Mat acc_ftly(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 7));

    // Extract detected 3D boxes - only extract local maxima from 3x3 neighborhood
    for (int i = 0; i < acc_prob.rows; ++i)
    {
        for (int j = 0; j < acc_prob.cols; ++j)
        {
            float conf = acc_prob.at<float>(i, j);
            if (conf >= 0.1)
            {
                // Check if it is a local maximum
                if (i > 0)
                {
                    if (j > 0 && acc_prob.at<float>(i-1, j-1) > conf) continue;
                    if (acc_prob.at<float>(i-1, j) > conf) continue;
                    if (j < acc_prob.cols-1 && acc_prob.at<float>(i-1, j+1) > conf) continue;
                }
                if (j > 0 && acc_prob.at<float>(i, j-1) > conf) continue;
                if (j < acc_prob.cols-1 && acc_prob.at<float>(i, j+1) > conf) continue;
                if (i < acc_prob.rows-1)
                {
                    if (j > 0 && acc_prob.at<float>(i+1, j-1) > conf) continue;
                    if (acc_prob.at<float>(i+1, j) > conf) continue;
                    if (j < acc_prob.cols-1 && acc_prob.at<float>(i+1, j+1) > conf) continue;
                }

                // Ok, this is a local maximum
                double fblx = acc_fblx.at<float>(i, j) / scale;
                double fbly = acc_fbly.at<float>(i, j) / scale;
                double fbrx = acc_fbrx.at<float>(i, j) / scale;
                double fbry = acc_fbry.at<float>(i, j) / scale;
                double rblx = acc_rblx.at<float>(i, j) / scale;
                double rbly = acc_rbly.at<float>(i, j) / scale;
                double ftly = acc_ftly.at<float>(i, j) / scale;

                if (pgp_p == NULL)
                {
                    // We do not have image projection matrices and ground planes - just output the detection
                    bounding_boxes.emplace_back(path_image, 1, conf, fblx, fbly, fbrx, fbry, rblx, rbly, ftly);
                }
                else
                {
                    // There are image projection matrices and ground planes - reconstruct the 3D bounding box
                    BB3D bb3d(path_image, 1, conf, fblx, fbly, fbrx, fbry, rblx, rbly, ftly);
                    cv::Mat X_3x8 = pgp_p->reconstructAndFixBB3D(bb3d);

                    // Now since the Z axis points forward, we can check if the reconstructed points are in
                    // front of the camera, if not then we can throw away this bounding box
                    if (!checkZ(X_3x8, pgp_p->C_3x1)) continue;

                    // Check if the bounding box is large enough - its bottom trapezoid should be of
                    // reasonable size
                    if (size_filter && !checkBBSize(X_3x8)) continue;

                    // Extract the 2D bounding box - project back the 3D one and find extremes
                    cv::Mat x_2x8 = pgp_p->projectXtox(X_3x8);
                    double xmin = DBL_MAX; double ymin = DBL_MAX; double xmax = DBL_MIN; double ymax = DBL_MIN;
                    for (int p = 0; p < 8; ++p)
                    {
                        const double u = x_2x8.at<double>(0, p);
                        const double v = x_2x8.at<double>(1, p);
                        if (u < xmin) xmin = u;
                        if (u > xmax) xmax = u;
                        if (v < ymin) ymin = v;
                        if (v > ymax) ymax = v;
                    }
                    // Set the 2D bounding box
                    bb3d.xmin = xmin;
                    bb3d.ymin = ymin;
                    bb3d.xmax = xmax;
                    bb3d.ymax = ymax;

                    bounding_boxes.push_back(bb3d);
                }
            }
        }
    }

    return bounding_boxes;
}

std::vector<BB3D> detectObjects (const std::string &path_image, const std::vector<double> &scales,
                                 const std::shared_ptr<caffe::Net<float>> &net,
                                 const std::map<std::string, PGP> &pgps, bool size_filter)
{
#ifdef MEASURE_TIME
    caffe::CPUTimer timer;
#endif
    std::vector<BB3D> bounding_boxes;

    caffe::Blob<float>* input_layer  = net->input_blobs()[0];

    std::vector<cv::Mat> input_channels;

#ifdef MEASURE_TIME
    timer.Start();
#endif
    // Load the image
    cv::Mat image = cv::imread(path_image, CV_LOAD_IMAGE_COLOR);
    // Convert to zero mean and unit variance
    cv::Mat imagef; image.convertTo(imagef, CV_32FC3);
    imagef -= cv::Scalar(128.0f, 128.0f, 128.0f);
    imagef *= 1.0f/128.0f;
#ifdef MEASURE_TIME
    timer.Stop(); std::cout << "Time to to read image: " << timer.MilliSeconds() << " ms" << std::endl;
#endif

    // Get the image projection matrix and ground plane if we have them
    const PGP* pgp_p = NULL;
    if (pgps.size() > 0)
    {
        // There are image projection matrices and ground planes - find the one for this image
        auto pgpi = pgps.find(path_image);
        if (pgpi != pgps.end())
        {
            pgp_p = &((*pgpi).second);
        }
        else
        {
            std::cerr << "WARNING: PGP entry not found for image '" << path_image << "'" << std::endl;
        }
    }

#ifdef MEASURE_TIME
    timer.Start();
#endif
    // Build the image pyramid and run detection on each scale of the pyramid
    for (double s: scales)
    {
        cv::Mat imagef_scaled;
        cv::resize(imagef, imagef_scaled, cv::Size(), s, s);

        // Reshape the network
        input_layer->Reshape(1, input_layer->shape(1), imagef_scaled.rows, imagef_scaled.cols);
        net->Reshape();

        // Prepare the cv::Mats for input
        wrapInputLayer(input_layer, input_channels);
        // Copy the image to the input layer of the network
        cv::split(imagef_scaled, input_channels);

        net->Forward();

        for (caffe::Blob<float>* output: net->output_blobs())
        {
            std::vector<BB3D> new_bbs = extract3DBoundingBoxes(output, path_image, s, pgp_p, size_filter);
            bounding_boxes.insert(bounding_boxes.end(), new_bbs.begin(), new_bbs.end());
        }
    }

#ifdef MEASURE_TIME
    timer.Stop(); std::cout << "Time net + bb extraction: " << timer.MilliSeconds() << " ms" << std::endl;
#endif

    return bounding_boxes;
}

std::vector<BB3D> nonMaximaSuppression (std::vector<BB3D> &bbs)
{
    // For now we do NMS in the image! Not in 3D!
    //
    // This is standard non-maxima suppression, where we throw away boxes, which have a higher 2D intersection
    // over union then given threshold with some other box with a higher confidence

    // Sort by confidence in the descending order
    std::sort(bbs.begin(), bbs.end(), [](const BB3D &a, const BB3D &b) { return a.conf > b.conf; });

    std::vector<BB3D> bbs_out;
    std::vector<bool> active(bbs.size(), true);

    for (int i = 0; i < bbs.size(); ++i)
    {
        if (active[i])
        {
            active[i] = false;
            bbs_out.push_back(bbs[i]);

            // Check intersection with all remaining bounding boxes
            for (int j = i; j < bbs.size(); ++j)
            {
                if (active[j] && iou2d(bbs_out.back(), bbs[j]) > IOU_2D_THRESHOLD)
                {
                    active[j] = false;
                }
            }
        }
    }

    return bbs_out;
}

void writeBoundingBoxes (const std::vector<BB3D> &bbs, std::ofstream &fout)
{
    for (const BB3D &bb: bbs)
    {
        // The line of a BB3TXT file is:
        // filename label confidence xmin ymin xmax ymax fblx fbly fbrx fbry rblx rbly ftly
        fout << bb.path_image << " 1 " << bb.conf << " " << bb.xmin << " " << bb.ymin << " " << bb.xmax
             << " " << bb.ymax << " " << bb.fblx << " " << bb.fbly << " " << bb.fbrx << " " << bb.fbry
             << " " << bb.rblx << " " << bb.rbly << " " << bb.ftly << std::endl;
    }
}

void runPyramidDetection (const std::string &path_prototxt, const std::string &path_caffemodel,
                          const std::string &path_image_list, const std::string &path_out,
                          const std::string &path_pgp, bool size_filter)
{
#ifdef CPU_ONLY
    caffe::Caffe::set_mode(caffe::Caffe::CPU);
#else
    caffe::Caffe::set_mode(caffe::Caffe::GPU);
#endif

#ifdef MEASURE_TIME
    caffe::CPUTimer timer;
#endif

    // Scaling factor is 1.5
//    const std::vector<double> scales = { 1.0, 0.66, 0.44, 0.29, 0.19 };
    const std::vector<double> scales = { 1.0 };

    // Create network and load trained weights from caffemodel file
    auto net = std::make_shared<caffe::Net<float>>(path_prototxt, caffe::TEST);
    net->CopyTrainedLayersFrom(path_caffemodel);

    caffe::Blob<float>* input_layer  = net->input_blobs()[0];
    caffe::Blob<float>* output_layer = net->output_blobs()[0];

    CHECK_EQ(net->num_inputs(), 1) << "Network should have exactly one input.";
    CHECK_EQ(input_layer->shape(1), 3) << "Input layer must have 3 channels.";
    CHECK_EQ(output_layer->shape(1), 8) << "Unsupported network, only 8 channels!";

    // Prepare the input channels
    std::vector<cv::Mat> input_channels;
    wrapInputLayer(input_layer, input_channels);

    std::ifstream infile(path_image_list.c_str());
    CHECK(infile) << "Unable to open image list TXT file '" << path_image_list << "'!";
    std::string line;

    // Load the P matrices and ground planes
    std::map<std::string, PGP> pgps;
    if (path_pgp != "") pgps = PGP::readPGPFile(path_pgp);

    std::ofstream fout; fout.open(path_out);
    CHECK(fout) << "Output file '" << path_out << "' could not have been created!";
    std::ofstream fout_nms;
    if (pgps.size() > 0) fout_nms.open(path_out.substr(0, path_out.size()-7) + "_nms.bb3txt");

    // -- RUN THE DETECTOR ON EACH IMAGE -- //
    while (std::getline(infile, line))
    {
        LOG(INFO) << line;
        CHECK(boost::filesystem::exists(line)) << "Image '" << line << "' not found!";

        // Detect bbs on the image
        std::vector<BB3D> bbs = detectObjects(line, scales, net, pgps, size_filter);

        // Save the bounding boxes before NMS to a BBTXT file
        writeBoundingBoxes(bbs, fout);

        // Only do NMS if we can reconstruct the 3D boxes
        if (pgps.size() > 0)
        {
#ifdef MEASURE_TIME
            timer.Start();
#endif
            // Non-maxima suppression
            bbs = nonMaximaSuppression(bbs);
#ifdef MEASURE_TIME
            timer.Stop(); std::cout << "Time to perform NMS: " << timer.MilliSeconds() << " ms" << std::endl;
#endif

            // Save the bounding boxes after NMS to a BBTXT file
            writeBoundingBoxes(bbs, fout_nms);
        }
    }

    fout.close();
    if (pgps.size() > 0) fout_nms.close();
}

// -----------------------------------------------  MAIN  ------------------------------------------------ //

struct ProgramArguments
{
    std::string path_prototxt;
    std::string path_caffemodel;
    std::string path_image_list;
    std::string path_out;
    std::string path_pgp;
    bool size_filter;
};

/**
 * @brief Parses arguments of the program
 */
void parseArguments (int argc, char** argv, ProgramArguments &pa)
{
    try {
        po::options_description desc("Arguments");
        desc.add_options()
            ("help", "Print help")
            ("prototxt", po::value<std::string>(&pa.path_prototxt)->required(),
             "Model file of the network (*.prototxt)")
            ("caffemodel", po::value<std::string>(&pa.path_caffemodel)->required(),
             "Weight file of the network (*.caffemodel)")
            ("image_list", po::value<std::string>(&pa.path_image_list)->required(),
             "Path to a TXT file with paths to the images to be tested")
            ("path_out", po::value<std::string>(&pa.path_out)->required(),
             "Path to the output BB3TXT file")
            ("pgp", po::value<std::string>(&pa.path_pgp)->default_value(""),
             "Path to a PGP file with calibration matrices and ground planes")
            ("size_filter", po::bool_switch(&pa.size_filter)->default_value(false),
             "Turns on filtering of all bounding boxes, which are too small")
        ;

        po::positional_options_description positional;
        positional.add("prototxt", 1);
        positional.add("caffemodel", 1);
        positional.add("image_list", 1);
        positional.add("path_out", 1);
        positional.add("pgp", 1);

        // Parse the input arguments
        po::variables_map vm;
        po::store(po::command_line_parser(argc, argv).options(desc).positional(positional).run(), vm);

        if (vm.count("help")) {
            std::cout << "Usage: ./macc3d_pyramid_test path/f.prototxt path/f.caffemodel path/image_list.txt path/out.bb3txt (path/calib.pgp)\n";
            std::cout << desc;
            exit(EXIT_SUCCESS);
        }

        po::notify(vm);

        if (!boost::filesystem::exists(pa.path_prototxt))
        {
            std::cerr << "ERROR: File '" << pa.path_prototxt << "' does not exist!" << std::endl;
            exit(EXIT_FAILURE);
        }
        if (!boost::filesystem::exists(pa.path_caffemodel))
        {
            std::cerr << "ERROR: File '" << pa.path_caffemodel << "' does not exist!" << std::endl;
            exit(EXIT_FAILURE);
        }
        if (!boost::filesystem::exists(pa.path_image_list))
        {
            std::cerr << "ERROR: File '" << pa.path_image_list << "' does not exist!" << std::endl;
            exit(EXIT_FAILURE);
        }
        if (boost::filesystem::exists(pa.path_out))
        {
            std::cerr << "ERROR: File '" << pa.path_out << "' already exists!" << std::endl;
            exit(EXIT_FAILURE);
        }
        if (pa.path_pgp != "" && !boost::filesystem::exists(pa.path_pgp))
        {
            std::cerr << "ERROR: File '" << pa.path_pgp << "' does not exist!" << std::endl;
            exit(EXIT_FAILURE);
        }
    }
    catch(std::exception& e)
    {
        std::cerr << e.what() << "\n";
        exit(EXIT_FAILURE);
    }
}

int main (int argc, char** argv)
{
    FLAGS_logtostderr = 1;
    FLAGS_minloglevel = ::google::INFO;
    ::google::InitGoogleLogging(argv[0]);

    ProgramArguments pa;
    parseArguments(argc, argv, pa);

std::cout << pa.size_filter << std::endl;
    runPyramidDetection(pa.path_prototxt, pa.path_caffemodel, pa.path_image_list, pa.path_out, pa.path_pgp, pa.size_filter);

    return EXIT_SUCCESS;
}

#else
int main(int argc, char** argv) {
    LOG(FATAL) << "This example requires OpenCV; compile with USE_OPENCV.";
}
#endif  // USE_OPENCV


#include "RaymarcherCave.h"

#include <math.h>
#include <string>
#include <iostream>

#include <boost/cast.hpp>

#include <vrj/vrjConfig.h>
#include <vrj/Display/Viewport.h>
#include <vrj/Draw/OpenGL/DrawManager.h>

#include <glm/glm.hpp>
#include <glm/ext.hpp>
#include <glm/common.hpp>

#include <glm/gtc/type_precision.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/quaternion.hpp>
#include <glm/gtc/matrix_inverse.hpp>
#include <glm/gtc/matrix_transform.hpp>

#include <glm/gtx/euler_angles.hpp>
#include <glm/gtx/quaternion.hpp>
#include <glm/gtx/common.hpp>
#include <glm/gtx/projection.hpp>
#include <glm/gtx/extend.hpp>
#include <glm/gtx/matrix_operation.hpp>
#include <glm/gtx/matrix_decompose.hpp>
#include <glm/gtx/norm.hpp>
#include <glm/gtx/normal.hpp>

#include "Shader.h"
#include "FPS.h"

using namespace vrj;
using namespace std;

#define PI 3.14159265358

//Global variables
//TODO: global variables are bad, they need to be included in the App class
GLuint cave_shader_programme;
float cave_time_counter;
glm::vec2 cave_iResolution;
glm::vec3 user_rotation;
glm::vec3 user_translation;
GLuint cave_vao;
FPS cave_counter;
float cave_width;
float cave_height;

//The quad needed to render for raytracing
GLfloat cave_points[] = {
	-1.0f, -1.0f,
	-1.0f, 1.0f,
	1.0f, -1.0f,
	1.0f, 1.0
};

//App lifecycle initialization
void CaveApp::init()
{
	std::cout << "---------- CaveApp:init() ---------------" << std::endl;
	mWand.init("VJWand");
	mHead.init("VJHead");
	mButton0.init("VJButton0");
	mButton1.init("VJButton1");
}

//App lifecycle initialization
void CaveApp::apiInit()
{
}

void CaveApp::bufferPreDraw()
{
}

void CaveApp::preFrame()
{
	mCurTime += 0.005f;
}

glm::mat4 get_model_matrix_glm(float angle_x, float angle_y, float angle_z, float trans_x, float trans_y, float trans_z){
	float * rot = new float[9];
	glm::mat4 model_mat = glm::mat4();
	glm::mat3 rotation_matrix = glm::mat3();
	glm::mat3 rotation_x(1, 0, 0, 0, cos((angle_x*PI) / 180), -sin((angle_x*PI) / 180), 0, sin((angle_x*PI) / 180), cos((angle_x*PI) / 180));
	glm::mat3 rotation_y(cos((angle_y*PI) / 180), 0, sin((angle_y*PI) / 180), 0, 1, 0, -sin((angle_y*PI) / 180), 0, cos((angle_y*PI) / 180));
	glm::mat3 rotation_z(cos((angle_z*PI) / 180), -sin((angle_z*PI) / 180), 0, sin((angle_z*PI) / 180), cos((angle_z*PI) / 180), 0, 0, 0, 1);
	rotation_matrix = rotation_z * rotation_y * rotation_x;
	rot[0] = rotation_matrix[0][0];
	rot[1] = rotation_matrix[1][0];
	rot[2] = rotation_matrix[2][0];
	rot[3] = rotation_matrix[0][1];
	rot[4] = rotation_matrix[2][2];
	rot[5] = rotation_matrix[2][1];
	rot[6] = rotation_matrix[0][2];
	rot[7] = rotation_matrix[1][2];
	rot[8] = rotation_matrix[1][1];
	model_mat[0][0] = rot[0];
	model_mat[1][0] = rot[1];
	model_mat[2][0] = rot[2];
	model_mat[3][0] = trans_x;
	model_mat[0][1] = rot[3];
	model_mat[1][1] = rot[4];
	model_mat[2][1] = rot[5];
	model_mat[3][1] = trans_y;
	model_mat[0][2] = rot[6];
	model_mat[1][2] = rot[7];
	model_mat[2][2] = rot[8];
	model_mat[3][2] = trans_z;
	model_mat[0][3] = 0;
	model_mat[1][3] = 0;
	model_mat[2][3] = 0;
	model_mat[3][3] = 1;
	return model_mat;
}

//TODO: get the width and the height of the viewport
void CaveApp::draw()
{
	//Start frame counter
	cave_counter.start_fps_counter();

	//Get width and height of the viewport dynamically
	vrj::opengl::DrawManager* drawMgr = boost::polymorphic_downcast<vrj::opengl::DrawManager*>(getDrawManager());
	vrj::opengl::UserData*    udata = drawMgr->currentUserData();
	vrj::ProjectionPtr const  proj = udata->getProjection();
	vrj::Frustum const& frust = proj->getFrustum();
	glm::mat4 view_mat = glm::make_mat4x4(proj->getViewMatrix().getData());
	glm::mat4 proj_mat = glm::frustum(frust[vrj::Frustum::VJ_LEFT], frust[vrj::Frustum::VJ_RIGHT], frust[vrj::Frustum::VJ_BOTTOM], frust[vrj::Frustum::VJ_TOP], frust[vrj::Frustum::VJ_NEAR], frust[vrj::Frustum::VJ_FAR]);
	glm::mat4 model_mat = get_model_matrix_glm(0.0f, 0.0f, 0.0f, 0.0f, 3.5f, 1.0f);
	glm::mat4 model_view_proj = proj_mat * view_mat * model_mat;

	glm::vec3 scale;
	glm::quat rotation;
	glm::vec3 translation;
	glm::vec3 skew;
	glm::vec4 perspective;
	glm::decompose(model_view_proj, scale, rotation, translation, skew, perspective);
	glm::vec3 euler_rotation = glm::eulerAngles(rotation) * (PI / 180.0f);

	glClearColor(0.1f, 0.1f, 0.1f, 1.0f);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glBindVertexArray(cave_vao);
	glUseProgram(cave_shader_programme);

	//Updating shader uniforms
	cave_iResolution = glm::vec2(cave_width, cave_height);
	glUniform2fv(glGetUniformLocation(cave_shader_programme, "iResolution"), 1, glm::value_ptr(cave_iResolution));
	cave_time_counter = cave_time_counter + 0.02f;
	glUniform1f(glGetUniformLocation(cave_shader_programme, "iGlobalTime"), cave_time_counter);
	user_rotation = glm::vec3(euler_rotation.x, euler_rotation.y, euler_rotation.z);
	glUniform3fv(glGetUniformLocation(cave_shader_programme, "user_rotation"), 1, glm::value_ptr(user_rotation));
	user_translation = glm::vec3(translation.x, translation.y, translation.z);
	glUniform3fv(glGetUniformLocation(cave_shader_programme, "user_translation"), 1, glm::value_ptr(user_translation));

	glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);

	cave_counter.end_fps_counter();
	//cave_counter.print_fps();
}

void CaveApp::intraFrame()
{
}

void CaveApp::postFrame()
{
}

void CaveApp::latePreFrame()
{
}

void CaveApp::contextPostDraw()
{
}

void CaveApp::contextPreDraw()
{
}

void CaveApp::initGLState()
{
	//Initialize OpenGL
	const GLubyte* renderer;
	const GLubyte* version;

	glewExperimental = GL_TRUE;
	glewInit();
	renderer = glGetString(GL_RENDERER);
	version = glGetString(GL_VERSION);
	printf("Renderer: %s\n", renderer);
	printf("OpenGL version supported %s\n", version);

	const char* env_p = std::getenv("SHADER_PATH");
	const char* vertex_shader_file_name = "vertex.vert";
	const char* fragment_shader_file_name = "fragment.frag";
	char vertex_shader_file_name_str[250];
	char fragment_shader_file_name_str[250];
	strcpy(vertex_shader_file_name_str, env_p);
	strcat(vertex_shader_file_name_str, "/");
	strcat(vertex_shader_file_name_str, vertex_shader_file_name);
	strcpy(fragment_shader_file_name_str, env_p);
	strcat(fragment_shader_file_name_str, "/");
	strcat(fragment_shader_file_name_str, fragment_shader_file_name);

	//Initialize Shader
	const char* vertex_shader = (const char*)Shader::load_shader(vertex_shader_file_name_str);
	const char* fragment_shader = (const char*)Shader::load_shader(fragment_shader_file_name_str);
	Shader s;
	s.vertex_shader = vertex_shader;
	s.fragment_shader = fragment_shader;

	//Get viewport information
	char* env_viewport_width = std::getenv("VIEWPORT_WIDTH");
	char* env_viewport_height = std::getenv("VIEWPORT_HEIGHT");
	cave_width = (float)atof(env_viewport_width);
	cave_height = (float)atof(env_viewport_height);

	//Create VBO
	GLuint vbo;
	glGenBuffers(1, &vbo);
	glBindBuffer(GL_ARRAY_BUFFER, vbo);
	glBufferData(GL_ARRAY_BUFFER, sizeof(cave_points), cave_points, GL_STATIC_DRAW);

	//Create VAO
	glGenVertexArrays(1, &cave_vao);
	glBindVertexArray(cave_vao);

	//Input uniforms positions
	glBindBuffer(GL_ARRAY_BUFFER, vbo);
	glVertexAttribPointer(0, 2, GL_FLOAT, GL_FALSE, 0, 0);
	glEnableVertexAttribArray(0);
	cave_iResolution = glm::vec2(cave_width, cave_height);
	glUniform2fv(glGetUniformLocation(cave_shader_programme, "iResolution"), 1, glm::value_ptr(cave_iResolution));
	cave_time_counter = 1.0f;
	glUniform1f(glGetUniformLocation(cave_shader_programme, "iGlobalTime"), cave_time_counter);
	user_rotation = glm::vec3(0.0f, 0.0f, 0.0f);
	glUniform3fv(glGetUniformLocation(cave_shader_programme, "user_rotation"), 1, glm::value_ptr(user_rotation));
	user_translation = glm::vec3(0.0f, 3.5f, 5.0f);
	glUniform3fv(glGetUniformLocation(cave_shader_programme, "user_translation"), 1, glm::value_ptr(user_translation));

	//Compile shader
	//Vertex shader
	GLuint vs = glCreateShader(GL_VERTEX_SHADER);
	glShaderSource(vs, 1, &s.vertex_shader, NULL);
	glCompileShader(vs);

	//Fragment shader
	GLuint fs = glCreateShader(GL_FRAGMENT_SHADER);
	glShaderSource(fs, 1, &s.fragment_shader, NULL);
	glCompileShader(fs);

	//Link shader
	cave_shader_programme = glCreateProgram();
	glAttachShader(cave_shader_programme, fs);
	glAttachShader(cave_shader_programme, vs);
	glLinkProgram(cave_shader_programme);

	//Use shader
	glUseProgram(cave_shader_programme);
}


// Copyright (c) 2015-2019 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "error_rate_stdev.hpp"

#include <deque>
#include <numeric>
#include <algorithm>
#include <iterator>
#include <map>
#include <cstddef>

#include <boost/variant.hpp>

#include "basics/cigar_string.hpp"
#include "basics/aligned_read.hpp"
#include "core/types/haplotype.hpp"
#include "io/variant/vcf_record.hpp"
#include "utils/maths.hpp"
#include "../facets/samples.hpp"
#include "../facets/read_assignments.hpp"

namespace octopus { namespace csr {

const std::string ErrorRateStdev::name_ = "ERS";

std::unique_ptr<Measure> ErrorRateStdev::do_clone() const
{
    return std::make_unique<ErrorRateStdev>(*this);
}

Measure::ResultType ErrorRateStdev::get_default_result() const
{
    return std::vector<boost::optional<double>> {};
}

namespace {

auto calculate_error_rate(const AlignedRead& read) noexcept
{
    return static_cast<double>(sum_non_matches(read.cigar())) / sequence_size(read);
}

boost::optional<double>
compute_error_rate_stdev(const Facet::SupportMaps::HaplotypeSupportMaps& assignments, const GenomicRegion& region) noexcept
{
    std::deque<double> error_rates {};
    for (const auto& p : assignments.assigned_wrt_haplotype) {
        for (const auto& read : overlap_range(p.second, region)) {
            error_rates.push_back(calculate_error_rate(read));
        }
    }
    for (const auto& read : overlap_range(assignments.ambiguous_wrt_haplotype, region)) {
        error_rates.push_back(calculate_error_rate(read.read));
    }
    if (!error_rates.empty()) {
        return maths::stdev(error_rates);
    } else {
        return boost::none;
    }
}

} // namespace

Measure::ResultType ErrorRateStdev::do_evaluate(const VcfRecord& call, const FacetMap& facets) const
{
    const auto& samples = get_value<Samples>(facets.at("Samples"));
    const auto& assignments = get_value<ReadAssignments>(facets.at("ReadAssignments")).haplotypes;
    std::vector<boost::optional<double>> result {};
    result.reserve(samples.size());
    for (const auto& sample : samples) {
        result.push_back(compute_error_rate_stdev(assignments.at(sample), mapped_region(call)));
    }
    return result;
}

Measure::ResultCardinality ErrorRateStdev::do_cardinality() const noexcept
{
    return ResultCardinality::samples;
}

const std::string& ErrorRateStdev::do_name() const
{
    return name_;
}

std::string ErrorRateStdev::do_describe() const
{
    return "Error rate standard deviation in reads overlapping the site";
}

std::vector<std::string> ErrorRateStdev::do_requirements() const
{
    return {"Samples", "ReadAssignments"};
}

} // namespace csr
} // namespace octopus