# SPDX-FileCopyrightText: Copyright © 2024-2026 Damian Anders <damian.anders@tu-dresden.de>
#
# SPDX-License-Identifier: AGPL-3.0-or-later

"""Identify areas of constant rotational speed and its frequency from an angle signal."""

import numpy as np
from numpy import typing as npt

__all__ = ["angle_values_to_rotation", "find_constant_angle_slope"]

def angle_values_to_rotation(time_values: npt.NDArray[np.float64],
							 angle_values: npt.NDArray[np.float64],
							 percentage: float = 75,
) -> tuple[np.float64, np.float64]:
	"""
	Calculates the velocity and offset based on the gradient
	of an angle sequence over time. This function determines velocity and
	offset by using the rate of change of the angle, selecting values above a
	specified percentile, and performing a linear fit using the
	least squares method.

	Parameters
	----------
	time_values : np.ndarray
		1D array containing the time values for the specified angle data.
	angle_values : np.ndarray
		1D array of angles corresponding to the specified time values.
	percentage : float, default=75
		Percentage used to determine percentile for the significant
		angle change rate.
		Defaults to 75.

	Returns
	-------
	tuple[np.float64, np.float64]
		A tuple where the first element represents the calculated velocity
		(slope) and the second element represents the calculated offset (y-intercept)

	Raises
	------
	ValueError
		If no valid angle gradients are found above the specified percentile.
	"""
	angleGradient = np.gradient(angle_values, time_values)	# Winkeländerungsgeschwindigkeit bestimmen
	angleSpeed = np.percentile(angleGradient, percentage)	# Grenzwert

	boolMask = angleGradient > angleSpeed	# Werte oberhalb des Grenzwerts

	boolMask[np.argmax(angle_values):] = False # Werte hinter maximalem Winkel nicht verwenden
	if not np.any(boolMask):
		raise ValueError("No valid angle gradients found above the specified percentile.")

	A = np.column_stack((time_values[boolMask], np.ones(np.count_nonzero(boolMask))))
	x, _, _, _ = np.linalg.lstsq(A, angle_values[boolMask])

	speed = x[0]
	offset = x[1]

	return speed, offset


def find_constant_angle_slope(time_values: npt.NDArray[np.float64],
							  angle_values: npt.NDArray[np.float64],
							  fit_slope: float,
							  fit_intercept: float,
							  percentage: float = 25,
) -> npt.NDArray[np.int_]:
	"""
	Find the slice indices of the longest continuous segment matching a linear fit.

	This function calculates the absolute deviation between a linear fit
	and the measured values. It determines a maximum deviation threshold
	using the specified percentile. The longest contiguous block of
	points falling strictly below this threshold is identified, and its
	start and end indices are returned.

	Parameters
	----------
	time_values : ndarray
		1D array containing the independent variable data with `float64` type.
	angle_values : ndarray
		1D array containing the measured dependent data with `float64` type.
	fit_slope : float
		The slope of the linear fit.
	fit_intercept : float
		The y-intercept of the linear fit.
	percentage : float, optional
		The percentile used to calculate the maximum allowed deviation
		threshold. Default is 25.

	Returns
	-------
	ndarray
		1D array of type `int_` containing the start and end indices.
		The end index is incremented by 1, making it directly usable
		for array slicing.

	Raises
	------
	ValueError
		If no constant gradient could be found (i.e., no values fall
		below the calculated threshold).
	"""
	abweichung = np.abs(time_values * fit_slope + fit_intercept	# Fit
						- angle_values)							#	Messung

	grenzwert = np.percentile(abweichung, percentage)	# maximaler Betrag der Abweichung
	idx_unterhalb_grenzwert = np.flatnonzero(abweichung < grenzwert)	# Indizes aller Werte unterhalb Grenzwert

	if idx_unterhalb_grenzwert.size == 0:
		raise ValueError('No constant gradient could be found.')

	idx_luecken = np.flatnonzero(np.diff(idx_unterhalb_grenzwert) > 1) + 1	# jeweils Index des ersten Werts einer Lücke
	idx_abschnitte = np.split(idx_unterhalb_grenzwert, idx_luecken)	# zusammenhängende Abschnitte unterhalb Grenzwert

	idx_laengste = max(idx_abschnitte, key=len)	# Indizes aller Werte des größten Abschnitts
	idx_start_end = idx_laengste[[0,-1]]	# Indizes des ersten und letzten Werts jenes Abschnitts
	idx_start_end[1] = idx_start_end[1] + 1	# damit "idx_start_end" direkt zum slicen nutzbar ist

	return idx_start_end
