import Tkinter, tkFileDialog
import math
# create a shorthand object for Tkinter so we don't have to type it
# all the time
tk = Tkinter


# create a class to build and manage the display
class TrackingApp:

    def __init__(self, width, height):

        # create a tk object, which is the root window
        self.root = tk.Tk()

        # width and height of the window
        self.initDx = width
        self.initDy = height

        # set up the geometry for the window
        self.root.geometry( "%dx%d+50+30" % (self.initDx, self.initDy) )

        # set the title of the window
        self.root.title("Colorful Dots")

        # set the maximum size of the window for resizing
        self.root.maxsize( 1024, 768 )

        # bring the window to the front
        self.root.lift()

        # setup the menus
        self.buildMenus()

        # build the objects on the Canvas
        self.buildCanvas()

        # set up the key bindings
        self.setBindings()

        # robot position as x, y, theta, object
        self.robot = [0, 0, 0, None]

        # robot velocity as (x, y) or (v, w)
        self.velocity = [0, 0, 0]

        # robot size
        self.radius = 10

        self.actionState = 'Idle'

        self.initRobot()

    def buildMenus(self):
        
        # create a new menu
        self.menu = tk.Menu(self.root)

        # set the root menu to our new menu
        self.root.config(menu = self.menu)

        # create a variable to hold the individual menus
        self.menulist = []

        # create a file menu
        filemenu = tk.Menu( self.menu )
        self.menu.add_cascade( label = "File", menu = filemenu )
        self.menulist.append(filemenu)

        # create another menu for data management
        editmenu = tk.Menu( self.menu )
        self.menu.add_cascade( label = "Edit", menu = editmenu )
        self.menulist.append(editmenu)

        # create another menu for specific control
        controlmenu = tk.Menu( self.menu )
        self.menu.add_cascade( label = "Control", menu = controlmenu )
        self.menulist.append(controlmenu)

        # create another menu for modes
        modemenu = tk.Menu( self.menu )
        self.menu.add_cascade( label = "Mode", menu = modemenu )
        self.menulist.append(modemenu)

        # menu text for the elements
        menutext = [ [ 'Quit  \xE2\x8C\x98-Q' ],
                     [ 'Cut', 'Copy', 'Paste' ],
                     [ 'Reset Robot' ],
                     [ 'Linear Achieve', 'Smooth Achieve', 'Pursuit' ] ]

        # menu callback functions
        menucmd = [ [ self.handleQuit ],
                    [ None, None, None ],
                    [ self.handleReset ],
                    [ self.handleLinearAchieve, self.handleSmoothAchieve, self.handlePursuit ] ]
        
        # build the menu elements and callbacks
        for i in range( len( self.menulist ) ):
            for j in range( len( menutext[i]) ):
                if menutext[i][j] != '-':
                    self.menulist[i].add_command( label = menutext[i][j], command=menucmd[i][j] )
                else:
                    self.menulist[i].add_separator()

    def buildCanvas(self):
        self.canvas = tk.Canvas( self.root, width=self.initDx, height=self.initDy )
        self.canvas.pack( expand=tk.YES, fill=tk.BOTH )

    def setBindings(self):
        self.root.bind( '<Button-1>', self.handleButton1 )
        self.root.bind( '<B1-Motion>', self.handleButton1Motion )

        self.root.bind( '<Command-q>', self.handleModQ )

        self.root.bind( '<Configure>', self.handleConfigure )

    def initRobot(self):
        self.robot[0] = self.initDx/2
        self.robot[1] = self.initDy/2
        self.robot[2] = 0

        self.velocity[0] = 0 # x
        self.velocity[1] = 0 # y
        self.velocity[2] = 0 # w

        self.robot[-1] = [ self.canvas.create_oval( self.robot[0] - self.radius,
                                                    self.robot[1] - self.radius,
                                                    self.robot[0] + self.radius,
                                                    self.robot[1] + self.radius ),
                           self.canvas.create_line( self.robot[0],
                                                    self.robot[1],
                                                    self.robot[0] + self.radius,
                                                    self.robot[1] ) ]

    def updateRobot(self):
        
        # update based on current velocities
        self.robot[0] += self.velocity[0]
        self.robot[1] += self.velocity[1]
        self.robot[2] += self.velocity[2]

        while self.robot[2] > math.pi:
            self.robot[2] -= 2.0 * math.pi
        while self.robot[2] < -math.pi:
            self.robot[2] += 2.0 * math.pi

        # update the body on the screen
        self.canvas.coords( self.robot[-1][0],
                            int(self.robot[0] - self.radius), 
                            int(self.robot[1] - self.radius), 
                            int(self.robot[0] + self.radius), 
                            int(self.robot[1] + self.radius) )

        # orientation
        self.canvas.coords( self.robot[-1][1],
                            int(self.robot[0]),
                            int(self.robot[1]),
                            int(self.robot[0] + self.radius*math.cos(self.robot[2]) ),
                            int(self.robot[1] - self.radius*math.sin(self.robot[2]) ) )

    def handleQuit(self):
        print 'Terminating'
        self.root.destroy()

    def handleModQ(self, event):
        self.handleQuit()

    # handle changes to the window size
    def handleConfigure(self, event):

        self.initDx = event.width
        self.initDy = event.height

    def handleButton1(self, event):
        print 'handling button 1'
        if self.actionState == 'Idle':
            return
        elif self.actionState == 'LinearAchieve':
            print 'starting linear achieve'
            self.baseClick = ( event.x, event.y )
            self.root.after( 100, self.execLinearAchieve )
        elif self.actionState == 'SmoothAchieve':
            print 'starting smooth achieve'
            self.baseClick = (event.x, event.y, math.atan2( -(event.y-self.robot[1]), event.x - self.robot[0] ) )
            self.root.after( 100, self.execSmoothAchieve )

    def handleButton1Motion(self, event):
        print 'handling button 1 motion'

    def execLinearAchieve( self ):
        print 'exec linear achieve'

        self.updateRobot()

        # calculate the new velocity
        deltax = self.baseClick[0] - (self.robot[0])
        deltay = self.baseClick[1] - (self.robot[1])

        if -2 < deltax < 2 and -2 < deltay < 2:
            print 'state reached'
            self.velocity[0] = 0.0
            self.velocity[1] = 0.0
            return

        maxvel = 100
        P = 1.5
        
        self.velocity[0] = deltax * P
        self.velocity[1] = deltay * P
        
        # proportional scaling if the velocities are above the maximum permissible
        if math.fabs(self.velocity[0]) > maxvel and math.fabs(self.velocity[0]) > math.fabs(self.velocity[1]):
            self.velocity[1] *= maxvel / math.fabs(self.velocity[0])
            if self.velocity[0] > 0:
                self.velocity[0] = maxvel;
            else:
                self.velocity[0] = -maxvel;

        if math.fabs(self.velocity[1]) > maxvel and math.fabs(self.velocity[1]) > math.fabs(self.velocity[0]):
            self.velocity[0] *= maxvel / math.fabs(self.velocity[1])
            if self.velocity[1] > 0:
                self.velocity[1] = maxvel;
            else:
                self.velocity[1] = -maxvel;

        for i in range(2):
            # need some minimum velocity if it's not zero
            if -1 < self.velocity[i] < 0:
                self.velocity[i] = -1
            if 0 < self.velocity[i] < 1:
                self.velocity[i] = 1


        self.root.after( 50, self.execLinearAchieve )

    def execSmoothAchieve( self ):
        print 'executing smooth achieve'
        
        # update the robot
        self.updateRobot()

        # test if we're done
        deltax = self.baseClick[0] - self.robot[0]
        deltay = self.baseClick[1] - self.robot[1]
        deltaw = 0 - self.robot[2]

        while deltaw > math.pi:
            deltaw -= 2.0 * math.pi
        while deltaw < -math.pi:
            deltaw += 2.0 * math.pi

        print "(deltax, deltay, deltaw) = %.2f %.2f %.2f" % (deltax, deltay, deltaw )

        if -2 < deltax < 2 and -2 < deltay < 2 and -0.1 < deltaw < 0.1:
            print 'state reached'
            self.velocity[0] = 0.0
            self.velocity[1] = 0.0
            return

        # calculate the new rho, alpha, and beta
        rho = math.sqrt( deltax * deltax + deltay * deltay )
        beta = math.atan2( -deltay, deltax )
        alpha = beta - self.robot[2]

        # make beta relative to goal orientation
        # beta -=  self.robot[2] 

        print "(rho, beta, alpha) = %.2f %.2f %.2f" % (rho, beta, alpha)
        

        # bound alpha and beta
        while alpha > math.pi:
            alpha -= 2.0 * math.pi
        while alpha < -math.pi:
            alpha += 2.0 * math.pi

        while beta > math.pi:
            beta -= 2.0 * math.pi
        while beta < -math.pi:
            beta += 2.0 * math.pi

        kp = 0.2
        ka = 2.0
        kb = 0.5

        # control law
        #rhop = -kp * rho * math.cos( alpha )
        #alphap = kp * math.sin( alpha ) - ka * alpha - kb * beta
        #betap = -kp * math.sin( alpha )

        # convert to v, w
        v = kp * rho
        w = ka * alpha + kb * beta

        # bound velocities
        if v > 50:
            v = 50.0
        if v < -50:
            v = -50.0

        if w > 0.5:
            w = 0.5
        if w < -0.5:
            w = -0.5

        # convert to x, y, w
        self.velocity[0] = v * math.cos( self.robot[2] )
        self.velocity[1] = -v * math.sin( self.robot[2] )
        self.velocity[2] = w


        print "velocity (%.2f %.2f %.2f)" % (self.velocity[0], self.velocity[1], self.velocity[2] )

        self.root.after( 50, self.execSmoothAchieve )


    def handleReset(self):
        print 'handling reset'
        self.actionState = 'Idle'
        self.robot[0] = self.initDx/2
        self.robot[1] = self.initDy/2
        self.robot[2] = 0
        self.velocity[0] = 0 # x ?
        self.velocity[1] = 0 # y ?
        self.velocity[2] = 0 # w ?
        self.canvas.coords( self.robot[-1][0], 
                            self.robot[0] - 10,
                            self.robot[1] - 10,
                            self.robot[0] + 10,
                            self.robot[1] + 10 )
        self.canvas.coords( self.robot[-1][1],
                            self.robot[0],
                            self.robot[1],
                            self.robot[0],
                            self.robot[1] - 10 )

    def handleLinearAchieve(self):
        print 'handling linear achieve'
        self.actionState = 'LinearAchieve'

    def handleSmoothAchieve(self):
        print 'handling smooth achieve'
        self.actionState = 'SmoothAchieve'

    def handlePursuit(self):
        print 'handling pursuit'

    def main(self):
        print 'Entering main loop'
        self.root.mainloop()

if __name__ == "__main__":
    dapp = TrackingApp(500, 500)
    dapp.main()