text:
#include <boost/version.hpp>
#if ((BOOST_VERSION / 100) % 1000) >= 53
#include <boost/thread/lock_guard.hpp>
#endif

#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <std_msgs/Float64.h>
#include <std_msgs/UInt8.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
//#include <opencv2/imgproc/imgproc.hpp>
//#include <opencv2/calib3d/calib3d.hpp>
#include <dynamic_reconfigure/server.h>
#include <turtlebot3_autorace_detect/DetectLaneParamsConfig.h>

// #include <iostream>
#include <algorithm>
#include <numeric>
#include <vector>
#include <gsl/gsl_multifit.h>
#include <stdbool.h>
#include <math.h>

#include "polyfit_queue.hpp"

using namespace std;
using namespace cv;

namespace lane_detection {

class DetectLaneNodelet : public nodelet::Nodelet
{
    // ROS communication
    boost::shared_ptr<image_transport::ImageTransport> it_;

    image_transport::Subscriber sub_projected_image_;

    boost::mutex connect_mutex_;
    image_transport::Publisher pub_image_lane_;
    image_transport::Publisher pub_image_yellow_lane_;
    image_transport::Publisher pub_image_white_lane_;

    ros::Publisher pub_lane_;
    ros::Publisher pub_yellow_lane_reliability_;
    ros::Publisher pub_white_lane_reliability_;

    // Dynamic reconfigure
    boost::recursive_mutex config_mutex_;
    typedef turtlebot3_autorace_detect::DetectLaneParamsConfig Config;
    typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
    boost::shared_ptr<ReconfigureServer> reconfigure_server_;
    // Config config_;

    // Parameters
    bool is_calibration_mode;
    int queue_size_;

    //// reconfigurable parameters
    int hue_white_l, hue_white_h;
    int saturation_white_l, saturation_white_h;
    int lightness_white_l, lightness_white_h;
    int hue_yellow_l, hue_yellow_h;
    int saturation_yellow_l, saturation_yellow_h;
    int lightness_yellow_l, lightness_yellow_h;

    // Properties
    uint8_t reliability_yellow_lane;
    uint8_t reliability_white_lane;

    polyfit_t white_lane_fit;
    polyfit_t yellow_lane_fit;
    polyfit_t lane_fit_bef;

    PolyfitQueue mov_avg_left;
    PolyfitQueue mov_avg_right;

    int counter;        // number of skipped input images

    // Methods
    virtual void onInit();

    void connectCB();
    void disconnectCB();
    void imageCB(const sensor_msgs::ImageConstPtr& image_msg);
    void configCB(Config &config, uint32_t level);

    vector<double> polynomialfit(int obs, int degree, int *dx, int *dy);
    // vector<double> polynomialfit(int obs, int degree, double *dx, double *dy);
    // vector<double> polyRegression(const vector<int>& x, const vector<int>& y);
    int count_nonzero(const Mat& image);
    int argmax(vector<int>& values, int first, int last);
    int arghalfmax(vector<int>& values, int first, int last, bool reverse=false);

