import numpy as np
import matplotlib.pyplot as plt
import csv
from scipy import signal
from skyfield.api import Topos, load, utc
from skyfield.units import Angle
from datetime import datetime
from tqdm.notebook import trange, tqdm
import json

satellites = load.tle_file('../tts/examples/active.txt')
by_name = {sat.name: sat for sat in satellites}

ts = load.timescale()
def time(dt):
    return ts.utc(datetime.fromtimestamp(1585993950.588545+dt,tz=utc))

def map_angle(angle):
    return 0.05 + angle.degrees * (0.35-0.05)/(180)

def azalt_to_duty(az, alt):
    if az.degrees > 180:
        return [map_angle(Angle(degrees=az.degrees%180)), map_angle(Angle(degrees=180-alt.degrees))]
    else:
        return [map_angle(az), map_angle(alt)]

def compute_true_trajectory(sat, loc, times):
    orients = []
    for dt in times:
        t = time(dt)
        diff = sat - loc
        topocentric = diff.at(t)
        alt, az, dist = topocentric.altaz()
        orients.append([az, alt])
    return orients

def compute_duty_cycle(x):
    peaks, props = signal.find_peaks(x, prominence=20)
    peaks = np.concatenate(([0], peaks))
    T_high = peaks[1::2] - peaks[::2]
    T_motor_drive = peaks[2]-peaks[0]
    return T_high/T_motor_drive, T_motor_drive


Fs = 102400 # hz
Ts = 1/Fs
station = Topos(37.5, 15.08)

X0 = np.genfromtxt('./rbs_m2-juliet20230hotel/signal_0.csv', delimiter=',')
X1 = np.genfromtxt('./rbs_m2-juliet20230hotel/signal_1.csv', delimiter=',')
X2 = np.genfromtxt('./rbs_m2-juliet20230hotel/signal_2.csv', delimiter=',')

visible = []
for sat in tqdm(satellites):
    alt, az, _ = (sat-station).at(time(60)).altaz()
    if alt.degrees > 0:
        visible.append(sat)

# generate tracks for all visible satellites
duty_traces = []
for sat in tqdm(visible):
    duty_traces.append([azalt_to_duty(*o)
                  for o in compute_true_trajectory(sat, station, times)])
dtraces = [np.array(x) for x in duty_traces]

for X in [X0, X1, X2]:
    measured_az, drive_period_az = compute_duty_cycle(X[:,1])
    measured_alt, drive_period_alt = compute_duty_cycle(X[:,2])

    errors = np.array([np.linalg.norm(measured_az[::60]-trace[:-1,0])
        + np.linalg.norm(measured_alt[::60]-trace[:-1,1])
        for trace in dtraces])

    print(np.argmin(errors), visible[np.argmin(errors)])