'''
Created on Nov 5, 2014

@author: jesse
'''
import Queue
import SocketServer
from BaseCommand import BaseCommand
from FlyCommand import FlyCommand
from TerminateCommand import TerminateCommand
from getch import getch
from multiprocessing import Process
import multiprocessing
from threading import Thread
import time
import math


class Android(object):
    '''
    Allows for live keyboard input to calibrate or fly the crazyflie.
    '''

    crazyFlie = None
    parameters = None
    data = None

    control = {}

    stopThread = False

    def __init__(self, crazyFlie, parameters, data, queue):
        '''
        Creates the new object.
        '''
        self.crazyFlie = crazyFlie
        self.queue = queue

    def start(self):
        t = Thread(target = self.run)
        t.start()

    def run(self):

        acceloServer = AcceloServer()
        t = Thread(target = acceloServer.run)
        t.start()

        prevCommandTime = time.time()

        noDataCount = 0
        while not self.stopThread:
            data = AcceloHandler.data
            if data is not None:
                command = FlyCommand()
                if "dir" in data:
                    dirData = data["dir"]
                    command.pitch = dirData[0]
                    command.roll = dirData[1]
                if "thr" in data:
                    thrData = data["thr"]
                    command.thrust = thrData[0]
                    command.yaw = thrData[1]
                
                print "Command pitch: " + str(command.pitch) + ", roll: " + str(command.roll)
                self.queue.addCommand(command)

            elif noDataCount < 1000:
                noDataCount += 1
            else:
                print "Receiving no data, stopping."
                self.queue.addCommand(TerminateCommand())
                self.stopThread = True
                acceloServer.stop()
            time.sleep(0.1)

class AcceloServer():

    HOST = ""
    PORT = 5555
    server = None

    def __init__(self):
        self.server = SocketServer.UDPServer((self.HOST, self.PORT), AcceloHandler)

    def run(self):
        self.server.serve_forever()

    def stop(self):
        self.server.shutdown()


class AcceloHandler(SocketServer.BaseRequestHandler):

    data = {}
    ips = {}

    def handle(self):
        addr = self.client_address[0]
        data = self.request[0].strip()

        if "dir" not in AcceloHandler.ips:
            print "{} controls direction.".format(addr)
            AcceloHandler.ips["dir"] = addr
            AcceloHandler.data["dir"] = {}
        elif "thr" not in AcceloHandler.ips and addr != AcceloHandler.ips["dir"]:
            print "{} controls thrust and yaw.".format(addr)
            AcceloHandler.ips["thr"] = addr
            AcceloHandler.data["thr"] = {}

        splitData = data.split(",")
        dirs = self.acceloToValue(float(splitData[2]), float(splitData[3]), float(splitData[4]))
        if addr == AcceloHandler.ips["dir"]:
            AcceloHandler.data["dir"] = dirs
        elif addr == AcceloHandler.ips["thr"]:
            # FIXME Very hacky, but see if it works.
            AcceloHandler.data["thr"] = (dirs[0]*-1000, 10*dirs[1])

    def acceloToValue(self, ax, ay, az):
        if az != 0:
            sDir = math.atan(-ax/az) * (90 / math.pi)
            fDir = math.atan(-ay/az) * (90 / math.pi)
        else:
            fDir = 0
            sDir = 0
        return (fDir, sDir)
