'''
Created on Nov 11, 2012

@author: attero
'''
import serial
import glob
import threading
import math

def mutex(m):
    def wrapper(self, *args, **kwargs):
        self.s.acquire()
        m()
        self.s.release()
    return wrapper

class CannonController :
    def __init__(self):
        self.s = threading.Semaphore()
        print("cannon controller started")
        self.position = [0, 0]
        self.fired = False
        self.laser = False
        ports = glob.glob("/dev/ttyACM*")
        
        if(len(ports) == 1):
            try:
                self.ser = serial.Serial(ports[0], 115200, timeout=1)
            except serial.SerialException:
                raise Exception("Could not open serial connction (busy?)")
            self.connected_to_serial = True
        else:
            self.ser = False
            self.connected_to_serial = False
            if len(ports) > 1 :
                raise Exception("To many devices to handle")
            elif len(ports) == 0 :
                print Exception("No device is connected")
            
    def set_position(self, point):
        self.position = point

    def translate_yaw(self, yaw):
        return yaw
            
    def fire(self):
        self.fired = True
        
    def enable_laser(self):
        self.laser = True
        
    def disable_laser(self):
        self.laser = False
        
    def move_left(self, change):
        if(self.position[0] >= change):
            self.position[0] -= change  
            
    def move_right(self, change):
        if(self.position[0] < 255 - change):
            self.position[0] += change 
            
    def move_up(self, change):
        if(self.position[1] < 255 - change):
            self.position[1] += change
            
    def move_down(self, change):
        if(self.position[1] >= change):
            self.position[1] -= change
            
    def check_validity_of_data(self):
        if(abs(self.position[0]) > 255):
            return False
        if (abs(self.position[1]) > 255):
            return False
        return True
    
    def get_data(self):
        if self.check_validity_of_data():
            data = []
            data.extend(self.position)
            if(self.fired):
                data.append(255)
                self.fired = False
            else:
                data.append(0)
            if(self.laser):
                data.append(255)
            else:
                data.append(0)
            return data
        else:
            return [0, 0, 0, 0]
        
    def get_data_to_send(self):
        data = self.get_data()
        data[0] = self.translate_yaw(data[0])
        return [chr(x) for x in data]
    
    def send_data(self):
        if(self.connected_to_serial): 
            data = self.get_data_to_send()
            self.ser.write('a')
            self.ser.write(data[0])
            print "wrote %i" % ord(data[0])
            self.ser.write('b')
            self.ser.write(data[1])
            self.ser.write('c')
            self.ser.write(data[2])
            self.ser.write('d')
            self.ser.write(data[3])
        else:
            print "Cannot send data, not connected..."

class HackWAWCannonController(CannonController):
    AXIS_DISTANCE = 20
    ARM_LENGTH = 1
    def translate_yaw(self, yaw):
        return yaw
        try:
            turret_angle = (yaw * math.pi) / 256 - (math.pi / 2)
            servo_angle = math.asin((self.ARM_LENGTH * math.tan(turret_angle)) / (self.AXIS_DISTANCE - self.ARM_LENGTH))
        except:
            return 0 if yaw == 0 else 255
        return int(((servo_angle + (math.pi / 2)) * 256) / math.pi)

class Gunpoint :
    def __init__(self, point1, point2, point3):
        self.vertical_angle = point3[1] - point1[1]
        self.horizontal_angle = point2[0] - point1[0]
        self.beginnig_horizontal = point1[0]
        self.beginnig_vertical =  point1[1]
        
    def calibrate(self, point1, point2, point3):
        self.vertical_angle = point3[1] - point1[1]
        self.horizontal_angle = point2[0] - point1[0]
        self.beginnig_horizontal = point1[0]
        self.beginnig_vertical = point1[1]
        
    def aim(self, point):
        horizontal = math.ceil(self.beginnig_horizontal + self.horizontal_angle*point[0])
        vertical = math.ceil(self.beginnig_vertical + self.vertical_angle*point[1])
        return [horizontal, vertical]