#!/usr/bin/python

# test the c328 camera using an FT232R breakout
# 10/2009 - http://jarv.org 

import sys
import serial
import time

def print_hex(hex_string):
        print "[",
        for c in hex_string:
                print "0x%02x" % ord(c),
        print "]",
        return

                 
def verify_msg(data,ser):
        ser_data = ser.read(3)
        if (ser_data != data):
                print_hex(ser_data)
                print "does not match",
                print_hex(data)
                print
                sys.exit(1)
 
        ser_data = ser_data + str(ser.read(3)) 
        return ser_data

def init():
        import optparse
        parser = optparse.OptionParser(
                usage = "%pog [options]",
                description = "Grab pictures from the C328-7640 module")
        parser.add_option("-p", "--port", dest="port", type="string",
                help="port that is used to connect to the serial device",
                default="/dev/ttyUSB1")
        parser.add_option("-b", "--baudrate", dest="baudrate", type="int",
                help="baudrate to use for the serial connection",
                default="57600")
        parser.add_option("-f", "--file", dest="file", type="str",
                help="Filename to write to",
                default="image.jpg")
        (options, args) = parser.parse_args()
       
       
        return options


MAX_ERRORS = 5
# get the port/baudrate from the command line
options = init()
port = options.port
baudrate = options.baudrate
filename = options.file
# connect to the serial port
ser = serial.Serial()
ser.port = port
ser.baudrate = baudrate
ser.timeout = 10  # 10 second timeout


try:
        ser.open()
except serial.SerialException, e:
        sys.stderr.write("Could not open serial port %s: %s\n" % (ser.portstr, e))
        sys.exit(1)
msg_sync = "\xAA\x0D\x00\x00\x00\x00"
ser.flushInput()
ser.flushOutput()


while not ser.inWaiting() == 12:
        ser.write(msg_sync); time.sleep(.1)
# verify ack and sync
ack = verify_msg("\xAA\x0E\x0D",ser)
sync = verify_msg("\xAA\x0D\x00",ser)

ser.write(ack)
time.sleep(.01)


print "Waiting 2 seconds"; time.sleep(2)
print "Initiating setup"

#Initial JPEG preview, VGA
ser.write("\xAA\x01\x00\x07\x00\x07");
ack = verify_msg("\xAA\x0E\x01",ser)
# Set Package Size (512 bytes)
ser.write("\xAA\x06\x08\x00\x02\x00");
ack = verify_msg("\xAA\x0E\x06",ser)
# Snapshot
ser.write("\xAA\x05\x00\x00\x00\x00");
ack = verify_msg("\xAA\x0E\x05", ser)
print "Requesting Picture"
# Get Picture
ser.write("\xAA\x04\x01\x00\x00\x00"); time.sleep(.01)
ack=verify_msg("\xAA\x0E\x04", ser)
data=verify_msg("\xAA\x0A\x01", ser)

size = data[3:6][::-1]  # last three bytes are the size
size_dec = int(size.encode("hex"), 16)


print "Requesting " + str(size_dec) + " Bytes"

# [ ID (2) ][ Data Size (2)][ Image Size (506) ][ Checksum (2) ]


got = 0
image = ''
i = 0
errors = 0

while (i < 65536):
        if (got >= size_dec): break
        lbyte = i >> 8
        hbyte = i & 0xff
        data = ''  # byte string for checksum
        request = "\xAA\x0E\x00\x00" + chr(hbyte) + chr(lbyte)
        ser.write(request)
        print_hex(request)
        # read the first 4 bytes which has the ID and payload size
        header=ser.read(4)
        data = data + header
        id_hex = header[0:2][::-1]
        id_dec = int(id_hex.encode("hex"),16)
        psize_hex = header[2:4][::-1]
        psize_dec = int(psize_hex.encode("hex"),16)
        # read the payload data
        payload = ser.read(psize_dec)
        data = data + payload
        image = image + payload
        got = got + psize_dec
        # read the checksum
        csdata = ser.read(2)
        cs_hex = csdata[0:1].encode("hex")
        cs_dec = int(cs_hex,16)


        # calculate the checksum
        cs_calc = 0
        for c in data:
                cs_calc = cs_calc + int(c.encode("hex"), 16)
        cs_calc = cs_calc & 0xff

        print "ID = " + str(id_dec) + " Size = " + str(psize_dec),
        print  "CS = 0x" + str(cs_hex),
        print  "CS_CALC = " + hex(cs_calc),
        print  " | " + str(got) + " bytes read"
        if (errors > MAX_ERRORS): break
        if (cs_dec != cs_calc):
               print "Checksum failure! retrying"
               errors = errors + 1
        else:
                i=i+1
 
       
if not (got == size_dec):
        print "ERROR: invalid payload"
        sys.exit(1)
       
print "Done requesting data"
ser.write("\xAA\x0E\x00\x00\xF0\xF0")

FILE =  open(filename, 'wb')
FILE.write(image)
FILE.close
print "Waiting 5 seconds"
time.sleep(5)