// Copyright (C) 2026 Kiyotsugu Arai
// SPDX-License-Identifier: LGPL-3.0-or-later

/**
 * @file cascade_classifier.hpp
 * @brief Cascade Classifier (OCV-27) + HOG+SVM (OCV-28) + 顔認識 (OCV-35)
 */

#ifndef CALX_IMAGE_CV_CASCADE_CLASSIFIER_HPP
#define CALX_IMAGE_CV_CASCADE_CLASSIFIER_HPP

#include <image/Image.hpp>
#include <math/core/matrix.hpp>
#include <vector>
#include <cmath>
#include <algorithm>
#include <array>
#include <numeric>
#include <functional>

namespace calx::image {

// ========================================================================
// 積分画像 (Cascade Classifier の基盤)
// ========================================================================

template<typename T>
Image<double> integralImage(const Image<T>& img) {
    Image<T> gray = (img.numChannels() >= 3) ? img.toGray() : img;
    int h = static_cast<int>(gray.height());
    int w = static_cast<int>(gray.width());
    Image<double> ii(h + 1, w + 1, 1, 0.0);
    for (int y = 0; y < h; ++y)
        for (int x = 0; x < w; ++x)
            ii(y+1, x+1) = double(gray(y,x)) + ii(y,x+1) + ii(y+1,x) - ii(y,x);
    return ii;
}

inline double integralSum(const Image<double>& ii, int x, int y, int w, int h) {
    return ii(y+h, x+w) - ii(y, x+w) - ii(y+h, x) + ii(y, x);
}

// ========================================================================
// OCV-27: Cascade Classifier (Haar-like 特徴)
// ========================================================================

struct DetectionRect {
    int x, y, width, height;
    double confidence;
};

/// Haar-like 特徴の種類
enum class HaarFeatureType { Edge, Line, Four };

struct HaarFeature {
    HaarFeatureType type;
    int x, y, w, h;  // 特徴窓の位置・サイズ (ウィンドウ内相対)
};

/// 弱分類器
struct WeakClassifier {
    HaarFeature feature;
    double threshold;
    double leftVal, rightVal;  // 閾値の左右の出力値
};

/// カスケードステージ
struct CascadeStage {
    std::vector<WeakClassifier> classifiers;
    double stageThreshold;
};

/// Cascade Classifier
class CascadeClassifier {
public:
    /// ステージを追加 (学習済みモデルの読み込み用)
    void addStage(CascadeStage stage) { stages_.push_back(std::move(stage)); }

    /// 検出
    template<typename T>
    std::vector<DetectionRect> detect(const Image<T>& img,
                                       double scaleFactor = 1.1,
                                       int minNeighbors = 3,
                                       int minSize = 24, int maxSize = 0) const {
        auto ii = integralImage(img);
        int h = static_cast<int>(img.height());
        int w = static_cast<int>(img.width());
        if (maxSize <= 0) maxSize = std::min(h, w);

        std::vector<DetectionRect> dets;

        for (int winSize = minSize; winSize <= maxSize; winSize = int(winSize * scaleFactor)) {
            int step = std::max(1, winSize / 10);
            for (int y = 0; y + winSize < h; y += step)
                for (int x = 0; x + winSize < w; x += step) {
                    double score = evaluateCascade(ii, x, y, winSize);
                    if (score > 0)
                        dets.push_back({x, y, winSize, winSize, score});
                }
        }

        // NMS (Non-Maximum Suppression)
        return nms(dets, minNeighbors);
    }

private:
    std::vector<CascadeStage> stages_;

    double evaluateCascade(const Image<double>& ii, int x, int y, int winSize) const {
        double totalScore = 0;
        for (const auto& stage : stages_) {
            double stageScore = 0;
            for (const auto& wc : stage.classifiers) {
                double fval = evalHaarFeature(ii, wc.feature, x, y, winSize);
                stageScore += (fval < wc.threshold) ? wc.leftVal : wc.rightVal;
            }
            if (stageScore < stage.stageThreshold) return -1;
            totalScore += stageScore;
        }
        return totalScore;
    }

