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.35

        #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("Robot heading" + str(robotHeading))
        print("Robotens hastighet" + str(speed))
        print("Distance to Carrot" + str(distanceToCarrot))

        time.sleep(0.05)

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

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


print("Path complete")
robot.setMotion(0, 0)