    int maskWhiteLaneFromHSV(const Mat& hsv_img, Mat& mask, vector<Point>& locations);
    int maskYellowLaneFromHSV(const Mat& hsv_img, Mat& mask, vector<Point>& locations);
    int fit_from_lines(const vector<Point>& nonzero_pts, vector<double>& lane_fit);
    int sliding_window(const Mat& image, const vector<Point>& nonzero_pts, int left_or_right, vector<double>& lane_fit);
    int getPointsFromPolyfit(const vector<double>& poly_fit, int size, vector<Point>& points);
    int getCenterPointsFromPolyfit(const vector<double>& left_poly_fit,
                const vector<double>& right_poly_fit,
                int offset, int size, vector<Point>& points);
    void make_lane(Mat& image, int white_fraction, int yellow_fraction);
};

// vector<double> DetectLaneNodelet::polynomialfit(int obs, int degree, double *dx, double *dy)
vector<double> DetectLaneNodelet::polynomialfit(int obs, int degree, int *dx, int *dy)
{
    //
    // ref. site : https://rosettacode.org/wiki/Polynomial_regression
    //
    gsl_multifit_linear_workspace *ws;
    gsl_matrix *cov, *X;
    gsl_vector *y, *c;
    double chisq;

    int i, j;

    X = gsl_matrix_alloc(obs, degree);
    y = gsl_vector_alloc(obs);
    c = gsl_vector_alloc(degree);
    cov = gsl_matrix_alloc(degree, degree);

    for(i=0; i < obs; i++) {
        for(j=0; j < degree; j++) {
            gsl_matrix_set(X, i, j, pow(dx[i], j));
        }
        gsl_vector_set(y, i, dy[i]);
    }

    ws = gsl_multifit_linear_alloc(obs, degree);
    gsl_multifit_linear(X, y, c, cov, &chisq, ws);

    /* store result ... */
    vector<double> coeffs;
    for(i=degree-1; i >= 0; i--)
    {
        coeffs.push_back(gsl_vector_get(c, i));
    }

    gsl_multifit_linear_free(ws);
    gsl_matrix_free(X);
    gsl_matrix_free(cov);
    gsl_vector_free(y);
    gsl_vector_free(c);
    return coeffs;
}

/*
// possible since C++17
vector<double> DetectLaneNodelet::polyRegression(const vector<int>& x, const vector<int>& y)
{
    int n = x.size();
    vector<int> r(n);
    iota(r.begin(), r.end(), 0);
    double xm = accumulate(x.begin(), x.end(), 0.0) / x.size();
    double ym = accumulate(y.begin(), y.end(), 0.0) / y.size();
    double x2m = transform_reduce(r.begin(), r.end(), 0.0, plus<double>{}, [](double a) {return a * a; }) / r.size();
    double x3m = transform_reduce(r.begin(), r.end(), 0.0, plus<double>{}, [](double a) {return a * a * a; }) / r.size();
    double x4m = transform_reduce(r.begin(), r.end(), 0.0, plus<double>{}, [](double a) {return a * a * a * a; }) / r.size();

    double xym = transform_reduce(x.begin(), x.end(), y.begin(), 0.0, plus<double>{}, multiplies<double>{});
    xym /= fmin(x.size(), y.size());

    double x2ym = transform_reduce(x.begin(), x.end(), y.begin(), 0.0, plus<double>{}, [](double a, double b) { return a * a * b; });
    x2ym /= fmin(x.size(), y.size());

    double sxx = x2m - xm * xm;
    double sxy = xym - xm * ym;
    double sxx2 = x3m - xm * x2m;
    double sx2x2 = x4m - x2m * x2m;
    double sx2y = x2ym - x2m * ym;

    vector<double> coeffs;
    double c = (sx2y * sxx - sxy * sxx2) / (sxx * sx2x2 - sxx2 * sxx2);
    coeffs.push_back(c);
    double b = (sxy * sx2x2 - sx2y * sxx2) / (sxx * sx2x2 - sxx2 * sxx2);
    coeffs.push_back(b);
    coeffs.push_back(ym - b * xm - c * x2m);

    return coeffs;
}
*/

int DetectLaneNodelet::count_nonzero(const Mat& image)
{
    vector<Point> locations;            // locations of non-zero pixels
    findNonZero(image, locations);      // find locatopns of non-zero elements
    return locations.size();            // return number of non-zero-element
}

int DetectLaneNodelet::argmax(vector<int>& values, int first, int last)
{
    int *data = values.data();
    int pos = first;
    if (first < last) {
        data += first;
        int max_value = *data++;
        for (int i=first+1; i<=last; i++) {
            if (max_value < *data) {
                max_value = *data;
                pos = i;
            }
            data++;
        }
    }
    return pos;
}

int DetectLaneNodelet::arghalfmax(vector<int>& values, int first, int last, bool reverse/*=false*/)
{
    int *data = values.data();
    int pos = first;
    if (first < last) {
        // get half of max value
        data += first;
        int max_value = *data++;
        for (int i=first+1; i<=last; i++, data++) {
            if (max_value < *data) {
                max_value = *data;
            }
        }

        // search position of half of max value
        // max_value /= 2;
        max_value /= 4;
        if (!reverse) {
            data = values.data() + first;
            for (pos=first; pos<=last; pos++, data++) {
                if (*data >= max_value)  break;
            }
        }
        else {
            data = values.data() + last;
            for (pos=last; pos>=first; pos--, data--) {
                if (*data >= max_value)  break;
            }
        }
    }
    return pos;
}

int DetectLaneNodelet::maskWhiteLaneFromHSV(const Mat& hsv_img, Mat& mask, vector<Point>& locations)
{
    // Define range of white color in HSV
    Scalar lower_white(hue_white_l, saturation_white_l, lightness_white_l);
    Scalar upper_white(hue_white_h, saturation_white_h, lightness_white_h);

    // Threshold the HSV image to get only white-color pixels
    inRange(hsv_img, lower_white, upper_white, mask);
    findNonZero(mask, locations);

    // Adjust lightness threshold value in terms of intensity of white-colors
    int fraction_num = locations.size();
    if (fraction_num > 0 && !is_calibration_mode) {
        if (fraction_num > 35000) {
            if (lightness_white_l < 250)  lightness_white_l += 5;
        }
        else if (fraction_num < 5000) {
            if (lightness_white_l > 50)  lightness_white_l -= 5;
        }
    }

    // Count much-short rows and adjust white-lane-reliability value
    int much_short_rows = 0;
    if (fraction_num > 0) {
        for (int row=0; row<mask.rows; row++) {
            uchar* ptr = mask.ptr<uchar>(row);
            int col = 0;
            for (col=0; col<mask.cols; col++, ptr++) {
                if (*ptr != 0)  break;
            }
            if (col >= mask.cols)  much_short_rows++;
        }
    }

    if (much_short_rows > 100) {
        if (reliability_white_lane >= 5)  reliability_white_lane -= 5;
    }
    else if (much_short_rows <= 100 ){
        if (reliability_white_lane <= 99)  reliability_white_lane += 5;
    }

    // Publish white-lane-reliability value and masked image
    std_msgs::UInt8 msg_white_lane_reliability;
    msg_white_lane_reliability.data = reliability_white_lane;
    pub_white_lane_reliability_.publish(msg_white_lane_reliability);

    if (is_calibration_mode) {
        sensor_msgs::ImagePtr white_lane_msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", mask).toImageMsg();
        pub_image_white_lane_.publish(white_lane_msg);
    }

    return fraction_num;
}

int DetectLaneNodelet::maskYellowLaneFromHSV(const Mat& hsv_img, Mat& mask, vector<Point>& locations)
{
    // Define range of yellow color in HSV
    Scalar lower_yellow(hue_yellow_l, saturation_yellow_l, lightness_yellow_l);
    Scalar upper_yellow(hue_yellow_h, saturation_yellow_h, lightness_yellow_h);

    // Threshold the HSV image to get only yellow-color pixels
    inRange(hsv_img, lower_yellow, upper_yellow, mask);
    findNonZero(mask, locations);

    // Adjust lightness threshold value in terms of intensity of yellow-colors
    int fraction_num = locations.size();
    if (fraction_num > 0 && !is_calibration_mode) {
        if (fraction_num > 35000) {
            if (lightness_yellow_l < 250)  lightness_yellow_l += 20;
        }
        else if (fraction_num < 5000) {
            if (lightness_yellow_l > 90)  lightness_yellow_l -= 20;
        }
    }

    // Count mush-short rows and adjust yellow-lane-reliability value
    int much_short_rows = 0;
    if (fraction_num > 0) {
        for (int row=0; row<mask.rows; row++) {
            uchar* ptr = mask.ptr<uchar>(row);
            int col = 0;
            for (col=0; col<mask.cols; col++, ptr++) {
                if (*ptr != 0)  break;
            }
            if (col >= mask.cols)  much_short_rows++;
        }
    }

    if (much_short_rows > 100) {
        if (reliability_yellow_lane >= 5)  reliability_yellow_lane -= 5;
    }
    else if (much_short_rows <= 100 ){
        if (reliability_yellow_lane <= 99)  reliability_yellow_lane += 5;
    }

    // Publish yellow-lane-reliability value and masked image
    std_msgs::UInt8 msg_yellow_lane_reliability;
    msg_yellow_lane_reliability.data = reliability_yellow_lane;
    pub_yellow_lane_reliability_.publish(msg_yellow_lane_reliability);

    if (is_calibration_mode) {
        sensor_msgs::ImagePtr yellow_lane_msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", mask).toImageMsg();
        pub_image_yellow_lane_.publish(yellow_lane_msg);
    }

    return fraction_num;
}

#define LANE_FIT_DEGREE     3       // 2-order polynomial fitting

int DetectLaneNodelet::fit_from_lines(const vector<Point>& nonzero_pts, vector<double>& lane_fit)
{
    int pt_size = nonzero_pts.size();
    vector<int> x_vals;
    vector<int> y_vals;
    int margin = 100;
    for (int i=0; i<pt_size; i++) {
        int x = nonzero_pts[i].x;
        int y = nonzero_pts[i].y;
        int  est_x = (int)(lane_fit[0]*y*y + lane_fit[1]*y + lane_fit[2]);
        if (abs(x - est_x) < margin) {
            x_vals.push_back(x);
            y_vals.push_back(y);
        }
    }

    // Fit a second-order polynormial to each
    int pts_size = y_vals.size();
    if (pts_size > 0) {
        // lane_fit = polyRegression(y_vals, x_vals);
        lane_fit = polynomialfit(pts_size, LANE_FIT_DEGREE, y_vals.data(), x_vals.data());
    }

    return pts_size;
}

#define LEFT_SIDE       0
#define RIGHT_SIDE      1

int DetectLaneNodelet::sliding_window(const Mat& image, const vector<Point>& nonzero_pts, int left_or_right, vector<double>& lane_fit)
{
    int width = image.cols;         // image width
    int height = image.rows;        // image height

    Mat out_image;
    if (is_calibration_mode) {
        cvtColor(image, out_image, CV_GRAY2BGR);
    }

    vector<int> histogram(width);
    for (int i=height/2; i<height; i++) {
        const uchar* ptr = image.ptr<uchar>(i);
        for (int j=0; j<width; j++) {
            if (*ptr++ != 0)  histogram[j]++;
        }
    }

    int lane_base = 0;
    if (left_or_right == LEFT_SIDE) {
        // lane_base = argmax(histogram, 0, width/2-1);
        lane_base = arghalfmax(histogram, 0, width/2-1);
    }
    else if (left_or_right == RIGHT_SIDE) {
        // lane_base = argmax(histogram, width/2, width-1);
        lane_base = arghalfmax(histogram, width/2, width-1, true);
    }

    int nwindows = 20;
    int window_height = height / nwindows;
//  int window_width = 100;
    int window_width = 150;     // 100 => 150 in terms of lane width
    int x_current = lane_base;
//    int minpixels = 50;
    int minpixels = 25;

    vector<Point> lane_inds;
    int acc_x_pos = 0;
    int x_count = 0;
    int win_top, win_left;

    // Step through the windows one by one
    for (int i=0; i<nwindows; i++) {
        // Identify window boundaries in x and y
        win_top = height - ((i+1) * window_height);
        win_left = x_current - (window_width/2);
        if (win_left < 0)  win_left = 0;
        acc_x_pos = 0;
        x_count = 0;

        // Identify the nonzero pixels in x and y within the window
        for (int k=0; k<nonzero_pts.size(); k++) {
            int x = nonzero_pts[k].x;
            int y = nonzero_pts[k].y;
            if ((y >= win_top && (y - win_top) < window_height) &&
                (x >= win_left && (x - win_left) < window_width)) {
                lane_inds.push_back(nonzero_pts[k]);
                acc_x_pos += x;
                x_count++;
            }
        }

        // If you found > minpix pixels, recenter next window on their mean position
        if (x_count > minpixels) {
            x_current = acc_x_pos / x_count;
        }

        if (is_calibration_mode) {
            //# Draw the windows on the visualization image
            rectangle(out_image, Point(win_left, win_top),
                Point(win_left+window_width-1, win_top+window_height-1),
                Scalar(0, 255, 0), 2);
        }
    }

    int pts_size = lane_inds.size();
    if (pts_size > 0) {
        // Extract line pixel positions
        vector<int> x_vals(pts_size);
        vector<int> y_vals(pts_size);
        int *xp = x_vals.data();
        int *yp = y_vals.data();
        for (int i=0; i<pts_size; i++) {
            *xp++ = lane_inds[i].x;
            *yp++ = lane_inds[i].y;
        }

        // Fit a second-order polynormial to each
        try {
            // lane_fit = polyRegression(y_vals, x_vals));
            lane_fit = polynomialfit(pts_size, LANE_FIT_DEGREE, y_vals.data(), x_vals.data());
            lane_fit_bef = lane_fit;
        } catch (int ex) {
            lane_fit = lane_fit_bef;
        }
    }
    else {
        lane_fit = lane_fit_bef;
    }

    if (is_calibration_mode) {
        // FOR TESTING...
        // vector<Point> pts;
        // getPointsFromPolyfit(lane_fit, out_image.rows, pts);
        // const Point* ppt[] = { pts.data() };
        // int npts[] = { out_image.rows };
        // polylines(out_image, ppt, npts, 1, false, Scalar(255, 0, 0), 12);

        if (left_or_right == LEFT_SIDE) {
            sensor_msgs::ImagePtr yellow_lane_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", out_image).toImageMsg();
            pub_image_yellow_lane_.publish(yellow_lane_msg);
        }
        else {
            sensor_msgs::ImagePtr white_lane_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", out_image).toImageMsg();
            pub_image_white_lane_.publish(white_lane_msg);
        }
    }

    return pts_size;
}

int DetectLaneNodelet::getPointsFromPolyfit(const vector<double>& poly_fit, int size, vector<Point>& points)
{
    for (int y=0; y<size; y++) {
        points.push_back(Point((int)(poly_fit[0]*y*y+poly_fit[1]*y+poly_fit[2]), y));
    }
    return size;
}

int DetectLaneNodelet::getCenterPointsFromPolyfit(const vector<double>& left_poly_fit, const vector<double>& right_poly_fit,
                int offset, int size, vector<Point>& points)
{
    int center_x;

    if (offset == 0) {
        for (int y=0; y<size; y++) {
            center_x = (int)((left_poly_fit[0]*y*y+left_poly_fit[1]*y+left_poly_fit[2]
                            + right_poly_fit[0]*y*y+right_poly_fit[1]*y+right_poly_fit[2])/2);
            points.push_back(Point(center_x, y));
        }
    }
    else if (offset > 0) {
        for (int y=0; y<size; y++) {
            center_x = (int)(left_poly_fit[0]*y*y+left_poly_fit[1]*y+left_poly_fit[2]) + offset;
            points.push_back(Point(center_x, y));
        }
    }
    else {
        for (int y=0; y<size; y++) {
            center_x = (int)(right_poly_fit[0]*y*y+right_poly_fit[1]*y+right_poly_fit[2]) + offset;
            points.push_back(Point(center_x, y));
        }
    }

    return size;
}

#define SEPARATION_DISTANCE         180     // 225     // 320 => 400

void DetectLaneNodelet::make_lane(Mat& image, int white_fraction, int yellow_fraction)
{
    // Create an image to draw the lines on
    Mat color_warp = Mat::zeros(image.rows, image.cols, image.type());
    Mat color_warp_lines = Mat::zeros(image.rows, image.cols, image.type());

    int npts[] = { image.rows };
    vector<Point> white_pts;
    vector<Point> yellow_pts;
    vector<Point> center_pts;

    if (white_fraction > 3000) {
        getPointsFromPolyfit(white_lane_fit, image.rows, white_pts);
        const Point* ppt[] = { white_pts.data() };
        polylines(color_warp_lines, ppt, npts, 1, false, Scalar(255, 255, 0), 25);
    }

    if (yellow_fraction > 3000) {
        getPointsFromPolyfit(yellow_lane_fit, image.rows, yellow_pts);
        const Point* ppt[] = { yellow_pts.data() };
        polylines(color_warp_lines, ppt, npts, 1, false, Scalar(0, 0, 255), 25);
    }

    bool is_center_x_exist = true;

    if (reliability_white_lane > 50 && reliability_yellow_lane > 50) {
        if (white_fraction > 3000 && yellow_fraction > 3000) {
            getCenterPointsFromPolyfit(yellow_lane_fit, white_lane_fit, 0, image.rows, center_pts);

            // Draw the lane onto the warped blank image
            // Concatenate white-point vector to yellow-point vector
            for (int i=white_pts.size()-1; i>=0; i--) {
                yellow_pts.push_back(white_pts[i]);
            }
            const Point* ppt2[] = { yellow_pts.data() };
            int npts2[] = { (int)yellow_pts.size() };
            fillPoly(color_warp, ppt2, npts2, 1, Scalar(0, 255, 0), 8);
        }
        else if (white_fraction > 3000 && yellow_fraction <= 3000) {
            getCenterPointsFromPolyfit(yellow_lane_fit, white_lane_fit, -SEPARATION_DISTANCE, image.rows, center_pts);
        }
        else if (white_fraction <= 3000 && yellow_fraction > 3000) {
            getCenterPointsFromPolyfit(yellow_lane_fit, white_lane_fit, SEPARATION_DISTANCE, image.rows, center_pts);
        }
    }
    else if (reliability_white_lane <= 50 && reliability_yellow_lane > 50) {
            getCenterPointsFromPolyfit(yellow_lane_fit, white_lane_fit, SEPARATION_DISTANCE, image.rows, center_pts);
    }
    else if (reliability_white_lane > 50 && reliability_yellow_lane <= 50) {
            getCenterPointsFromPolyfit(yellow_lane_fit, white_lane_fit, -SEPARATION_DISTANCE, image.rows, center_pts);
    }
    else {
        is_center_x_exist = false;
    }

    // Publishes lane center
    if (is_center_x_exist) {
        const Point* ppt[] = { center_pts.data() };
        polylines(color_warp_lines, ppt, npts, 1, false, Scalar(0, 255, 255), 12);

        std_msgs::Float64Ptr msg_desired_center = boost::shared_ptr<std_msgs::Float64>(new std_msgs::Float64());
        msg_desired_center->data = center_pts[450].x;
        // msg_desired_center->data = center_pts[350].x;
        pub_lane_.publish(msg_desired_center);
    }

    // Combine the result with the original image
    addWeighted(image, 1, color_warp, 0.2, 0, image);
    addWeighted(image, 1, color_warp_lines, 1, 0, image);

    // Publishes lane image
    sensor_msgs::ImagePtr image_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
    pub_image_lane_.publish(image_msg);
}

void DetectLaneNodelet::onInit()
{
    // Get NodeHandles
    ros::NodeHandle &nh         = getNodeHandle();
    ros::NodeHandle &private_nh = getPrivateNodeHandle();
    it_.reset(new image_transport::ImageTransport(nh));

    // Read parameters
    private_nh.param("queue_size", queue_size_, 5);
    private_nh.param("is_detection_calibration_mode", is_calibration_mode, false);

    NODELET_INFO("[Detct Lane] queue_size = %d", queue_size_);
    NODELET_INFO("[Detct Lane] is_calibration_mode = %s", (is_calibration_mode?"True":"False"));

    // Initialize properties
    reliability_white_lane = 100;
    reliability_yellow_lane = 100;

    counter = 0;

    // Set up dynamic reconfigure
    if (is_calibration_mode) {
        reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh));
        ReconfigureServer::CallbackType f = boost::bind(&DetectLaneNodelet::configCB, this, _1, _2);
        reconfigure_server_->setCallback(f);
    }
//    else {
        private_nh.param("detect/lane/white/hue_l", hue_white_l, 0);
        private_nh.param("detect/lane/white/hue_h", hue_white_h, 25);
        private_nh.param("detect/lane/white/saturation_l", saturation_white_l, 0);
        private_nh.param("detect/lane/white/saturation_h", saturation_white_h, 36);
        private_nh.param("detect/lane/white/lightness_l", lightness_white_l, 180);
        private_nh.param("detect/lane/white/lightness_h", lightness_white_h, 255);
        private_nh.param("detect/lane/yellow/hue_l", hue_yellow_l, 27);
        private_nh.param("detect/lane/yellow/hue_h", hue_yellow_h, 41);
        private_nh.param("detect/lane/yellow/saturation_l", saturation_yellow_l, 130);
        private_nh.param("detect/lane/yellow/saturation_h", saturation_yellow_h, 255);
        private_nh.param("detect/lane/yellow/lightness_l", lightness_yellow_l, 160);
        private_nh.param("detect/lane/yellow/lightness_h", lightness_yellow_h, 255);

        NODELET_INFO("[Detect Lane] Extrinsic Camera Calibration parameter");
        NODELET_INFO("hue_white_l: %d, hue_white_h: %d", hue_white_l, hue_white_h);
        NODELET_INFO("saturation_white_l: %d, saturation_white_h: %d", saturation_white_l, saturation_white_h);
        NODELET_INFO("lightness_white_l: %d, lightness_white_h: %d", lightness_white_l, lightness_white_h);
        NODELET_INFO("hue_yellow_l: %d, hue_yellow_h: %d", hue_yellow_l, hue_yellow_h);
        NODELET_INFO("saturation_yellow_l: %d, saturation_yellow_h: %d", saturation_yellow_l, saturation_yellow_h);
        NODELET_INFO("lightness_yellow_l: %d, lightness_yellow_h: %d", lightness_yellow_l, lightness_yellow_h);