    static double evalHaarFeature(const Image<double>& ii, const HaarFeature& f,
                                   int ox, int oy, int winSize) {
        double scale = double(winSize) / 24.0;  // base window = 24x24
        int fx = ox + int(f.x * scale), fy = oy + int(f.y * scale);
        int fw = int(f.w * scale), fh = int(f.h * scale);
        if (fw < 1 || fh < 1) return 0;

        switch (f.type) {
        case HaarFeatureType::Edge:
            return integralSum(ii, fx, fy, fw/2, fh)
                 - integralSum(ii, fx+fw/2, fy, fw-fw/2, fh);
        case HaarFeatureType::Line:
            return integralSum(ii, fx, fy, fw, fh/3)
                 - integralSum(ii, fx, fy+fh/3, fw, fh/3)
                 + integralSum(ii, fx, fy+2*fh/3, fw, fh-2*fh/3);
        case HaarFeatureType::Four: {
            int hw = fw/2, hh = fh/2;
            return integralSum(ii, fx, fy, hw, hh)
                 - integralSum(ii, fx+hw, fy, fw-hw, hh)
                 - integralSum(ii, fx, fy+hh, hw, fh-hh)
                 + integralSum(ii, fx+hw, fy+hh, fw-hw, fh-hh);
        }
        }
        return 0;
    }

