#!/usr/bin/env python2

import serial
import sys
import time
import threading
import ossaudiodev, fcntl
import math
import array

class SerialReader(threading.Thread):
    def __init__(self, path):
        super(SerialReader, self).__init__()

        self._stop = threading.Event()
        self._path = path;
        self.vals = [0] * 4

    def run(self):
        uart = serial.Serial(self._path, 115200, timeout=1)
        uart.write(chr(0x81))

        try:
            data = ""
            while not self._stop.isSet():
                data += uart.read(1)
            
                if len(data) == 3:
                    n = ord(data[0])
                    val = (ord(data[1]) << 8) + ord(data[2])

                    self.vals[n] = val

                    data = ""
        finally:
            uart.write(chr(0x82))
            uart.close()

    def stop(self):
        self._stop.set()

rate = 22050.0

audio = ossaudiodev.open("/dev/dsp", "w")
audio.speed(int(rate))
audio.setfmt(ossaudiodev.AFMT_U16_LE)

buf = array.array("i", [(4 << 16) | 8])
fcntl.ioctl(audio.fileno(), -1073459190, buf, True)

reader = SerialReader("/dev/ttyUSB0")
reader.start()

a = 0.0

fzero = 0.0
azero = 0.0

time.sleep(1)

fzero = reader.vals[2]
azero = reader.vals[3]

try:
    while True:
        freq = (float(reader.vals[2]) - fzero + 400) * 1.2 + 200
        print freq
        print reader.vals
        
        data = ""
        step = freq * math.pi * 2 / rate
        factor = max(0.0, min((float(reader.vals[3]) - azero + 100) * 0.02, 1.0))

        for i in range(0, 128):
            sample = int(math.sin(a) * 16000 * factor + 32768)
            a += step
            data += chr(sample & 0xFF) + chr(sample >> 8)
        
#        audio.sync()
        audio.writeall(data)
finally:
    reader.stop()