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

"""Simulate position sensor signals for an eccentrically rotating circle."""

from typing import Literal

import numpy as np
import numpy.typing as npt
from scipy import signal as ssig

__all__ = ["eccentric_signals",
		   "surface_profile"]


def eccentric_signals(phi: npt.NDArray[np.float64],
					  radius: float,
					  exzentrizitaet: npt.NDArray[np.float64],
					  posSensor: npt.NDArray[np.float64],
					  modus: Literal['xy', 'ra'] = 'xy',
)-> tuple[npt.NDArray[np.float64],
		  npt.NDArray[np.float64],
		  npt.NDArray[np.float64]]:
	"""
	Simulate position sensor signals for an eccentrically rotating circle.

	Notes
	_____
	All angles are in radians.

	Parameters
	----------
	phi : (M,) array_like
		Angular lines of the shaft's position at which the sensor values are to be calculated.
	radius : scalar
		Radius of the circle.
	exzentrizitaet : (2,) array_like
		Centre point displacement of the circle, see ``modus``
	posSensor : (N,) array_like
		Angular positions of the sensors
	modus : {'xy', 'ra'}, optional
		Format of the transferred eccentricity. ``“xy”`` for Cartesian coordinates, ``“ra”`` for polar coordinates.
		Defaults to ``'xy'``

	Returns
	-------
	delta_s : (M,N) ndarray
		Simulated signals.
	gamma : (M,N) ndarray
		Angle between the x-axis and the intersection of the sensor axis with the eccentric circle.
	l_1 : (M,N) ndarray
		 Distance of the centre point shift parallel to the sensor axis.
	"""

	phi = np.asarray(phi, dtype=np.float64)
	radius = np.float64(radius)
	exzentrizitaet = np.asarray(exzentrizitaet, dtype=np.float64)
	posSensor = np.atleast_1d(np.asarray(posSensor, dtype=np.float64))

	if 'xy' in modus:
		absExz = np.linalg.norm(exzentrizitaet)		# Betrag der Exzentrizität
		alpha = np.arctan2(exzentrizitaet[1], exzentrizitaet[0])	# Offset-Winkel des Ortsvektors der Exzentrizität
	elif 'ra' in modus:
		absExz = exzentrizitaet[0]
		alpha = exzentrizitaet[1]
	else:
		raise ValueError('"modus" muss "xy" oder "ra" sein.')

	if absExz > radius:
		raise ValueError('Exzentrizität zu groß, Kreis verlässt Messachse(n).')

	winkel = phi + alpha				# laufender Winkel des Ortsvektors
	xR_phi = absExz * np.cos(winkel)	# laufende x- und y-Koordinaten des tatsächlichen Mittelpunkts
	yR_phi = absExz * np.sin(winkel)

	delta_s = np.zeros((phi.size, posSensor.size), dtype=np.float64)	# Arrays initialisieren
	gamma = np.zeros(delta_s.shape)
	l_1 = np.zeros(delta_s.shape)
	for a in range(posSensor.size):								# für jede Sensorposition:
		k = absExz * np.sin(winkel - posSensor[a])				#	Hilfsstrecke k
		l_1[:, a] = absExz * np.cos(winkel - posSensor[a])		#	Hilfsstrecke l_1, Mittelpunktverschiebung parallel zur Sensorachse
		l_2 = np.sqrt(np.square(radius) - np.square(k))			#	Hilfsstrecke l_2
		s = l_1[:, a] + l_2										#	Abstand Sollachse zu Schnittpunkt Messachse mit Kreis
		delta_s[:, a] = s - radius								#	Abweichung Messwert von von (theor.) Messwert Soll-Kreisbahn
		gammaRoh = np.arctan2(s*np.sin(posSensor[a])-yR_phi,	#	Winkel zwischen Schnittpunkt Messachse, verschobenem Mittelpunkt, und verschobener x-Achse
							  s*np.cos(posSensor[a])-xR_phi)
		gamma[:, a] = np.unwrap(gammaRoh)						#		Sprünge aus Winkel entfernen

	return delta_s, gamma, l_1


