import numpy as np
import sys

def spherical_to_cartesian(radius, theta_deg, phi_deg):
    # Controllo se radius e angoli sono all'interno dei range specificati
    #if not (radius >= 100):
    #    return "Errore: radius deve essere maggiore di 100 cm."
    #if not (0 <= theta_deg <= 90):
    #    return "Errore: Theta deve essere compreso tra 0 e 90 gradi."
    #if not (0 <= phi_deg <= 360):
    #    return "Errore: Phi deve essere compreso tra 0 e 360 gradi."
    
    # Conversione degli angoli da gradi a radianti
    theta_rad = np.radians(theta_deg)
    phi_rad = np.radians(phi_deg)
    
    # Conversione delle coordinate sferiche in cartesiane
    x = radius * np.sin(theta_rad) * np.cos(phi_rad)
    y = radius * np.sin(theta_rad) * np.sin(phi_rad)
    z = radius * np.cos(theta_rad)
    
    return x, y, z, theta_rad, phi_rad

def tangent_plane(theta_rad, phi_rad):
    # Calcolo di rot1 (-sin(phi), cos(phi), 0)
    rot1_x = -np.sin(phi_rad)
    rot1_y = np.cos(phi_rad)
    rot1_z = 0
    
    # Calcolo di rot2 (cos(theta)*cos(phi), cos(theta)*sin(phi), -sin(theta))
    rot2_x = np.cos(theta_rad) * np.cos(phi_rad)
    rot2_y = np.cos(theta_rad) * np.sin(phi_rad)
    rot2_z = -np.sin(theta_rad)
    
    return (rot1_x, rot1_y, rot1_z), (rot2_x, rot2_y, rot2_z)

def beam_direction(theta_rad, phi_rad):
    # Calcolo della dir del fascio
    dir_x = -np.sin(theta_rad) * np.cos(phi_rad)
    dir_y = -np.sin(theta_rad) * np.sin(phi_rad)
    dir_z = -np.cos(theta_rad)
    
    return dir_x, dir_y, dir_z

# togli i commenti se vuoi questo script indipendente
# Input dell'utente
#radius = float(input("Inserisci il radius (in cm, maggiore di 100): "))
#theta_deg = float(input("Inserisci theta (in gradi, tra 0 e 90): "))
#phi_deg = float(input("Inserisci phi (in gradi, tra 0 e 360): "))

# Trasformazione coordinate
#risultato = spherical_to_cartesian(radius, theta_deg, phi_deg)
#if isinstance(risultato, str):
#    print(risultato)
#    sys.exit()
#else:
#    x, y, z, theta_rad, phi_rad = risultato
#    print(f"/gps/pos/centre {x:.3f}, {y:.3f}, {z:.3f} cm")

# Calcolo delle rotazioni per piano tangente
#rot1, rot2 = tangent_plane(theta_rad, phi_rad)
#print(f"/gps/pos/rot1 {rot1[0]:.3f} {rot1[1]:.3f} {rot1[2]:.3f}")
#print(f"/gps/pos/rot2 {rot2[0]:.3f} {rot2[1]:.3f} {rot2[2]:.3f}")
    
# Calcolo della dir del fascio
#dir = beam_direction(theta_rad, phi_rad)
#print(f"/gps/direction {dir[0]:.3f} {dir[1]:.3f} {dir[2]:.3f}")