//    }

    // Initialize image-transport publisher
    // Monitor whether anyone is subscribed to lane-detection output
    ros::SubscriberStatusCallback connect_cb = boost::bind(&DetectLaneNodelet::connectCB, this);
    ros::SubscriberStatusCallback disconnect_cb = boost::bind(&DetectLaneNodelet::disconnectCB, this);
    // Make sure we don't enter connectCb() between advertising and assigning to pub_lane_
    boost::lock_guard<boost::mutex> lock(connect_mutex_);

    pub_lane_ = nh.advertise<std_msgs::Float64>("/detect/lane", 1, connect_cb, disconnect_cb);
    pub_yellow_lane_reliability_ = nh.advertise<std_msgs::UInt8>("/detect/yellow_lane_relibility", 1);
    pub_white_lane_reliability_ = nh.advertise<std_msgs::UInt8>("/detect/white_lane_relibility", 1);

    pub_image_lane_ = it_->advertise("/detect/image_output", 1);
    if (is_calibration_mode) {
        pub_image_white_lane_ = it_->advertise("/detect/image_output_sub1", 1);
        pub_image_yellow_lane_ = it_->advertise("/detect/image_output_sub2", 1);
    }
}

void DetectLaneNodelet::configCB(Config &config, uint32_t level)
{
    NODELET_INFO("[Detect Lane] Extrinsic Camera Calibration parameter reconfigured to");
    NODELET_INFO("hue_white_l: %d, hue_white_h: %d", config.hue_white_l, config.hue_white_h);
    NODELET_INFO("saturation_white_l: %d, saturation_white_h: %d", config.saturation_white_l, config.saturation_white_h);
    NODELET_INFO("lightness_white_l: %d, lightness_white_h: %d", config.lightness_white_l, config.lightness_white_h);
    NODELET_INFO("hue_yellow_l: %d, hue_yellow_h: %d", config.hue_yellow_l, config.hue_yellow_h);
    NODELET_INFO("saturation_yellow_l: %d, saturation_yellow_h: %d", config.saturation_yellow_l, config.saturation_yellow_h);
    NODELET_INFO("lightness_yellow_l: %d, lightness_yellow_h: %d", config.lightness_yellow_l, config.lightness_yellow_h);

    boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
    // config_ = config;
    hue_white_l = config.hue_white_l;
    hue_white_h = config.hue_white_h;
    saturation_white_l = config.saturation_white_l;
    saturation_white_h = config.saturation_white_h;
    lightness_white_l = config.lightness_white_l;
    lightness_white_h = config.lightness_white_h;
    hue_yellow_l = config.hue_yellow_l;
    hue_yellow_h = config.hue_yellow_h;
    saturation_yellow_l = config.saturation_yellow_l;
    saturation_yellow_h = config.saturation_yellow_h;
    lightness_yellow_l = config.lightness_yellow_l;
    lightness_yellow_h = config.lightness_yellow_h;
}

void DetectLaneNodelet::connectCB()
{
    boost::lock_guard<boost::mutex> lock(connect_mutex_);
    if (!sub_projected_image_) {
        NODELET_INFO("[Detct Lane] Activate image subscriber...");
        image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
        sub_projected_image_ = it_->subscribe("/detect/image_input", queue_size_, &DetectLaneNodelet::imageCB, this, hints);
    }
}

void DetectLaneNodelet::disconnectCB()
{
    boost::lock_guard<boost::mutex> lock(connect_mutex_);
    if (pub_lane_.getNumSubscribers() == 0) {
        NODELET_INFO("[Detct Lane] Shutdown image subscriber...");
        sub_projected_image_.shutdown();
    }
}

void DetectLaneNodelet::imageCB(const sensor_msgs::ImageConstPtr& image_msg)
{
    // Change the frame rate by yourself. Now, it is set to 1/3 (10fps).
    // Unappropriate value of frame rate may cause huge delay on entire recognition process.
    // This is up to your computer's operating power.
    if (counter % 3 != 0) {
        counter += 1;
        return;
    }
    else counter = 1;

    cv_bridge::CvImagePtr cv_ptr;
    try {
        cv_ptr = cv_bridge::toCvCopy(image_msg, "bgr8");
    }
    catch (cv_bridge::Exception& e) {
        NODELET_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    cv::Mat cv_image;
    (cv_ptr->image).copyTo(cv_image);

    // Find White and Yellow Lanes
    Mat hsv_img;
    cvtColor(cv_image, hsv_img, CV_BGR2HSV);

    cv::Mat cv_white_lane;
    vector<Point> white_lane_pts;
    int white_fraction = maskWhiteLaneFromHSV(hsv_img, cv_white_lane, white_lane_pts);

    cv::Mat cv_yellow_lane;
    vector<Point> yellow_lane_pts;
    int yellow_fraction = maskYellowLaneFromHSV(hsv_img, cv_yellow_lane, yellow_lane_pts);
    // NODELET_INFO("yellow_fraction = %d, white_fraction = %d", yellow_fraction, white_fraction);

    // try {
    //     NODELET_INFO("Execute fit_from_lines()...");
    //     if (yellow_fraction > 3000) {
    //         fit_from_lines(cv_yellow_lane, yellow_lane_fit);
    //         mov_avg_left.append(yellow_lane_fit);
    //     }

    //     if (white_fraction > 3000) {
    //         fit_from_lines(cv_white_lane, white_lane_fit);
    //         mov_avg_right.append(white_lane_fit);
    //     }
    // }
    // catch (int ex) {
        // NODELET_INFO("Execute sliding_window() on yellow mask...");
        if (yellow_fraction > 3000) {
            sliding_window(cv_yellow_lane, yellow_lane_pts, LEFT_SIDE, yellow_lane_fit);
            // mov_avg_left.init(yellow_lane_fit);
            mov_avg_left.append(yellow_lane_fit);
        }

        // NODELET_INFO("Execute sliding_window() on white mask...");
        if (white_fraction > 3000) {
            sliding_window(cv_white_lane, white_lane_pts, RIGHT_SIDE, white_lane_fit);
//            mov_avg_right.init(white_lane_fit);
            mov_avg_right.append(white_lane_fit);
        }
    // }

    // NODELET_INFO("Get average lane fit....");
    yellow_lane_fit = mov_avg_left.mean();
    white_lane_fit = mov_avg_right.mean();
    // NODELET_INFO("yellow_lane_fit : %f %f %f", yellow_lane_fit.at(0), yellow_lane_fit.at(1), yellow_lane_fit.at(2));
    // NODELET_INFO("white_lane_fit : %f %f %f", white_lane_fit.at(0), white_lane_fit.at(1), white_lane_fit.at(2));

    make_lane(cv_image, white_fraction, yellow_fraction);
}

}       // lane_detection namespace

// Register nodelet
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(lane_detection::DetectLaneNodelet, nodelet::Nodelet)


