###########################################################################
# cgkit - Python Computer Graphics Kit
# Copyright (C) 2004 Matthias Baas (baas@ira.uka.de)
#
# This library is free software; you can redistribute it and/or
# modify it under the terms of the GNU Lesser General Public
# License as published by the Free Software Foundation; either
# version 2.1 of the License, or (at your option) any later version.
#
# This library is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
# Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public
# License along with this library; if not, write to the Free Software
# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
#
# cgkit homepage: http://cgkit.sourceforge.net
###########################################################################
# $Id: flockofbirds.py,v 1.2 2005/07/07 08:24:57 mbaas Exp $

## \file flockofbirds.py
## Contains the FlockOfBirds class.

import types
import component
import eventmanager, events
from slots import *
from cgtypes import *
import fob

# FlockOfBirds
class FlockOfBirds(component.Component):
    """Receives tracker values from a Flock of Birds motion tracker.

    For each bird the component creates the following four slots:

    - \c pos<n>_slot (\c Vec3Slot) - Position
    - \c angle<n>_slot (\c Vec3Slot) - Euler angles
    - \c matrix<n>_slot (\c Mat3Slot) - Rotation matrix
    - \c quat<n>_slot (\c QuatSlot) - Quaternion

    where <n> is the number of the bird (1-based). Not all of the slots
    are active at the same time. You can select which slot should carry
    the corresponding value via the bird mode (see constructor).

    """

    def __init__(self,
                 name = "FlockOfBirds",
                 com_port = 0,
                 baud_rate = 115200,
                 timeout = 2.0,
                 num_birds = 1,
                 bird_mode = "p",
                 hemisphere = "forward",
                 auto_insert = True):
        """Constructor.

        The bird mode is one of "p" (position), "a" (angles), "m" (matrix),
        "q" (quaternion) or a combination like "pa", "pm" and "pq".
        The argument \a bird_mode can either be a mode string that will
        be used for all birds, or it can be a list of mode strings for
        every bird.

        \param name (\c str) Component name
        \param com_port (\c int) COM port to use for communicating with the flock (0,1,...)
        \param baud_rate (\c int) Baud rate
        \param timeout (\c float) Time out value in seconds for RS232 operations
        \param num_birds (\c int) Number of birds to use (including the ERC)
        \param bird_mode (\c str or \c list) Tracker mode (selects what kind of data the birds will send)
        \param hemisphere (\c str) Hemisphere setting for the transmitter ("forward", "aft", "upper", "lower", "left", "right")
        \param auto_insert (\c bool) True if the component should be inserted into the scene automatically
        """
        
        component.Component.__init__(self, name, auto_insert)

        self.com_port = com_port
        self.baud_rate = baud_rate
        self.timeout = timeout
        self.num_birds = num_birds
        self.hemisphere = hemisphere

        if isinstance(bird_mode, types.StringTypes):
            self.bird_mode = self.num_birds*[bird_mode]
        else:
            self.bird_mode = bird_mode

        self.bird_mode = map(lambda x: x.upper(), self.bird_mode)

        if len(self.bird_mode)!=self.num_birds:
            raise ValueError, "%d bird modes expected (got %d)"%(self.num_birds, len(self.bird_mode))

        # Create slots
        self._slots = []
        for i in range(self.num_birds):
            p = Vec3Slot()
            a = Vec3Slot()
            m = Mat3Slot(mat3(1))
            q = QuatSlot(quat(1))
            self._slots.append((p,a,m,q))
            setattr(self, "pos%d_slot"%(i+1), p)
            setattr(self, "angle%d_slot"%(i+1), a)
            setattr(self, "matrix%d_slot"%(i+1), m)
            setattr(self, "quat%d_slot"%(i+1), q)
            self.addSlot("pos%d"%(i+1), p)
            self.addSlot("angle%d"%(i+1), a)
            self.addSlot("matrix%d"%(i+1), m)
            self.addSlot("quat%d"%(i+1), q)

        # Initialize the FOB...
        
        self._fob = fob.FOB()
        self._fob.open(self.com_port, self.baud_rate, self.timeout)
        
        self._fob.fbbAutoConfig(numbirds=self.num_birds)
        self._fob.fbbReset()
        hs = { "forward":fob.FORWARD,
               "aft":fob.AFT,
               "upper":fob.UPPER,
               "lower":fob.LOWER,
               "left":fob.LEFT,
               "right":fob.RIGHT }
        for i in range(1, self.num_birds):
            self._fob.hemisphere(hs[self.hemisphere.lower()], addr=i+1)
            self.setTrackerMode(self.bird_mode[i], i+1)
        self._fob.run()
        
        eventmanager.eventManager().connect(events.STEP_FRAME, self)
        

    ## protected:

    def onStepFrame(self):
        # Poll values
        for i in range(1, self.num_birds):
            values = self._fob.point(i+1)
            if values==None:
                print "No values received from bird %d."%(i+1)                
            else:
                # 1. Convert values into proper types
                #    (position, angles, matrix, quat)
                # 2. Set the values on the corresponding slots
#                print values
                ps,as,ms,qs = self._slots[i]
                m = self.bird_mode[i]
                if m[0]=="P":
                    p = vec3(values[:3])
                    ps.setValue(p)
                c = m[-1]                    
                if c=="A":
                    a = vec3(values[-3:])
                    as.setValue(a)
                elif c=="M":
                    m = mat3(values[-9:])
                    ms.setValue(m)
                elif c=="Q":
                    q = quat(values[-4:])
                    qs.setValue(q)
                    


    # setTrackerMode
    def setTrackerMode(self, mode, addr):
        """Issue the appropriate fob command to set the tracker mode.

        \param mode (\c str) Tracker mode ("P", "A", "M", "Q", "PA", "PM", "PQ")
        \param addr (\c int) Bird address
        """
        if mode=="P":
            self._fob.position(addr)
        elif mode=="A":
            self._fob.angles(addr)
        elif mode=="M":
            self._fob.matrix(addr)
        elif mode=="Q":
            self._fob.quaternion(addr)
        elif mode=="PA":
            self._fob.position_angles(addr)            
        elif mode=="PM":
            self._fob.position_matrix(addr)
        elif mode=="PQ":
            self._fob.position_quaternion(addr)
        else:
            raise ValueError, "Invalid tracker mode: %s"%mode
        
