from tkinter import *
from tkinter import messagebox
from tkinter import simpledialog
from tkinter import filedialog
import time
import random
import sys
import math

tk = Tk()
msg = messagebox
sdg = simpledialog

#Global var1iables
bgColor = "grey"
canvasW= 1235 #1588 #1244
canvasH = 600 #762 #600
defaultRobotColor = "#ffa500"
doneExecutingRobotColor = "#0000ff"
frontClawColor = "green"
backClawColor = "red"
direction = "forward"
lineSquaring = False

#Create canvas
tk.geometry(str(canvasW) + "x" + str(canvasH) + "+0+0")
tk.title("First Mission Mapper")
canvas = Canvas(tk, width=canvasW, height=canvasH, bg=bgColor)
canvas.pack()

photo = PhotoImage(file="img/map.gif")
bgImage = canvas.create_image(0, 0, anchor=NW, image=photo)

# Robot class
class Robot:
    def __init__(self, x, y):
        self.color = defaultRobotColor
        self.angle = 0
        self.vertices = [
            [x, y],
            [x+50, y+25],
            [x, y+50],
            [x+25, y+25]
        ]
        self.robot = canvas.create_polygon(self.vertices, fill=self.color)
    def move(self, forward):
        cosineX = math.cos(math.radians(self.angle))
        cosineY = math.cos(math.radians(90-self.angle))
        newX = cosineX
        newY = cosineY
        if not forward:
            newX *= -1
            newY *= -1
        canvas.move(self.robot, newX, newY)
        for point in self.vertices:
            point[0] += newX
            point[1] += newY
    def rotateCalculate(self, angle):
        angle = self.angle+angle
        self.angle = angle
        angle = math.radians(angle)
        cos_val = math.cos(angle)
        sin_val = math.sin(angle)
        center = [self.vertices[3][0], self.vertices[3][1]]
        cx, cy = center
        new_points = []
        for x_old, y_old in self.vertices:
            x_old -= cx
            y_old -= cy
            x_new = x_old * cos_val - y_old * sin_val
            y_new = x_old * sin_val + y_old * cos_val
            new_points.append([x_new + cx, y_new + cy])
        return new_points
    def rotate(self, angle):
        canvas.delete(self.robot)
        new_points = self.rotateCalculate(angle)
        self.robot = canvas.create_polygon(new_points, fill=self.color)
    def changeState(self, color):
        canvas.itemconfig(self.robot, fill=color)
        self.color = color

# Set robot
robot = Robot(50, 475)

# Set memory
pathObjects = []
toDo = [None]
checkPoints = [[robot.vertices[3][0], robot.vertices[3][1]]]
frontClawValues = [0]
backClawValues = [0]

directory = None

# Open allready made mission for editing
def openMission(event):
    global directory
    
    # Choose mission
    directory = filedialog.askdirectory()
    
    # Open files
    toDoFile = open(directory + "/txt/toDo.txt","r")
    xCoordsFile = open(directory + "/txt/xCoords.txt","r")
    yCoordsFile = open(directory + "/txt/yCoords.txt","r")
    frontClawFile = open(directory + "/txt/frontClaw.txt","r")
    backClawFile = open(directory + "/txt/backClaw.txt","r")

    # Store lines
    toDo = toDoFile.readlines()
    xCoords = xCoordsFile.readlines()
    yCoords = yCoordsFile.readlines()
    frontClaw = frontClawFile.readlines()
    backClaw = backClawFile.readlines()

    # Set pointer for files
    checkPointNumber = 0
    frontClawValueNumber = 0
    backClawValueNumber = 0

    # Generate mission
    for command in toDo:
        command = float(command)
        if command == 1:
            direction = "forward"
            addCheckPoint(int(xCoords[checkPointNumber]), int(yCoords[checkPointNumber]), "forward")
            checkPointNumber += 1
        elif command == 2:
            direction = "backward"
            addCheckPoint(int(xCoords[checkPointNumber]), int(yCoords[checkPointNumber]), "backward")
            checkPointNumber += 1
        elif command == 3:
            addFrontClaw(str(int(frontClaw[frontClawValueNumber])))
            frontClawValueNumber += 1
        elif command == 4:
            addBackClaw(str(int(backClaw[backClawValueNumber])))
            backClawValueNumber += 1
        elif command == 5:
            direction = "forward"
            addLineSquaring(direction)
            addCheckPoint(int(xCoords[checkPointNumber]), int(yCoords[checkPointNumber]), "forward")
            checkPointNumber += 1
        elif command == 6:
            direction = "backward"
            addLineSquaring(direction)
            addCheckPoint(int(xCoords[checkPointNumber]), int(yCoords[checkPointNumber]), "backward")
            checkPointNumber += 1
        tk.update()

    # Close files
    toDoFile.close()
    xCoordsFile.close()
    yCoordsFile.close()
    frontClawFile.close()
    backClawFile.close()

