#pragma once

#include <algorithm>
#include <cmath>
#include <iostream>
#include <vector>

#include <opencv2/core/types.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/video.hpp>

#include "chicken_counter/config.hpp"
#include "chicken_counter/types.hpp"

namespace cc {

class BackwardMotionDetector {
public:
    BackwardMotionDetector() {}

    BackwardMotionDetector(const MotionConfig& config,
                           const RoiConfig& roi,
                           bool verbose = false)
        : config(config), roi(roi), verbose(verbose)
    {
        int x1 = roi.points[0].x, y1 = roi.points[0].y;
        int x2 = x1, y2 = y1;
        for (const auto& p : roi.points) {
            if (p.x < x1) x1 = p.x; if (p.y < y1) y1 = p.y;
            if (p.x > x2) x2 = p.x; if (p.y > y2) y2 = p.y;
        }
        roi_bounds = cv::Rect(x1, y1, x2 - x1, y2 - y1);
    }

    MotionState update(const cv::Mat& frame,
                       const std::vector<TrackObservation>& tracks,
                       int frame_index)
    {
        if (!config.enabled) return state;

        int stride = std::max(1, config.stride_frames);
        if (frame_index % stride != 0) return state;

        cv::Mat gray;
        cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);

        cv::Mat gray_roi = gray(roi_bounds);

        double scale = config.flow_scale;
        if (scale < 1.0) {
            int tw = std::max(1, static_cast<int>(gray_roi.cols * scale));
            int th = std::max(1, static_cast<int>(gray_roi.rows * scale));
            cv::resize(gray_roi, gray_roi, {tw, th}, 0, 0, cv::INTER_AREA);
        } else {
            scale = 1.0;
        }

        cv::Mat mask(gray_roi.size(), CV_8UC1, cv::Scalar(255));
        int r = config.block_radius;
        for (const auto& track : tracks) {
            int lx1 = static_cast<int>((std::max(0, track.bbox_x1 - r) - roi_bounds.x) * scale);
            int ly1 = static_cast<int>((std::max(0, track.bbox_y1 - r) - roi_bounds.y) * scale);
            int lx2 = static_cast<int>((std::min(roi_bounds.x + roi_bounds.width, track.bbox_x2 + r) - roi_bounds.x) * scale);
            int ly2 = static_cast<int>((std::min(roi_bounds.y + roi_bounds.height, track.bbox_y2 + r) - roi_bounds.y) * scale);
            if (lx2 <= lx1 || ly2 <= ly1) continue;
            cv::rectangle(mask, {lx1, ly1}, {lx2, ly2}, 0, -1);
        }

        std::vector<cv::Point2f> points;
        cv::goodFeaturesToTrack(gray_roi, points, config.max_corners,
            config.quality_level, config.min_distance, mask, config.block_radius);

        if (previous_gray.empty() || static_cast<int>(points.size()) < config.min_features) {
            previous_gray = gray_roi.clone();
            return state;
        }

        std::vector<cv::Point2f> next_points;
        std::vector<uint8_t> status;
        std::vector<float> err;
        cv::calcOpticalFlowPyrLK(previous_gray, gray_roi, points, next_points, status, err);

        previous_gray = gray_roi.clone();

        if (next_points.empty() || status.empty()) return state;

        int valid_count = 0;
        double flow_sum = 0.0;
        for (size_t i = 0; i < status.size(); ++i) {
            if (!status[i]) continue;
            double v = (config.axis == "vertical")
                ? (next_points[i].y - points[i].y)
                : (next_points[i].x - points[i].x);
            flow_sum += v;
            valid_count++;
        }
        if (valid_count < config.min_features) return state;

        double median_speed = flow_sum / valid_count * config.forward_sign;

        double alpha = config.ema_alpha;
        state.smoothed_speed = static_cast<float>(
            alpha * median_speed + (1.0 - alpha) * state.smoothed_speed);

        if (state.smoothed_speed <= config.reverse_enter_threshold) {
            ++state.consecutive_reverse_frames;
        } else if (state.smoothed_speed > config.reverse_exit_threshold) {
            state.consecutive_reverse_frames = 0;
            state.backward_active = false;
        }

        if (state.consecutive_reverse_frames >= config.debounce_frames) {
            bool was_active = state.backward_active;
            state.backward_active = true;
            if (verbose && !was_active) {
                std::cerr << "[motion] BACKWARD TRIGGERED! smoothed="
                          << state.smoothed_speed
                          << " consecutive=" << state.consecutive_reverse_frames << "\n";
            }
        }

        if (verbose) {
            ++update_count;
            std::cerr << "[motion #" << update_count
                      << "] features=" << valid_count
                      << " median_speed=" << median_speed
                      << " smoothed=" << state.smoothed_speed
                      << " consecutive_rev=" << state.consecutive_reverse_frames
                      << " backward=" << state.backward_active << "\n";
        }

        return state;
    }

    MotionState state;

private:
    MotionConfig config;
    RoiConfig roi;
    bool verbose;
    cv::Rect roi_bounds;
    cv::Mat previous_gray;
    int update_count = 0;
};

} // namespace cc