"""
Step calibration: counts motor steps needed for one full turntable revolution.
Sends steps in batches, watches encoder, stops at 2400 counts (1 rev).
"""
import serial, time, sys

PORT     = '/dev/ttyACM0'
BAUD     = 115200
TARGET   = 2400   # encoder counts per revolution (unchanged)
BATCH    = 20     # steps per command
DELAY_MS = 8      # ms between steps — half-step needs faster rate to count cleanly

def read_enc(s):
    while True:
        line = s.readline().decode(errors='replace').strip()
        if '[ALIVE]' in line and 'enc=' in line:
            return int(line.split('enc=')[1].split()[0])

s = serial.Serial(PORT, BAUD, timeout=2)
s.setDTR(False); s.setRTS(False)
time.sleep(1)
s.reset_input_buffer()

# Zero encoder
s.write(b'ZERO\n')
time.sleep(0.3)
enc_start = read_enc(s)
print(f"Encoder zeroed. Starting calibration — DO NOT move turntable manually.")
print(f"Target: {TARGET} counts = 1 full revolution\n")

total_steps = 0
last_enc    = 0
start_time  = time.time()

while True:
    s.write(f'STEP {BATCH} {DELAY_MS}\n'.encode())

    # Drain responses until we get the STEP confirmation
    deadline = time.time() + 5
    while time.time() < deadline:
        line = s.readline().decode(errors='replace').strip()
        if line:
            print(f"  {line}")
        if '[STEP]' in line:
            # Extract current encoder from ALIVE or next ENC line
            break

    total_steps += BATCH

    # Get current encoder position
    s.write(b'ENC\n')
    deadline = time.time() + 3
    enc_now = last_enc
    while time.time() < deadline:
        line = s.readline().decode(errors='replace').strip()
        if '[ENC]' in line and 'counts=' in line:
            enc_now = abs(int(line.split('counts=')[1].split()[0]))
            break

    elapsed = time.time() - start_time
    print(f"  >> steps={total_steps}  enc={enc_now}  ({enc_now/TARGET*100:.1f}% of rev)  t={elapsed:.1f}s")

    if enc_now >= TARGET:
        break

    last_enc = enc_now

s.write(b'RELEASE\n')
time.sleep(0.3)

elapsed = time.time() - start_time
print(f"\n{'='*50}")
print(f"CALIBRATION RESULT")
print(f"  Steps for 1 revolution : {total_steps}")
print(f"  Encoder counts reached : {enc_now}")
print(f"  Time elapsed           : {elapsed:.1f}s")
print(f"  Degrees per step       : {360/total_steps:.4f}°")
print(f"  Steps per degree       : {total_steps/360:.2f}")
print(f"{'='*50}")
s.close()