// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

#include "BundlerLib.h"

#include <Eigen/Core>
#include <Eigen/StdVector>

#include <unordered_set>

#include <g2o/config.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/robust_kernel_impl.h>

#include <array>

using namespace g2o;

namespace mage
{
    class EdgeScaleConstraint : public  BaseMultiEdge<1, number_t>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

        EdgeScaleConstraint()
        {
            _vertices.resize(2);
            BaseMultiEdge::resize(2);
        }

        void SetWeight(number_t weight)
        {
            m_weight = weight;
        }

        bool read(std::istream& /*is*/) override { return true; }

        bool write(std::ostream& /*os*/) const override { return true; }

        void computeError() override
        {
            const auto v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
            const auto v2 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
            Vector3 dt = v2->estimate().translation() - v1->estimate().translation();
            _error[0] = (_measurement - dt.norm())*m_weight;
        }

    protected:
        number_t m_weight = 0;
    };

    class EdgeRotationConstraint : public  BaseMultiEdge<1, Quaternion>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

        EdgeRotationConstraint()
        {
            _vertices.resize(2);
            BaseMultiEdge::resize(2);
        }

        bool read(std::istream& /*is*/) override { return true; }
        bool write(std::ostream& /*os*/) const override { return true; }

        void SetWeight(number_t value)
        {
            m_weight = value;
        }

        Quaternion GetRelativeRotation() const
        {
            auto v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
            auto v2 = static_cast<const VertexSE3Expmap*>(_vertices[1]);

            return (v1->estimate().inverse() * v2->estimate()).rotation();
        }

        void computeError() override
        {
            _error[0] = GetRelativeRotation().angularDistance(_measurement) * m_weight;
        }

    protected:
        number_t m_weight = 0;
    };

    class StepOptimizer : private SparseOptimizer
    {
    public:
        using SparseOptimizer::setVerbose;
        using SparseOptimizer::setAlgorithm;
        using SparseOptimizer::activeEdges;

        bool addParameter(Parameter* p)
        {
            m_dirty = true;
            return SparseOptimizer::addParameter(p);
        }

        bool addVertex(Vertex* v)
        {
            m_dirty = true;
            return SparseOptimizer::addVertex(v);
        }

        bool addEdge(Edge* e)
        {
            m_dirty = true;
            return SparseOptimizer::addEdge(e);
        }

        bool removeEdge(Edge* e)
        {
            m_dirty = true;
            return SparseOptimizer::removeEdge(e);
        }

        void SetCurrentLambda(number_t lambda)
        {
            // The user lambda is checked by the solver on the 0th iteration;
            // consequently, while setting the lambda does NOT actually dirty
            // the optimizer, it does require the iteration to be reset to 0.
            m_iteration = 0;
            static_cast<OptimizationAlgorithmLevenberg*>(_algorithm)->setUserLambdaInit(lambda);
        }

        bool Step()
        {
            if (m_dirty)
            {
                InitializeOptimization();
            }

            if (m_useless || SparseOptimizer::terminate())
                return false;

            preIteration(m_iteration);
            auto result = _algorithm->solve(m_iteration);
            postIteration(m_iteration);

            m_iteration++;

            return result == OptimizationAlgorithm::OK;
        }

    private:
        bool m_dirty{ true };
        bool m_useless{ false };
        int m_iteration{ 0 };

        void InitializeOptimization()
        {
            SparseOptimizer::initializeOptimization();

            m_useless = _ivMap.size() == 0;
            if (!m_useless)
                _algorithm->init();

            m_iteration = 0;
            m_dirty = false;
        }
    };

    struct BundlerLib::Impl
    {
        int currentId;

        const std::unique_ptr<StepOptimizer> Optimizer;
        const std::unique_ptr<OptimizationAlgorithmLevenberg> Algorithm;
        std::vector<g2o::VertexSE3Expmap, Eigen::aligned_allocator<g2o::VertexSE3Expmap>> PoseVertices;
        std::vector<g2o::VertexSBAPointXYZ, Eigen::aligned_allocator<g2o::VertexSBAPointXYZ>> PointVertices;
        std::vector<g2o::EdgeProjectXYZ2UV, Eigen::aligned_allocator<g2o::EdgeProjectXYZ2UV>> ObservationEdges;
        std::vector<g2o::RobustKernelHuber> HuberKernels;
        std::vector<EdgeScaleConstraint> FixedDistanceConstraints;
        std::vector<EdgeRotationConstraint, Eigen::aligned_allocator<EdgeRotationConstraint>> RelativeRotationConstraints;
        std::vector<EdgeSE3Expmap, Eigen::aligned_allocator<EdgeSE3Expmap>> RelativeTransformConstraints;
    };

    BundlerLib::BundlerLib(const BundlerParameters& bundlerParameters)
        : m_impl{ new Impl{
                std::numeric_limits<int>::max() - 1,
                std::make_unique<StepOptimizer>(),
                std::make_unique<g2o::OptimizationAlgorithmLevenberg>(
                    std::make_unique<g2o::BlockSolver_6_3>(
                        std::make_unique<g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>>()))
            } },
        m_bundlerParameters(bundlerParameters)
    {
        m_impl->Optimizer->setVerbose(false);
        m_impl->Optimizer->setAlgorithm(m_impl->Algorithm.get());
    }

    void BundlerLib::AllocateCameras(size_t count)
    {
        assert(m_impl->PoseVertices.empty() && "can only allocate once");
        m_impl->PoseVertices.resize(count);

        for (size_t i = 0; i < count; ++i)
        {
            m_impl->PoseVertices[i].setId(gsl::narrow<int>(i));
        }
    }

    void BundlerLib::AllocateMapPoints(size_t count)
    {
        m_impl->PointVertices.resize(count);

        for (size_t i = 0; i < count; ++i)
        {
            m_impl->PointVertices[i].setId(--m_impl->currentId);
        }
    }

    void BundlerLib::AllocateObservations(size_t count)
    {
        m_impl->ObservationEdges.resize(count);

        for (size_t i = 0; i < count; ++i)
        {
            m_impl->ObservationEdges[i].setId(--m_impl->currentId);
        }

        m_impl->HuberKernels.resize(count);
    }

    void BundlerLib::AllocateFixedDistanceConstraints(size_t count)
    {
        m_impl->FixedDistanceConstraints.resize(count);

        for (size_t i = 0; i < count; ++i)
        {
            m_impl->FixedDistanceConstraints[i].setId(--m_impl->currentId);
        }
    }

    void BundlerLib::AllocateRelativeRotationConstraints(size_t count)
    {
        m_impl->RelativeRotationConstraints.resize(count);

        for (size_t i = 0; i < count; ++i)
        {
            m_impl->RelativeRotationConstraints[i].setId(--m_impl->currentId);
        }
    }

    void BundlerLib::AllocateRelativeTransformConstraints(size_t count)
    {
        m_impl->RelativeTransformConstraints.resize(count);

        for (size_t i = 0; i < count; ++i)
        {
            m_impl->RelativeTransformConstraints[i].setId(--m_impl->currentId);
        }
    }

    void BundlerLib::SetCameraPose(size_t idx, Eigen::Map<const Eigen::Vector3f> position, Eigen::Map<const Eigen::Matrix3f> orientation, Eigen::Map<const Eigen::Vector4f> intrinsics, bool isFixed)
    {
        auto vertex = &m_impl->PoseVertices[idx];

        // TODO(14118744) remove this allocation as well
        auto cam_params = new g2o::CameraParameters(intrinsics[2], g2o::Vector2(intrinsics[0], intrinsics[1]), 0.0);
        cam_params->setId(gsl::narrow<int>(idx));
        m_impl->Optimizer->addParameter(cam_params);

        vertex->setFixed(isFixed);
        vertex->setEstimate(
            g2o::SE3Quat(Eigen::Quaternionf{ orientation }.normalized().cast<number_t>(), position.cast<number_t>())
        );

        m_impl->Optimizer->addVertex(vertex);
    }

    void BundlerLib::FixCameraPose(size_t idx, bool value)
    {
        m_impl->PoseVertices[idx].setFixed(value);
    }

    void BundlerLib::SetMapPoint(size_t idx, Eigen::Map<const Eigen::Vector3f> point)
    {
        auto vertex = &m_impl->PointVertices[idx];

        vertex->setMarginalized(true);  // Identify these as not-camera-parameters.
        vertex->setEstimate(point.cast<number_t>());
        vertex->setFixed(m_bundlerParameters.ArePointsFixed);

        m_impl->Optimizer->addVertex(vertex);
    }

    void BundlerLib::SetObservation(size_t idx, Eigen::Map<const Eigen::Vector2f> position, size_t cameraIndex, size_t mapPointIndex, float informationMatrixScalar)
    {
        auto edge = &m_impl->ObservationEdges[idx];

        edge->setVertex(0, &m_impl->PointVertices[mapPointIndex]);
        edge->setVertex(1, &m_impl->PoseVertices[cameraIndex]);
        edge->setMeasurement(position.cast<number_t>());
        edge->information() = informationMatrixScalar * Matrix2::Identity();

        edge->setParameterId(0, gsl::narrow_cast<int>(cameraIndex));

        auto kernel = &m_impl->HuberKernels[idx];
        edge->setRobustKernel(kernel);

        m_impl->Optimizer->addEdge(edge);
    }

    void BundlerLib::SetFixedDistanceConstraint(size_t idx, size_t cameraIndex1, size_t cameraIndex2, float distance, float weight)
    {
        auto edge = &m_impl->FixedDistanceConstraints[idx];

        edge->SetWeight(weight);
        edge->setVertex(0, &m_impl->PoseVertices[cameraIndex1]);
        edge->setVertex(1, &m_impl->PoseVertices[cameraIndex2]);
        edge->setInformation(Eigen::Matrix<number_t, 1, 1>::Identity());
        edge->setMeasurement(distance);

        m_impl->Optimizer->addEdge(edge);
    }

    void BundlerLib::SetRelativeRotationConstraint(size_t idx, size_t cameraIndex1, size_t cameraIndex2, const Eigen::Quaternionf& deltaRotation, float weight)
    {
        auto edge = &m_impl->RelativeRotationConstraints[idx];

        edge->SetWeight(weight);
        edge->setVertex(0, &m_impl->PoseVertices[cameraIndex1]);
        edge->setVertex(1, &m_impl->PoseVertices[cameraIndex2]);
        edge->setInformation(Eigen::Matrix<number_t, 1, 1>::Identity());

        edge->setMeasurement(deltaRotation.cast<number_t>());

        m_impl->Optimizer->addEdge(edge);
    }

    void BundlerLib::SetRelativeTransformConstraint(
        size_t idx, size_t cameraIndex1, size_t cameraIndex2, Eigen::Map<const Eigen::Vector3f> deltaPosition, const Eigen::Quaternionf& deltaRotation, float weight)
    {
        auto edge = &m_impl->RelativeTransformConstraints[idx];

        edge->setVertex(0, &m_impl->PoseVertices[cameraIndex1]);
        edge->setVertex(1, &m_impl->PoseVertices[cameraIndex2]);
        edge->setInformation(Eigen::Matrix<number_t, 6, 6>::Identity() * weight);

        edge->setMeasurement({ deltaRotation.cast<number_t>(), deltaPosition.cast<number_t>() });

        m_impl->Optimizer->addEdge(edge);
    }

    BundlerLib::~BundlerLib() = default;

    void BundlerLib::SetCurrentLambda(float userLambda)
    {
        m_impl->Optimizer->SetCurrentLambda(userLambda);
    }

    float BundlerLib::GetCurrentLambda() const
    {
        return static_cast<float>(m_impl->Algorithm->currentLambda());
    }

    float BundlerLib::StepBundleAdjustment(gsl::span<const float> huberWidthPerIteration, float maxErrorSquare, std::vector<unsigned int>& outliers)
    {
        float priorHuberWidth = -1.f;
        for (float huberWidth : huberWidthPerIteration)
        {
            assert(huberWidth >= 0.f && "Huber widths must be nonnegative");

            if (huberWidth != priorHuberWidth)
            {
                for (auto& huberKernel : m_impl->HuberKernels)
                {
                    huberKernel.setDelta(huberWidth);
                }

                priorHuberWidth = huberWidth;
            }

            if (!m_impl->Optimizer->Step())
                break;
        }

        std::vector<OptimizableGraph::Edge*> edges2Remove;

        int count = 0;
        number_t error = 0;

        for (auto edge : m_impl->Optimizer->activeEdges())
        {
            auto data = edge->errorData();

            number_t sumSquares = 0;

            for (int i = 0; i < edge->dimension(); i++)
            {
                sumSquares += data[i]*data[i];
            }

            // check to see that the point projects in front of the camera (because reprojection error cannot differenciate)
            const auto* vert = static_cast<const g2o::VertexSBAPointXYZ*>(edge->vertex(0));
            const auto* poseVertex = static_cast<const VertexSE3Expmap*>(edge->vertex(1));

            SE3Quat worldPose = poseVertex->estimate().inverse();
            g2o::Vector3 worldPos = vert->estimate();
            g2o::Vector3 projectedVec = worldPos - worldPose.translation();

            // now the sign of the dot product of the forward vector from the projection matrix and a vector to the projected point
            // will tell us if the point is in front of the camera or behind it
            const g2o::Vector3 forward = worldPose.rotation() * g2o::Vector3{ 0, 0, 1 };
            const number_t dotProd = projectedVec.dot(forward);

            if (dotProd <= 0)
            {
                edges2Remove.push_back(edge);
            }
            else if (sumSquares > maxErrorSquare)
            {
                edges2Remove.push_back(edge);
            }
            else
            {
                error += sumSquares;
                count++;
            }
        }

        if (!edges2Remove.empty())
        {
            for (auto edge : edges2Remove)
            {
                // only remove point if it is a camera/point edge.
                // TODO(14118778) PERF: don't do a linear search
                auto itr = std::find_if(m_impl->ObservationEdges.begin(), m_impl->ObservationEdges.end(),
                    [edge](const g2o::EdgeProjectXYZ2UV& obs) { return &obs == edge; });

                if (itr != m_impl->ObservationEdges.end())
                {
                    m_impl->Optimizer->removeEdge(edge);
                    outliers.push_back(gsl::narrow<unsigned int>( std::distance(m_impl->ObservationEdges.begin(), itr) ));
                }
            }
        }

        return (float)(error / count);
    }

    namespace
    {
        inline g2o::Quaternion ToQuat(const g2o::Vector4& rotation)
        {
            return g2o::Quaternion{ rotation.eval().data() };
        }
    }

    void BundlerLib::GetPose(size_t idx, Eigen::Map<Eigen::Vector3f> position, Eigen::Map<Eigen::Matrix3f> orientation) const
    {
        const auto& vertex = m_impl->PoseVertices[idx];

        const auto& pose = vertex.estimate();

        position = pose.translation().cast<float>();
        orientation = pose.rotation().normalized().toRotationMatrix().cast<float>();
    }

    void BundlerLib::GetPoint(size_t idx, Eigen::Map<Eigen::Vector3f> position) const
    {
        const auto& vertex = m_impl->PointVertices[idx];
        position = vertex.estimate().cast<float>();
    }
}