    static std::vector<DetectionRect> nms(std::vector<DetectionRect>& dets, int minNeighbors) {
        std::sort(dets.begin(), dets.end(), [](auto& a, auto& b) {
            return a.confidence > b.confidence;
        });
        std::vector<bool> suppress(dets.size(), false);
        std::vector<DetectionRect> result;

        for (std::size_t i = 0; i < dets.size(); ++i) {
            if (suppress[i]) continue;
            int neighbors = 0;
            for (std::size_t j = i + 1; j < dets.size(); ++j) {
                if (suppress[j]) continue;
                // IoU チェック
                int x1 = std::max(dets[i].x, dets[j].x);
                int y1 = std::max(dets[i].y, dets[j].y);
                int x2 = std::min(dets[i].x+dets[i].width, dets[j].x+dets[j].width);
                int y2 = std::min(dets[i].y+dets[i].height, dets[j].y+dets[j].height);
                if (x1 < x2 && y1 < y2) {
                    double inter = (x2-x1) * (y2-y1);
                    double a1 = dets[i].width * dets[i].height;
                    double a2 = dets[j].width * dets[j].height;
                    if (inter / (a1 + a2 - inter) > 0.3) {
                        suppress[j] = true;
                        ++neighbors;
                    }
                }
            }
            if (neighbors >= minNeighbors - 1)
                result.push_back(dets[i]);
        }
        return result;
    }
};

// ========================================================================
// OCV-28: HOG 特徴量
// ========================================================================

/// HOG 記述子を計算
template<typename T>
std::vector<double> computeHOG(const Image<T>& img,
                                int cellSize = 8, int blockSize = 2, int nBins = 9) {
    Image<T> gray = (img.numChannels() >= 3) ? img.toGray() : img;
    int h = static_cast<int>(gray.height());
    int w = static_cast<int>(gray.width());
    constexpr double pi = 3.14159265358979323846;

    int cellsY = h / cellSize, cellsX = w / cellSize;

    // セルごとのヒストグラム
    std::vector<std::vector<std::vector<double>>> cellHist(
        cellsY, std::vector<std::vector<double>>(cellsX, std::vector<double>(nBins, 0)));

    for (int y = 1; y < h - 1; ++y)
        for (int x = 1; x < w - 1; ++x) {
            double gx = double(gray(y, x+1)) - double(gray(y, x-1));
            double gy = double(gray(y+1, x)) - double(gray(y-1, x));
            double mag = std::sqrt(gx*gx + gy*gy);
            double angle = std::atan2(gy, gx);
            if (angle < 0) angle += pi;  // [0, π)

            int cy = y / cellSize, cx = x / cellSize;
            if (cy >= cellsY || cx >= cellsX) continue;
            int bin = std::min(int(angle * nBins / pi), nBins - 1);
            cellHist[cy][cx][bin] += mag;
        }

    // ブロック正規化 (L2-Hys)
    std::vector<double> descriptor;
    int blockCells = blockSize;
    for (int by = 0; by + blockCells <= cellsY; ++by)
        for (int bx = 0; bx + blockCells <= cellsX; ++bx) {
            std::vector<double> block;
            for (int cy = by; cy < by + blockCells; ++cy)
                for (int cx = bx; cx < bx + blockCells; ++cx)
                    block.insert(block.end(), cellHist[cy][cx].begin(), cellHist[cy][cx].end());
            // L2 正規化
            double norm = 0;
            for (double v : block) norm += v * v;
            norm = std::sqrt(norm + 1e-6);
            for (double& v : block) v = std::min(v / norm, 0.2);
            // 再正規化
            norm = 0;
            for (double v : block) norm += v * v;
            norm = std::sqrt(norm + 1e-6);
            for (double& v : block) v /= norm;
            descriptor.insert(descriptor.end(), block.begin(), block.end());
        }
    return descriptor;
}

// ========================================================================
// OCV-35: 顔認識 (Eigenfaces / LBPH)
// ========================================================================

/// LBP (Local Binary Pattern) ヒストグラム
template<typename T>
std::vector<double> computeLBPHistogram(const Image<T>& img, int gridX = 8, int gridY = 8) {
    Image<T> gray = (img.numChannels() >= 3) ? img.toGray() : img;
    int h = static_cast<int>(gray.height());
    int w = static_cast<int>(gray.width());

    int cellH = h / gridY, cellW = w / gridX;
    std::vector<double> hist(gridX * gridY * 256, 0);

    for (int gy = 0; gy < gridY; ++gy)
        for (int gx = 0; gx < gridX; ++gx) {
            int y0 = gy * cellH, x0 = gx * cellW;
            int y1 = std::min(y0 + cellH, h - 1);
            int x1 = std::min(x0 + cellW, w - 1);

            for (int y = y0 + 1; y < y1; ++y)
                for (int x = x0 + 1; x < x1; ++x) {
                    T center = gray(y, x);
                    uint8_t lbp = 0;
                    lbp |= (gray(y-1,x-1) >= center) << 7;
                    lbp |= (gray(y-1,x)   >= center) << 6;
                    lbp |= (gray(y-1,x+1) >= center) << 5;
                    lbp |= (gray(y,x+1)   >= center) << 4;
                    lbp |= (gray(y+1,x+1) >= center) << 3;
                    lbp |= (gray(y+1,x)   >= center) << 2;
                    lbp |= (gray(y+1,x-1) >= center) << 1;
                    lbp |= (gray(y,x-1)   >= center) << 0;
                    hist[(gy * gridX + gx) * 256 + lbp] += 1.0;
                }
        }

    // 正規化
    double total = 0;
    for (double v : hist) total += v;
    if (total > 0) for (double& v : hist) v /= total;
    return hist;
}

/// LBPH 顔認識器
class LBPHFaceRecognizer {
public:
    /// 学習: labels[i] は images[i] のラベル (人物ID)
    template<typename T>
    void train(const std::vector<Image<T>>& images, const std::vector<int>& labels) {
        histograms_.clear();
        labels_.clear();
        for (std::size_t i = 0; i < images.size(); ++i) {
            histograms_.push_back(computeLBPHistogram(images[i]));
            labels_.push_back(labels[i]);
        }
    }

    /// 予測: 最も近い学習サンプルのラベルと距離を返す
    template<typename T>
    std::pair<int, double> predict(const Image<T>& img) const {
        auto hist = computeLBPHistogram(img);
        int bestLabel = -1;
        double bestDist = 1e30;

        for (std::size_t i = 0; i < histograms_.size(); ++i) {
            double dist = chiSquareDistance(hist, histograms_[i]);
            if (dist < bestDist) { bestDist = dist; bestLabel = labels_[i]; }
        }
        return {bestLabel, bestDist};
    }

private:
    std::vector<std::vector<double>> histograms_;
    std::vector<int> labels_;

    static double chiSquareDistance(const std::vector<double>& a, const std::vector<double>& b) {
        double sum = 0;
        for (std::size_t i = 0; i < a.size() && i < b.size(); ++i) {
            double d = a[i] - b[i];
            double s = a[i] + b[i];
            if (s > 1e-10) sum += d * d / s;
        }
        return sum;
    }
};

} // namespace calx::image

#endif
