package network.rs485.ben.computervision

import org.opencv.core.*
import org.opencv.imgproc.Imgproc
import kotlin.math.pow
import kotlin.math.roundToInt
import kotlin.math.sqrt

internal typealias Pixel = Pair<Int, Int>
internal typealias Line = Pair<Pixel, Pixel>

internal val Pixel.x: Int
    get() = first

internal val Pixel.y: Int
    get() = second

internal operator fun Pixel.plus(other: Pixel): Pixel = Pixel(x + other.x, y + other.y)

internal operator fun Pixel.minus(other: Pixel): Pixel = Pixel(x - other.x, y - other.y)

internal operator fun Mat.get(pixel: Pixel): DoubleArray? =
    get(/* row = */ pixel.y, /* col = */ pixel.x)

fun Point.rounded(): Pixel = x.roundToInt() to y.roundToInt()

internal operator fun Mat.get(point: Point): DoubleArray? =
    get(/* row = */ point.y.toInt(), /* col = */ point.x.toInt())

class Vector2D(x: Double, y: Double) : Point(x, y) {
    val magnitude: Double
        get() = sqrt(x.pow(2.0) + y.pow(2.0))

    constructor(other: Point) : this(other.x, other.y)

    fun normalize() = apply {
        magnitude.let {
            x /= it
            y /= it
        }
    }

    operator fun times(factor: Double): Vector2D = Vector2D(x * factor, y * factor)

    operator fun div(divider: Double): Vector2D = Vector2D(x / divider, y / divider)

    operator fun plus(point: Point) = Vector2D(x + point.x, y + point.y)

}

@JvmName("pixelToVec2D")
internal fun Pixel.vec2D(): Vector2D = Vector2D(x.toDouble(), y.toDouble())

@JvmName("lineToVec2D")
internal fun Line.vec2D(): Vector2D = Vector2D((second.x - first.x).toDouble(), (second.y - first.y).toDouble())

internal object LineComparator : Comparator<Line> {
    override fun compare(line1: Line, line2: Line): Int {
        assert(line1.first.y == line1.second.y)
        assert(line2.first.y == line2.second.y)
        val rowCompare = line1.first.y.compareTo(line2.first.y)
        if (rowCompare != 0) return rowCompare
        val lineStartCompare = line1.first.x.compareTo(line2.first.x)
        if (lineStartCompare != 0) return lineStartCompare
        return line1.second.x.compareTo(line2.second.x)
    }
}

internal fun Line.intersects(other: Line): Boolean {
    assert(first.y == second.y)
    assert(other.first.y == other.second.y)
    if (first.y != other.first.y) return false
    if (first.x == other.first.x) return true
    return if (first.x < other.first.x) {
        second.x >= other.first.x
    } else {
        other.second.x >= first.x
    }
}

internal fun Mat.drawLine(line: Line, color: Scalar) = Imgproc.line(
    /* img = */ this,
    /* pt1 = */ line.first.vec2D(),
    /* pt2 = */ line.second.vec2D(),
    /* color = */ color,
)

internal fun Mat.filterPixels(filterFunc: (value: DoubleArray) -> Boolean): List<Pixel> =
    (0 until rows()).flatMap { row ->
        (0 until cols())
            .filter { col -> filterFunc(get(row, col)) }
            .map { col -> Pixel(col, row) }
    }

internal fun Mat.sumOfHist(
    indices: IntRange,
    maximum: Double,
    mapFunc: (level: Int, value: Double) -> Double,
): Double {
    var sum: Double = 0.toDouble()
    for (idx in indices) {
        sum += mapFunc(idx, get(idx, 0)[0])
        if (sum > maximum) break
    }
    return sum
}

fun <T : Number> Mat.searchGreatest(center: Pixel, direction: Vector2D, searchRange: Iterable<T>): Pixel? =
    searchRange.map { factor: Number -> (direction * factor.toDouble()).rounded() + center }
        .filter { it.x in (0 until width()) && it.y in (0 until height()) }
        .lastOrNull { pixel -> get(pixel)?.let { it[0] > 0 } ?: false }