from Robot import *
from Path import *
from math import *
import json

robot = Robot()

# load a path file
p = Path("Path-around-table-and-back.json")
path = p.getPath()

newPath = []

#skapar en lista med heltal
for i in range(len(path)):
    newPath.append(i)

#stor forloop som räknar alla koordinater
for i in newPath[::4]:

    increaseSpeed = 0

    robotRunning = True

    while robotRunning is True:

        #koordinats position, lista av dictionaries
        coordinate_x = path[i]['X']
        coordinate_y = path[i]['Y']

        #Räkna ut robotens vinkel
        robotHeading = robot.getHeading()

        #robotens position, returnerar två värden
        robotPos = robot.getPosition()

        dx = coordinate_x - robotPos["X"]
        dy = coordinate_y - robotPos["Y"]

        #Räknar ut vinkeln från roboten till carrot
        angle = atan2(dy, dx)

        speed = 0.40

        #räknar ut avstånd till carrot
        distanceToCarrot = sqrt(dx**2 + dy**2)

        timeToCarrot = distanceToCarrot/speed

        #räknar ut anglevi
        robotAngle = (angle) - (robotHeading) / timeToCarrot

        #Robotens hastighet + rotationshastighet
        robot.setMotion(speed, robotAngle)


        print(robotHeading)
        print(angle)
        print(robotPos)['X']
        print(robotPos)['Y']
        print(coordinate_x)
        print(coordinate_y)

        time.sleep(0.05)

        if increaseSpeed == 5:
            print("Speed speed")
            speed = + 0.01
            increaseSpeed = 0

        if distanceToCarrot < 0.4:
            print("Carrot found!")
            robotRunning = False
            time.sleep(0.05)

print("Path complete")
robot.setMotion(0.0)