#!/usr/bin/python
"""
Author: Jeremy M. Stober
Program: camera.py
Date: Monday, April  9 2007
Description: A simple roving eye robot.
"""

import os, sys, getopt, pdb, string
import Image, ImageChops, ImageFilter
from random import gauss
import functional
from numpy import *
import sensors

class Camera:

    def __init__(self, im, size = 80, x = 0, y = 0, theta = 0, cfilter = functional.identity):
        self.x = x
        self.y = y
        self.theta = 0
        self.im = im
        self.size = size
        self.offset = self.size / 2
        self.history = [(x,y,theta)]
        self.center = array(im.size) / 2
        self.xbound = self.center[0] + self.size
        self.ybound = self.center[1] + self.size

        # add a filter if we want to degrade snapshots in any way
        self.filter = cfilter

        # since we translate and rotate the image - the camera box never changes
        self.camerabox = (self.center[0] - self.offset,
                          self.center[1] - self.offset,
                          self.center[0] + self.offset,
                          self.center[1] + self.offset)

    def addfilter(self, cfilter):
        self.filter = cfilter

    def move(self, command):
        if self.checkbounds(command):
            self.history.append(command)
            self.x = self.x + command[0]
            self.y = self.y + command[1]
            self.theta = self.theta + command[2]
            return 1
        else:
            return 0 # stall

    def checkbounds(self, command):
        """ if false camera is out of bounds """
        """ make sure self.x and self.y are at least size pixels away from image border """
        return abs(self.x + command[0]) < self.xbound and abs(self.y + command[1]) < self.ybound

    def snap(self):
        nim = ImageChops.offset(self.im, self.x, self.y).rotate(self.theta)
        snap = nim.crop(self.camerabox)
        return self.filter(snap)

def blur(image):
    """ Another Simple Feature """
    return image.filter(ImageFilter.BLUR)

def blank(image):
    """ A particularly simple filter. """
    return Image.new(image.mode, image.size)

def randomMotion():
    xmotion = gauss(0,3)
    ymotion = gauss(0,3)
    theta = gauss(0,3)
    return (int(xmotion), int(ymotion), int(theta))

def record(snap, log):
    binary = snap.convert('1')
    log.write(str.join(" ", [str(i) for i in binary.getdata()]))
    log.write("\n")

def runcamera(image, size, thumb, commands, directory):
    im = Image.open(image)
    camera = Camera(im, size = size)

    text = ["p%d" % i for i in range(thumb ** 2)]
    text += ["x","y","thr"]
    logfile_thumb = sensors.header(text, "%s/camera_small.dat" % directory)

    text = ["p%d" % i for i in range(size ** 2)]
    text += ["x","y","thr"]
    logfile_large = sensors.header(text, "%s/camera_large.dat" % directory)

    x,y,theta = (0,0,0)

    step = 0

    for command in commands:
        print command
        (x,y,theta,nsteps) = command

        for i in range(nsteps):
            if not camera.move((x,y,theta)):
                print "Stall!"
            step += 1

            img = camera.snap()
            img.save("%s/%.5d.jpg" % (directory, step))

            # record large image trace
            trace = list(img.getdata())
            trace += [x,y,theta]
            sensors.record(trace,logfile_large)

            # record small image trace
            img.thumbnail((thumb,thumb))
            trace = list(img.getdata())
            trace += [x,y,theta]
            sensors.record(trace, logfile_thumb)

    logfile_thumb.close()
    logfile_large.close()


def randomcamera(image, size, thumb, iterations, directory):
    im = Image.open(image)
    camera = Camera(im, size = size)

    text = ["p%d" % i for i in range(thumb ** 2)]
    text += ["x","y","thr"]
    logfile_thumb = sensors.header(text, "%s/camera_small.dat" % directory)

    text = ["p%d" % i for i in range(size ** 2)]
    text += ["x","y","thr"]
    logfile_large = sensors.header(text, "%s/camera_large.dat" % directory)

    x,y,theta = (0,0,0)

    for i in range(iterations):
        if i % 10 == 0:
            (x,y,theta) = randomMotion()

        if not camera.move((x,y,theta)):
            print "Stall!"

        img = camera.snap()
        img.save("%s/%.5d.jpg" % (directory, i))

        # record large image trace
        trace = list(img.getdata())
        trace += [x,y,theta]
        sensors.record(trace,logfile_large)

        # record small image trace
        img.thumbnail((thumb,thumb))
        trace = list(img.getdata())
        trace += [x,y,theta]
        sensors.record(trace, logfile_thumb)

    logfile_thumb.close()
    logfile_large.close()


def main():

    options = ['help', 'image=', 'directory=', 'time=', 'size=', 'thumb=', 'commands=']
    def usage():
	print sys.argv[0] + " Options: " + string.join(options," ")

    try:
        (options, args) = getopt.getopt(sys.argv[1:], '', options)
    except getopt.GetoptError:
        # print help information and exit:
        usage()
        sys.exit(2)

    image = None
    directory = None
    time = None
    size = None
    thumb = None
    commands = None
    for o, a in options:
        if o in ('--help'):
            usage()
            sys.exit()
        elif o in ('--size'):
            size = int(a)
        elif o in ('--image'):
            image = a
        elif o in ('--directory'):
            directory = a
        elif o in ('--time'):
            time = int(a)
        elif o in ('--commands'):
            fd = open(a)
            commands = [[int(num) for num in line.split()] for line in fd.readlines()]
        elif o in ('--thumb'):
            thumb = int(a)

    
    if time:
        randomcamera(image, size, thumb, time, directory)
    elif commands:
        runcamera(image,size,thumb, commands, directory)


if __name__ == "__main__":
    main()