#include <opencv2/opencv.hpp>

#include <cstdint>

enum
{
    BAT_MODE_FLOOR = 0
    , BAT_MODE_CEIL = 1
    , BAT_MODE_INVALID
};

cv::Mat bilateral_adaptive_threshold(cv::Mat const& input
    , int32_t kernel_size = 20
    , int32_t C = 0
    , int32_t mode = BAT_MODE_FLOOR
    , uint8_t true_value = 255
    , uint8_t false_value = 0)
{
    CV_Assert(input.channels() == 1);
    CV_Assert(input.type() == CV_8UC1);
    CV_Assert(kernel_size > 0);
    CV_Assert(mode < BAT_MODE_INVALID);

    // Prepare kernels

    cv::Mat kernel_left(cv::Mat::ones(1, kernel_size + 1, CV_16S));
    kernel_left.at<int16_t>(0, kernel_size) = -kernel_size;

    cv::Mat kernel_right(cv::Mat::ones(1, kernel_size + 1, CV_16S));
    kernel_right.at<int16_t>(0, 0) = -kernel_size;

    cv::Mat kernel_up(cv::Mat::ones(kernel_size + 1, 1, CV_16S));
    kernel_up.at<int16_t>(kernel_size, 0) = -kernel_size;

    cv::Mat kernel_down(cv::Mat::ones(kernel_size + 1, 1, CV_16S));
    kernel_down.at<int16_t>(0, 0) = -kernel_size;

    double delta(0.0);
    switch (mode) {
    case BAT_MODE_FLOOR: delta = C; break;
    case BAT_MODE_CEIL: delta = -C; break;
    default: break;
    };

    cv::Mat left_thresh, right_thresh, up_thresh, down_thresh;
    cv::filter2D(input, left_thresh, CV_16S, kernel_left
        , cv::Point(kernel_size, 0), delta, cv::BORDER_CONSTANT);
    cv::filter2D(input, right_thresh, CV_16S, kernel_right
        , cv::Point(0, 0), delta, cv::BORDER_CONSTANT);
    cv::filter2D(input, up_thresh, CV_16S, kernel_up
        , cv::Point(0, kernel_size), delta, cv::BORDER_CONSTANT);
    cv::filter2D(input, down_thresh, CV_16S, kernel_down
        , cv::Point(0, 0), delta, cv::BORDER_CONSTANT);

    cv::Mat result(input.rows, input.cols, input.type(), cv::Scalar(false_value));
    cv::Mat mask;
    if (mode == BAT_MODE_FLOOR) {
        mask = ((0 > left_thresh) & (0 > right_thresh))
            | ((0 > up_thresh) & (0 > down_thresh));
    } else if (mode == BAT_MODE_CEIL) {
        mask = ((0 < left_thresh) & (0 < right_thresh))
            | ((0 < up_thresh) & (0 < down_thresh));
    }
    result.setTo(true_value, mask);
    return result;
}

int main()
{
    cv::Mat img_in(cv::imread("foo_in.png", -1));
    cv::Mat img_out(cv::imread("foo_out.png", -1));

    cv::Mat dummy(img_in), result;

    int64 total_t(0);
    for (int i(0); i < 256; ++i) {
        int64 start_t(cv::getTickCount());
        result = bilateral_adaptive_threshold(img_in);
        total_t += cv::getTickCount() - start_t;
        dummy += result;
    }

    std::cout << (total_t / (cv::getTickFrequency() * 256) * 1000.0) << "\n";

    CV_Assert(cv::countNonZero(result == img_out) == result.rows * result.cols);


    return 0;
}