import json import time from typing import Tuple, Dict, List, Iterable, Optional import numpy as np from scipy.optimize import curve_fit import statistics import math from aaredaqlib.logger_config import setup_logger from aaredaqlib.models import MLBoxModel, MLOutputModel logger = setup_logger(__name__, '/tmp/mxlogs') def box_height_from_tuple(box: tuple[float, float, float, float]) -> float: x1, y1, x2, y2 = box return abs(y2 - y1) def box_area_from_tuple(box: tuple[float, float, float, float]) -> float: x1, y1, x2, y2 = box return abs(y2 - y1) * abs(x2 -x1) def prepare_samples(boxes_by_angle: dict[int, tuple[float,float,float,float]], area = False) -> List[Tuple[float, float]]: # angles in degrees -> (theta_rad, height) samples = [] for deg, box in boxes_by_angle.items(): v = box_area_from_tuple(box) if area else box_height_from_tuple(box) samples.append((float(deg), v)) return samples def cos_model(theta_deg: float | np.ndarray, A: float, B: float, phi_rad: float, C: float): return A + B * np.cos(C * np.deg2rad(theta_deg) - phi_rad) def mad_filter(samples: List[Tuple[float, float]], k: float = 3.5) -> List[Tuple[float, float]]: if not samples: return samples ys = [y for _, y in samples] med = statistics.median(ys) mad = statistics.median([abs(y - med) for y in ys]) or 1.0 return [(d, y) for d, y in samples if abs(y - med) <= k * mad] def samples_to_json(samples): output_data = { 'timestamp': time.ctime(), 'scan_results': samples, 'total_results': len(samples) } with open("cos_test.json", 'w') as f: json.dump(output_data, f, indent=2) return def fit_metrics(y_true: np.ndarray, y_pred: np.ndarray) -> tuple[float, float, float]: resid = y_true - y_pred rmse = float(np.sqrt(np.mean(resid**2))) mae = float(np.mean(np.abs(resid))) # R² with protection against zero variance ss_tot = float(np.sum((y_true - np.mean(y_true))**2)) r2 = float(1.0 - np.sum(resid**2) / ss_tot) if ss_tot > 0 else float("nan") return rmse, mae, r2 def fit_cosine(samples: List[Tuple[float, float]]) -> dict: samples = mad_filter(samples, k=3.5) if len(samples) < 3: A = sum(y for _, y in samples) / max(1, len(samples)) return {"A": A, "B": 1.0, "phi_rad": 0.0, "C": 1.0, "rmse": None, "mae": None, "r2": None} degs = np.array([d for d, _ in samples], dtype=float) ys = np.array([y for _, y in samples], dtype=float) # Initial guess via linearized cosine/sine fit cosv = np.cos(np.deg2rad(degs)) sinv = np.sin(np.deg2rad(degs)) X = np.column_stack([np.ones_like(degs), cosv, sinv]) # [A, C, S] try: beta, _, _, _ = np.linalg.lstsq(X, ys, rcond=None) A0, C0, S0 = beta.tolist() except Exception: A0, C0, S0 = float(np.mean(ys)), 0.0, 0.0 B0 = float(math.hypot(C0, S0)) phi0 = float(math.atan2(S0, C0)) C0 = 1.0 # Bounds to keep B reasonable and phi in [-pi, pi] y_std = float(np.std(ys)) or 1.0 B_max = 10.0 * y_std bounds = ([-np.inf, 0.0, -math.pi, -np.inf], [np.inf, B_max, math.pi, np.inf]) try: popt, _ = curve_fit(cos_model, degs, ys, p0=[A0, B0, phi0, C0], bounds=bounds, maxfev=10000) logger.info(f"fit cosine: {popt}") A, B, phi, C = map(float, popt) B = max(0.0, B) yhat = cos_model(degs, A, B, phi, C) rmse, mae, r2 = fit_metrics(ys, yhat) return {"A": A, "B": B, "phi_rad": phi, "C": C, "rmse": rmse, "mae": mae, "r2": r2} except Exception as e: # Fallback to initial logger.info(f"error in curve fit {e}") yhat0 = cos_model(degs, A0, max(0.0, B0), phi0, C0) rmse0, mae0, r2_0 = fit_metrics(ys, yhat0) return {"A": float(A0), "B": max(0.0, float(B0)), "phi_rad": float(phi0), "C": float(C0), "rmse": rmse0, "mae": mae0, "r2": r2_0} def get_samples_out(boxes): samples_out = [] for deg, box in boxes.items(): h = box_height_from_tuple(box) a = box_area_from_tuple(box) samples_out.append({"angle_deg": float(deg), "height": float(h), "area": float(a)}) samples_out.sort(key=lambda x: x["angle_deg"]) return samples_out def choose_best_fit(fits_by_name: Dict[str, Dict]) -> Tuple[Optional[float], Optional[Dict], Optional[str]]: def key(entry: Dict): params = entry.get("params") or {} rmse = params.get("rmse") mae = params.get("mae") r2 = params.get("r2") logger.info(f"params: {params}") # Treat None/NaN as non-comparable if rmse is None or mae is None or r2 is None: return None try: logger.info(f"rmse: {rmse}, mae: {mae}, r2: {r2}") return float(rmse), float(mae), -float(r2) except Exception: return None best_name = None best_fit = None best_key = None for name, entry in fits_by_name.items(): k = key(entry) logger.info(f"name: {name}, entry: {entry}, key : {k}") if k is None: continue if best_key is None or k < best_key: best_key = k best_fit = entry best_name = name if best_fit is None: return None, None, None best_angle = best_fit.get("angle") best_angle = float(best_angle) if isinstance(best_angle, (int, float)) else None return (best_angle, best_fit, best_name) def get_flat_face(boxes: dict[int, tuple[float,float,float,float]], start_angle:int, end_angle:int, area:bool = False) -> tuple[int, dict]: samples = prepare_samples(boxes, area=area) parameters = fit_cosine(samples) def safe_best_angle(params: dict) -> int: if not params: return 0 search_grid = range(start_angle, end_angle, 1) A = float(params.get("A", 0.0)) B = float(max(0.0, params.get("B", 0.0))) C = float(params.get("C", 0.0)) phi = float(params.get("phi_rad", 0.0)) if B <= 1e-9 or not math.isfinite(A) or not math.isfinite(phi): return 0 return max(search_grid, key=lambda d: cos_model(d, A, B, phi, C)) best_fit_angle= safe_best_angle(parameters) return best_fit_angle, parameters def chose_best_angle(boxes: dict[int, tuple[float,float,float,float]], fit_results) -> int: measured_angles = list(boxes.keys()) choose_best_fit(fit_results) candidates = [a for a in (fit_results["Area"]["angle"], fit_results["Height"]["angle"]) if isinstance(a, (int, float))] if measured_angles and candidates: chosen = min(candidates, key=lambda a: min(abs(((a - m + 180) % 360) - 180) for m in measured_angles)) else: chosen = candidates[0] if candidates else 0 return chosen