text:
/* Copyright (C) 2014 InfiniDB, Inc.
   Copyright (C) 2019 MariaDB Corporation

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

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

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

//  $Id: groupconcat.cpp 9705 2013-07-17 20:06:07Z pleblanc $

#include <iostream>
//#define NDEBUG
#include <cassert>
#include <string>
using namespace std;

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

#include "errorids.h"
#include "exceptclasses.h"
using namespace logging;

#include "returnedcolumn.h"
#include "aggregatecolumn.h"
#include "arithmeticcolumn.h"
#include "functioncolumn.h"
#include "constantcolumn.h"
#include "rowcolumn.h"
#include "groupconcatcolumn.h"
#include "calpontsystemcatalog.h"
using namespace execplan;

#include "rowgroup.h"
#include "rowaggregation.h"
using namespace rowgroup;

#include "dataconvert.h"
using namespace dataconvert;

#include "groupconcat.h"

using namespace ordering;

#include "jobstep.h"
#include "jlf_common.h"
#include "limitedorderby.h"
#include "mcs_decimal.h"

namespace joblist
{

// GroupConcatInfo class implementation
GroupConcatInfo::GroupConcatInfo()
{
}

GroupConcatInfo::~GroupConcatInfo()
{
}

void GroupConcatInfo::prepGroupConcat(JobInfo& jobInfo)
{
    RetColsVector::iterator i = jobInfo.groupConcatCols.begin();

    while (i != jobInfo.groupConcatCols.end())
    {
        GroupConcatColumn* gcc = dynamic_cast<GroupConcatColumn*>(i->get());
        const RowColumn* rcp = dynamic_cast<const RowColumn*>(gcc->aggParms()[0].get());

        SP_GroupConcat groupConcat(new GroupConcat);
        groupConcat->fSeparator = gcc->separator();
        groupConcat->fDistinct = gcc->distinct();
        groupConcat->fSize = gcc->resultType().colWidth;
        groupConcat->fRm = jobInfo.rm;
        groupConcat->fSessionMemLimit = jobInfo.umMemLimit;
        groupConcat->fTimeZone = jobInfo.timeZone;

        int key = -1;
        const vector<SRCP>& cols = rcp->columnVec();

        for (uint64_t j = 0, k = 0; j < cols.size(); j++)
        {
            const ConstantColumn* cc = dynamic_cast<const ConstantColumn*>(cols[j].get());

            if (cc == NULL)
            {
                key = getColumnKey(cols[j], jobInfo);
                fColumns.insert(key);
                groupConcat->fGroupCols.push_back(make_pair(key, k++));
            }
            else
            {
                groupConcat->fConstCols.push_back(make_pair(cc->constval(), j));
            }
        }

        vector<SRCP>& orderCols = gcc->orderCols();

        for (vector<SRCP>::iterator k = orderCols.begin(); k != orderCols.end(); k++)
        {
            if (dynamic_cast<const ConstantColumn*>(k->get()) != NULL)
                continue;

            key = getColumnKey(*k, jobInfo);
            fColumns.insert(key);
            groupConcat->fOrderCols.push_back(make_pair(key, k->get()->asc()));
        }

        fGroupConcat.push_back(groupConcat);

        i++;
    }

    // Rare case: all columns in group_concat are constant columns, use a column in column map.
    if (jobInfo.groupConcatCols.size() > 0 && fColumns.size() == 0)
    {
        int key = -1;

        for (vector<uint32_t>::iterator i = jobInfo.tableList.begin();
                i != jobInfo.tableList.end() && key == -1;
                i++)
        {
            if (jobInfo.columnMap[*i].size() > 0)
            {
                key = *(jobInfo.columnMap[*i].begin());
            }
        }

        if (key != -1)
        {
            fColumns.insert(key);
        }
        else
        {
            throw runtime_error("Empty column map.");
        }
    }
}

uint32_t GroupConcatInfo::getColumnKey(const SRCP& srcp, JobInfo& jobInfo)
{
    int colKey = -1;
    const SimpleColumn* sc = dynamic_cast<const SimpleColumn*>(srcp.get());

    if (sc != NULL)
    {
        if (sc->schemaName().empty())
        {
            // bug3839, handle columns from subquery.
            SimpleColumn tmp(*sc, jobInfo.sessionId);
            tmp.oid(tableOid(sc, jobInfo.csc) + 1 + sc->colPosition());
            colKey = getTupleKey(jobInfo, &tmp);
        }
        else
        {
            colKey = getTupleKey(jobInfo, sc);
        }

        // check if this is a dictionary column
        if (jobInfo.keyInfo->dictKeyMap.find(colKey) != jobInfo.keyInfo->dictKeyMap.end())
            colKey = jobInfo.keyInfo->dictKeyMap[colKey];
    }
    else
    {
        const ArithmeticColumn* ac = dynamic_cast<const ArithmeticColumn*>(srcp.get());
        const FunctionColumn* fc = dynamic_cast<const FunctionColumn*>(srcp.get());

        if (ac != NULL || fc != NULL)
        {
            colKey = getExpTupleKey(jobInfo, srcp->expressionId());
        }
        else
        {
            cerr << "Unsupported GROUP_CONCAT column. " << srcp->toString() << endl;
            throw runtime_error("Unsupported GROUP_CONCAT column.");
        }
    }

    return colKey;
}

void GroupConcatInfo::mapColumns(const RowGroup& projRG)
{
    map<uint32_t, uint32_t> projColumnMap;
    const vector<uint32_t>& keysProj = projRG.getKeys();

    for (uint64_t i = 0; i < projRG.getColumnCount(); i++)
        projColumnMap[keysProj[i]] = i;

    for (vector<SP_GroupConcat>::iterator k = fGroupConcat.begin(); k != fGroupConcat.end(); k++)
    {
        vector<uint32_t> pos;
        vector<uint32_t> oids;
        vector<uint32_t> keys;
        vector<uint32_t> scale;
        vector<uint32_t> precision;
        vector<CalpontSystemCatalog::ColDataType> types;
        vector<uint32_t> csNums;
        pos.push_back(2);

        vector<pair<uint32_t, uint32_t> >::iterator i1 = (*k)->fGroupCols.begin();

        while (i1 != (*k)->fGroupCols.end())
        {
            map<uint32_t, uint32_t>::iterator j = projColumnMap.find(i1->first);

            if (j == projColumnMap.end())
            {
                cerr << "Concat Key:" << i1->first << " is not projected." << endl;
                throw runtime_error("Project error.");
            }

            pos.push_back(pos.back() + projRG.getColumnWidth(j->second));
            oids.push_back(projRG.getOIDs()[j->second]);
            keys.push_back(projRG.getKeys()[j->second]);
            types.push_back(projRG.getColTypes()[j->second]);
            csNums.push_back(projRG.getCharsetNumber(j->second));
            scale.push_back(projRG.getScale()[j->second]);
            precision.push_back(projRG.getPrecision()[j->second]);

            i1++;
        }

        vector<pair<uint32_t, bool> >::iterator i2 = (*k)->fOrderCols.begin();

        while (i2 != (*k)->fOrderCols.end())
        {
            map<uint32_t, uint32_t>::iterator j = projColumnMap.find(i2->first);

            if (j == projColumnMap.end())
            {
                cerr << "Order Key:" << i2->first << " is not projected." << endl;
                throw runtime_error("Project error.");
            }

            vector<uint32_t>::iterator i3 = find(keys.begin(), keys.end(), j->first);
            int idx = 0;

            if (i3 == keys.end())
            {
                idx = keys.size();

                pos.push_back(pos.back() + projRG.getColumnWidth(j->second));
                oids.push_back(projRG.getOIDs()[j->second]);
                keys.push_back(projRG.getKeys()[j->second]);
                types.push_back(projRG.getColTypes()[j->second]);
                csNums.push_back(projRG.getCharsetNumber(j->second));
                scale.push_back(projRG.getScale()[j->second]);
                precision.push_back(projRG.getPrecision()[j->second]);
            }
            else
            {
                idx = std::distance(keys.begin(), i3);
            }

            (*k)->fOrderCond.push_back(make_pair(idx, i2->second));

            i2++;
        }

        (*k)->fRowGroup = RowGroup(oids.size(), pos, oids, keys, types, csNums, scale, precision, projRG.getStringTableThreshold(), false);
        (*k)->fMapping = makeMapping(projRG, (*k)->fRowGroup);
    }
}

shared_array<int> GroupConcatInfo::makeMapping(const RowGroup& in, const RowGroup& out)
{
    // For some reason using the rowgroup mapping fcns don't work completely right in this class
    shared_array<int> mapping(new int[out.getColumnCount()]);

    for (uint64_t i = 0; i < out.getColumnCount(); i++)
    {
        for (uint64_t j = 0; j < in.getColumnCount(); j++)
        {
            if ((out.getKeys()[i] == in.getKeys()[j]))
            {
                mapping[i] = j;
                break;
            }
        }
    }

    return mapping;
}

const string GroupConcatInfo::toString() const
{
    ostringstream oss;
    oss << "GroupConcatInfo: toString() to be implemented.";
    oss << endl;

    return oss.str();
}

GroupConcatAgUM::GroupConcatAgUM(rowgroup::SP_GroupConcat& gcc) : GroupConcatAg(gcc)
{
    initialize();
}

GroupConcatAgUM::~GroupConcatAgUM()
{
}

void GroupConcatAgUM::initialize()
{
    if (fGroupConcat->fDistinct || fGroupConcat->fOrderCols.size() > 0)
        fConcator.reset(new GroupConcatOrderBy());
    else
        fConcator.reset(new GroupConcatNoOrder());

    fConcator->initialize(fGroupConcat);

    fGroupConcat->fRowGroup.initRow(&fRow, true);
    fData.reset(new uint8_t[fRow.getSize()]);
    fRow.setData(fData.get());
}

void GroupConcatAgUM::processRow(const rowgroup::Row& inRow)
{
    applyMapping(fGroupConcat->fMapping, inRow);
    fConcator->processRow(fRow);
}

void GroupConcatAgUM::merge(const rowgroup::Row& inRow, int64_t i)
{
    uint8_t* data = inRow.getData();
    joblist::GroupConcatAgUM* gccAg = *((joblist::GroupConcatAgUM**)(data + inRow.getOffset(i)));

    fConcator->merge(gccAg->concator().get());
}

void GroupConcatAgUM::getResult(uint8_t* buff)
{
    fConcator->getResult(buff, fGroupConcat->fSeparator);
}

uint8_t* GroupConcatAgUM::getResult()
{
    return fConcator->getResult(fGroupConcat->fSeparator);
}

void GroupConcatAgUM::applyMapping(const boost::shared_array<int>& mapping, const Row& row)
{
    // For some reason the rowgroup mapping fcns don't work right in this class.
    for (uint64_t i = 0; i < fRow.getColumnCount(); i++)
    {
        if (fRow.getColumnWidth(i) > datatypes::MAXLEGACYWIDTH)
        {
            if (fRow.getColTypes()[i] == execplan::CalpontSystemCatalog::CHAR ||
                fRow.getColTypes()[i] == execplan::CalpontSystemCatalog::VARCHAR ||
                fRow.getColTypes()[i] == execplan::CalpontSystemCatalog::TEXT)
            {
                fRow.setStringField(row.getConstString(mapping[i]), i);
            }
            else if (fRow.getColTypes()[i] == execplan::CalpontSystemCatalog::LONGDOUBLE)
            {
                fRow.setLongDoubleField(row.getLongDoubleField(mapping[i]), i);
            }
            else if (datatypes::isWideDecimalType(fRow.getColType(i), fRow.getColumnWidth(i)))
            {
                row.copyBinaryField<int128_t>(fRow, i, mapping[i]);
            }
        }
        else
        {
            fRow.setIntField(row.getIntField(mapping[i]), i);
        }
    }
}

// GroupConcator class implementation
GroupConcator::GroupConcator() : fCurrentLength(0), fGroupConcatLen(0), fConstantLen(0)
{
}

GroupConcator::~GroupConcator()
{
}

void GroupConcator::initialize(const rowgroup::SP_GroupConcat& gcc)
{
    // MCOL-901 This value comes from the Server and it is
    // too high(3MB) to allocate it for every instance.
    fGroupConcatLen = gcc->fSize;
    fCurrentLength -= strlen(gcc->fSeparator.c_str());
    fTimeZone = gcc->fTimeZone;

    fConstCols = gcc->fConstCols;
    fConstantLen = strlen(gcc->fSeparator.c_str());

    for (uint64_t i = 0; i < fConstCols.size(); i++)
        fConstantLen += strlen(fConstCols[i].first.c_str());
}

void GroupConcator::outputRow(std::ostringstream& oss, const rowgroup::Row& row)
{
    const CalpontSystemCatalog::ColDataType* types = row.getColTypes();
    vector<uint32_t>::iterator i = fConcatColumns.begin();
    vector<pair<string, uint32_t> >::iterator j = fConstCols.begin();

    uint64_t groupColCount = fConcatColumns.size() + fConstCols.size();

    for (uint64_t k = 0; k < groupColCount; k++)
    {
        if (j != fConstCols.end() &&  k == j->second)
        {
            oss << j->first;
            j++;
            continue;
        }

        switch (types[*i])
        {
            case CalpontSystemCatalog::TINYINT:
            case CalpontSystemCatalog::SMALLINT:
            case CalpontSystemCatalog::MEDINT:
            case CalpontSystemCatalog::INT:
            case CalpontSystemCatalog::BIGINT:
            {
                int64_t intVal = row.getIntField(*i);

                oss << intVal;

                break;
            }

            case CalpontSystemCatalog::DECIMAL:
            case CalpontSystemCatalog::UDECIMAL:
            {
                oss << fixed << row.getDecimalField(*i);
                break;
            }

            case CalpontSystemCatalog::UTINYINT:
            case CalpontSystemCatalog::USMALLINT:
            case CalpontSystemCatalog::UMEDINT:
            case CalpontSystemCatalog::UINT:
            case CalpontSystemCatalog::UBIGINT:
            {
                uint64_t uintVal = row.getUintField(*i);
                int scale = (int) row.getScale(*i);

                if (scale == 0)
                {
                    oss << uintVal;
                }
                else
                {
                    oss << fixed << datatypes::Decimal(datatypes::TSInt128((int128_t) uintVal),
                                                       scale,
                                                       datatypes::INT128MAXPRECISION);
                }

                break;
            }

            case CalpontSystemCatalog::CHAR:
            case CalpontSystemCatalog::VARCHAR:
            case CalpontSystemCatalog::TEXT:
            {
                oss << row.getStringField(*i).c_str();
                break;
            }

            case CalpontSystemCatalog::DOUBLE:
            case CalpontSystemCatalog::UDOUBLE:
            {
                oss << setprecision(15) << row.getDoubleField(*i);
                break;
            }

            case CalpontSystemCatalog::LONGDOUBLE:
            {
                oss << setprecision(15) << row.getLongDoubleField(*i);
                break;
            }

            case CalpontSystemCatalog::FLOAT:
            case CalpontSystemCatalog::UFLOAT:
            {
                oss << row.getFloatField(*i);
                break;
            }

            case CalpontSystemCatalog::DATE:
            {
                oss << DataConvert::dateToString(row.getUintField(*i));
                break;
            }

            case CalpontSystemCatalog::DATETIME:
            {
                oss << DataConvert::datetimeToString(row.getUintField(*i));
                break;
            }

            case CalpontSystemCatalog::TIMESTAMP:
            {
                oss << DataConvert::timestampToString(row.getUintField(*i), fTimeZone);
                break;
            }

            case CalpontSystemCatalog::TIME:
            {
                oss << DataConvert::timeToString(row.getUintField(*i));
                break;
            }

            default:
            {
                break;
            }
        }

        i++;
    }
}

bool GroupConcator::concatColIsNull(const rowgroup::Row& row)
{
    bool ret = false;

    for (vector<uint32_t>::iterator i = fConcatColumns.begin(); i != fConcatColumns.end(); i++)
    {
        if (row.isNullValue(*i))
        {
            ret = true;
            break;
        }
    }

    return ret;
}

int64_t GroupConcator::lengthEstimate(const rowgroup::Row& row)
{
    int64_t rowLen = fConstantLen;  // fixed constant and separator length
    const CalpontSystemCatalog::ColDataType* types = row.getColTypes();

    // null values are already skipped.
    for (vector<uint32_t>::iterator i = fConcatColumns.begin(); i != fConcatColumns.end(); i++)
    {
        if (row.isNullValue(*i))
            continue;

        int64_t fieldLen = 0;

        switch (types[*i])
        {
            case CalpontSystemCatalog::TINYINT:
            case CalpontSystemCatalog::SMALLINT:
            case CalpontSystemCatalog::MEDINT:
            case CalpontSystemCatalog::INT:
            case CalpontSystemCatalog::BIGINT:
            {
                int64_t v = row.getIntField(*i);

                if (v < 0) fieldLen++;

                while ((v /= 10) != 0) fieldLen++;

                fieldLen += 1;
                break;
            }

            case CalpontSystemCatalog::UTINYINT:
            case CalpontSystemCatalog::USMALLINT:
            case CalpontSystemCatalog::UMEDINT:
            case CalpontSystemCatalog::UINT:
            case CalpontSystemCatalog::UBIGINT:
            {
                uint64_t v = row.getUintField(*i);

                while ((v /= 10) != 0) fieldLen++;

                fieldLen += 1;
                break;
            }

            case CalpontSystemCatalog::DECIMAL:
            case CalpontSystemCatalog::UDECIMAL:
            {
                fieldLen += 1;

                break;
            }

            case CalpontSystemCatalog::CHAR:
            case CalpontSystemCatalog::VARCHAR:
            case CalpontSystemCatalog::TEXT:
            {
                fieldLen += row.getConstString(*i).length();
                break;
            }

            case CalpontSystemCatalog::DOUBLE:
            case CalpontSystemCatalog::UDOUBLE:
            case CalpontSystemCatalog::FLOAT:
            case CalpontSystemCatalog::UFLOAT:
            case CalpontSystemCatalog::LONGDOUBLE:
            {
                fieldLen = 1; // minimum length
                break;
            }

            case CalpontSystemCatalog::DATE:
            {
                fieldLen = 10; // YYYY-MM-DD
                break;
            }

            case CalpontSystemCatalog::DATETIME:
            case CalpontSystemCatalog::TIMESTAMP:
            {
                fieldLen = 19; // YYYY-MM-DD HH24:MI:SS
                // Decimal point and milliseconds
                uint64_t colPrecision = row.getPrecision(*i);

                if (colPrecision > 0 && colPrecision < 7)
                {
                    fieldLen += colPrecision + 1;
                }

                break;
            }

            case CalpontSystemCatalog::TIME:
            {
                fieldLen = 10; //  -HHH:MI:SS
                // Decimal point and milliseconds
                uint64_t colPrecision = row.getPrecision(*i);

                if (colPrecision > 0 && colPrecision < 7)
                {
                    fieldLen += colPrecision + 1;
                }

                break;
            }

            default:
            {
                break;
            }
        }

        rowLen += fieldLen;
    }

    return rowLen;
}

const string GroupConcator::toString() const
{
    ostringstream oss;
    oss << "GroupConcat size-" << fGroupConcatLen;
    oss << "Concat   cols: ";
    vector<uint32_t>::const_iterator i = fConcatColumns.begin();
    vector<pair<string, uint32_t> >::const_iterator j = fConstCols.begin();
    uint64_t groupColCount = fConcatColumns.size() + fConstCols.size();

    for (uint64_t k = 0; k < groupColCount; k++)
    {
        if (j != fConstCols.end() &&  k == j->second)
        {
            oss << 'c' << " ";
            j++;
        }
        else
        {
            oss << (*i) << " ";
            i++;
        }
    }

    oss << endl;

    return oss.str();
}

// GroupConcatOrderBy class implementation
GroupConcatOrderBy::GroupConcatOrderBy()
{
    fRule.fIdbCompare = this;
}

GroupConcatOrderBy::~GroupConcatOrderBy()
{
}

void GroupConcatOrderBy::initialize(const rowgroup::SP_GroupConcat& gcc)
{
    GroupConcator::initialize(gcc);

    fOrderByCond.resize(0);

    for (uint64_t i = 0; i < gcc->fOrderCond.size(); i++)
        fOrderByCond.push_back(IdbSortSpec(gcc->fOrderCond[i].first, gcc->fOrderCond[i].second));

    fDistinct = gcc->fDistinct;
    fRowsPerRG = 128;
    fErrorCode = ERR_AGGREGATION_TOO_BIG;
    fRm = gcc->fRm;
    fSessionMemLimit = gcc->fSessionMemLimit;

    vector<std::pair<uint32_t, uint32_t> >::iterator i = gcc->fGroupCols.begin();

    while (i != gcc->fGroupCols.end())
        fConcatColumns.push_back((*(i++)).second);

    IdbOrderBy::initialize(gcc->fRowGroup);
}

uint64_t GroupConcatOrderBy::getKeyLength() const
{
    // only distinct the concatenated columns
    return fConcatColumns.size(); // cols 0 to fConcatColumns.size() - 1 will be compared
}

void GroupConcatOrderBy::processRow(const rowgroup::Row& row)
{
    // check if this is a distinct row
    if (fDistinct && fDistinctMap->find(row.getPointer()) != fDistinctMap->end())
        return;

    // this row is skipped if any concatenated column is null.
    if (concatColIsNull(row))
        return;

    // if the row count is less than the limit
    if (fCurrentLength < fGroupConcatLen)
    {
        copyRow(row, &fRow0);
        // the RID is no meaning here, use it to store the estimated length.
        int16_t estLen = lengthEstimate(fRow0);
        fRow0.setRid(estLen);
        OrderByRow newRow(fRow0, fRule);
        fOrderByQueue.push(newRow);
        fCurrentLength += estLen;

        // add to the distinct map
        if (fDistinct)
            fDistinctMap->insert(fRow0.getPointer());

        fRowGroup.incRowCount();
        fRow0.nextRow();

        if (fRowGroup.getRowCount() >= fRowsPerRG)
        {
            fDataQueue.push(fData);

            uint64_t newSize = fRowsPerRG * fRowGroup.getRowSize();
            fMemSize += newSize;

            if (!fRm->getMemory(newSize, fSessionMemLimit))
            {
                cerr << IDBErrorInfo::instance()->errorMsg(fErrorCode)
                     << " @" << __FILE__ << ":" << __LINE__;
                throw IDBExcept(fErrorCode);
            }

            fData.reinit(fRowGroup, fRowsPerRG);
            fRowGroup.setData(&fData);
            fRowGroup.resetRowGroup(0);
            fRowGroup.getRow(0, &fRow0);
        }
    }

    else if (fOrderByCond.size() > 0 && fRule.less(row.getPointer(), fOrderByQueue.top().fData))
    {
        OrderByRow swapRow = fOrderByQueue.top();
        fRow1.setData(swapRow.fData);
        fOrderByQueue.pop();
        fCurrentLength -= fRow1.getRelRid();
        fRow2.setData(swapRow.fData);

        if (!fDistinct)
        {
            copyRow(row, &fRow1);
        }
        else
        {
            // only the copyRow does useful work here
            fDistinctMap->erase(swapRow.fData);
            copyRow(row, &fRow2);
            fDistinctMap->insert(swapRow.fData);
        }

        int16_t estLen = lengthEstimate(fRow2);
        fRow2.setRid(estLen);
        fCurrentLength += estLen;

        fOrderByQueue.push(swapRow);
    }
}

void GroupConcatOrderBy::merge(GroupConcator* gc)
{
    GroupConcatOrderBy* go = dynamic_cast<GroupConcatOrderBy*>(gc);

    while (go->fOrderByQueue.empty() == false)
    {
        const OrderByRow& row = go->fOrderByQueue.top();

        // check if the distinct row already exists
        if (fDistinct && fDistinctMap->find(row.fData) != fDistinctMap->end())
        {
            ; // no op;
        }

        // if the row count is less than the limit
        else if (fCurrentLength < fGroupConcatLen)
        {
            fOrderByQueue.push(row);
            row1.setData(row.fData);
            fCurrentLength += row1.getRelRid();

            // add to the distinct map
            if (fDistinct)
                fDistinctMap->insert(row.fData);

        }

        else if (fOrderByCond.size() > 0 && fRule.less(row.fData, fOrderByQueue.top().fData))
        {
            OrderByRow swapRow = fOrderByQueue.top();
            row1.setData(swapRow.fData);
            fOrderByQueue.pop();
            fCurrentLength -= row1.getRelRid();

            if (fDistinct)
            {
                fDistinctMap->erase(swapRow.fData);
                fDistinctMap->insert(row.fData);
            }

            row1.setData(row.fData);
            fCurrentLength += row1.getRelRid();

            fOrderByQueue.push(row);
        }

        go->fOrderByQueue.pop();
    }
}

void GroupConcatOrderBy::getResult(uint8_t* buff, const string& sep)
{
    ostringstream oss;
    bool addSep = false;

    // need to reverse the order
    stack<OrderByRow> rowStack;

    while (fOrderByQueue.size() > 0)
    {
        rowStack.push(fOrderByQueue.top());
        fOrderByQueue.pop();
    }

    while (rowStack.size() > 0)
    {
        if (addSep)
            oss << sep;
        else
            addSep = true;

        const OrderByRow& topRow = rowStack.top();
        fRow0.setData(topRow.fData);
        outputRow(oss, fRow0);
        rowStack.pop();
    }

    int64_t resultSize = oss.str().size();
    resultSize = (resultSize > fGroupConcatLen) ? fGroupConcatLen : resultSize;
    fOutputString.reset(new uint8_t[resultSize + 2]);
    fOutputString[resultSize] = '\0';
    fOutputString[resultSize + 1] = '\0';

    strncpy((char*)fOutputString.get(),
        oss.str().c_str(), resultSize);
}

uint8_t* GroupConcator::getResult(const string& sep)
{
    getResult(fOutputString.get(), sep);
    return fOutputString.get();
}

const string GroupConcatOrderBy::toString() const
{
    string baseStr = GroupConcator::toString();

    ostringstream oss;
    oss << "OrderBy   cols: ";
    vector<IdbSortSpec>::const_iterator i = fOrderByCond.begin();

    for (; i != fOrderByCond.end(); i++)
        oss << "(" << i->fIndex << ","
            << ((i->fAsc) ? "Asc" : "Desc") << ","
            << ((i->fNf) ? "null first" : "null last") << ") ";

    if (fDistinct)
        oss << endl << " distinct";

    oss << endl;

    return (baseStr + oss.str());
}

// GroupConcatNoOrder class implementation
GroupConcatNoOrder::GroupConcatNoOrder() :
    fRowsPerRG(128), fErrorCode(ERR_AGGREGATION_TOO_BIG), fMemSize(0), fRm(NULL)
{
}

GroupConcatNoOrder::~GroupConcatNoOrder()
{
    if (fRm)
        fRm->returnMemory(fMemSize, fSessionMemLimit);
}

void GroupConcatNoOrder::initialize(const rowgroup::SP_GroupConcat& gcc)
{
    GroupConcator::initialize(gcc);

    fRowGroup = gcc->fRowGroup;
    fRowsPerRG = 128;
    fErrorCode = ERR_AGGREGATION_TOO_BIG;
    fRm = gcc->fRm;
    fSessionMemLimit = gcc->fSessionMemLimit;

    vector<std::pair<uint32_t, uint32_t> >::iterator i = gcc->fGroupCols.begin();

    while (i != gcc->fGroupCols.end())
        fConcatColumns.push_back((*(i++)).second);

    uint64_t newSize = fRowsPerRG * fRowGroup.getRowSize();
    fMemSize += newSize;

    if (!fRm->getMemory(newSize, fSessionMemLimit))
    {
        cerr << IDBErrorInfo::instance()->errorMsg(fErrorCode)
             << " @" << __FILE__ << ":" << __LINE__;
        throw IDBExcept(fErrorCode);
    }

    fData.reinit(fRowGroup, fRowsPerRG);
    fRowGroup.setData(&fData);
    fRowGroup.resetRowGroup(0);
    fRowGroup.initRow(&fRow);
    fRowGroup.getRow(0, &fRow);
}

void GroupConcatNoOrder::processRow(const rowgroup::Row& row)
{
    // if the row count is less than the limit
    if (fCurrentLength < fGroupConcatLen && concatColIsNull(row) == false)
    {
        copyRow(row, &fRow);

        // the RID is no meaning here, use it to store the estimated length.
        int16_t estLen = lengthEstimate(fRow);
        fRow.setRid(estLen);
        fCurrentLength += estLen;
        fRowGroup.incRowCount();
        fRow.nextRow();

        if (fRowGroup.getRowCount() >= fRowsPerRG)
        {
            uint64_t newSize = fRowsPerRG * fRowGroup.getRowSize();

            fMemSize += newSize;

            if (!fRm->getMemory(newSize, fSessionMemLimit))
            {
                cerr << IDBErrorInfo::instance()->errorMsg(fErrorCode)
                     << " @" << __FILE__ << ":" << __LINE__;
                throw IDBExcept(fErrorCode);
            }

            fDataQueue.push(fData);
            fData.reinit(fRowGroup, fRowsPerRG);
            fRowGroup.setData(&fData);
            fRowGroup.resetRowGroup(0);
            fRowGroup.getRow(0, &fRow);
        }
    }
}

void GroupConcatNoOrder::merge(GroupConcator* gc)
{
    GroupConcatNoOrder* in = dynamic_cast<GroupConcatNoOrder*>(gc);

    while (in->fDataQueue.size() > 0)
    {
        fDataQueue.push(in->fDataQueue.front());
        in->fDataQueue.pop();
    }

    fDataQueue.push(in->fData);
    fMemSize += in->fMemSize;
    in->fMemSize = 0;
}

void GroupConcatNoOrder::getResult(uint8_t* buff, const string& sep)
{
    ostringstream oss;
    bool addSep = false;

    fDataQueue.push(fData);

    while (fDataQueue.size() > 0)
    {
        fRowGroup.setData(&fDataQueue.front());
        fRowGroup.getRow(0, &fRow);

        for (uint64_t i = 0; i < fRowGroup.getRowCount(); i++)
        {
            if (addSep)
                oss << sep;
            else
                addSep = true;

            outputRow(oss, fRow);
            fRow.nextRow();
        }

        fDataQueue.pop();
    }

    int64_t resultSize = oss.str().size();
    resultSize = (resultSize > fGroupConcatLen) ? fGroupConcatLen : resultSize;
    fOutputString.reset(new uint8_t[resultSize + 2]);
    fOutputString[resultSize] = '\0';
    fOutputString[resultSize + 1] = '\0';

    strncpy((char*)fOutputString.get(),
        oss.str().c_str(), resultSize);
}

const string GroupConcatNoOrder::toString() const
{
    return GroupConcator::toString();
}

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

ext: cpp

max_stars_repo_name: zettadb/zettalib

max_issues_repo_name: zettadb/zettalib

max_forks_repo_name: zettadb/zettalib


text:
#include <jsoncpp/json/json.h>

#include <boost/lexical_cast.hpp>

#include <unordered_map>
#include <iostream>
#include <fstream>
#include <vector>
#include <thread>
#include <cmath>

const std::unordered_map<std::string, int> type_to_weight =
        {
            {"motorway", 2},
            {"trunk", 2},
            {"primary", 5},
            {"secondary", 4},
            {"tertiary", 3},
//            {"unclassified", 1},
//            {"residential", 1},
        };

struct Point
{
    double lat = 0.0;
    double lon = 0.0;
};

struct Way
{
    std::string type_;
    std::vector<Point> points_;

};

struct Line
{
    Point start_;
    Point finish_;
    int weight_ = 0;
};

struct PointUtils
{
    static Point Add(const Point& start, const Point& shift)
    {
        Point point;
        point.lat = start.lat + shift.lat;
        point.lon = start.lon + shift.lon;
        return point;
    }

    static Point Substruct(const Point& start, const Point& finish)
    {
        Point point;
        point.lat = start.lat - finish.lat;
        point.lon = start.lon - finish.lon;
        return point;
    }

    static Point Mult(const Point& start, const int n, const int m)
    {
        Point point;
        point.lat = start.lat * n;
        point.lon = start.lon * m;
        return point;
    }

    static double GetDistance(const Point& one, const Point& two)
    {
        double x2 = two.lat;
        double x1 = one.lat;
        double y2 = two.lon;
        double y1 = one.lon;
        return sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) );
    }

    static double GetMinLengthFromLineToPoint(const Line& line, const Point& point)
    {
        const double x0 = point.lat;
        const double y0 = point.lon;
        const double x1 = line.start_.lat;
        const double y1 = line.start_.lon;
        const double x2 = line.finish_.lat;
        const double y2 = line.finish_.lon;

        const double diff_y = y2 - y1;
        const double diff_x = x2 - x1;
        const double divide = std::sqrt(diff_y * diff_y + diff_x * diff_x);

        const double mnogo = std::abs(diff_y * x0 - diff_x * y0 + x2 * y1 - y2 * x1);

        const double min_lenght = mnogo / divide;

        double min = std::min(min_lenght, GetDistance(line.start_, point));
        min = std::min(min, GetDistance(line.finish_, point));

        return min;
    }
};

using Ways = std::vector<Way>;

class WaysReader
{
public:
    static Ways Read(std::istream& stream)
    {
        Json::Reader json_reader;
        Json::Value values;
        const bool is_correct = json_reader.parse(stream, values);

        if (!is_correct)
        {
            throw std::runtime_error("Can't read way file");
        }

        Ways ways;

        for (const auto& val : values)
        {
            auto way = Way();
            way.type_ = val["type"].asString();
            for (const auto& point : val["points"])
            {
                Point raw_point;
                raw_point.lat = boost::lexical_cast<double>(point[0].asString());
                raw_point.lon = boost::lexical_cast<double>(point[1].asString());
                way.points_.push_back(raw_point);
            }
            ways.push_back(way);
        }

        return ways;
    }
};

using Lines = std::vector<Line>;

struct CreateTileOptions
{
    Point start_point;
    Point finish_point;
    int n = 0;
    int m = 0;
};

struct Tile
{
    std::array<double, 256*256> weights{};
    int x_index = 0;
    int y_index = 0;
    Point begin_point;
    Point end_point;
    Lines lines;

    bool IsPointInTile(const Point& point) const
    {
        return point.lat > begin_point.lat && point.lat < end_point.lat && point.lon > begin_point.lon && point.lon < end_point.lon;
    }

    Point GetPointCenterPixel(const size_t point_number) const
    {
        const Point distance_point = PointUtils::Substruct(end_point, begin_point);

        Point step_point;
        step_point.lat = distance_point.lat / 512;
        step_point.lon = distance_point.lon / 512;

        const Point first_point = PointUtils::Add(begin_point, step_point);

        step_point.lat = distance_point.lat / 256;
        step_point.lon = distance_point.lon / 256;

        const Point full_shift_point = PointUtils::Mult(step_point, point_number / 256, point_number % 256);

        return PointUtils::Add(first_point, full_shift_point);
    }

};

void CreateTile(const CreateTileOptions& options, const size_t x, const size_t y, Tile &tile)
{

    const Point shift_point = PointUtils::Substruct(options.finish_point, options.start_point);

    Point step_point;
    step_point.lat = shift_point.lat / options.n;
    step_point.lon = shift_point.lon / options.m;

    tile.begin_point = PointUtils::Add(options.start_point, PointUtils::Mult(step_point, x, y));
    tile.end_point = PointUtils::Add(options.start_point, PointUtils::Mult(step_point, x + 1, y + 1));
    tile.x_index = x;
    tile.y_index = y;

    std::cerr << tile.begin_point.lat << " " << tile.begin_point.lon << " ";
    std::cerr << tile.end_point.lat << " " << tile.end_point.lon << std::endl;
}

void DistributeLine(const CreateTileOptions& options, const size_t x, const size_t y, const Ways &ways, std::vector<Tile>& tiles)
{
    auto& current_tile = tiles[y * options.n + x];

    for (const auto& way : ways)
    {
        const auto iter = type_to_weight.find(way.type_);
        if (iter == type_to_weight.end())
        {
            continue;
        }

        for (unsigned j = 0; j < way.points_.size() - 1; ++j)
        {

            const auto& point = way.points_[j];
            if (current_tile.IsPointInTile(point))
            {
                Line line;
                line.start_ = point;
                line.finish_ = way.points_[j + 1];
                line.weight_ = iter->second;
                current_tile.lines.push_back(line);
            }
        }
    }
}

std::pair<double, double> GetWeigth(const Point& center_pixel, const Lines& lines)
{
    double one = 0.0, two = 0.0;

    for (const auto& line : lines)
    {
        const double d = PointUtils::GetMinLengthFromLineToPoint(line, center_pixel);
        if (d <= 1e-6)
        {
            return {line.weight_, 1.0};
        }
        one += line.weight_ / d;
//        double okto_norm = 1 / std::pow(d, 8);
//        one += okto_norm * line.weight_;
//        two += okto_norm;
    }

    return {one, two};
}

static const int y_tile_part = 5;

void CalculateWeight(
        const CreateTileOptions& options,
        const size_t x,
        const size_t y,
        std::vector<Tile>& tiles)
{

    int current_tile_num = x + y * options.n;

    Tile &tile = tiles[current_tile_num];
    std::cerr << tile.lines.size() << std::endl;
    for (int i = 0; i < 256 * 256; ++i)
    {
        const Point &center_pixel = tile.GetPointCenterPixel(i);
        double one = 0.0, two = 0.0;
        for (const auto& lines : {
            tile.lines,
            tiles[(options.n + 1) * y + x - 1].lines,
            tiles[(options.n + 1) * y + x + 1].lines,
            tiles[(options.n + 1) * y + x].lines,
            tiles[(options.n - 1) * y + x - 1].lines,
            tiles[(options.n - 1) * y + x + 1].lines,
            tiles[(options.n - 1) * y + x].lines,
            tiles[(options.n) * y + x + 1].lines,
            tiles[(options.n) * y + x - 1].lines})
        {
            const auto [one_data, two_data] = GetWeigth(center_pixel, lines);
            one += one_data;
            two += two_data;
        }

        two = 1.0;

        tile.weights[i] = one / two;
    }
}

void CreateTileThread(
        const CreateTileOptions& options,
        std::vector<Tile>& tiles,
        const size_t start_x,
        const size_t finish_x)
{
    for (size_t tile_number_x = start_x; tile_number_x < finish_x; ++tile_number_x)
    {
        for (int tile_number_y = 0; tile_number_y < options.m / y_tile_part; ++tile_number_y)
        {
            std::cerr << "Create tile " << tile_number_x << " " << tile_number_y << std::endl;
            CreateTile(options, tile_number_x, tile_number_y, tiles[tile_number_y * options.n + tile_number_x]);
        }
    }
}

void DistributeLineThread(
        const CreateTileOptions &options,
        const Ways &ways,
        const size_t start_x,
        const size_t finish_x,
        std::vector<Tile>& tiles)
{
    for (size_t tile_number_x = start_x; tile_number_x < finish_x; ++tile_number_x)
    {
        for (int tile_number_y = 0; tile_number_y < options.m / y_tile_part; ++tile_number_y)
        {
            DistributeLine(options, tile_number_x, tile_number_y, ways, tiles);
        }
    }
}

void CalculateWeightThread(
        const CreateTileOptions& options,
        const size_t start_x,
        const size_t finish_x,
        std::vector<Tile>& tiles)
{
    std::cerr << "Start calculate weight " << start_x << " " << finish_x << std::endl;
    for (size_t tile_number_x = start_x; tile_number_x < finish_x; ++tile_number_x)
    {
        for (int tile_number_y = 1; tile_number_y < options.m / y_tile_part - 1; ++tile_number_y)
        {
            std::cerr << "Calculate weight for tile " << tile_number_x << " " << tile_number_y << std::endl;
            CalculateWeight(options, tile_number_x, tile_number_y, tiles);
        }
    }
}

std::vector<Tile> CreateTiles(const CreateTileOptions& options, const Ways& ways)
{
    std::cerr << "Start create tile" << std::endl;
    std::vector<Tile> tiles(options.m * options.n);

    const unsigned int thread_count = std::thread::hardware_concurrency();

    const unsigned x_step = options.n / thread_count;

    {
        std::vector<std::thread> threads;
        for (size_t thread_number = 0; thread_number < thread_count - 1; ++thread_number)
        {
            threads.emplace_back(CreateTileThread,
                                 options,
                                 std::ref(tiles),
                                 x_step * thread_number,
                                 (x_step + 1) * thread_number);
        }
        threads.emplace_back(CreateTileThread, options, std::ref(tiles), x_step * (thread_count - 1), options.n);

        for (auto& thread : threads)
        {
            thread.join();
        }
    }

    std::cerr << "Start distribute line" << std::endl;
    std::cerr << ways.size() << std::endl;

    {
        std::vector<std::thread> threads;
        for (size_t thread_number = 0; thread_number < thread_count - 1; ++thread_number)
        {
            threads.emplace_back(DistributeLineThread,
                                 options,
                                 ways,
                                 x_step * thread_number,
                                 (x_step + 1) * thread_number,
                                 std::ref(tiles));
        }
        threads.emplace_back(DistributeLineThread,
                          options,
                          ways,
                          x_step * (thread_count - 1),
                          options.n,
                          std::ref(tiles));

        for (auto& thread : threads)
        {
            thread.join();
        }
    }

    {
        std::vector<std::thread> threads;
        for (size_t thread_number = 0; thread_number < thread_count - 1; ++thread_number)
        {
            threads.emplace_back(CalculateWeightThread,
                              options,
                              x_step * thread_number + 1,
                              (x_step + 1) * thread_number,
                              std::ref(tiles));
        }
        threads.emplace_back(CalculateWeightThread,
                             options,
                             x_step * (thread_count - 1),
                             options.n - 1,
                             std::ref(tiles));

        for (auto& thread : threads)
        {
            thread.join();
        }
    }

    return tiles;
}

void WriteTile(const Tile &tile, const std::string path_to_result, const int shift_x, const int shift_y)
{
    const std::string file_name = path_to_result + "/" + std::to_string(tile.x_index + shift_x) + "_" + std::to_string(shift_y - tile.y_index) + ".csv";

    std::ofstream stream(file_name);

    stream << "lat;lng;weight;index" << std::endl;
    for (int i = 0; i < 256 * 256; ++i)
    {
        const Point &point = tile.GetPointCenterPixel(i);
        stream << point.lat << ";"
               << point.lon << ";"
               << tile.weights[i] << ";"
               << i << std::endl;
    }
}

int main(int argc, const char *argv[])
{
    /*
     * leftBottomLat: 54.80068486732233
     * leftBottomLng: 82.529296875
     * leftBottomTileX: 5975
     * leftBottomTileY: 2598
     * numTileX: 15
     * numTileY: 16
     * rightUpperLat: 55.254077067072714
     * rightUpperLng: 83.2763671875
     * rightUpperTileX: 5990
     * rightUpperTileY: 2582
     */

    CreateTileOptions options;

    {
        options.start_point.lat = 54.80068486732233;
        options.start_point.lon = 82.529296875;
        options.finish_point.lat = 55.254077067072714;
        options.finish_point.lon = 83.2763671875;
        options.n = 17;
        options.m = 18;
    }

    std::ifstream file("../graph_dump.json");

    if (!file.is_open())
    {
        throw std::runtime_error("Can't open file");
    }

    const auto& ways = WaysReader::Read(file);

    const auto& tiles = CreateTiles(options, ways);
    for (const auto& tile : tiles)
    {
        if (tile.x_index != 0 && tile.x_index != options.n - 1 &&
            tile.y_index != 0 && tile.y_index != options.m - 1)
        {
            WriteTile(tile, "../tiles", 5974, 2599);
        }
    }
}

