#!/usr/bin/env python2
import time
import serial
import sys
import random

verboseLevel = 1

def int2bin (n, count=16):
    """returns the binary of integer n, using count number of digits"""
    return "".join([str((n >> y) & 1) for y in range(count-1, -1, -1)])

class Sensors:
    SERIAL_TIMEOUT_SEC = 0.5
    SERIAL_PORT = "/dev/ttyACM0"
    SERIAL_BAUD_RATE = 115200
    # Commands
    CMD_ALL_SENSORS = 'A'

    ANSWER_OK       = 'OK'
    ANSWER_ERROR    = 'ERROR'

    def __init__ (self, serial_=None, debugLevel=0):
        self.debugLevel = debugLevel
        if self.debugLevel >= 100:
            print "Sensors.__init__"
        self.serial = serial_
        self.ok = False
        if self.serial is None:
            if self.debugLevel >= 20:
                print "Opening serial port %s... " % (self.SERIAL_PORT),
            try:
                self.serial = serial.Serial (self.SERIAL_PORT, self.SERIAL_BAUD_RATE, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE, self.SERIAL_TIMEOUT_SEC)
                #self.serial.open ()
            except serial.serialutil.SerialException, errStr:
                print errStr
            else:
                self.ok = True
                if self.debugLevel >= 20:
                    print "Ok."
        else:
            # TODO exception handling
            self.serial.open ()

    def __del__ (self):
        if self.debugLevel >= 100:
            print "Sensors.__del__"
        if self.serial is not None and self.serial.isOpen ():
            if self.debugLevel >= 20:
                print "Closing serial port..."
            self.serial.close ()

    def isOk (self):
        return self.ok
    
    # Transmit command and receives answer.
    # @return true: If answer was OK, otherwise false.
    def getValues (self):
        command = self.CMD_ALL_SENSORS
        if self.debugLevel >= 75:
            print "Command: %s" % (command)
        answer = []
        if self.serial is not None and self.serial.isOpen ():
            self.serial.write (command + '\n')
            end = False
            i = 0
            while not end:
                # Read line and remove whitespaces from the end
                a = self.serial.readline ().rstrip ()
                if a != self.ANSWER_OK and a != self.ANSWER_ERROR:
                    answer.append (a)
                else:
                    end = True
                i += 1
                if i > 20:
                    end = True
        else:
            # TODO throw exception
            if self.debugLevel >= 5:
                print "Sensors.sendCommand: throw exception"
        if self.debugLevel >= 75:
            print "Answer: %s" % (answer)
    	return answer[1:]

# Append lines to a file
def writeFile (outfn, outfilecontent):
    if verboseLevel >= 20:
        print ""
        print "Writing to file", outfn, "..."
    try:
        outfile = open (outfn, "a")
        for l in outfilecontent:
            outfile.write (l)
        if verboseLevel >= 20:
            print len (outfilecontent), "lines were written."
        outfile.close ()
    except IOError, errmsg:
        print "Cannot open file", outfn, "Error:", errmsg
    except:
        print "Unexpected error:", sys.exc_info()[0]
        raise


if __name__ == "__main__":
    #print "Opening sensors port... ",
    sensors = Sensors (None, 15)
    if not sensors.isOk ():
        # Error text has already printed
        #print "ERROR: Cannot initialize debug port!"
        sys.exit (1)
    #print "Ok."
    timestamp = time.strftime ("%Y-%m-%d %H:%M:%S", time.localtime ())
    line = timestamp + " " + " ".join (sensors.getValues ()) + "\n"
    writeFile ("/home/ivanovp/sensors.log", line)

# vim:set sw=4: