from IPython.display import display, Latex, Math

import phm

p = phm.phm()

bin(p.read_inputs())

def shutup():
    p.set_frequency(0)
    p.write_outputs(0b1000)

def shutup_handler(self, etype, value, tb, tb_offset=None):
    shutup()
    print 'KeyboardInterrupt'

get_ipython().set_custom_exc((KeyboardInterrupt,), shutup_handler)

def notify_speaker():
    p.set_frequency(50)

def notify_led():
    p.set_voltage(3000)

notify = notify_speaker

p.write_outputs(0b1000)

f = 0
stepup = 20
stepdown = stepup * 3
dt = 0.1
fmax = 550
nmax = 15
nmin = 0
while True:
    d = p.read_inputs()
    if d is None:
        continue

    if d & 0b1000:
        f += stepup
        f = min(f, fmax)
    else:
        f -= stepdown
        f = max(0,f)
    p.set_frequency(f)
    p.write_outputs(int(1.*f * nmax / fmax))
    time.sleep(dt)


p.write_outputs(0b1110)

def magnetize():
    p.write_outputs(0b1000)

def drop():
    p.write_outputs(0b0010)

magnetize()

def wait(beats=3):
    time.sleep(beats * 60. / tempo)

C4 = 130.81
def ready_beep(beats=1):
    p.write_outputs(0b1100)
    p.set_frequency(C4)
    wait(beats)
    p.write_outputs(0b1000)
    p.set_frequency(0)

def go_beep(beats=1):
    p.write_outputs(0b1010)
    p.set_frequency(C4*2)
    wait(beats)
    p.set_frequency(0)

def catch_beep(beats=1):
    p.write_outputs(0b1110)
    p.set_frequency(C4*4)
    wait(beats)
    p.set_frequency(0)
    p.write_outputs(0b000)


tempo = 600

def gate_blocked():
    mask = None
    while mask is None:
        mask = p.read_inputs()
    return (mask & 0b1000)
    

def time_drop():
    print "waiting for signal ...",
    sys.stdout.flush()
    while not gate_blocked():
        time.sleep(0.1)
    print "ready ...",
    sys.stdout.flush()
    while gate_blocked():
        ready_beep()
        wait(4)
    go_beep(3)
    print "go!"
    sys.stdout.flush()
    start = time.time()
    drop()
    cycles = 0
    while not gate_blocked():
        cycles += 1
    stop = time.time()
    t = stop - start
    err = 0.5 * t / cycles
    t -= err
    tms = 1e3*t
    print u"%i ± %.1f ms" % (1e3 * t, 1e3 * err)
    catch_beep()
    magnetize()
    return t, err

time_drop()

%pylab inline

ds = [0, 10, 15, 20, 25]
ts = [0]
errs = [0]

for d in ds[1:]:
    print "move magnet to %i cm" % d
    t, err = time_drop()
    ts.append(t)
    errs.append(err)

ds = np.array(ds)
ts = np.array(ts)
errs = np.array(errs)


plot(ds, ts, '-o')

plot(ts, ds, '-o')
xlabel("t")
ylabel("d")

from scipy.optimize import leastsq

err_f = lambda a: a*ts*ts - ds

a = leastsq(err_f, 1)[0][0]

%config InlineBackend.figure_format = 'svg'

tt = np.linspace(0, ts[-1])
plot(tt, a * tt**2, '-', ts, ds, '-o');

g_sin_theta = 2 * a

sin_theta = 2.2 / 30

g = g_sin_theta / sin_theta

display(Latex("$g = %.1f$ cm/s" % g))

g_true = 980.7
dg = (g - g_true) / g_true
display(Latex("$g_{true} = %.1f$ cm/s" % g_true))
display(Latex(r"error $= %.1f$ %%" % (100*dg)))