""" Easy Interactive Camera-Projector Homography - Blackboard Example

Copyright (c) 2010, Nirav Patel <http://eclecti.cc>

Permission to use, copy, modify, and/or distribute this software for any
purpose with or without fee is hereby granted, provided that the above
copyright notice and this permission notice appear in all copies.

THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
"""

#!/usr/bin/env python

import sys
import numpy
import pygame
import pygame.camera
import pygame.gfxdraw
from pygame.locals import *

class Blackboard:
    def __init__(self, matrix):
        pygame.init()
        pygame.mouse.set_visible(False)
        display_resolutions = pygame.display.list_modes()
        display_res = display_resolutions[0]
        self.display = pygame.display.set_mode(display_res,pygame.FULLSCREEN)
        self.display.fill((0,0,0))
        pygame.display.flip()
        self.mat = matrix
        
        # start the camera and find its resolution
        pygame.camera.init()
        clist = pygame.camera.list_cameras()
        if len(clist) == 0:
            raise IOError('No cameras found.  The IRCamera class needs a camera supported by Pygame')
        self.resolution = (640,480)
        self.camera = pygame.camera.Camera(clist[0], self.resolution, "RGB")
        self.camera.start()
        # get the actual camera resolution
        self.resolution = self.camera.get_size()
        self.snapshot = pygame.surface.Surface(self.resolution, 0, self.display)
        self.white = pygame.surface.Surface(display_res,0,self.display)
        self.white.fill((1,1,1))
        self.last_point = None
        self.fade = self.fadestart = 2
        
    def points_from_blob(self, surface, thresh):
        # threshold the IR image into a binary image
        mask = pygame.mask.from_threshold(surface,(255,255,255),(thresh,thresh,thresh))
        # find the largest blob of light in it
        cc = mask.connected_component()
        count = cc.count()
        drawlist = []
        
        scaling = 2
        if count > 100:
            # turn the blob into a list of points
            plist = cc.outline(1)
            centroid = cc.centroid()
            c = numpy.array([centroid[0],centroid[1],1])
            c = numpy.dot(self.mat,c)
            for point in plist:
                p = numpy.array([point[0],point[1],1])
                # convert each point from camera to display coordinates
                p = numpy.dot(self.mat,p)
                # normalize it
                norm_point = (p[0]/p[2], p[1]/p[2])
                # scale it bigger or smaller based on the scaling factor
                scale_point = (norm_point[0]+(c[0]-norm_point[0])/scaling,
                               norm_point[1]+(c[1]-norm_point[1])/scaling)
                drawlist.append(scale_point)
              
        return drawlist
        
    def get_and_flip(self):
        # gradually fade the image back to black
        if self.fade <= 0:
            self.display.blit(self.white,(0,0),None,BLEND_SUB)
            self.fade = self.fadestart
        else:
            self.fade -= 1
    
        self.snapshot = self.camera.get_image(self.snapshot)
        for i in range(1,4):
            # see if we have an IR blob at varying thresholds of intensity
            drawlist = self.points_from_blob(self.snapshot, 255-i*30)
            if len(drawlist) > 2:
                # if we have a blob, draw it with varying alpha values
                pygame.gfxdraw.filled_polygon(self.display, drawlist, pygame.Color(255,0,0,i*10))

        pygame.display.flip()
            
    def run(self):
        going = True
        
        while going:
            self.get_and_flip()
            events = pygame.event.get()
            for e in events:
                if e.type == QUIT or (e.type == KEYDOWN and e.key == K_ESCAPE):
                    going = False

if __name__ == '__main__':
    matrix_file = 'homography.npy'
    if len(sys.argv) > 2:
        matrix_file = sys.argv[2]

    matrix = numpy.load(matrix_file)
    wb = Blackboard(matrix)
    wb.run()