max_stars_repo_name: uchr/Hackathon-Urbaton

max_issues_repo_name: uchr/Hackathon-Urbaton

max_forks_repo_name: uchr/Hackathon-Urbaton


text:
#include <chrono>
#include <vector>
#include <unordered_map>
#include <queue>

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

#include <gflags/gflags.h>
#include <glog/logging.h>

#include <nabo/nabo.h>

#include <opengv/absolute_pose/CentralAbsoluteAdapter.hpp>
#include <opengv/absolute_pose/methods.hpp>
#include <opengv/sac/Ransac.hpp>
#include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp>

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

namespace py = pybind11;

template <template <typename, typename> class Container, typename Type>
using Aligned = Container<Type, Eigen::aligned_allocator<Type>>;

constexpr int kLocalDescriptorSize = 256;
constexpr int kGlobalDescriptorSize = 1024;

struct Image {
  // Undistorted keypoints in the normalized image plane.
  Eigen::Matrix<float, 2, Eigen::Dynamic> normalized_keypoints;
  // 256dim for local descriptors.
  Eigen::Matrix<float, kLocalDescriptorSize, Eigen::Dynamic> local_descriptors;

  // -1 if no correspondence.
  Eigen::VectorXi point_indices;
};

struct Point3d {
  Eigen::Vector3f xyz;

  std::vector<int> observing_images;
};

class HLoc {
public:
  HLoc() {
    image_descriptors_.resize(kGlobalDescriptorSize, 0);
    ratio_test_value_ = 0.9;
  }

  // Adds an Image and returns its index.
  int addImage(Eigen::Ref<Eigen::Matrix<float, 1, kGlobalDescriptorSize, Eigen::RowMajor>> global_descriptor,
               Eigen::Ref<Eigen::Matrix<float, 2, Eigen::Dynamic, Eigen::RowMajor>> normalized_keypoints,
               Eigen::Ref<Eigen::Matrix<float, kLocalDescriptorSize, Eigen::Dynamic, Eigen::RowMajor>> local_descriptors) {
    CHECK_EQ(normalized_keypoints.cols(), local_descriptors.cols());

    const int num_keypoints = normalized_keypoints.cols();
    const int new_image_index = images_.size();

    LOG_IF(WARNING, num_keypoints == 0) << "Adding frame " << new_image_index
                                        << " with no keypoints.";

    Image new_image;
    new_image.normalized_keypoints = normalized_keypoints;
    new_image.local_descriptors = local_descriptors;
    new_image.point_indices =  Eigen::VectorXi::Constant(num_keypoints, -1);
    images_.push_back(new_image);

    CHECK_EQ(image_descriptors_.cols(), new_image_index);
    image_descriptors_.conservativeResize(Eigen::NoChange,
                                          image_descriptors_.cols() + 1);
    image_descriptors_.col(new_image_index) = global_descriptor;

    return new_image_index;
  }

  // Adds a 3D points and returns its index.
  int add3dPoint(Eigen::Ref<Eigen::Matrix<float, 1, 3, Eigen::RowMajor>> xyz,
                 Eigen::Ref<Eigen::Matrix<int, Eigen::Dynamic, 2, Eigen::RowMajor>> observing_images_and_kpts) {
    const int new_point_index = points_.size();
    Point3d new_point;
    new_point.xyz = xyz;
    for (int i = 0; i < observing_images_and_kpts.rows(); ++i) {
      const int observing_image_idx = observing_images_and_kpts(i, 0);
      CHECK_LT(observing_image_idx, images_.size());
      CHECK_GE(observing_image_idx, 0);
      new_point.observing_images.push_back(observing_image_idx);

      const int observing_kpt_idx = observing_images_and_kpts(i, 1);
      CHECK_GE(observing_kpt_idx, 0);
      CHECK_LT(observing_kpt_idx, images_[observing_image_idx].point_indices.size());
      images_[observing_image_idx].point_indices(observing_kpt_idx) = new_point_index;
    }
    points_.push_back(new_point);

    return new_point_index;
  }

  py::tuple localize(Eigen::Ref<Eigen::Matrix<float, 1, kGlobalDescriptorSize, Eigen::RowMajor>> global_descriptor,
               Eigen::Ref<Eigen::Matrix<float, 2, Eigen::Dynamic, Eigen::RowMajor>> row_normalized_keypoints,
               Eigen::Ref<Eigen::Matrix<float, kLocalDescriptorSize, Eigen::Dynamic, Eigen::RowMajor>> row_local_descriptors) {
    CHECK_EQ(row_normalized_keypoints.cols(), row_local_descriptors.cols());

    // Global retrieval first.
    constexpr int kNumNeighbors = 10;
    Eigen::VectorXi indices(kNumNeighbors);
    Eigen::VectorXf dists2(kNumNeighbors);

    auto global_start = std::chrono::high_resolution_clock::now();
    CHECK_EQ(global_descriptor.rows(), 1);
    CHECK_EQ(global_descriptor.cols(), kGlobalDescriptorSize);
    nns_->knn(global_descriptor, indices, dists2, kNumNeighbors, 0, Nabo::NNSearchF::SORT_RESULTS | Nabo::NNSearchF::ALLOW_SELF_MATCH);

    auto covis_start = std::chrono::high_resolution_clock::now();

    std::vector<std::vector<int>> components = covisibilityClustering(indices);

    auto ransac_start = std::chrono::high_resolution_clock::now();

    // Copy data as row-major sucks for our arrays.
    Eigen::Matrix<float, 2, Eigen::Dynamic> normalized_keypoints = row_normalized_keypoints;
    Eigen::Matrix<float, kLocalDescriptorSize, Eigen::Dynamic> local_descriptors = row_local_descriptors;

    int num_components_tested = 0;
    bool pnp_success = false;
    int num_inliers = 0;
    int num_iters = 0;

    int total_local_ms = 0;
    int total_pnp_ms = 0;

    int num_db_landmarks = 0;
    int num_matches = 0;

    int last_component_size = -1;

    for (std::vector<int>& component : components) {
      // Limit component size to 5.
      if (component.size() > 5) {
        component.resize(5);
      }

      ++num_components_tested;
      last_component_size = component.size();

      int time_local;
      int time_pnp;
      pnp_success = localizeLocally(component, normalized_keypoints, local_descriptors,
                          &num_db_landmarks, &num_matches, &num_inliers, &num_iters, &time_local, &time_pnp);
      total_local_ms += time_local;
      total_pnp_ms += time_pnp;

      // Break the loop if we succeed.
      if (pnp_success) {
        break;
      }
    }

    auto ransac_end = std::chrono::high_resolution_clock::now();
    auto dur_ransac_ms = std::chrono::duration_cast<std::chrono::milliseconds>(ransac_end - ransac_start);
    auto dur_covis_ms = std::chrono::duration_cast<std::chrono::milliseconds>(ransac_start - covis_start);
    auto dur_global_ms = std::chrono::duration_cast<std::chrono::milliseconds>(covis_start - global_start);
    LOG(INFO) << dur_global_ms.count() << " " << dur_covis_ms.count() << " "
              << dur_ransac_ms.count() << " 2d3d,pnp(" << total_local_ms
              << ", " << total_pnp_ms << ") global/covis/local [ms]";

    return py::make_tuple(pnp_success, components.size(), num_components_tested, last_component_size,
                          num_db_landmarks,
                          num_matches, num_inliers,
                          num_iters, dur_global_ms.count(),
                          dur_covis_ms.count(), total_local_ms, total_pnp_ms);
  }

  void buildIndex() {
    LOG(INFO) << "Found " << images_.size() << " images and "
              << points_.size() << " 3D points. Building index.";
    CHECK_EQ(images_.size(), image_descriptors_.cols());

    CHECK_EQ(image_descriptors_.rows(), kGlobalDescriptorSize);
    CHECK_GT(image_descriptors_.cols(), 0);
    nns_ = Nabo::NNSearchF::createKDTreeLinearHeap(image_descriptors_);
  }

private:
  std::unordered_set<int> getConnectedImages(const int image_idx) const {
    CHECK_LT(image_idx, images_.size());
    CHECK_GE(image_idx, 0);

    const Eigen::VectorXi& point_indices = images_[image_idx].point_indices;
    std::unordered_set<int> connected_images;
    for (int i = 0; i < point_indices.size(); ++i) {
      const int connected_point_idx = point_indices(i);
      CHECK_GT(connected_point_idx, -1);
      CHECK_LT(connected_point_idx, points_.size());
      CHECK_GE(connected_point_idx, 0);
      connected_images.insert(points_[connected_point_idx].observing_images.begin(),
                              points_[connected_point_idx].observing_images.end());
    }
    return connected_images;
  }

  bool doPnpRansac(const opengv::points_t& points, const opengv::bearingVectors_t& bearing_vectors, int* num_inliers, int* num_iters) const {
    CHECK_NOTNULL(num_inliers);
    CHECK_NOTNULL(num_iters);

    CHECK_EQ(points.size(), bearing_vectors.size());

    const double focal_length = 800;
    const double pixel_sigma_px = 15;
    const double ransac_threshold = 1.0 - cos(atan(pixel_sigma_px / focal_length));
    const int max_ransac_iters = 1000;

    opengv::absolute_pose::CentralAbsoluteAdapter adapter(bearing_vectors,
      points);

    opengv::sac::Ransac<
      opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem> ransac;

      std::shared_ptr<opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem>absposeproblem_ptr(
        new opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem(
          adapter, opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem::KNEIP));

    ransac.sac_model_ = absposeproblem_ptr;
    ransac.threshold_ = ransac_threshold;
    ransac.max_iterations_ = max_ransac_iters;
    const bool ransac_success = ransac.computeModel() && ransac.inliers_.size() > 11;

    *num_inliers = ransac.inliers_.size();
    *num_iters = ransac.iterations_;

    LOG(INFO) << "Ransac " << ransac_success << ": " << *num_inliers << " inliers, " << *num_iters << " it, " << points.size() << " points.";

    return ransac_success;
  }