#pragma once
/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file interpolateddiscountcurvelinearzerp.hpp
    \brief interpolated discount term structure
    \ingroup termstructures
*/

#include <ql/math/interpolations/linearinterpolation.hpp>
#include <ql/patterns/lazyobject.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/time/calendars/nullcalendar.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {
using namespace QuantLib;
//! InterpolatedDiscountCurveLinearZero as in QuantLib, but with floating discount quotes and floating reference date
/*! InterpolatedDiscountCurveLinearZero as in QuantLib, but with
    floating discount quotes and floating reference date,
    reference date is always the global evaluation date,
    i.e. settlement days are zero and calendar is NullCalendar()

        \ingroup termstructures
*/
class InterpolatedDiscountCurveLinearZero : public YieldTermStructure, public LazyObject {
public:
    //! \name Constructors
    //@{
    //! times based constructor, note that times should be consistent with day counter dc passed
    InterpolatedDiscountCurveLinearZero(const std::vector<Time>& times, const std::vector<Handle<Quote> >& quotes,
                               const DayCounter& dc)
        : YieldTermStructure(dc), times_(times), quotes_(quotes), data_(times_.size(), 1.0),
          today_(Settings::instance().evaluationDate()) {
        for (Size i = 0; i < quotes.size(); ++i) {
            QL_REQUIRE(times_.size() > 1, "at least two times required");
            QL_REQUIRE(times_.size() == quotes.size(), "size of time and quote vectors do not match");
            QL_REQUIRE(times_[0] == 0.0, "First time must be 0, got " << times_[0]);
            QL_REQUIRE(!quotes[i].empty(), "quote at index " << i << " is empty");
            registerWith(quotes_[i]);
        }
        interpolation_ = boost::make_shared<LinearInterpolation>(times_.begin(), times_.end(), data_.begin());
        registerWith(Settings::instance().evaluationDate());
    }
    //! date based constructor
    InterpolatedDiscountCurveLinearZero(const std::vector<Date>& dates, const std::vector<Handle<Quote> >& quotes,
                                        const DayCounter& dc)
        : YieldTermStructure(dc), times_(dates.size(), 0.0), quotes_(quotes), data_(dates.size(), 1.0),
          today_(Settings::instance().evaluationDate()) {
        for (Size i = 0; i < dates.size(); ++i)
            times_[i] = dc.yearFraction(today_, dates[i]);
        for (Size i = 0; i < quotes.size(); ++i) {
            QL_REQUIRE(times_.size() > 1, "at least two times required");
            QL_REQUIRE(times_.size() == quotes.size(), "size of time and quote vectors do not match");
            QL_REQUIRE(times_[0] == 0.0, "first time must be 0, got " << times_[0]);
            QL_REQUIRE(!quotes[i].empty(), "quote at index " << i << " is empty");
            registerWith(quotes_[i]);
        }
        interpolation_ = boost::make_shared<LinearInterpolation>(times_.begin(), times_.end(), data_.begin());
        registerWith(Settings::instance().evaluationDate());
    }
    //@}

    Date maxDate() const { return Date::maxDate(); }
    void update() {
        LazyObject::update();
        TermStructure::update();
    }
    const Date& referenceDate() const {
        calculate();
        return today_;
    }

    Calendar calendar() const { return NullCalendar(); }
    Natural settlementDays() const { return 0; }

protected:
    void performCalculations() const {
        today_ = Settings::instance().evaluationDate();
        data_[0] = std::log(quotes_[1]->value()) / -times_[1];
        for (Size i = 1; i < times_.size(); ++i) {
            data_[i] = std::log(quotes_[i]->value()) / -times_[i];
        }
        interpolation_->update();
    }

    DiscountFactor discountImpl(Time t) const {
        calculate();
        if (t <= this->times_.back())
            return std::exp((*interpolation_)(t, true) * -t);
        // flat fwd extrapolation
        Time tMax = this->times_.back();
        Rate zMax = this->data_.back();
        Rate instFwdMax = zMax + tMax * this->interpolation_->derivative(tMax);
		Rate r = (zMax * tMax + instFwdMax * (t - tMax)) / t;
        return std::exp(r * -t);
    }

private:
    std::vector<Time> times_;
    std::vector<Handle<Quote> > quotes_;
    mutable std::vector<Real> data_;
    mutable Date today_;
    boost::shared_ptr<Interpolation> interpolation_;
};

} // namespace QuantExt


/**
 * Beck Pang, 20181029, the ROS node to add the perspective controller with wheel command
 * Reference:
 *  Chaumette, François, and Seth Hutchinson. "Visual servo control. I. Basic approaches.
 *  Chaumette, François, and Seth Hutchinson. "Visual servo control. II. Advanced approaches.
 */

#include <iostream>
#include <ros/ros.h>
#include <cmath>
#include <Eigen/Dense>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TwistStamped.h>
#include "rm_cv/vertice.h"
#include "filt.h"
// #include "rm_cv/ArmorRecord.h"
#include "III_VisualServoController.h"
#include "camera_model/camera_models/CameraFactory.h"

#include <dynamic_reconfigure/server.h>
#include <visual_servo_control/tuningConfig.h>

using namespace std;
using namespace Eigen;

ros::Publisher cmd_pub;
ros::Publisher kalman_output_pub;
ros::Publisher omega_raw_pub;
ros::Publisher omega_visual_pub;
ros::Publisher kalman_input_pub;
ros::Publisher delayed_gyro_pub;
ros::Publisher debug_z_pub;
string cv_topic;
string omega_input_topic;
string kalman_input_topic;
string kalman_output_topic;
string omega_raw_topic;
string omega_visual_topic;
string publisher_topic;
string delayed_gyro_topic;
string debug_distance_topic;

// Camera
std::string cfg_file_name;

camera_model::CameraPtr m_camera;

// Controller
int n = 4;
int m = 2;
double Kp = 1.0;
double Kd = 0.0;
double Kf_r0 = 0.01;
double Kf_q0 = 1.0;
double FIR_gain = 1.0;
double ctrl_freq = 30;
double distance_cuttoff_freq = 3;

double target_Z = 1.0;
const double MAX_DISTANCE = 3.0;
double pixel_x_max = 640;
double pixel_y_max = 512;
double pixel_dx = 68;
double pixel_dy = 25;
double Kp_z = 0.0;
double Kd_z = 0.0;

double z_filt = 1.0;
double error_z = 0.0;

// low pass Filter for distance
Filter *z_low_pass;

// Handle acyronized observer and control
VectorXd prev_ctl_val(6);
int cv_state_machine = 0;

bool visual_updated = false;
bool gyro_updated = false;
MatrixXd target_image_frame(n, m);

VisualServoController ctl;

enum class fsm {idle = 0, once = 1, multi = 2};

MatrixXd last_input_image_frame(n, m);

/**
 * Publish the linear and angular command to the chassis
 * @param cmd
 * @param pub
 */
void
publish_cmd(const Eigen::VectorXd &cmd, const ros::Publisher& pub)
{
    geometry_msgs::Twist vel_msg;
    vel_msg.linear.x  = cmd[0];
    vel_msg.linear.y  = cmd[1];
    vel_msg.linear.z  = cmd[2];
    vel_msg.angular.x = cmd[3];
    vel_msg.angular.y = cmd[4];
    vel_msg.angular.z = cmd[5];
    pub.publish(vel_msg);
}

void
publish_angular_velocity(const Eigen::VectorXd &estimated_omega, const ros::Publisher& pub)
{
    geometry_msgs::TwistStamped omega_msg;
    omega_msg.header.stamp = ros::Time::now();
    omega_msg.twist.angular.x = estimated_omega[0];
    omega_msg.twist.angular.y = estimated_omega[1];
    omega_msg.twist.angular.z = estimated_omega[2];
    pub.publish(omega_msg);
}

void
publish_distance_debug(double z_in, double z_out)
{
    geometry_msgs::TwistStamped distance_msg;
    distance_msg.header.stamp = ros::Time::now();
    distance_msg.twist.linear.z = z_in;
    distance_msg.twist.angular.z = z_out;
    debug_z_pub.publish(distance_msg);
}

void
visual_handle_feature(const Eigen::MatrixXd &input_pixel)
{
    visual_updated = true;

    Vector3d input_pixel_output[n];
    MatrixXd input_image_frame(n, m);

    for (int i = 0; i < n; ++i) {
        m_camera->liftSphere(input_pixel.row(i), input_pixel_output[i] );
        input_image_frame.row(i) << input_pixel_output[i](0), input_pixel_output[i](1);
    }
    // std::cout << "input in image frame " << std::endl << input_image_frame << std::endl;

    last_input_image_frame = input_image_frame;
}

void
visual_handle_distance(const Eigen::MatrixXd &input_pixel)
{
    double pixel_dy_n = 0.5 * ( input_pixel(1, 1) - input_pixel(0, 1) + input_pixel(3, 1) - input_pixel(2, 1) );

    double z_raw;
    if (pixel_dy_n * MAX_DISTANCE > pixel_dy) {
        z_raw  = target_Z * pixel_dy / pixel_dy_n;
    }
    else {
        z_raw  = target_Z;
    }

    double z_filt = FIR_gain * z_low_pass->do_sample(z_raw);

    ctl.setZ(z_filt);
    publish_distance_debug(z_raw * 1000, z_filt * 1000);

    error_z = target_Z - z_filt;
}

void
visual_feature_cb(const rm_cv::vertice::ConstPtr cv_ptr)
{
    // convert the pixel value to image coordinate value
    MatrixXd input_pixel(n, m);

    for (int i = 0; i < n; ++i) {
        input_pixel(i, 0) = cv_ptr->vertex[i].x;
        input_pixel(i, 1) = cv_ptr->vertex[i].y;
    }

    visual_handle_feature(input_pixel);

    visual_handle_distance(input_pixel);
}