def surface_profile(phi: npt.NDArray[np.float64],
					f_crit: float,
					seed: int = 555,
					filter_order: int = 10,
) -> npt.NDArray[np.float64]:
	"""
	Generates a simulated surface profile with random form deviations.

	Creates a synthetic roundness error signal by applying a low-pass filter to
	normally distributed random values. The function ensures the generated profile
	is smooth and continuous at the transition from $2\\pi$ back to $0$.

	Parameters
	----------
	phi : npt.NDArray[np.float64]
	    Array of angular positions (radians) representing the circumference.
	    Its length determines the number of samples in the output.
	f_crit : float
	    Cutoff frequency for the Butterworth filter, expressed as a multiple
	    of the fundamental rotational frequency.
	seed : int, default=555
	    Seed for the random number generator to ensure reproducible profiles.
	filter_order : int, default=10
	    The order of the Butterworth low-pass filter used for smoothing.

	Returns
	-------
	npt.NDArray[np.float64]
	    A normalized (std=1) array of radial deviations corresponding to `phi`.

	Raises
	------
	Exception
	    If the algorithm fails to find a segment that meets the continuity
	    constraints within the search limit.

	Notes
	-----
	The continuity constraint requires that the jump between the last and first
	sample of the profile is within the tolerance of the average signal gradient.
	"""
	laengeGenerieren = 5	# initiale Länge des zu generierenden Fehlersignals als Vielfaches von "phi.size"
	einschwingen = filter_order * 100	# Einschwingphase des Filters großzügig abschneiden
	a = 0	# initialer Startpunkt des Fensters

	while laengeGenerieren < 25:	# Grenze für die Länge des abzusuchenden Fehlersignals
		fehlerRoh = np.asarray(np.random.default_rng(seed).standard_normal(laengeGenerieren*phi.size))	# normalverteiltes Fehlersignal erstellen
		fehlerSmooth = np.asarray(_butterworth_lowpass(fehlerRoh, filter_order, phi.size, f_crit))		#	und durch Tiefpassfilterung glätten
		tol = np.abs(np.mean(np.diff(fehlerSmooth)))	# Sprung bei Übergang vom Ende zum Anfang des Fehlersignals soll nicht größer als
														#	Median der Sprünge zwischen den Werten des Signals sein

		while a < ((laengeGenerieren-1)*phi.size - einschwingen):									# Fenster im Bereich des Fehlersignals
			werteSprung = fehlerSmooth[einschwingen+a] - fehlerSmooth[einschwingen+a+phi.size-1]	# Betrag des Sprungs von letztem Wert des Fensters zu erstem Wert des Fensters
			if np.abs(werteSprung) <= tol:															# wenn Betrag innerhalb der Toleranz liegt:
				fehlerAusschnitt = fehlerSmooth[einschwingen+a:einschwingen+phi.size+a]				#	passende Werte ausschneiden
				fehlerSkaliert = fehlerAusschnitt / np.std(fehlerAusschnitt)						#	und deren Standardabweichung auf 1 normieren
				return fehlerSkaliert
			a = a+1		# Fenster verschieben
		laengeGenerieren = laengeGenerieren + 1	# wenn im Bereich des Fehlersignals keine passende Fensterposition gefunden wird:
												#	längeres Fehlersignal generieren und ab aktueller Fensterposition weitersuchen
	raise Exception('No matching result found. Change the “seed” parameter and try again.')


def _butterworth_lowpass(signal: npt.NDArray[np.float64],
						 filter_order: int,
						 f_sampling: float,
						 f_crit: float,
) -> npt.NDArray[np.float64]:
	"""
	Applies a zero-phase Butterworth low-pass filter to the signal.

	Parameters
	----------
	signal : npt.NDArray[np.float64]
	    The input signal to be filtered.
	filter_order : int
	    The order of the filter.
	f_sampling : float
	    The sampling frequency of the signal.
	f_crit : float
	    The cutoff frequency in the same units as `fs`.

	Returns
	-------
	npt.NDArray[np.float64]
	    The filtered signal.
	"""
	butter_sos = ssig.butter(filter_order,  # Filterkoeffizienten erstellen
							 f_crit,
							 btype='lowpass',
							 output='sos',
							 fs=f_sampling)

	return ssig.sosfiltfilt(butter_sos, signal, axis=0)