#!/usr/bin/env python

import serial, time
DEBUG=True
READ=0 #read
WRITE=1 #write
class Servo:
    def __init__(self, pin, board, number=0):
        self.pin = pin
        self.board=board
        self.number=number
    def attach(self):
        if self.board.sendCmd('S', 5, self.number, self.pin, 0)=='0':
            return True
        print "Servo: Error attach %s" % self.pin
        return False
    def detach(self):
        if self.board.sendCmd('S', 9, self.number, self.pin, 0)=='0':
            return True
        print "Servo: Error detach %s" % self.pin
        return False
    def attached(self):
        if self.board.sendCmd('S', 8, self.number, self.pin, 0)=='1':
            return True
        return False
    def write(self,value):
        if self.board.sendCmd('S', 6, self.number, self.pin, value)=='0':
            return True
        print "Servo: Error write %s = %s" % (self.pin,value)
        return False
    def read(self):
        return self.board.sendCmd('S', 7, self.number, self.pin, 0)

class Arduino(object):

    __OUTPUT_PINS = -1

    def __init__(self, port, baudrate=9600):
        self.serial = serial.Serial(port, baudrate)
        self.servo_index=0
        
    def init(self):
        self.sendCmd('I',)

    def __str__(self):
        return "Arduino is on port %s at %d baudrate" %(self.serial.port, self.serial.baudrate)

    def attachServo(self,pin):
        s = Servo(pin,self,self.servo_index)
        s.attach()
        self.servo_index+=1
        return s
        
    def getServo(self,pin,number):
        ''' number - index of serv[] array '''
        return Servo(pin,self,number)

    def output(self, pinArray):
        if(isinstance(pinArray, list) or isinstance(pinArray, tuple)):
            self.__OUTPUT_PINS = pinArray
            for each_pin in pinArray:
                self.sendCmd('C',each_pin,1)
        return True

    def setLow(self, pin):
        self.sendCmd('D',WRITE,pin,0)
        return True

    def setHigh(self, pin):
        self.sendCmd('D',WRITE,pin,1)
        return True

    def getState(self, pin):
        return self.__formatPinState(self.sendCmd('D',READ,pin))

    def analogWrite(self, pin, value):
        self.sendCmd('A',WRITE,pin,value)
        return True

    def analogRead(self, pin):
        return int(self.sendCmd('A',READ,pin))

    def turnOff(self):
        for each_pin in self.__OUTPUT_PINS:
            self.setLow(each_pin)
        return True

    def sendCmd(self, *args):
        line=""
        for arg in args:
            line = line + "%s " % arg
        #print "dbg:sendCmd('%s!')" % line
        self.serial.write("%s!" % line)
        self.serial.flush()
        res = self.serial.readline().replace("\r\n","")
        if DEBUG:
            print "dbg:sendCmd('%s!')=%s" % (line, res)
        return res

    def __formatPinState(self, pinValue):
        if pinValue=='1':
            return True
        else:
            return False

    def close(self):
        self.serial.close()
        return True

    def __del__(self):
        #close serial connection once program ends
        #this fixes the problem of port getting locked or unrecoverable in some linux systems
        self.serial.close()