/*
void
visual_feature_cb_with_z(const rm_cv::ArmorRecord::ConstPtr cv_ptr)
{
    MatrixXd input_pixel(n, m);

    for (int i = 0; i < n; ++i) {
        input_pixel(i, 0) = cv_ptr->vertex[i].x;
        input_pixel(i, 1) = cv_ptr->vertex[i].y;
    }

    visual_handle_feature(input_pixel);

    double z_raw  = cv_ptr->armorPose.linear.z;
    double z_filt = FIR_gain * z_low_pass->do_sample(z_raw);
    ctl.setZ(z_filt);
    publish_distance_debug(z_raw, z_filt);
}
 */

void
omega_cam_cb(const geometry_msgs::TwistStamped::ConstPtr omega_ptr){
    gyro_updated = true;

    MatrixXd input_omega(3, 1);
    input_omega(0, 0) = omega_ptr->twist.angular.x;
    input_omega(1, 0) = omega_ptr->twist.angular.y;
    input_omega(2, 0) = omega_ptr->twist.angular.z;

    Eigen::MatrixXd end_R_cam(3, 3);

    end_R_cam <<
        0, -1, 0,
        0, 0, -1,
        1, 0, 0;

    VectorXd input_omega_cam = end_R_cam * input_omega;
    // std::cout << "input in omega" << std::endl << input_omega << std::endl;
    publish_angular_velocity(input_omega_cam, omega_raw_pub);

    ctl.updateOmega(input_omega_cam);
}

/**
 * callback function for dynamic reconfigure
 */
void configCallback(visual_servo_control::tuningConfig &config, uint32_t level __attribute__((unused)))
{
    Kp = config.Kp;
    Kd = config.Kd;
    Kp_z = config.Kp_z;
    Kd_z = config.Kd_z;
    Kf_r0 = config.Kf_r0;
    Kf_q0 = config.Kf_q0;
    FIR_gain = config.FIR_gain;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "four_point_visual_servo");
    ros::NodeHandle nh("~");

    nh.param("Kp", Kp, -1.0);
    nh.param("Kd", Kd, 0.0);
    nh.param("Kp_z", Kp_z, 0.0);
    nh.param("Kd_z", Kd_z, 0.0);
    nh.param("Kf_r0", Kf_r0, 0.01);
    nh.param("Kf_r0", Kf_r0, 1.0);
    nh.param("ctrl_freq", ctrl_freq, 30.0);
    nh.param("distance_cuttoff_freq", distance_cuttoff_freq, 3.0);
    nh.param("target_Z", target_Z, 1.0);
    nh.param("pixel_dx", pixel_dx, 68.0);
    nh.param("pixel_dy", pixel_dy, 25.0);
    nh.param("FIR_gain", FIR_gain, 1.0);
    nh.param("cv_topic", cv_topic, string("/detected_vertice"));
    nh.param("omega_input_topic", omega_input_topic, string("/can_receive_1/end_effector_omega"));

    nh.param("publisher_topic", publisher_topic, string("/cmd_vel"));
    nh.param("kalman_input_topic",  kalman_input_topic,  string("/visual_servo/kalman_input"));
    nh.param("kalman_output_topic", kalman_output_topic, string("/visual_servo/kalman_output"));
    nh.param("omega_raw_topic", omega_raw_topic, string("/visual_servo/raw_omega_cam"));
    nh.param("omega_visual_topic", omega_visual_topic, string("/visual_servo/omega_visual"));
    nh.param("delayed_gyro_topic", delayed_gyro_topic, string("/visual_servo/delayed_gyro"));
    nh.param("debug_distance_topic", debug_distance_topic, string("/visual_servo/distance_compare"));

    nh.param("cfg_file_name", cfg_file_name, string("/home/nvidia/ws/src/6_controller/gimbal_controller/cfg/camera_tracking_camera_calib.yaml"));

    // dynamic reconfigure server
    dynamic_reconfigure::Server<visual_servo_control::tuningConfig> dr_server;
    dynamic_reconfigure::Server<visual_servo_control::tuningConfig>::CallbackType dr_callback;
    dr_callback = boost::bind(&configCallback, _1, _2);
    dr_server.setCallback(dr_callback);

    ros::Subscriber sub1 = nh.subscribe(cv_topic, 10, visual_feature_cb);
    ros::Subscriber sub2 = nh.subscribe(omega_input_topic, 10, omega_cam_cb);
    cmd_pub = nh.advertise<geometry_msgs::Twist>(publisher_topic, 10);
    omega_raw_pub     = nh.advertise<geometry_msgs::TwistStamped>(omega_raw_topic, 10);
    omega_visual_pub  = nh.advertise<geometry_msgs::TwistStamped>(omega_visual_topic, 10);
    kalman_input_pub  = nh.advertise<geometry_msgs::TwistStamped>(kalman_input_topic, 10);
    kalman_output_pub = nh.advertise<geometry_msgs::TwistStamped>(kalman_output_topic, 10);
    delayed_gyro_pub  = nh.advertise<geometry_msgs::TwistStamped>(delayed_gyro_topic, 10);
    debug_z_pub       = nh.advertise<geometry_msgs::TwistStamped>(debug_distance_topic, 10);

    // create a camera model
    m_camera = camera_model::CameraFactory::instance()->generateCameraFromYamlFile(cfg_file_name);

    fsm finite_state = fsm::idle;

    // create the low pass filter
    z_low_pass = new Filter(LPF, 10, 2 * ctrl_freq, distance_cuttoff_freq);

    // setup the target coordinate
    MatrixXd target_pixel(n, m);
    MatrixXd target_image_frame(n, m);

    double pixel_x_down= (pixel_x_max - pixel_dx) * 0.5;
    double pixel_x_top_= (pixel_x_max + pixel_dx) * 0.5;
    double pixel_y_down= (pixel_y_max - pixel_dy) * 0.5;
    double pixel_y_top_= (pixel_y_max + pixel_dy) * 0.5;

    target_pixel <<
            pixel_x_down, pixel_y_down,
    	    pixel_x_down, pixel_y_top_,
    	    pixel_x_top_, pixel_y_down,
    	    pixel_x_top_, pixel_y_top_; // 1000 mm

    Vector3d target_pixel_output[n];

    for	(int i=0; i < n; ++i) {
        m_camera->liftSphere(target_pixel.row(i), target_pixel_output[i] );
        target_image_frame.row(i) << target_pixel_output[i](0), target_pixel_output[i](1);
    }
    std::cout << "target in image frame " << std::endl << target_image_frame << std::endl;

    // set up the controller
    ctl.setTarget(target_image_frame);

    ctl.setTargetZ(target_Z);

    ctl.setZ(target_Z);

    ctl.setControlFrequency(ctrl_freq);

    ctl.initKalmanFilter(Kf_r0, Kf_q0, 1 / ctrl_freq);

    prev_ctl_val.setZero();

    Eigen::MatrixXd cam_R_end(6, 6);
    cam_R_end.setIdentity();

    // linear velocity vx, vy, and vz also need to switch to chassis frame
    cam_R_end.block(0, 0, 3, 3) <<
           0, -1, 0,
           -1, 0, 0,
           0,  0, 1;

    cam_R_end.block(3, 3, 3, 3) <<
         0, 0, 1,
        -1, 0, 0,
         0,-1, 0;
    std::cout << "cam_R_end_effector: " << std::endl << cam_R_end << std::endl;

    ros::Rate rate(ctrl_freq);

    while (ros::ok()) {
        /**
         * Change setable values in the controller with dynamics configure
         */
        ctl.setKp(Kp);
        ctl.setKd(Kd);
        ctl.setKalmanR(Kf_r0);
        ctl.setKalmanQ(Kf_q0);

        /**
         * finite finite_state machine
         */
        switch (finite_state){
            case fsm::idle:
                if (visual_updated) finite_state = fsm::once;
                else                finite_state = fsm::idle;
                break;
            case fsm::once:
                if (visual_updated) finite_state = fsm::multi;
                else                finite_state = fsm::idle;
                break;
            case fsm::multi:
                if (visual_updated) finite_state = fsm::multi;
                else                finite_state = fsm::idle;
                break;
        }

        if (finite_state == fsm::idle) {
            ctl.finite_state = 0;

            publish_cmd(prev_ctl_val, cmd_pub);
            prev_ctl_val.setZero();
        }
        else if (finite_state == fsm::once) {
            ctl.finite_state = 1;

            ctl.updateFeatures(last_input_image_frame);
            VectorXd ctl_val = ctl.control();

            VectorXd ctl_val_rot = cam_R_end * ctl_val;
            ctl_val_rot(0) += Kp_z * error_z;

            publish_cmd(ctl_val_rot, cmd_pub);
            prev_ctl_val = ctl_val_rot;
        }
        else if (finite_state == fsm::multi) {
            ctl.finite_state = 2;

            ctl.updateFeatures(last_input_image_frame);
            VectorXd ctl_val = ctl.control();

            VectorXd ctl_val_rot = cam_R_end * ctl_val;
            ctl_val_rot(0) += Kp_z * error_z;

            publish_cmd(ctl_val_rot, cmd_pub);
            prev_ctl_val = ctl_val_rot;
        }

        if (finite_state == fsm::multi && gyro_updated) {
            VectorXd kalman_output = ctl.getKalmanOutput();
            VectorXd kalman_input  = ctl.getKalmanInput();
            VectorXd raw_visual_w  = ctl.getRawVisualOmega();
            VectorXd delay_gyro    = ctl.getDelayedGyro();
            publish_angular_velocity(kalman_output, kalman_output_pub);
            publish_angular_velocity(kalman_input, kalman_input_pub);
            publish_angular_velocity(raw_visual_w, omega_visual_pub);
            publish_angular_velocity(delay_gyro, delayed_gyro_pub);
        }

        // remove flag
        visual_updated = false;
        gyro_updated = false;
        rate.sleep();
        ros::spinOnce();
    }
}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2014-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#include <rokko/localized_matrix.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

#define BOOST_TEST_MODULE test_matrix_major
#ifndef BOOST_TEST_DYN_LINK
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

BOOST_AUTO_TEST_CASE(test_matrix_major) {
  int dim = 3;

  // M = 1 2 3
  //     4 5 6
  //     7 8 9

  rokko::localized_matrix<double, rokko::matrix_row_major> M0(dim,dim); // row major
  M0 << 1,2,3,4,5,6,7,8,9;
  std::cout << M0 << std::endl;
  BOOST_CHECK_EQUAL(M0(0,0),1.0);
  BOOST_CHECK_EQUAL(M0(0,1),2.0);
  BOOST_CHECK_EQUAL(M0(1,0),4.0);
  double* ptr0 = &M0(0,0);
  BOOST_CHECK_EQUAL(*(ptr0 + 1), 2.0); // row major
  BOOST_CHECK_EQUAL((M0.row(0))(1), 2.0);
  BOOST_CHECK_EQUAL((M0.col(0))(1), 4.0);

  rokko::localized_matrix<double, rokko::matrix_col_major> M1(dim,dim); // column major
  M1 << 1,2,3,4,5,6,7,8,9;
  std::cout << M1 << std::endl;
  BOOST_CHECK_EQUAL(M1(0,0),1.0);
  BOOST_CHECK_EQUAL(M1(0,1),2.0);
  BOOST_CHECK_EQUAL(M1(1,0),4.0);
  double* ptr1 = &M1(0,0);
  BOOST_CHECK_EQUAL(*(ptr1 + 1), 4.0); // column major
  BOOST_CHECK_EQUAL((M1.row(0))(1), 2.0);
  BOOST_CHECK_EQUAL((M1.col(0))(1), 4.0);

  boost::numeric::ublas::matrix<double> M2(dim,dim); // row major
  for (int i = 0; i < 3; ++i) for (int j = 0; j < 3; ++j) M2(i,j) = M0(i,j);
  std::cout << M2 << std::endl;
  BOOST_CHECK_EQUAL(M2(0,0),1.0);
  BOOST_CHECK_EQUAL(M2(0,1),2.0);
  BOOST_CHECK_EQUAL(M2(1,0),4.0);
  double* ptr2 = &M2(0,0);
  BOOST_CHECK_EQUAL(*(ptr2 + 1), 2.0); // row major

  boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> M3(dim,dim); // column major
  for (int i = 0; i < 3; ++i) for (int j = 0; j < 3; ++j) M3(i,j) = M0(i,j);
  std::cout << M3 << std::endl;
  BOOST_CHECK_EQUAL(M3(0,0),1.0);
  BOOST_CHECK_EQUAL(M3(0,1),2.0);
  BOOST_CHECK_EQUAL(M3(1,0),4.0);
  double* ptr3 = &M3(0,0);
  BOOST_CHECK_EQUAL(*(ptr3 + 1), 4.0); // column major

  Eigen::MatrixXd M4(dim, dim); // column major
  for (int i = 0; i < 3; ++i) for (int j = 0; j < 3; ++j) M4(i,j) = M0(i,j);
  std::cout << M4 << std::endl;
  BOOST_CHECK_EQUAL(M4(0,0),1.0);
  BOOST_CHECK_EQUAL(M4(0,1),2.0);
  BOOST_CHECK_EQUAL(M4(1,0),4.0);
  double* ptr4 = &M4(0,0);
  BOOST_CHECK_EQUAL(*(ptr4 + 1), 4.0); // column major
}


