import numpy as np
import robot



def navigateToWaypoint(X,Y):


    #assuming robot is at (0,0,0) 
    
    #xnew = cos(theta)*d
    #ynew = sin(theta)*d

    d = np.sqrt(X*X + Y*Y)
    sin_theta = Y/d
    cos_theta = X/d

    #theta is in radians
    theta_rad = np.arccos(cos_theta)
    
    #turn theta into degrees
    theta_deg = theta_rad * 360.0 / (2.0* np.pi)

    #call function that rotates robot by theta_deg degrees
    robot.left(theta_deg)
    

    #call function to move robot straight for d cm
    robot.forwards(d)



def path_planning(current_loc, goal):

    #need to translate our frame of reference
    x_c, y_c, theta_c = current_loc  #!!!!!make sure that theta_c is in degrees!
    x_g, y_g = goal

    
    d = np.sqrt((x_c-x_g)**2+(y_c-y_g)**2)
    

    #calculate angle of goal point with respect to real frame
    sin_theta = y_g/d
    cos_theta = x_g/d

    #theta is in radians
    theta_rad = np.arccos(cos_theta)
    
    #turn theta into degrees
    theta_deg = theta_rad * 360 / 2* np.pi  
    
    #subtract the angle robot is facing  with respect to real frame from angle of goal point
    #to obtain abgle the robot needs to turn to
    #if this angle is negative, robot has to turn this amount right
    #if angle is positive, robot has to turn this amount left
    theta_to_move = theta_deg - theta_c
    
    print theta_to_move

    if theta_to_move < 0:
        #turn right amount theta_to_move
        print 'moving right'
        robot.right(int(theta_to_move))

    else: 
        print 'moving left'
        #turn left amount theta_to_move
        robot.left(int(theta_to_move))
    
    #xnew = cos(theta)*d
    #ynew = sin(theta)*d
    X = abs(x_c - x_g)
    Y = abs(y_c - y_g)
    d = np.sqrt(X*X + Y*Y)

    
    #after turned into correct direction, move d cm forward
    robot.forwards(d)
    
path_planning((0,0,0),(4,1))
    
    


    

@@ -92,22 +92,17 @@
  (0,0,0)
- %0A    
+%0A
 %0A    #xn
@@ -267,36 +267,32 @@
 ccos(cos_theta)%0A
-    
 %0A    #turn theta
@@ -440,20 +440,16 @@
 ta_deg)%0A
-    
 %0A%0A    #c
@@ -517,16 +517,49 @@
 ds(d)%0A%0A%0A
+path_planning((0,0,0),(0,-1))%0A888
 %0Adef pat
@@ -738,20 +738,16 @@
 = goal%0A%0A
-    
 %0A    d =
@@ -782,20 +782,16 @@
 _g)**2)%0A
-    
 %0A%0A    #c
@@ -953,20 +953,16 @@
 _theta)%0A
-    
 %0A    #tu
@@ -1003,41 +1003,31 @@
 g = 
-theta_rad * 360 / 2* np.pi  %0A    
+np.rad2deg(theta_rad)%0A%0A
 %0A   
@@ -1334,20 +1334,16 @@
 theta_c%0A
-    
 %0A    pri
@@ -1506,17 +1506,16 @@
    else:
- 
 %0A       
@@ -1614,20 +1614,16 @@
 _move))%0A
-    
 %0A    #xn
@@ -1739,20 +1739,16 @@
 + Y*Y)%0A%0A
-    
 %0A    #af
@@ -1822,20 +1822,16 @@
 ards(d)%0A
-    
 %0Apath_pl
@@ -1856,21 +1856,4 @@
 1))%0A
-    %0A    %0A%0A%0A    %0A

use numpy radian converter