package odometry

import "math"

// rotateGray rotates a grayscale matrix by angleDeg (positive = clockwise)
// around its center, using bilinear interpolation. The output has the same
// dimensions as the input.
func rotateGray(g [][]float64, size int, angleDeg float64) [][]float64 {
	theta := angleDeg * math.Pi / 180.0
	cosT := math.Cos(theta)
	sinT := math.Sin(theta)
	cy := float64(size-1) / 2.0
	cx := float64(size-1) / 2.0

	out := make([][]float64, size)
	for i := range out {
		out[i] = make([]float64, size)
	}

	for oy := 0; oy < size; oy++ {
		for ox := 0; ox < size; ox++ {
			// Map output pixel to input coordinates via inverse rotation.
			dy := float64(oy) - cy
			dx := float64(ox) - cx
			sx := cx + dx*cosT + dy*sinT
			sy := cy - dx*sinT + dy*cosT

			// Bilinear interpolation with clamp at borders.
			x0 := int(math.Floor(sx))
			y0 := int(math.Floor(sy))
			fx := sx - float64(x0)
			fy := sy - float64(y0)

			x0 = clampInt(x0, 0, size-1)
			y0 = clampInt(y0, 0, size-1)
			x1 := clampInt(x0+1, 0, size-1)
			y1 := clampInt(y0+1, 0, size-1)

			out[oy][ox] = (1-fy)*(1-fx)*g[y0][x0] +
				(1-fy)*fx*g[y0][x1] +
				fy*(1-fx)*g[y1][x0] +
				fy*fx*g[y1][x1]
		}
	}
	return out
}

func absf(x float64) float64 {
	if x < 0 {
		return -x
	}
	return x
}