// Copyright (C) 2010  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "tester.h"
#include <dlib/svm.h>
#include <dlib/rand.h>
#include <dlib/string.h>
#include <vector>
#include <sstream>
#include <ctime>
#include <dlib/data_io.h>

namespace
{
    using namespace test;
    using namespace dlib;
    using namespace std;
    dlib::logger dlog("test.sldf");

    class sldf_tester : public tester
    {
        /*!
            WHAT THIS OBJECT REPRESENTS
                This object represents a unit test.  When it is constructed
                it adds itself into the testing framework.
        !*/
    public:
        sldf_tester (
        ) :
            tester (
                "test_sldf",       // the command line argument name for this test
                "Run tests on the simplify_linear_decision_function routines.", // the command line argument description
                0                     // the number of command line arguments for this test
            )
        {
        }

        dlib::rand rnd;

        void perform_test (
        )
        {
            print_spinner();
            typedef std::map<unsigned long,double> sample_type;

            typedef matrix<double,0,1> dense_sample_type;

            typedef sparse_linear_kernel<sample_type> kernel_type;
            typedef linear_kernel<dense_sample_type> dense_kernel_type;

            svm_nu_trainer<kernel_type> linear_trainer;
            linear_trainer.set_nu(0.2);
            svm_nu_trainer<dense_kernel_type> dense_linear_trainer;
            dense_linear_trainer.set_nu(0.2);

            std::vector<sample_type> samples;
            std::vector<double> labels;

            // make an instance of a sample vector so we can use it below
            sample_type sample;

            // Now lets go into a loop and randomly generate 300 samples.
            double label = +1;
            for (int i = 0; i < 300; ++i)
            {
                // flip this flag
                label *= -1;

                sample.clear();

                // now make a random sparse sample with at most 10 non-zero elements
                for (int j = 0; j < 10; ++j)
                {
                    int idx = rnd.get_random_32bit_number()%100;
                    double value = rnd.get_random_double();

                    sample[idx] = label*value;
                }

                // Also save the samples we are generating so we can let the svm_c_linear_trainer
                // learn from them below.
                samples.push_back(sample);
                labels.push_back(label);
            }

            {
                print_spinner();
                dlog << LINFO << " test with sparse samples ";
                decision_function<kernel_type> df = linear_trainer.train(samples, labels);

                dlog << LINFO << "df.basis_vectors.size(): "<< df.basis_vectors.size();
                DLIB_TEST(df.basis_vectors.size() > 4);

                dlog << LINFO << "test scores: "<< test_binary_decision_function(df, samples, labels);

                // save the outputs of the decision function before we mess with it
                std::vector<double> prev_vals;
                for (unsigned long i = 0; i < samples.size(); ++i)
                    prev_vals.push_back(df(samples[i]));

                df = simplify_linear_decision_function(df);

                dlog << LINFO << "df.basis_vectors.size(): "<< df.basis_vectors.size();
                DLIB_TEST(df.basis_vectors.size() == 1);

                dlog << LINFO << "test scores: "<< test_binary_decision_function(df, samples, labels);

                // now check that the simplified decision function still produces the same results
                std::vector<double> cur_vals;
                for (unsigned long i = 0; i < samples.size(); ++i)
                    cur_vals.push_back(df(samples[i]));

                const double err = max(abs(mat(cur_vals) - mat(prev_vals)));
                dlog << LINFO << "simplify error: "<< err;
                DLIB_TEST(err < 1e-13);

            }

            // same as above but call simplify_linear_decision_function() two times
            {
                print_spinner();
                dlog << LINFO << " test with sparse samples ";
                decision_function<kernel_type> df = linear_trainer.train(samples, labels);

                dlog << LINFO << "df.basis_vectors.size(): "<< df.basis_vectors.size();
                DLIB_TEST(df.basis_vectors.size() > 4);

                dlog << LINFO << "test scores: "<< test_binary_decision_function(df, samples, labels);

                // save the outputs of the decision function before we mess with it
                std::vector<double> prev_vals;
                for (unsigned long i = 0; i < samples.size(); ++i)
                    prev_vals.push_back(df(samples[i]));

                df = simplify_linear_decision_function(df);
                df = simplify_linear_decision_function(df);

                dlog << LINFO << "df.basis_vectors.size(): "<< df.basis_vectors.size();
                DLIB_TEST(df.basis_vectors.size() == 1);

                dlog << LINFO << "test scores: "<< test_binary_decision_function(df, samples, labels);

                // now check that the simplified decision function still produces the same results
                std::vector<double> cur_vals;
                for (unsigned long i = 0; i < samples.size(); ++i)
                    cur_vals.push_back(df(samples[i]));

                const double err = max(abs(mat(cur_vals) - mat(prev_vals)));
                dlog << LINFO << "simplify error: "<< err;
                DLIB_TEST(err < 1e-13);

            }

            {
                print_spinner();
                dlog << LINFO << " test with dense samples ";
                std::vector<dense_sample_type> dense_samples(sparse_to_dense(samples));

                // In addition to the rule we learned with the pegasos trainer lets also use our linear_trainer
                // to learn a decision rule.
                decision_function<dense_kernel_type> dense_df = dense_linear_trainer.train(dense_samples, labels);

                dlog << LINFO << "dense_df.basis_vectors.size(): "<< dense_df.basis_vectors.size();
                DLIB_TEST(dense_df.basis_vectors.size() > 4);

                dlog << LINFO << "test scores: "<< test_binary_decision_function(dense_df, dense_samples, labels);

                // save the outputs of the decision function before we mess with it
                std::vector<double> prev_vals;
                for (unsigned long i = 0; i < dense_samples.size(); ++i)
                    prev_vals.push_back(dense_df(dense_samples[i]));

                dense_df = simplify_linear_decision_function(dense_df);

                dlog << LINFO << "dense_df.basis_vectors.size(): "<< dense_df.basis_vectors.size();
                DLIB_TEST(dense_df.basis_vectors.size() == 1);

                dlog << LINFO << "test scores: "<< test_binary_decision_function(dense_df, dense_samples, labels);

                // now check that the simplified decision function still produces the same results
                std::vector<double> cur_vals;
                for (unsigned long i = 0; i < dense_samples.size(); ++i)
                    cur_vals.push_back(dense_df(dense_samples[i]));

                const double err = max(abs(mat(cur_vals) - mat(prev_vals)));
                dlog << LINFO << "simplify error: "<< err;
                DLIB_TEST(err < 1e-13);
            }

            // same as above but call simplify_linear_decision_function() two times
            {
                print_spinner();
                dlog << LINFO << " test with dense samples ";
                std::vector<dense_sample_type> dense_samples(sparse_to_dense(samples));

                // In addition to the rule we learned with the pegasos trainer lets also use our linear_trainer
                // to learn a decision rule.
                decision_function<dense_kernel_type> dense_df = dense_linear_trainer.train(dense_samples, labels);

                dlog << LINFO << "dense_df.basis_vectors.size(): "<< dense_df.basis_vectors.size();
                DLIB_TEST(dense_df.basis_vectors.size() > 4);

                dlog << LINFO << "test scores: "<< test_binary_decision_function(dense_df, dense_samples, labels);

                // save the outputs of the decision function before we mess with it
                std::vector<double> prev_vals;
                for (unsigned long i = 0; i < dense_samples.size(); ++i)
                    prev_vals.push_back(dense_df(dense_samples[i]));

                dense_df = simplify_linear_decision_function(dense_df);
                dense_df = simplify_linear_decision_function(dense_df);

                dlog << LINFO << "dense_df.basis_vectors.size(): "<< dense_df.basis_vectors.size();
                DLIB_TEST(dense_df.basis_vectors.size() == 1);

                dlog << LINFO << "test scores: "<< test_binary_decision_function(dense_df, dense_samples, labels);

                // now check that the simplified decision function still produces the same results
                std::vector<double> cur_vals;
                for (unsigned long i = 0; i < dense_samples.size(); ++i)
                    cur_vals.push_back(dense_df(dense_samples[i]));

                const double err = max(abs(mat(cur_vals) - mat(prev_vals)));
                dlog << LINFO << "simplify error: "<< err;
                DLIB_TEST(err < 1e-13);
            }

            {
                print_spinner();

                dlog << LINFO << " test with sparse samples and a vector normalizer";
                std::vector<dense_sample_type> dense_samples(sparse_to_dense(samples));
                std::vector<dense_sample_type> norm_samples;

                // make a normalizer and normalize everything
                vector_normalizer<dense_sample_type> normalizer;
                normalizer.train(dense_samples);
                for (unsigned long i = 0; i < dense_samples.size(); ++i)
                    norm_samples.push_back(normalizer(dense_samples[i]));

                normalized_function<decision_function<dense_kernel_type> > dense_df;

                dense_df.normalizer = normalizer;
                dense_df.function = dense_linear_trainer.train(norm_samples, labels);

                dlog << LINFO << "dense_df.function.basis_vectors.size(): "<< dense_df.function.basis_vectors.size();
                DLIB_TEST(dense_df.function.basis_vectors.size() > 4);

                dlog << LINFO << "test scores: "<< test_binary_decision_function(dense_df, dense_samples, labels);

                // save the outputs of the decision function before we mess with it
                std::vector<double> prev_vals;
                for (unsigned long i = 0; i < dense_samples.size(); ++i)
                    prev_vals.push_back(dense_df(dense_samples[i]));

                decision_function<dense_kernel_type> simple_df = simplify_linear_decision_function(dense_df);

                dlog << LINFO << "simple_df.basis_vectors.size(): "<< simple_df.basis_vectors.size();
                DLIB_TEST(simple_df.basis_vectors.size() == 1);

                dlog << LINFO << "test scores: "<< test_binary_decision_function(simple_df, dense_samples, labels);

                // now check that the simplified decision function still produces the same results
                std::vector<double> cur_vals;
                for (unsigned long i = 0; i < dense_samples.size(); ++i)
                    cur_vals.push_back(simple_df(dense_samples[i]));

                const double err = max(abs(mat(cur_vals) - mat(prev_vals)));
                dlog << LINFO << "simplify error: "<< err;
                DLIB_TEST(err < 1e-13);

            }

        }
    };

    // Create an instance of this object.  Doing this causes this test
    // to be automatically inserted into the testing framework whenever this cpp file
    // is linked into the project.  Note that since we are inside an unnamed-namespace
    // we won't get any linker errors about the symbol a being defined multiple times.
    sldf_tester a;

}


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <vector>
#include <list>
#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <iterator>
#include <iostream>

namespace boost
{
  struct edge_component_t
  {
    enum
    { num = 555 };
    typedef edge_property_tag kind;
  }
  edge_component;
}