  bool localizeLocally(const std::vector<int>& frame_component,
                       const Eigen::Matrix<float, 2, Eigen::Dynamic>& normalized_keypoints,
                       const Eigen::Matrix<float, kLocalDescriptorSize, Eigen::Dynamic>& local_descriptors,
                       int* num_db_landmarks, int* num_matches, int* num_inliers, int* num_iters, int* time_local_ms, int* time_ransac_ms) const {
    CHECK_NOTNULL(num_db_landmarks);
    CHECK_NOTNULL(num_matches);
    CHECK_NOTNULL(time_local_ms);
    CHECK_NOTNULL(time_ransac_ms);

    CHECK(!frame_component.empty());

    auto local_prep_start = std::chrono::high_resolution_clock::now();

    // First check the total size of the db to preallocate memory.
    int total_num_db_points = 0;
    for (const int frame_idx : frame_component) {
      total_num_db_points += images_[frame_idx].point_indices.size();
    }

    if (total_num_db_points == 0) {
      // No db points, quit early.
      *num_db_landmarks = 0;
      *num_matches = 0;
      *num_inliers = 0;
      *num_iters = 0;
      auto local_bailout = std::chrono::high_resolution_clock::now();
      *time_local_ms = std::chrono::duration_cast<std::chrono::milliseconds>(local_bailout - local_prep_start).count();
      *time_ransac_ms = 0;

      return false;
    }

    Eigen::MatrixXf db_local_descriptors(kLocalDescriptorSize, total_num_db_points);
    Eigen::VectorXi db_point_indices(total_num_db_points);

    // Stick descriptors and point indices together.
    int index = 0;
    for (const int frame_idx : frame_component) {
      const Image& image = images_[frame_idx];
      const int num_points = image.point_indices.size();

      // Copy point indices.
      db_point_indices.segment(index, num_points) = image.point_indices;
      // Copy descriptors.
      db_local_descriptors.block(0, index, kLocalDescriptorSize, num_points) = image.local_descriptors;

      index += num_points;
    }

    *num_db_landmarks = total_num_db_points;

    constexpr int kLocalNumNeighbors = 2;
    Eigen::MatrixXi indices(kLocalNumNeighbors, local_descriptors.cols());
    Eigen::MatrixXf dists2(kLocalNumNeighbors, local_descriptors.cols());

    auto local_build = std::chrono::high_resolution_clock::now();

    CHECK_EQ(db_local_descriptors.rows(), kLocalDescriptorSize);
    CHECK_GT(db_local_descriptors.cols(), 0);
    Nabo::NNSearchF* local_nns = Nabo::NNSearchF::createKDTreeTreeHeap(db_local_descriptors);

    auto local_search = std::chrono::high_resolution_clock::now();

    CHECK_EQ(local_descriptors.rows(), kLocalDescriptorSize);
    CHECK_GT(local_descriptors.cols(), 0);
    local_nns->knn(local_descriptors, indices, dists2, kLocalNumNeighbors, 0, Nabo::NNSearchF::SORT_RESULTS | Nabo::NNSearchF::ALLOW_SELF_MATCH);

    auto filtering_start = std::chrono::high_resolution_clock::now();

    delete local_nns;

    const Eigen::MatrixXf dists = dists2.array().sqrt();

    const double kRatioTestValue = 0.9;

    // Assuming all matches will fit, will shrink to size below the loop.
    opengv::points_t points(indices.cols());
    opengv::bearingVectors_t bearing_vectors(indices.cols());
    int idx = 0;
    for (int i = 0; i < indices.cols(); ++i) {
      const int point_idx_0 = db_point_indices(indices(0, i));
      const int point_idx_1 = db_point_indices(indices(1, i));
      if (point_idx_0 == point_idx_1 || dists(0, i) < kRatioTestValue * dists(1, i)) {
        bearing_vectors[idx] = Eigen::Vector3f(
          normalized_keypoints(0, i), normalized_keypoints(1, i), 1).cast<double>();
        bearing_vectors[idx].normalize();

        CHECK_LT(db_point_indices(indices(0, i)), points_.size());
        points[idx] = points_[db_point_indices(indices(0, i))].xyz.cast<double>();
        ++idx;
      }
    }
    points.resize(idx);
    bearing_vectors.resize(idx);

    *num_matches = idx;

    auto pnp_start = std::chrono::high_resolution_clock::now();

    if (points.size() < 12) {
      // Bail out early.
      *num_inliers = 0;
      *num_iters = 0;
      auto local_bailout = std::chrono::high_resolution_clock::now();
      *time_local_ms = std::chrono::duration_cast<std::chrono::milliseconds>(pnp_start - local_prep_start).count();
      *time_ransac_ms = 0;

      return false;
    }

    const bool pnp_success = doPnpRansac(points, bearing_vectors, num_inliers, num_iters);

    auto pnp_end = std::chrono::high_resolution_clock::now();

    LOG(INFO) << "Num db/query " << db_local_descriptors.cols() << " / " << local_descriptors.cols();

    *time_local_ms = std::chrono::duration_cast<std::chrono::milliseconds>(pnp_start - local_prep_start).count();
    *time_ransac_ms = std::chrono::duration_cast<std::chrono::milliseconds>(pnp_end - pnp_start).count();

    return pnp_success;
  }

  std::vector<std::vector<int>> covisibilityClustering(const Eigen::VectorXi& indices) const {
    std::unordered_set<int> visited;
    std::vector<std::vector<int>> components;

    std::unordered_set<int> frame_ids;
    for (int i = 0; i < indices.size(); ++i) {
      frame_ids.insert(indices(i));
    }

    for (int i = 0; i < indices.size(); ++i) {
      if (visited.count(indices(i)) > 0u) {
        continue;
      }

      // New component.
      components.resize(components.size() + 1);

      std::queue<int> queue;
      queue.push(indices(i));
      while (!queue.empty()) {
        const int exploration_frame = queue.front();
        queue.pop();

        // If already there, look at the next item. If not there, insert.
        if (!visited.insert(exploration_frame).second) {
          continue;
        }

        components.back().push_back(exploration_frame);
        std::unordered_set<int> connected_frames = getConnectedImages(exploration_frame);

        for (const int connected_frame : connected_frames) {
          if (frame_ids.count(connected_frame) > 0u && visited.count(connected_frame) == 0u) {
            queue.push(connected_frame);
          }
        }
      }
    }

    return components;
  }

  std::vector<Image> images_;
  Aligned<std::vector, Point3d> points_;
  Nabo::NNSearchF* nns_;
  // 1024dim for global descriptor. Eigen is column major by default.
  Eigen::MatrixXf image_descriptors_;
  double ratio_test_value_;
};

PYBIND11_MODULE(_hloc_cpp, m) {
    m.doc() = "pybind11 Hierarchical Localization cpp backend";

    py::class_<HLoc>(m, "HLoc")
    .def(py::init())
    .def("addImage", &HLoc::addImage, py::return_value_policy::copy)
    .def("add3dPoint", &HLoc::add3dPoint, py::return_value_policy::copy)
    .def("buildIndex", &HLoc::buildIndex)
    .def("localize", &HLoc::localize, py::return_value_policy::copy);
}

ext: cc

max_stars_repo_name: maxtomCMU/hfnet


text:
#include "demo.h"
#include <cmath>
#include <boost/python.hpp>
#include "base/py.h"
#include "base/log.h"
#include "math/matrix-inl.h"
#include "engine/resourceref.h"
#include "engine/engine.h"
#include "physics/physics.h"

extern "C" NEGINE_EXPORT PyObject* PyInit_negine_runtime(void);

using namespace boost::python;

static Application* intstance_ = nullptr;

Application& globalApp() {
    ASSERT(intstance_ != nullptr);
    return *intstance_;
}

Demo::Demo(const std::string& filename) {
    intstance_ = this;

    PyImport_AppendInittab("negine_core", PyInit_negine_core);
    PyImport_AppendInittab("negine_runtime", PyInit_negine_runtime);
    Py_Initialize();
    object m = import("__main__");
    object global = object(m.attr("__dict__"));

    exec_file(filename.c_str(), global, global);

    keypressed_ = 0;

    cam_ = new GameCamera("camera1", Engine::scene());

    cam_->transform->setPosition(math::vec3f(0.f, 0.f, -5.f));
    cam_->transform->setPitch(50 * math::deg_to_rad);
    cam_->transform->setHead(180 * math::deg_to_rad);
    cam_->camera->setPerspective(width_ / (f32)height_, 45.0f, 1, 1000);
    cam_->camera->update();

    ResourceRef checkers("checkers");
    ResourceRef("default_fbo").setResource(nullptr);

    GL_ASSERT(GL);

    ResourceRef model("themodel");
    model.loadDefault<opengl::Model>("sphere.obj");

    obj_ = new GameObject("theObject", Engine::scene());

    obj_->transform->setPosition(math::vec3f(0, 0, 0));
    obj_->transform->setHead(0.0f);
    obj_->transform->setPitch(0.0f);
    obj_->model->model_ = model;

    opengl::Material* material = new opengl::Material;
    material->modeMap["normal"] = ResourceRef("prog1");
    ResourceRef maa("default_material", material);

    opengl::RenderPass rp;
    rp.target = "fbo";
    rp.mode = "normal";
    rp.generator = "scene";
    rp.viewport = math::vec4f(0, 0, 640, 480);
    rp.clear = true;
    rp.depthTest = true;
    rp.depthWrite = true;
    rp.cullBackFace = false;
    rp.blend = false;
    rp.clearColor = math::vec4f(1.0f, 0.0f, 0.0f, 1.0f);
    pipeline_.push_back(rp);

    opengl::RenderPass rp2;
    rp2.target = "default_fbo";
    rp2.mode = "prog2";
    rp2.generator = "fullscreen";
    rp2.viewport = math::vec4f(0, 0, 640, 480);
    rp2.clear = true;
    rp2.depthTest = false;
    rp2.depthWrite = false;
    rp2.cullBackFace = false;
    rp2.blend = false;
    rp2.clearColor = math::vec4f(1.0f, 0.0f, 0.0f, 1.0f);
    pipeline_.push_back(rp2);

    phys::Body* plane = new phys::Body(Engine::physics(), phys::Body::Plane, 0.0f, math::vec3f(0, 1, 0));
    phys::Body* ball = new phys::Body(Engine::physics(), phys::Body::Sphere, 1.0f, math::vec3f(0, 50, 0));
    ball->setTransform(obj_->transform);
    phys::Body* ball2 = new phys::Body(Engine::physics(), phys::Body::Sphere, 1.0f, math::vec3f(0.5f, 20.f, 0.f));

    GameObject* oo = new GameObject("theObject2", Engine::scene());

    oo->transform->setPosition(math::vec3f(0.5f, 2.0f, 0.0f));
    oo->model->model_ = model;
    oo->transform->update();
    ball2->setTransform(oo->transform);

    timer_.reset();

    cam_->transform->update();
    obj_->transform->update();
}

Demo::~Demo() {
}

void Demo::OnFrame() {
    Engine::physics().simulate(timer_.reset() / 1000.f);
    UpdateWorld();
    Engine::renderer().render(GL, pipeline_, cam_->camera);
    GL_ASSERT(GL);
    SDLApp::OnFrame();
}

void Demo::OnKeyboardDown(u8 key) {
    if (key == 'w') {
        keypressed_ |= 1;
    }
    else if (key == 's') {
        keypressed_ |= 2;
    }
    else if (key == 'a') {
        keypressed_ |= 4;
    }
    else if (key == 'd') {
        keypressed_ |= 8;
    }
    else if (key == 'j') {
        keypressed_ |= 16;
    }
    else if (key == 'k') {
        keypressed_ |= 32;
    }
    else if (key == 'h') {
        keypressed_ |= 64;
    }
    else if (key == 'l') {
        keypressed_ |= 128;
    }
}

void Demo::OnKeyboardUp(u8 key) {
    if (key == 'w') {
        keypressed_ ^= 1;
    }
    else if (key == 's') {
        keypressed_ ^= 2;
    }
    else if (key == 'a') {
        keypressed_ ^= 4;
    }
    else if (key == 'd') {
        keypressed_ ^= 8;
    }
    else if (key == 'j') {
        keypressed_ ^= 16;
    }
    else if (key == 'k') {
        keypressed_ ^= 32;
    }
    else if (key == 'h') {
        keypressed_ ^= 64;
    }
    else if (key == 'l') {
        keypressed_ ^= 128;
    }
}

void Demo::OnMotion(i32 x, i32 y, i32 dx, i32 dy) {
    game::Transform* cam = Engine::scene().getTyped<game::Transform>("camera1");
    cam->turnHead(math::deg_to_rad * dx);

    if (fabs(cam->pitch() + math::deg_to_rad * dy) < math::pi / 2.0f) {
        cam->turnPitch(math::deg_to_rad * dy);
    }
    else if (dy != 0) {
        cam->setPitch(math::pi / 2.0f * dy / fabs((f32)dy));
    }
    if (dx != 0 || dy != 0)
        cam->update();
}

void Demo::UpdateWorld() {
    const f32 speed = 0.001f;

    game::Transform* cam = Engine::scene().getTyped<game::Transform>("camera1");

    if (keypressed_ & 1) {
        cam->moveForward(speed);
    }

    if (keypressed_ & 2) {
        cam->moveBackward(speed);
    }

    if (keypressed_ & 4) {
        cam->moveLeft(speed);
    }

    if (keypressed_ & 8) {
        cam->moveRight(speed);
    }

    if (keypressed_ & 16) {
        if (fabs(cam->pitch() + math::deg_to_rad) < math::pi / 2.0f)
            cam->turnPitch(math::deg_to_rad);
    }

    if (keypressed_ & 32) {
        if (fabs(cam->pitch() - math::deg_to_rad) < math::pi / 2.0f)
            cam->turnPitch(-math::deg_to_rad);
    }

    if (keypressed_ & 64) {
        cam->turnHead(math::deg_to_rad);
    }

    if (keypressed_ & 128) {
        cam->turnHead(-math::deg_to_rad);
    }

    if (keypressed_ != 0)
        cam->update();

    //game::Transform* obj = scene_.getTyped<game::Transform>("theObject");

    //obj->turnHead(0.001f);
    //obj->update();
}

max_stars_repo_name: alexa-infra/negine

max_issues_repo_name: alexa-infra/negine

max_forks_repo_name: alexa-infra/negine