def save(event):
    global directory
    
    # Open files
    if directory == None:
        directory = filedialog.askdirectory()
    toDoFileTxt = open(directory + "/txt/toDo.txt","w")
    xCoordsFileTxt = open(directory + "/txt/xCoords.txt","w")
    yCoordsFileTxt = open(directory + "/txt/yCoords.txt","w")
    frontClawFileTxt = open(directory + "/txt/frontClaw.txt","w")
    backClawFileTxt = open(directory + "/txt/backClaw.txt","w")
    toDoFileRtf = open(directory + "/rtf/toDo.rtf","w")
    xCoordsFileRtf = open(directory + "/rtf/xCoords.rtf","w")
    yCoordsFileRtf = open(directory + "/rtf/yCoords.rtf","w")
    frontClawFileRtf = open(directory + "/rtf/frontClaw.rtf","w")
    backClawFileRtf = open(directory + "/rtf/backClaw.rtf","w")
    
    # Set pointer for files
    checkPointNumber = 1
    frontClawValueNumber = 1
    backClawValueNumber = 1

    # Read commands
    for command in toDo:
        toDoFileTxt.write(str(command) + "\n")
        toDoFileRtf.write("{:.4f}".format(command)+chr(13))

        # Turn and move
        if command == 1 or command == 2 or command == 5 or command == 6:
            checkPoint = checkPoints[checkPointNumber]
            mouseX = checkPoint[0]
            mouseY = checkPoint[1]
            xCoordsFileTxt.write(str(mouseX) + "\n")
            yCoordsFileTxt.write(str(mouseY) + "\n")
            xCoordsFileRtf.write("{:.4f}".format(mouseX)+chr(13))
            yCoordsFileRtf.write("{:.4f}".format(mouseY)+chr(13))
            checkPointNumber += 1

        # Front Claw
        elif command == 3:
            frontClawValue = int(frontClawValues[frontClawValueNumber])
            frontClawFileTxt.write(str(frontClawValue) + "\n")
            frontClawFileRtf.write("{:.4f}".format(frontClawValue)+chr(13))
            frontClawValueNumber += 1

        # Back Claw
        elif command == 4:
            backClawValue = int(backClawValues[backClawValueNumber])
            backClawFileTxt.write(str(backClawValue) + "\n")
            backClawFileRtf.write("{:.4f}".format(backClawValue)+chr(13))
            backClawValueNumber += 1

    # Write end of file character for rtf files
    toDoFileRtf.write("{:.4f}".format(0)+chr(13))
    xCoordsFileRtf.write("{:.4f}".format(0)+chr(13))
    yCoordsFileRtf.write("{:.4f}".format(0)+chr(13))
    frontClawFileRtf.write("{:.4f}".format(10000)+chr(13))
    backClawFileRtf.write("{:.4f}".format(10000)+chr(13))

    # Close files
    toDoFileTxt.close()
    xCoordsFileTxt.close()
    yCoordsFileTxt.close()
    frontClawFileTxt.close()
    backClawFileTxt.close()
    toDoFileRtf.close()
    xCoordsFileRtf.close()
    yCoordsFileRtf.close()
    frontClawFileRtf.close()
    backClawFileRtf.close()

    robot.changeState(doneExecutingRobotColor)
    tk.update()
    time.sleep(1)
    robot.changeState(defaultRobotColor)

def addCheckPointEvent(event):
    global direction
    addCheckPoint(event.x, event.y, direction)