int
main()
{
  using namespace boost;
  typedef adjacency_list < vecS, vecS, undirectedS,
    no_property, property < edge_component_t, std::size_t > >graph_t;
  typedef graph_traits < graph_t >::vertex_descriptor vertex_t;
  graph_t g(9);
  add_edge(0, 5, g);
  add_edge(0, 1, g);
  add_edge(0, 6, g);
  add_edge(1, 2, g);
  add_edge(1, 3, g);
  add_edge(1, 4, g);
  add_edge(2, 3, g);
  add_edge(4, 5, g);
  add_edge(6, 8, g);
  add_edge(6, 7, g);
  add_edge(7, 8, g);

  property_map < graph_t, edge_component_t >::type
    component = get(edge_component, g);

  std::size_t num_comps = biconnected_components(g, component);
  std::cerr << "Found " << num_comps << " biconnected components.\n";

  std::vector<vertex_t> art_points;
  articulation_points(g, std::back_inserter(art_points));
  std::cerr << "Found " << art_points.size() << " articulation points.\n";

  std::cout << "graph A {\n" << "  node[shape=\"circle\"]\n";

  for (std::size_t i = 0; i < art_points.size(); ++i) {
    std::cout << (char)(art_points[i] + 'A')
              << " [ style=\"filled\", fillcolor=\"red\" ];"
              << std::endl;
  }

  graph_traits < graph_t >::edge_iterator ei, ei_end;
  for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    std::cout << (char)(source(*ei, g) + 'A') << " -- "
              << (char)(target(*ei, g) + 'A')
              << "[label=\"" << component[*ei] << "\"]\n";
  std::cout << "}\n";

  return 0;
}


// Copyright (C) 2014 The Regents of the University of California (Regents)
// and Google, Inc. All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California, Google,
//       nor the names of its contributors may be used to endorse or promote
//       products derived from this software without specific prior written
//       permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu), John Flynn (jflynn@google.com)

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <glog/logging.h>

#include <limits>
#include <cmath>
#include <vector>

#include "gtest/gtest.h"
#include "theia/math/util.h"
#include "theia/test/test_utils.h"
#include "theia/util/util.h"
#include "theia/util/random.h"
#include "theia/sfm/pose/three_point_relative_pose_partial_rotation.h"
#include "theia/sfm/pose/test_util.h"

namespace theia {

using Eigen::AngleAxisd;
using Eigen::Matrix3d;
using Eigen::Vector3d;
using Eigen::Quaterniond;

// Tests that the three point pose works correctly by taking the passed
// points_3d, projecting them to get image one rays, transforming by
// (expected_rotation, expected_translation) to get image two rays and then
// verifying that the ThreePointRelativePosePartialRotation function returns
// (test_rotation, test_translation_direction) among its solutions.
// Noise can be added to the image projections by setting projection_noise.
// The thresholds for rotation and translation similarity can be controlled
// by max_rotation_difference and max_difference_between_translation.
// If projection_noise_std_dev is non-zero then a random noise generator must
// be passed in rng.
void TestThreePointResultWithNoise(const Vector3d& axis,
                                   const Vector3d points_3d[3],
                                   const double projection_noise_std_dev,
                                   const Quaterniond& expected_rotation,
                                   const Vector3d& expected_translation,
                                   const double max_rotation_difference,
                                   const double max_angle_between_translation) {
  // Calculates the image rays in both views.
  Vector3d view_one_rays[3];
  Vector3d view_two_rays[3];
  for (int i = 0; i < 3; ++i) {
    const Vector3d proj_3d =
        expected_rotation * points_3d[i] + expected_translation;
    view_one_rays[i] = points_3d[i].normalized();
    view_two_rays[i] = proj_3d.normalized();
  }
  if (projection_noise_std_dev) {
    // Adds noise to both of the rays.
    for (int i = 0; i < 3; ++i) {
      Eigen::Vector2d view_one_ray = view_one_rays[i].hnormalized();
      AddNoiseToProjection(projection_noise_std_dev, &view_one_ray);
      view_one_rays[i] = view_one_ray.homogeneous().normalized();

      Eigen::Vector2d view_two_ray = view_two_rays[i].hnormalized();
      AddNoiseToProjection(projection_noise_std_dev, &view_two_ray);
      view_two_rays[i] = view_two_ray.homogeneous().normalized();
    }
  }

  // Calculates the essential matrix, this may return multiple solutions.
  std::vector<Quaterniond> soln_rotations;
  std::vector<Vector3d> soln_translations;
  ThreePointRelativePosePartialRotation(axis,
                                        view_one_rays,
                                        view_two_rays,
                                        &soln_rotations,
                                        &soln_translations);
  EXPECT_GT(soln_rotations.size(), 0);

  // Among the returned solutions verify that at least one is close to the
  // expected translation and rotation.
  bool matched_transform = false;
  for (int n = 0; n < soln_rotations.size(); ++n) {
    double rotation_difference =
        expected_rotation.angularDistance(soln_rotations[n]);

    bool matched_rotation = (rotation_difference < max_rotation_difference);

    // The translation is only known up to scale so this verifies that the
    // translations have matching directions.
    const double translation_angle_difference =
        AngleAxisd(Quaterniond::FromTwoVectors(soln_translations[n],
                                               expected_translation)).angle();

    bool matched_translation =
        (translation_angle_difference < max_angle_between_translation);

    if (matched_translation && matched_rotation) {
      matched_transform = true;
    }
  }
  CHECK(matched_transform);
}

// A basic test with a few points and no noise.
void BasicTest() {
  const Vector3d points_3d[3] = {Vector3d(-1.0, 3.0, 3.0),
                                  Vector3d(1.0, -1.0, 2.0),
                                  Vector3d(2.0, 1.0, 3.0)};
  const Vector3d axis = Vector3d(0.0, 0.0, 1.0).normalized();
  Quaterniond kExpectedRotation(AngleAxisd(DegToRad(51.0), axis));

  const Vector3d kExpectedTranslation(1.0, 1.0, 1.0);

  const double kProjectionNoise = 0.0;
  const double kMaxAllowedRotationDifference = 1e-5;
  const double kMaxAllowedAngleBetweenTranslations = 1e-8;

  TestThreePointResultWithNoise(axis,
                                points_3d,
                                kProjectionNoise,
                                kExpectedRotation,
                                kExpectedTranslation,
                                kMaxAllowedRotationDifference,
                                kMaxAllowedAngleBetweenTranslations);
}

TEST(ThreePointRelativePosePartialRotationTest, Basic) {
  BasicTest();
}

// A basic test with a few points, no noise and no rotation.
TEST(ThreePointRelativePosePartialRotationTest, NoRotationTest) {
  const Vector3d points_3d[3] = {Vector3d(-1.0, 3.0, 3.0),
                                  Vector3d(1.0, -1.0, 2.0),
                                  Vector3d(2.0, 1.0, 3.0)};
  // This test fails with these points.
  // const Vector3d points_3d[3] = {Vector3d(-1.0, 3.0, -3.0),
  //                                 Vector3d(1.0, -1.0, 2.0),
  //                                 Vector3d(2.0, 1.0, 9.0)};
  const Vector3d axis = Vector3d(0.0, 0.0, 1.0).normalized();
  Quaterniond kExpectedRotation(AngleAxisd(0.0, axis));

  const Vector3d kExpectedTranslation(1.0, 1.0, 1.0);

  double kProjectionNoise = 0.0;
  double kMaxAllowedRotationDifference = 1e-5;
  double kMaxAllowedAngleBetweenTranslations = 1e-4;
  TestThreePointResultWithNoise(axis,
                                points_3d,
                                kProjectionNoise,
                                kExpectedRotation,
                                kExpectedTranslation,
                                kMaxAllowedRotationDifference,
                                kMaxAllowedAngleBetweenTranslations);
}

// Tests a variety of axes, angles and translations with added projection noise.
TEST(ThreePointRelativePosePartialRotationTest, NoiseTest) {
  const Vector3d kPoints3D[3] = { Vector3d(-1.0, 3.0, 3.0),
                                  Vector3d(1.0, -1.0, 2.0),
                                  Vector3d(2.0, 1.0, 3.0) };

  const Vector3d kAxes[7] = {
      Vector3d(0.0, 0.0, 1.0).normalized(),
      Vector3d(0.0, 1.0, 0.0).normalized(),
      Vector3d(1.0, 0.0, 0.0).normalized(),
      Vector3d(1.0, 0.0, 1.0).normalized(),
      Vector3d(0.0, 1.0, 1.0).normalized(),
      Vector3d(1.0, 1.0, 0.0).normalized(),
      Vector3d(1.0, 1.0, 1.0).normalized()
  };

  const double kAngles[7] = { 11.0, 7.0, 5.0, 2.0, 3.0, 13.0, 12.0 };

  const Vector3d kTranslations[7] = {
      Vector3d(1.0, 1.0, 1.0),
      Vector3d(3.0, 2.0, 13.0),
      Vector3d(4.0, 5.0, 11.0),
      Vector3d(1.0, 2.0, 15.0),
      Vector3d(3.0, 1.5, 91.0),
      Vector3d(6.0, 3.0, 2.0),
      Vector3d(13.0, 1.0, 15.0)
  };

  InitRandomGenerator();
  for (int transform_index = 0; transform_index < THEIA_ARRAYSIZE(kAxes);
       ++transform_index) {
    Quaterniond kExpectedRotation(
        AngleAxisd(DegToRad(kAngles[transform_index]), kAxes[transform_index]));
    kExpectedRotation.normalize();

    const double kProjectionNoise = 1.0 / 512;
    const double kMaxAllowedRotationDifference = DegToRad(8.0);
    const double kMaxAllowedAngleBetweenTranslations = DegToRad(2.0);
    TestThreePointResultWithNoise(kAxes[transform_index],
                                  kPoints3D,
                                  kProjectionNoise,
                                  kExpectedRotation,
                                  kTranslations[transform_index],
                                  kMaxAllowedRotationDifference,
                                  kMaxAllowedAngleBetweenTranslations);
  }
}

// Tests that the solver degrade gracefully when the passed axis is not exactly
// correct.
TEST(ThreePointRelativePosePartialRotationTest, IncorrectAxisTest) {
  const Vector3d kPoints3D[3] = { Vector3d(-1.0, 3.0, 3.0),
                                  Vector3d(1.0, -1.0, 2.0),
                                  Vector3d(2.0, 1.0, 3.0) };

  const Vector3d kAxes[7] = {
      Vector3d(0.0, 0.0, 1.0).normalized(),
      Vector3d(0.0, 1.0, 0.0).normalized(),
      Vector3d(1.0, 0.0, 0.0).normalized(),
      Vector3d(1.0, 0.0, 1.0).normalized(),
      Vector3d(0.0, 1.0, 1.0).normalized(),
      Vector3d(1.0, 1.0, 0.0).normalized(),
      Vector3d(1.0, 1.0, 1.0).normalized()
  };

  const double kAngles[7] = { 11.0, 7.0, 5.0, 2.0, 3.0, 13.0, 12.0 };

  const Vector3d kTranslations[7] = {
      Vector3d(1.0, 1.0, 1.0),
      Vector3d(3.0, 2.0, 13.0),
      Vector3d(4.0, 5.0, 11.0),
      Vector3d(1.0, 2.0, 15.0),
      Vector3d(3.0, 1.5, 91.0),
      Vector3d(6.0, 3.0, 2.0),
      Vector3d(13.0, 1.0, 15.0)
  };

  // The axes are perturbed by these rotation to simulate the axis not being
  // perfectly known.
  const Quaterniond kAxisPerturbations[6] = {
    Quaterniond(AngleAxisd(DegToRad(1.0), Vector3d(1.0, 0, 0))),
    Quaterniond(AngleAxisd(DegToRad(-1.0), Vector3d(1.0, 0, 0))),
    Quaterniond(AngleAxisd(DegToRad(1.0), Vector3d(0.0, 1.0, 0))),
    Quaterniond(AngleAxisd(DegToRad(-1.0), Vector3d(0.0, 1.0, 0))),
    Quaterniond(AngleAxisd(DegToRad(1.0), Vector3d(0.0, 0.0, 1.0))),
    Quaterniond(AngleAxisd(DegToRad(-1.0), Vector3d(0.0, 0.0, 1.0)))
  };

  for (int transform_index = 0;
       transform_index < THEIA_ARRAYSIZE(kAxes);
       ++transform_index) {
    for (int axis_rotation_index = 0;
         axis_rotation_index < THEIA_ARRAYSIZE(kAxisPerturbations);
         ++axis_rotation_index) {
      // Perturbs the axis by the axis perturbation.
      const Vector3d perturbed_axis =
          kAxisPerturbations[axis_rotation_index] * kAxes[transform_index];
      // Uses the perturbed rotation as the expected rotation, but the original
      // axis is passed as the axis.
      Quaterniond kExpectedRotation(
          AngleAxisd(DegToRad(kAngles[transform_index]), perturbed_axis));

      // Tests the ThreePointRelativePosePartialRotation function with fairly
      // large thresholds on the allowed translation and rotation differences.
      const double kProjectionNoise = 0.0;
      const double kMaxAllowedRotationDifference =
          DegToRad(2.0);
      const double kMaxAllowedAngleBetweenTranslations =
          DegToRad(2.0);

      TestThreePointResultWithNoise(kAxes[transform_index],
                                    kPoints3D,
                                    kProjectionNoise,
                                    kExpectedRotation,
                                    kTranslations[transform_index],
                                    kMaxAllowedRotationDifference,
                                    kMaxAllowedAngleBetweenTranslations);
    }
  }
}

// Comprehensive test with many points and different axes and translations.
TEST(ThreePointRelativePosePartialRotationTest, ManyPoints) {
  const Vector3d kAxes[7] = {
      Vector3d(0.0, 0.0, 1.0).normalized(),
      Vector3d(0.0, 1.0, 0.0).normalized(),
      Vector3d(1.0, 0.0, 0.0).normalized(),
      Vector3d(1.0, 0.0, 1.0).normalized(),
      Vector3d(0.0, 1.0, 1.0).normalized(),
      Vector3d(1.0, 1.0, 0.0).normalized(),
      Vector3d(1.0, 1.0, 1.0).normalized()
  };
  const double kAngles[7] = {11.0, 7.0, 5.0, 2.0, 3.0, 13.0, 12.0};

  const Vector3d kTranslations[7] = {
      Vector3d(1.0, 1.0, 1.0),
      Vector3d(3.0, 2.0, 13.0),
      Vector3d(4.0, 5.0, 11.0),
      Vector3d(1.0, 2.0, 15.0),
      Vector3d(3.0, 1.5, 91.0),
      Vector3d(6.0, 3.0, 2.0),
      Vector3d(13.0, 1.0, 15.0)
  };

  InitRandomGenerator();
  for (int i = 0; i < 1000; ++i) {
    for (int transform_index = 0; transform_index < THEIA_ARRAYSIZE(kAxes);
         ++transform_index) {
      std::vector<Vector3d> random_points;
      CreateRandomPointsInFrustum(1.0,
                                  1.0,
                                  2.0,
                                  10.0,
                                  3,
                                  &random_points);

      const Vector3d points_3d[3] = { random_points[0],
                                      random_points[1],
                                      random_points[2] };
      Quaterniond kExpectedRotation(AngleAxisd(
          DegToRad(kAngles[transform_index]), kAxes[transform_index]));

      const double kProjectionNoise = 0.0;
      const double kMaxAllowedRotationDifference = 1e-5;
      const double kMaxAllowedAngleBetweenTranslations = 1e-4;
      TestThreePointResultWithNoise(kAxes[transform_index],
                                    points_3d,
                                    kProjectionNoise,
                                    kExpectedRotation,
                                    kTranslations[transform_index],
                                    kMaxAllowedRotationDifference,
                                    kMaxAllowedAngleBetweenTranslations);
    }
  }
}

}  // namespace theia


