/**
 * @brief The 'regression2D' method can be used to fit a line to a given point set.
 * @param points_begin      set begin iterator
 * @param points_end        set end iterator
 * @param fit_start         the start of the line fit
 * @param fit_end           the set termintating iterator position
 * @param line              the parameterized line to work with
*/
inline void regression2D(const std::vector<LaserBeam>::const_iterator &points_begin, const std::vector<LaserBeam>::const_iterator &points_end,
                         Eigen::ParametrizedLine<double, 2> &line)
{
    std::vector<LaserBeam>::const_iterator back_it = points_end;
    --back_it;
    Eigen::Vector2d front (points_begin->posX(), points_end->posY());
    Eigen::Vector2d back (back_it->posX(), back_it->posY());

    unsigned int size = std::distance(points_begin, points_end);
    Eigen::MatrixXd A(size, 2);
    Eigen::VectorXd b(size);
    A.setOnes();

    Eigen::Vector2d dxy = (front - back).cwiseAbs();
    bool solve_for_x = dxy.x() > dxy.y();
    if(solve_for_x) {
        /// SOLVE FOR X
        int i = 0;
        for(std::vector<LaserBeam>::const_iterator it = points_begin ; it != points_end ; ++it, ++i)
        {
            A(i,1) = it->posX();
            b(i)   = it->posY();
        }
    } else {
        /// SOLVE FOR Y
        int i = 0;
        for(std::vector<LaserBeam>::const_iterator it = points_begin ; it != points_end ; ++it, ++i)
        {
            A(i,1) = it->posY();
            b(i)   = it->posX();
        }
    }

    Eigen::VectorXd weights = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    double          alpha   = weights(0);
    double          beta    = weights(1);
    Eigen::Vector2d from;
    Eigen::Vector2d to;

    if(solve_for_x) {
        from(0) = 0.0;
        from(1) = alpha;
        to(0)   = 1.0;
        to(1)   = alpha + beta;
    } else {
        from(0) = alpha;
        from(1) = 0.0;
        to(0)   = alpha + beta;
        to(1)   = 1.0;
    }

    Eigen::Vector2d fit_start;
    Eigen::Vector2d fit_end;
    line = Eigen::ParametrizedLine<double, 2>(from, (to - from).normalized());
    fit_start = line.projection(front);
    fit_end   = line.projection(back);
    line = Eigen::ParametrizedLine<double, 2>(fit_start, (fit_end - fit_start));
}
/**
 * @brief Calculate the distance between two Eigen vectors given by iterators.
 * @param first             the first Eigen Vector2d
 * @param second            the second Eigen Vector2d
 * @return                  the distance
 */
inline double distance(const std::vector<LaserBeam>::const_iterator &first, const std::vector<LaserBeam>::const_iterator &second)
{
    return (Eigen::Vector2d(first->posX(), first->posY()) - Eigen::Vector2d(second->posX(), second->posY())).norm();
}