def addCheckPoint(mouseX, mouseY, direction):
    global lineSquaring
    oldX = checkPoints[-1][0]
    oldY = checkPoints[-1][1]
    if direction == "forward":
        if lineSquaring == True:
            pathObjects[-1].append(canvas.create_line(oldX, oldY, mouseX, mouseY, width=5, fill="green", dash=(5, 5), arrow="last"))
            canvas.tag_lower(pathObjects[-1][-1])
            lineSquaring = False
        else:
            pathObjects.append([canvas.create_line(oldX, oldY, mouseX, mouseY, width=5, fill="green", arrow="last")])
            canvas.tag_lower(pathObjects[-1][0])
            toDo.append(1)
    else:
        if lineSquaring == True:
            pathObjects[-1].append(canvas.create_line(oldX, oldY, mouseX, mouseY, width=5, fill="red", dash=(5, 5), arrow="last"))
            canvas.tag_lower(pathObjects[-1][-1])
            lineSquaring = False
        else:
            pathObjects.append([canvas.create_line(oldX, oldY, mouseX, mouseY, width=5, fill="red", arrow="last")])
            canvas.tag_lower(pathObjects[-1][0])
            toDo.append(2)
    canvas.tag_lower(bgImage)
    checkPoints.append([mouseX, mouseY])

def addFrontClawEvent(event):
    addFrontClaw(simpledialog.askstring("Front Claw", "Enter value:"))

def addFrontClaw(value):
    oldX = checkPoints[-1][0]
    oldY = checkPoints[-1][1]
    toDo.append(3)
    pathObjects.append([canvas.create_oval(oldX-15, oldY-15, oldX+15, oldY+15, fill=frontClawColor, outline=""), canvas.create_text(oldX, oldY, text=value, fill="white")])
    frontClawValues.append(value)

def addBackClawEvent(event):
    addBackClaw(simpledialog.askstring("Back Claw", "Enter value:"))

def addBackClaw(value):
    oldX = checkPoints[-1][0]
    oldY = checkPoints[-1][1]
    toDo.append(4)
    pathObjects.append([canvas.create_oval(oldX-15, oldY-15, oldX+15, oldY+15, fill=backClawColor, outline=""), canvas.create_text(oldX, oldY, text=value, fill="white")])
    backClawValues.append(value)

def addLineSquaringEvent(event):
    global direction
    addLineSquaring(direction)

def addLineSquaring(direction):
    global lineSquaring
    oldX = checkPoints[-1][0]
    oldY = checkPoints[-1][1]
    lineSquaring = True
    if direction == "forward":
        pathObjects.append([canvas.create_oval(oldX-15, oldY-15, oldX+15, oldY+15, fill="gray"), canvas.create_oval(oldX-6, oldY-12, oldX+6, oldY, fill="green"), canvas.create_oval(oldX-3, oldY+4, oldX+3, oldY+10, fill="green")])
        toDo.append(5)
    else:
        pathObjects.append([canvas.create_oval(oldX-15, oldY-15, oldX+15, oldY+15, fill="gray"), canvas.create_oval(oldX-6, oldY-12, oldX+6, oldY, fill="red"), canvas.create_oval(oldX-3, oldY+4, oldX+3, oldY+10, fill="red")])
        toDo.append(6)

def delete(event):
    if toDo == []:
        return
    if toDo[-1] == 1 or toDo[-1] == 2 or toDo[-1] == 5 or toDo[-1] == 6:
        checkPoints.pop()
    elif toDo[-1] == 3:
        frontClawValues.pop()
    elif toDo[-1] == 4:
        backClawValues.pop()
    for index in range(len(pathObjects[-1])):
        canvas.delete(pathObjects[-1][index])
    pathObjects.pop()
    toDo.pop()
    tk.update()

previewLine = canvas.create_line(0, 0, 0, 0)

def previewPath(event):
    global previewLine, direction, toDo
    canvas.delete(previewLine)
    oldX = checkPoints[-1][0]
    oldY = checkPoints[-1][1]
    if direction == "forward":
        previewLine = canvas.create_line(oldX, oldY, event.x, event.y, width=75, fill="green", smooth=True)
    else:
        previewLine = canvas.create_line(oldX, oldY, event.x, event.y, width=75, fill="red", smooth=True)
    if toDo == []:
        toDo = [None]
    elif toDo[0] == None:
        toDo.pop(0)

def forward(event):
    global direction
    direction = "forward"

def backward(event):
    global direction
    direction = "backward"

tk.bind("<Command-o>", openMission)

tk.bind("<Up>", forward)
tk.bind("<Down>", backward)
tk.bind("f", addFrontClawEvent)
tk.bind("b", addBackClawEvent)
tk.bind("l", addLineSquaringEvent)

tk.bind("<Command-s>", save)

tk.bind("<BackSpace>", delete)
canvas.bind("<Button-1>", addCheckPointEvent)
canvas.bind("<Motion>", previewPath)