// Copyright (C) 2008-2018 Lorenzo Caminiti
// Distributed under the Boost Software License, Version 1.0 (see accompanying
// file LICENSE_1_0.txt or a copy at http://www.boost.org/LICENSE_1_0.txt).
// See: http://www.boost.org/doc/libs/release/libs/contract/doc/html/index.html

#include <boost/contract.hpp>

int gcd(int const a, int const b) {
    int result;
    boost::contract::check c = boost::contract::function()
        .precondition([&] {
            BOOST_CONTRACT_ASSERT(a > 0);
            BOOST_CONTRACT_ASSERT(b > 0);
        })
        .postcondition([&] {
            BOOST_CONTRACT_ASSERT(result <= a);
            BOOST_CONTRACT_ASSERT(result <= b);
        })
    ;

    int x = a, y = b;
    while(x != y) {
        if(x > y) x = x - y;
        else y = y - x;
    }
    return result = x;
}

//[check_macro
int main() {
    // Implementation checks (via macro, disable run-/compile-time overhead).
    BOOST_CONTRACT_CHECK(gcd(12, 28) == 4);
    BOOST_CONTRACT_CHECK(gcd(4, 14) == 2);

    return 0;
}
//]


/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2017 Sylko Olzscher
 *
 */
#ifndef CYNG_CRYPTO_ROTATING_COUNTER_HPP
#define CYNG_CRYPTO_ROTATING_COUNTER_HPP

#include <boost/assert.hpp>

namespace cyng
{

	/**
	 *	iota() as iterator
	 */
	template < typename T, T FIRST, T LAST >
	class circular_counter
	{
	public:
		typedef	T counter_type;

	public:
		circular_counter()
			: index_(FIRST)
		{
			static_assert(FIRST != LAST, "invalid range");
		}

		circular_counter(circular_counter const& other)
			: index_(other.index_)
		{}

		/**
		 *	@return current element
		 */
		T operator*() const
		{
			return index_;
		}

		/**
		 *	Reset internal position to initial state.
		 */
		void reset()
		{
			index_ = FIRST;
		}

		/**
		 *	@return next element of range
		 *	not thread safe(!)
		 */
		T operator()()
		{
			if (index_ == LAST)
			{
				T tmp = index_;
				index_ = FIRST;
				return tmp;
			}
			BOOST_ASSERT_MSG(index_ < LAST, "counter out of range");
			return index_++;
		}

	private:
		T index_;
	};

	/**
	 * Generate a sequence of linear rising numbers (start, end]. After
	 * reaching one element before end it starts from the start
	 * value again.
	 *
	 * Works best with POD types.
	 */
	template < typename T>
	class ring_counter
	{
	public:
		ring_counter(T start, T end)
		: start_(start)
		, end_(end)
		, pos_(start)
		{
			BOOST_ASSERT_MSG(start_ < end_, "invalid range");
		}

		/**
		 * Possible overflow!
		 *
		 *	@return next element of range
		 */
		T operator()()
		{
			const T tmp = (pos_ % end_);

			if (tmp < pos_)
			{
				//
				//	go back to start
				//
				pos_ = start_;
			}
			pos_++;

			return tmp;
		}

		/**
		 *	@return current element
		 */
		T operator*() const
		{
			return (pos_ == end_)
			? start_
			: pos_
			;
		}

	private:
		T const start_;
		T const end_;
		T pos_;
	};

}	//	cyng

#endif // CYNG_CRYPTO_ROTATING_COUNTER_HPP


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/core/tag_of.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/comparable.hpp>
#include <laws/constant.hpp>
#include <laws/euclidean_ring.hpp>
#include <laws/group.hpp>
#include <laws/hashable.hpp>
#include <laws/logical.hpp>
#include <laws/monoid.hpp>
#include <laws/orderable.hpp>
#include <laws/ring.hpp>

#include <type_traits>
using namespace boost::hana;

struct inherit_simple : std::integral_constant<int, 3> { };
struct inherit_no_default : std::integral_constant<int, 3> {
    inherit_no_default() = delete;
};

struct incomplete;
struct empty_type { };
struct non_pod { virtual ~non_pod() { } };

int main() {
    auto ints = make<tuple_tag>(
        std::integral_constant<int, -10>{},
        std::integral_constant<int, -2>{},
        std::integral_constant<int, 0>{},
        std::integral_constant<int, 1>{},
        std::integral_constant<int, 3>{}
    );
    (void)ints;

#if BOOST_HANA_TEST_PART == 1
    //////////////////////////////////////////////////////////////////////////
    // Make sure the tag is detected properly
    //////////////////////////////////////////////////////////////////////////
    {
        static_assert(std::is_same<
            tag_of_t<inherit_simple>,
            ext::std::integral_constant_tag<int>
        >{}, "");
        static_assert(std::is_same<
            tag_of_t<inherit_no_default>,
            ext::std::integral_constant_tag<int>
        >{}, "");
        static_assert(std::is_same<
            tag_of_t<std::is_pointer<int*>>,
            ext::std::integral_constant_tag<bool>
        >{}, "");

        static_assert(!std::is_same<
            tag_of_t<incomplete>,
            ext::std::integral_constant_tag<int>
        >{}, "");
        static_assert(!std::is_same<
            tag_of_t<empty_type>,
            ext::std::integral_constant_tag<int>
        >{}, "");
        static_assert(!std::is_same<
            tag_of_t<non_pod>,
            ext::std::integral_constant_tag<int>
        >{}, "");
        static_assert(!std::is_same<
            tag_of_t<void>,
            ext::std::integral_constant_tag<int>
        >{}, "");
    }

    //////////////////////////////////////////////////////////////////////////
    // Interoperation with hana::integral_constant
    //////////////////////////////////////////////////////////////////////////
    {
        BOOST_HANA_CONSTANT_CHECK(std::integral_constant<int, 1>{} == int_c<1>);
        BOOST_HANA_CONSTANT_CHECK(std::integral_constant<int, 1>{} == long_c<1>);

        BOOST_HANA_CONSTANT_CHECK(std::integral_constant<int, 2>{} != int_c<3>);
    }

#elif BOOST_HANA_TEST_PART == 2
    //////////////////////////////////////////////////////////////////////////
    // Constant
    //////////////////////////////////////////////////////////////////////////
    {
        // value
        static_assert(value(std::integral_constant<int, 0>{}) == 0, "");
        static_assert(value(std::integral_constant<int, 1>{}) == 1, "");
        static_assert(value(std::integral_constant<int, 3>{}) == 3, "");

        // laws
        test::TestConstant<ext::std::integral_constant_tag<int>>{ints, tuple_t<int, long, long long>};
    }

#elif BOOST_HANA_TEST_PART == 3
    //////////////////////////////////////////////////////////////////////////
    // Monoid, Group, Ring, EuclideanRing
    //////////////////////////////////////////////////////////////////////////
    {
        test::TestMonoid<ext::std::integral_constant_tag<int>>{ints};
        test::TestGroup<ext::std::integral_constant_tag<int>>{ints};
        test::TestRing<ext::std::integral_constant_tag<int>>{ints};
        test::TestEuclideanRing<ext::std::integral_constant_tag<int>>{ints};
    }

#elif BOOST_HANA_TEST_PART == 4
    //////////////////////////////////////////////////////////////////////////
    // Logical
    //////////////////////////////////////////////////////////////////////////
    {
        auto t = test::ct_eq<3>{};
        auto e = test::ct_eq<4>{};

        // eval_if
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                eval_if(std::true_type{}, always(t), always(e)),
                t
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                eval_if(std::false_type{}, always(t), always(e)),
                e
            ));
        }

        // not_
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                not_(std::true_type{}),
                std::false_type{}
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                not_(std::false_type{}),
                std::true_type{}
            ));
        }

        auto ints = make<tuple_tag>(
            std::integral_constant<int, -2>{},
            std::integral_constant<int, 0>{},
            std::integral_constant<int, 1>{},
            std::integral_constant<int, 3>{}
        );

        auto bools = make<tuple_tag>(std::true_type{}, std::false_type{});

        // laws
        test::TestLogical<ext::std::integral_constant_tag<int>>{ints};
        test::TestLogical<ext::std::integral_constant_tag<bool>>{bools};
    }

#elif BOOST_HANA_TEST_PART == 5
    //////////////////////////////////////////////////////////////////////////
    // Comparable and Hashable
    //////////////////////////////////////////////////////////////////////////
    test::TestComparable<ext::std::integral_constant_tag<int>>{ints};
    test::TestHashable<ext::std::integral_constant_tag<void>>{ints};

#elif BOOST_HANA_TEST_PART == 6
    //////////////////////////////////////////////////////////////////////////
    // Orderable
    //////////////////////////////////////////////////////////////////////////
    test::TestOrderable<ext::std::integral_constant_tag<int>>{ints};
#endif
}


#include <blitzml/sparse_linear/sparse_linear_solver.h>

#include <blitzml/base/math_util.h>
#include <blitzml/base/vector_util.h>
#include <blitzml/base/timer.h>
#include <blitzml/base/subproblem_controller.h>

#include <blitzml/smooth_loss/squared_loss.h>
#include <blitzml/smooth_loss/logistic_loss.h>
#include <blitzml/smooth_loss/squared_hinge_loss.h>
#include <blitzml/smooth_loss/smoothed_hinge_loss.h>
#include <blitzml/smooth_loss/huber_loss.h>

using std::vector;

