# inter - Robot simulation interface. {{{
#
# Copyright (C) 2008 Nicolas Schodet
#
# APBTeam:
#        Web: http://apbteam.org/
#      Email: team AT apbteam DOT org
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 2 of the License, or
# (at your option) any later version.
#
# This program 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 General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
#
# }}}
"""Inter, coupled with a mex Node."""
from math import pi
import time

from inter import Inter, Obstacle
from dist_sensor import DistSensor
from path import Path
from Tkinter import *
from mex.node import Node
from mex.msg import Msg

import asserv
import io

class InterNode (Inter):
    """Inter, coupled with a mex Node."""

    # There is 900 tick per seconds, to permit AVR to have a 4.44444 ms
    # period.
    TICK = 900.0

    def __init__ (self):
        Inter.__init__ (self)
        self.node = Node ()
        self.asserv_link = asserv.Mex (self.node)
        self.asserv_link.position.register (self.notify_position)
        self.asserv_link.aux[0].register (self.notify_aux0)
        self.io_link = io.Mex (self.node)
        self.notify_jack ()
        self.notify_color_switch ()
        self.jackVar.trace_variable ('w',
                lambda *args: self.notify_jack ())
        self.colorVar.trace_variable ('w',
                lambda *args: self.notify_color_switch ())
        for i in range (len (self.io_link.servo)):
            self.io_link.servo[i].register (
                    lambda i = i: self.notify_servo (i))
        self.tk.createfilehandler (self.node, READABLE, self.read)
        self.date = 0
        self.synced = True
        self.step_after = None
        self.step_time = None
        self.obstacles = [ ]
        self.dist_sensors = [
                DistSensor (self.tableview.robot, (150, 127), 0, 800),
                DistSensor (self.tableview.robot, (150, 0), 0, 800),
                DistSensor (self.tableview.robot, (150, -127), 0, 800),
                DistSensor (self.tableview.robot, (-70, 100), pi, 800),
                DistSensor (self.tableview.robot, (-70, -100), pi, 800),
                ]
        for s in self.dist_sensors:
            s.obstacles = self.obstacles
            s.hide = True
            s.register (self.update_sharps)
        self.path = Path (self.tableview.table)
        self.io_link.path.register (self.notify_path)

    def createWidgets (self):
        Inter.createWidgets (self)
        self.nowLabel = Label (self.rightFrame, text = 'Now: 0 s')
        self.nowLabel.pack ()
        self.stepButton = Button (self.rightFrame, text = 'Step',
                command = self.step)
        self.stepButton.pack ()
        self.stepSizeScale = Scale (self.rightFrame, orient = HORIZONTAL,
                from_ = 0.05, to = 1.0, resolution = 0.05)
        self.stepSizeScale.pack ()
        self.playVar = IntVar ()
        self.playButton = Checkbutton (self.rightFrame, variable =
                self.playVar, text = 'Play', command = self.play)
        self.playButton.pack ()
        self.tableview.bind ('<2>', self.place_obstacle)
        self.showSensorsVar = IntVar ()
        self.showSensorsButton = Checkbutton (self.sensorFrame, variable =
                self.showSensorsVar, text = 'Show sensors', command =
                self.show_sensors)
        self.showSensorsButton.pack ()

    def step (self):
        """Do a step.  Signal to the Hub we are ready to wait to the next step
        date."""
        self.node.wait_async (self.date
                + int (self.stepSizeScale.get () * self.TICK))
        self.synced = False
        self.step_after = None
        self.step_time = time.time ()

    def play (self):
        """Activate auto-steping."""
        if self.playVar.get ():
            if self.step_after is None and self.synced:
                self.step ()
            self.stepButton.configure (state = DISABLED)
        else:
            if self.step_after is not None:
                self.after_cancel (self.step_after)
                self.step_after = None
            self.stepButton.configure (state = NORMAL)

    def read (self, file, mask):
        """Handle event on the Node."""
        self.node.read ()
        if not self.synced and self.node.sync ():
            self.synced = True
            self.date = self.node.date
            self.nowLabel.configure (text = 'Now: %.2f s' % (self.date
                / self.TICK))
            self.update ()
            if self.playVar.get ():
                assert self.step_after is None
                next = self.step_time + self.stepSizeScale.get ()
                delay = next - time.time ()
                if delay > 0:
                    self.step_after = self.after (int (delay * 1000),
                            self.step)
                else:
                    self.step ()

    def notify_position (self):
        self.tableview.robot.pos = self.asserv_link.position.pos
        self.tableview.robot.angle = self.asserv_link.position.angle
        self.tableview.robot.update ()

    def notify_aux0 (self):
        self.actuatorview.arm.angle = self.asserv_link.aux[0].angle
        self.actuatorview.arm.update ()

    def notify_jack (self):
        self.io_link.jack.state = self.jackVar.get ()
        self.io_link.jack.notify ()

    def notify_color_switch (self):
        self.io_link.color_switch.state = self.colorVar.get ()
        self.io_link.color_switch.notify ()

    def notify_servo (self, i):
        servo = self.io_link.servo[i]
        trap = self.actuatorview.rear.traps[i]
        trap.pos = servo.value
        trap.update ()

    def update_sharps (self):
        for ds, adc in zip (self.dist_sensors, self.io_link.adc):
            d = ds.distance or 800
            d /= 10
            if d > 10:
                v = 0.000571429 * d*d + -0.0752381 * d + 2.89107
            else:
                v = 2.2 / 10 * d
            v = int (round (v * 1024 / 5))
            assert v >= 0 and v < 1024
            adc.value = v
            adc.notify ()

    def notify_path (self):
        self.path.path = self.io_link.path.path
        self.path.update ()

    def place_obstacle (self, ev):
        pos = self.tableview.screen_coord ((ev.x, ev.y))
        if self.obstacles:
            self.obstacles[0].pos = pos
        else:
            self.obstacles.append (Obstacle (self.tableview.table, pos, 150))
        for d in self.obstacles + self.dist_sensors:
            d.update ()
        self.update ()

    def show_sensors (self):
        hide = not self.showSensorsVar.get ()
        for i in self.dist_sensors:
            i.hide = hide
            i.update ()
        self.update ()

if __name__ == '__main__':
    import mex.hub
    import utils.forked
    h = mex.hub.Hub (min_clients = 1)
    fh = utils.forked.Forked (h.wait)
    try:
        app = InterNode ()
        app.mainloop()
    finally:
        fh.kill ()
        import time
        time.sleep (1)