namespace BlitzML {

value_t SparseLinearSolver::compute_dual_obj() const {
  value_t loss = 0.;
  for (index_t j = 0; j < num_examples; ++j) {
    loss += loss_function->compute_loss(Aomega[j], data->b_value(j));
  }
  return -(loss + l1_penalty * l1_norm(omega));
}

value_t SparseLinearSolver::compute_primal_obj_x() const {
  value_t obj = 0.;
  for (index_t j = 0; j < num_examples; ++j) {
    obj += loss_function->compute_conjugate(kappa_x * x[j], data->b_value(j));
  }
  return obj;
}

value_t SparseLinearSolver::compute_primal_obj_y() const {
  value_t obj = 0.;
  for (index_t j = 0; j < num_examples; ++j) {
    obj += loss_function->compute_conjugate(y[j], data->b_value(j));
  }
  return obj;
}

void SparseLinearSolver::update_subproblem_obj_vals() {
  value_t primal_obj_x = compute_primal_obj_x();
  value_t dual_obj = compute_dual_obj();
  obj_vals.set_primal_obj_x(primal_obj_x);
  obj_vals.set_dual_obj(dual_obj);
}

void SparseLinearSolver::solve_subproblem() {
  SubproblemController controller(subproblem_params);

  SubproblemState initial_state;
  set_initial_subproblem_state(initial_state);

  set_initial_subproblem_z();

  for (int newton_itr = 0; newton_itr < subproblem_params.max_iterations; ++newton_itr) {
    setup_proximal_newton_problem();
    value_t cd_threshold = compute_cd_threshold();

    scale_Delta_Aomega_by_2nd_derivatives();

    int cd_itr, max_num_cd_itr, min_num_cd_itr;
    set_max_and_min_num_cd_itr(max_num_cd_itr, min_num_cd_itr);
    Timer t1;
    for (cd_itr = 1; cd_itr <= max_num_cd_itr; ++cd_itr) {
      value_t est_grad_sq = update_coordinates_in_working_set();
      if (cd_itr >= min_num_cd_itr) {
        if (controller.should_compute_duality_gap()) {
          break;
        }
        if (est_grad_sq < cd_threshold) {
          break;
        }
      }
    }

    unscale_Delta_Aomega_by_2nd_derivatives();

    perform_backtracking();
    update_bias_subproblem();

    update_kappa_x();
    update_subproblem_obj_vals();
    update_z();

    bool sufficient_dual_progress =
           check_sufficient_dual_progress(initial_state, subproblem_params.epsilon);

    if (controller.should_terminate(obj_vals, sufficient_dual_progress)
        || (newton_itr == subproblem_params.max_iterations)) {
      return;
    }
  }
}

value_t SparseLinearSolver::update_coordinates_in_working_set() {
  value_t ret = 0.;
  for (const_index_itr ind = ws.begin_indices(); ind != ws.end_indices(); ++ind) {
    value_t subgrad = update_feature(*ind);
    ret += sq(subgrad);
  }
  ws.shuffle_indices();
  return ret;
}

void SparseLinearSolver::
set_max_and_min_num_cd_itr(int &max_num_cd_itr, int &min_num_cd_itr) const {
  if (iteration_number == 1) {
    min_num_cd_itr = 1;
    max_num_cd_itr = 1;
  } else {
    min_num_cd_itr = 3;
    max_num_cd_itr = 64;
  }
}

value_t SparseLinearSolver::compute_cd_threshold() const {
  value_t ret = 0.;
  for (const_index_itr i = ws.begin(); i != ws.end(); ++i) {
    if (omega[*i] != 0.) {
      ret += sq(fabs(ATx[*i]) - l1_penalty);
    } else {
      ret += sq(soft_threshold(ATx[*i], l1_penalty));
    }
  }
  return 0.25 * ret;
}

value_t SparseLinearSolver::norm_diff_sq_z_initial_x(const SubproblemState& initial_state) const {
  value_t norm_diff_sq = 0.;
  for (index_t j = 0; j < num_examples; ++j) {
    norm_diff_sq += sq(initial_state.x[j] - kappa_z * z[j]);
  }
  return norm_diff_sq;
}

void SparseLinearSolver::setup_subproblem_no_working_sets() {
  Solver::setup_subproblem_no_working_sets();
  subproblem_params.max_iterations = 1;
}

void SparseLinearSolver::set_initial_subproblem_z() {
  z = y;
  for (const_index_itr i = ws.begin_sorted(); i != ws.end_sorted(); ++i) {
    ATz[*i] = ATy[*i];
  }
  kappa_z = 1.0;
  obj_vals.set_primal_obj_z(obj_vals.primal_obj_y());
  z_match_y = true;
  z_match_x = false;
}

void SparseLinearSolver::setup_proximal_newton_problem() {
  Delta_omega.assign(ws.size(), 0.);
  Delta_Aomega.assign(num_examples, 0.);
  value_t hessian_extra = 1e-12;
  update_newton_2nd_derivatives(hessian_extra);
  Delta_bias = (use_bias) ? -sum_x / sum_newton_2nd_derivatives : 0.;
  inv_newton_2nd_derivative_cache.resize(ws.size());
  col_ip_newton_2nd_derivative_cache.resize(ws.size());

  bool s_d_is_const = is_vector_const(newton_2nd_derivatives, 1e-12);

  for (size_t ind = 0; ind != ws.size(); ++ind) {
    index_t i = ws.ith_member(ind);
    const Column& col = *A_cols[i];

    value_t weighted_norm_sq = 0;
    if (s_d_is_const) {
      value_t const_val = newton_2nd_derivatives[0];
      weighted_norm_sq = const_val / (inv_lipschitz_cache[i] * loss_function->lipschitz_constant());
    } else {
      weighted_norm_sq = col.weighted_norm_sq(newton_2nd_derivatives);
    }

    if (use_bias) {
      if (s_d_is_const) {
        value_t const_val = newton_2nd_derivatives[0];
        col_ip_newton_2nd_derivative_cache[ind] = col_means_cache[i] * const_val * num_examples;
      } else {
        col_ip_newton_2nd_derivative_cache[ind] = col.inner_product(newton_2nd_derivatives);
        weighted_norm_sq += col_means_cache[i] * (col_means_cache[i] * sum_newton_2nd_derivatives - 2 * col_ip_newton_2nd_derivative_cache[ind]);
      }
    }

    if (weighted_norm_sq > 0.) {
      inv_newton_2nd_derivative_cache[ind] = 1. / weighted_norm_sq;
    } else {
      inv_newton_2nd_derivative_cache[ind] = -1.;
    }
  }
}

void SparseLinearSolver::update_z() {
  bool is_best = obj_vals.primal_obj_x() <= obj_vals.primal_obj_z();
  if (is_best) {
    obj_vals.set_primal_obj_z(obj_vals.primal_obj_x());
    z = x;
    kappa_z = kappa_x;
    for (const_index_itr i = ws.begin_sorted(); i != ws.end_sorted(); ++i) {
      ATz[*i] = ATx[*i];
    }
    z_match_x = true;
    z_match_y = false;
  } else {
    z_match_x = false;
  }
}

void SparseLinearSolver::scale_Delta_Aomega_by_2nd_derivatives() {
  for (index_t i = 0; i < num_examples; ++i) {
    Delta_Aomega[i] *= newton_2nd_derivatives[i];
  }
}

void SparseLinearSolver::unscale_Delta_Aomega_by_2nd_derivatives() {
  for (index_t i = 0; i < num_examples; ++i) {
    Delta_Aomega[i] /= newton_2nd_derivatives[i];
  }
}

value_t SparseLinearSolver::update_feature(index_t working_set_ind) {
  // note: Delta_Aomega is scaled by newton 2nd derivatives
  value_t inv_L = inv_newton_2nd_derivative_cache[working_set_ind];
  if (inv_L < 0) {
    return 0.;
  }

  index_t feature_ind = ws.ith_member(working_set_ind);
  const Column& col = *A_cols[feature_ind];
  value_t grad = ATx[feature_ind] +
         col.inner_product(Delta_Aomega) +
         col_ip_newton_2nd_derivative_cache[working_set_ind] * Delta_bias;

  value_t current_value = omega[feature_ind] + Delta_omega[working_set_ind];
  value_t shrunk_grad = fabs(grad) - l1_penalty;
  if (current_value == 0. && shrunk_grad < 0) {
    return 0.;
  }
  value_t pre_shrink = current_value - grad * inv_L;
  value_t new_value = soft_threshold(pre_shrink, l1_penalty * inv_L);

  value_t delta = new_value - current_value;
  col.weighted_add_multiple(Delta_Aomega, newton_2nd_derivatives, delta);
  Delta_omega[working_set_ind] += delta;

  if (use_bias) {
    value_t grad_bias =
                  col_ip_newton_2nd_derivative_cache[working_set_ind]  * delta;
    Delta_bias -= grad_bias * (1 / sum_newton_2nd_derivatives);
  }

  return shrunk_grad;
}

void SparseLinearSolver::update_bias_subproblem() {
  update_bias();
}

void SparseLinearSolver::update_bias(int max_newton_itr) {
  if (!use_bias) {
    return;
  }

  Delta_Aomega.assign(num_examples, 0.);

  for (int newton_itr = 0; newton_itr < max_newton_itr; ++newton_itr) {
    value_t change = perform_newton_update_on_bias();
    if (fabs(change) == 0.0) {
      return;
    }
  }
}

value_t SparseLinearSolver::perform_newton_update_on_bias() {
  update_newton_2nd_derivatives();
  Delta_bias = (sum_newton_2nd_derivatives > 0) ?
                  -sum_x / sum_newton_2nd_derivatives : -100 * sign(sum_x);
  Delta_bias = (fabs(Delta_bias) < 100) ? Delta_bias : 100 * sign(Delta_bias);

  value_t step_size = 1.;
  unsigned backtrack_itr = 0;
  while (++backtrack_itr <= 6) {
    update_x(step_size);
    if (sum_x * Delta_bias <= 0.) {
      value_t change = step_size * Delta_bias;
      bias += change;
      add_scalar_to_vector(Aomega, change);
      return change;
    }

    if (backtrack_itr <= 2) {
      step_size *= 0.5;
    } else {
      step_size *= 0.1;
    }

    if (step_size * Delta_bias == 0.) {
      break;
    }
  }
  update_x(0.);
  return 0;
}

void SparseLinearSolver::perform_backtracking() {
  value_t step_size = 1.;
  unsigned backtrack_itr = 0;
  while (++backtrack_itr) {
    update_x(step_size);

    value_t derivative = compute_backtracking_step_size_derivative(step_size);
    if (derivative <= 1e-12) {
      break;
    } else if (backtrack_itr > MAX_BACKTRACK_STEPS) {
      compute_Aomega();
      update_x(0.);
      return;
    }

    step_size *= 0.5;
  }

  for (const_index_itr ind = ws.begin_indices(); ind != ws.end_indices(); ++ind) {
    omega[ws.ith_member(*ind)] += step_size * Delta_omega[*ind];
  }
  bias += step_size * Delta_bias;
  for (index_t i = 0; i < num_examples; ++i) {
    Aomega[i] += step_size * (Delta_Aomega[i] + Delta_bias);
  }
}

value_t SparseLinearSolver::
compute_backtracking_step_size_derivative(value_t step_size) const {
  value_t derivative_loss = inner_product(Delta_Aomega, x) + Delta_bias * sum_x;

  value_t derivative_l1 = 0.;
  for (const_index_itr ind = ws.begin_indices(); ind != ws.end_indices(); ++ind) {
    index_t i = ws.ith_member(*ind);
    value_t omega_i = omega[i] + step_size * Delta_omega[*ind];
    if (omega_i == 0.) {
      derivative_l1 -= l1_penalty * fabs(Delta_omega[*ind]);
    } else {
      derivative_l1 += l1_penalty * Delta_omega[*ind] * sign(omega_i);
    }
  }

  return derivative_loss + derivative_l1;
}

void SparseLinearSolver::update_x(value_t step_size) {
  const value_t* labels = data->b_values();
  for (index_t i = 0; i < num_examples; ++i) {
    value_t diff_aiTomega = step_size * (Delta_Aomega[i] + Delta_bias);
    x[i] = loss_function->compute_deriative(Aomega[i] + diff_aiTomega, labels[i]);
  }
  sum_x = sum_vector(x);
  z_match_x = false;
  z_match_y = false;
}

void SparseLinearSolver::update_newton_2nd_derivatives(value_t epsilon_to_add) {
  for (index_t i = 0; i < num_examples; ++i) {
    value_t a_dot_omega = Aomega[i];
    value_t label = data->b_value(i);
    newton_2nd_derivatives[i] =
          loss_function->compute_2nd_derivative(a_dot_omega, label) + epsilon_to_add;
  }
  sum_newton_2nd_derivatives = sum_vector(newton_2nd_derivatives);
}

void SparseLinearSolver::update_kappa_x() {
  value_t max_abs_grad = l1_penalty;
  for (const_index_itr i = ws.begin_sorted(); i != ws.end_sorted(); ++i) {
    value_t grad = A_cols[*i]->inner_product(x);
    ATx[*i] = grad;
    if (fabs(grad) > max_abs_grad) {
      max_abs_grad = fabs(grad);
    }
  }
  kappa_x = (l1_penalty < max_abs_grad) ? l1_penalty / max_abs_grad : 1.;
}

value_t SparseLinearSolver::compute_alpha() {
  update_non_working_set_gradients();
  value_t alpha = 1.;
  for (index_t j = 0; j < num_components; ++j) {
    value_t alpha_j = compute_alpha_for_feature(j);
    if (alpha_j < alpha) {
      alpha = alpha_j;
    }
  }
  return alpha;
}

void SparseLinearSolver::update_non_working_set_gradients() {
  for (index_t i = 0; i < num_components; ++i) {
    if (!ws.is_in_working_set(i)) {
      ATx[i] = A_cols[i]->inner_product(x);
    }
  }

  if (z_match_y) {
    ATz = ATy;
  } else if (z_match_x) {
    ATz = ATx;
  } else {
    for (index_t i = 0; i < num_components; ++i) {
      if (!ws.is_in_working_set(i)) {
        ATz[i] = A_cols[i]->inner_product(z);
      }
    }
  }
}

value_t SparseLinearSolver::compute_alpha_for_feature(index_t i) const {
  if (ws.is_in_working_set(i)) {
    return 1.;
  }
  value_t kappa_x_AiTz = kappa_z * ATz[i];
  if (fabs(kappa_x_AiTz) <= l1_penalty) {
    return 1.;
  }
  value_t AiTy = ATy[i];
  if (kappa_x_AiTz == AiTy) {
    return 1.;
  }
  value_t value = (kappa_x_AiTz < 0) ? -l1_penalty : l1_penalty;
  return (value - AiTy) / (kappa_x_AiTz - AiTy);
}

void SparseLinearSolver::update_y() {
  value_t alpha_kappa_z = alpha * kappa_z;
  for (index_t j = 0; j < num_examples; ++j) {
    y[j] = alpha_kappa_z * z[j] + (1 - alpha) * y[j];
  }
  for (index_t i = 0; i < num_components; ++i) {
    ATy[i] = alpha_kappa_z * ATz[i] + (1 - alpha) * ATy[i];
  }
}

unsigned short SparseLinearSolver::compute_priority_value(index_t i) const {
  if (omega[i] != 0.) {
    return 0;
  }

  value_t ATyi = ATy[i];
  value_t ATxi = ATx[i];
  value_t diffi = ATxi - ATyi;
  value_t inv_norm_sq = inv_lipschitz_cache[i] * loss_function->lipschitz_constant();

  unsigned short top = capsule_candidates.size();
  unsigned short bottom = 0;
  bool in_working_set = false;
  do {
    unsigned short ind = (top + bottom) / 2;
    const CapsuleParams& cap = capsule_candidates[ind];

    value_t beta_l = cap.left_beta;
    value_t val_l = ATyi + beta_l * diffi;

    value_t beta_r = cap.right_beta;
    value_t val_r = ATyi + beta_r * diffi;

    value_t val = std::max(std::fabs(val_l), std::fabs(val_r));
    value_t dist_sq = (val >= l1_penalty) ? 0.
                    : sq(l1_penalty - val) * inv_norm_sq;

    in_working_set = (dist_sq <= sq(cap.radius));
    if (in_working_set) {
      top = ind;
    } else {
      bottom = ind;
    }
  } while (top > bottom + 1);

  return top;
}

void SparseLinearSolver::screen_components() {
  vector<bool> should_screen;
  bool any_screened = mark_components_to_screen(should_screen);
  if (any_screened) {
    apply_screening_result(should_screen);
  }
}

bool SparseLinearSolver::mark_components_to_screen(vector<bool> &should_screen) const {
  value_t d_sq = compute_d_sq();
  value_t thresh_sq = obj_vals.duality_gap() - d_sq / 4;
  if (thresh_sq <= 0.) {
    return false;
  }

  bool impossible_thresh = sq(l1_penalty) * max_inv_lipschitz_cache < thresh_sq;
  if (impossible_thresh && iteration_number != 1) {
    return false;
  }

  should_screen.assign(num_components, false);
  bool any_screened = false;
  for (index_t i = 0; i < num_components; ++i) {
    if (inv_lipschitz_cache[i] < 0) {
      should_screen[i] = true;
      any_screened = true;
    }

    if (omega[i] != 0.) {
      continue;
    }

    value_t val = (ATx[i] + ATy[i]) / 2;
    value_t dist = l1_penalty - fabs(val);
    if (dist < 0.) {
      continue;
    }

    bool screen_i = (sq(dist) * inv_lipschitz_cache[i] >= thresh_sq);
    if (screen_i) {
      should_screen[i] = true;
      any_screened = true;
    }
  }

  return any_screened;
}

void SparseLinearSolver::
apply_screening_result(const vector<bool> &should_screen) {
  index_t i = 0;
  for (index_t ind = 0; ind < num_components; ++ind) {
    if (!should_screen[ind]) {
      ATy[i] = ATy[ind];
      ATx[i] = ATx[ind];
      omega[i] = omega[ind];
      inv_lipschitz_cache[i] = inv_lipschitz_cache[ind];
      col_means_cache[i] = col_means_cache[ind];
      A_cols[i] = A_cols[ind];
      screen_indices_map[i] = screen_indices_map[ind];
      ++i;
    }
  }
  num_components = i;
  omega.resize(num_components);
  ws.reduce_max_size(num_components);
  if (!use_working_sets()) {
    ws.clear();
    for (index_t i = 0; i < num_components; ++i) {
      ws.add_index(i);
    }
  }
}

size_t SparseLinearSolver::size_of_component(index_t i) const {
  return A_cols[i]->nnz();
}

index_t SparseLinearSolver::initial_problem_size() const {
  return data->num_cols();
}

void SparseLinearSolver::initialize(value_t *initial_conditions) {
  deserialize_parameters();
  set_data_dimensions();
  initialize_proximal_newton_variables();
  cache_feature_info();
  initialize_blitz_variables(initial_conditions);
}

value_t SparseLinearSolver::strong_convexity_constant() const {
  return 1 / loss_function->lipschitz_constant();
}

void SparseLinearSolver::deserialize_parameters() {
  l1_penalty = (*params)[0];
  if ((*params)[2] != 0.) {
    use_bias = true;
  } else {
    use_bias = false;
  }
  int loss_type = static_cast<int>((*params)[3]);
  delete_loss_function();
  switch (loss_type) {
    case 0:
      loss_function = new SquaredLoss();
      break;
    case 1:
      loss_function = new HuberLoss();
      break;
    case 2:
      loss_function = new LogisticLoss();
      break;
    case 3:
      loss_function = new SquaredHingeLoss();
      break;
    case 4:
      loss_function = new SmoothedHingeLoss();
      break;
    default:
      loss_function = new SquaredLoss();
      break;
  }
}

void SparseLinearSolver::set_data_dimensions() {
  num_examples = data->num_rows();
  num_components = data->num_cols();
}

void SparseLinearSolver::initialize_blitz_variables(
    value_t* initial_conditions) {
  initialize_model(initial_conditions);

  compute_Aomega();

  x.resize(num_examples);
  ATx.resize(num_components);
  kappa_x = 1.;

  y.assign(num_examples, 0.);
  ATy.assign(num_components, 0.);

  update_x(0.0);
  update_bias(15);

  z = x;
  ATz.assign(num_components, 0.);
  kappa_z = 1.0;
  z_match_x = true;
  z_match_y = false;
}

void SparseLinearSolver::initialize_model(value_t* initial_conditions) {
  if (initial_conditions != NULL) {
    omega.assign(initial_conditions, initial_conditions + num_components);
  } else {
    omega.assign(num_components, 0.);
  }
  bias = 0.;
}

void SparseLinearSolver::compute_Aomega() {
  Aomega.assign(num_examples, bias);
  for (index_t i = 0; i < num_components; ++i) {
    if (omega[i] != 0.) {
      A_cols[i]->add_multiple(Aomega, omega[i]);
    }
  }
}

void SparseLinearSolver::cache_feature_info() {
  A_cols.resize(num_components);
  inv_lipschitz_cache.resize(num_components);
  col_means_cache.resize(num_components);
  value_t lipschitz_constant = loss_function->lipschitz_constant();
  for (index_t i = 0; i < num_components; ++i) {
    A_cols[i] = data->column(i);
    const Column& col = *A_cols[i];
    value_t norm_sq = col.l2_norm_sq();
    value_t mean = col.mean();
    if (use_bias) {
      norm_sq -= num_examples * sq(mean);
    }
    if (norm_sq <= 1e-12) {
      norm_sq = -1.;
    }
    inv_lipschitz_cache[i] = 1 / (norm_sq * lipschitz_constant);
    col_means_cache[i] = mean;
  }

  max_inv_lipschitz_cache = max_vector(inv_lipschitz_cache);

  screen_indices_map.resize(num_components);
  for (index_t i = 0; i < num_components; ++i) {
    screen_indices_map[i] = i;
  }
}

void SparseLinearSolver::initialize_proximal_newton_variables() {
  newton_2nd_derivatives.resize(num_examples);
  Delta_Aomega.assign(num_examples, 0);
  Delta_bias = 0.;
}

void SparseLinearSolver::log_variables(Logger &logger) const {
  std::vector<index_t> indices;
  std::vector<value_t> values;
  size_t nnz_weights = l0_norm(omega);
  indices.reserve(nnz_weights);
  values.reserve(nnz_weights);
  for (index_t k = 0; k < num_components; ++k) {
    if (omega[k] != 0.) {
      indices.push_back(screen_indices_map[k]);
      values.push_back(omega[k]);
    }
  }
  if (log_vectors()) {
    logger.log_vector<index_t>("weight_indices", indices);
    logger.log_vector<value_t>("weight_values", values);
    logger.log_vector<value_t>("z", z);
  }
  logger.log_value<value_t>("bias", bias);
  logger.log_value<value_t>("l1_penalty", l1_penalty);
  logger.log_value<size_t>("number_nonzero_weights", nnz_weights);
}

void SparseLinearSolver::fill_result(value_t *result) const {
  size_t ind = 0;

  // fill in weights
  for (index_t i = 0; i < data->num_cols(); ++i) {
    result[ind++] = 0.;
  }
  for (size_t ind = 0; ind < omega.size(); ++ind) {
    result[screen_indices_map[ind]] = omega[ind];
  }
  result[ind++] = bias;

  // fill in dual solution
  for (index_t i = 0; i < num_examples; ++i) {
    result[ind++] = y[i];
  }

  result[ind++] = obj_vals.duality_gap();
  result[ind++] = obj_vals.dual_obj();
}

value_t SparseLinearSolver::compute_max_l1_penalty(
    const Dataset *data, const Parameters *params) {
  this->data = data;
  this->params = params;

  initialize(NULL);

  for (index_t j = 0; j < num_components; ++j) {
    ATx[j] = A_cols[j]->inner_product(x);
  }
  return max_abs(ATx);
}

void SparseLinearSolver::delete_loss_function() {
  if (loss_function != NULL) {
    delete loss_function;
    loss_function = NULL;
  }
}

} // namespace BlitzML


#ifndef BASIS_TEMPLATE_HPP
#define BASIS_TEMPLATE_HPP

#include <armadillo>

class Basis {

public:

    int _nX;
    int _nK;
    int _nM;
    int _nU;
    int _nKU;
    Basis(int _nX, int _nK, int _nU, int _nM, int _nKU) : _nX(_nX), _nK(_nK), _nU(_nU), _nM(_nM), _nKU(_nKU) { ; }
    virtual ~Basis() { ; }
    virtual arma::vec fk(const arma::vec& x, const arma::vec& u) = 0;
    virtual arma::vec fkx(const arma::vec& x) = 0;
    virtual arma::vec fku(const arma::vec& x, const arma::vec& u) = 0;
    virtual arma::mat fkudu(const arma::vec& x, const arma::vec& u) = 0;

};

#endif