""""
@author: hbartos
"""

import numpy as np
import matplotlib.pyplot as plt


#the generalized kinematic system formula integrated with Eulers method
def ODE(xc, yc, theta_c,r,d,t,w1,w2, number):
    i = 0
    coords = []
    new_coords = [xc, yc]
    coords.append(new_coords)
    for i in range (0, number):
         theta_n = theta_c + (r*t/(2.0*d))*(w1-w2)
         xn = xc + (r*t/2.0)*(w1+w2)*np.cos(theta_n)
         yn = yc + (r*t/2.0)*(w1+w2)*np.sin(theta_n)
         new_coords = [xn, yn]
         coords.append(new_coords)
         xc = xn
         yc = yn
         theta_c = theta_n
         i = i+1
   
    return (coords, xn,yn,theta_n)



def main():
    r = 0.04 # wheel radius
    d = 0.11 # distance from wheel to center of robot
    
    speeds_durations = np.array([
        [ 12, -30, 3 ],
        [ 10, -14, 2],
        [12, -15, 3],
        [8, -18, 1]])
    print (speeds_durations)
    speeds= []
    
    #read speeds and durations into second-by-second wheel velocities 
    for s in speeds_durations:
        dur = s[2]
        i = 1
        for i in range (0,dur):
            speeds.append([s[0], s[1]])
            i = i+1
    print('speeds:', speeds)
    
    
   # initialize three coordinate arrays
    
    coords0_5 = []
    coords0_25 = []
    coords0_125 = []
    
    
    
    
    # for t = .5
    #starting location:
    xc = 0
    yc = 0
    theta_c = 0
    t = .5
    number = 2
    
    for i in speeds:
        w1 = i[0]
        w2 = i[1]
        coords, xc, yc, theta_c = ODE(xc, yc, theta_c,r,d,t,w1,w2, number)
        coords0_5.append(coords)
        print ('coords 0.5', coords0_5)
        
        
    # for t = .25
    xc = 0
    yc = 0
    theta_c = 0
    t= .25
    number = 4
  
    for i in speeds:
        w1 = i[0]
        w2 = i[1]
        coords, xc, yc, theta_c = ODE(xc, yc, theta_c,r,d,t,w1,w2, number)
        coords0_25.append(coords)
       
        print ('coords 0.25', coords0_25)
    
    # for t = .125
    xc = 0
    yc = 0
    theta_c = 0
    t = .125
    number = 8
    
    for i in speeds:
        w1 = i[0]
        w2 = i[1]
        coords, xc, yc, theta_c = ODE(xc, yc, theta_c,r,d,t,w1,w2, number)
        coords0_125.append(coords)
        print ('coords 0.125', coords0_125)
        
# lets make these lists plottable...
    eighth_seconds = np.array(coords0_125)
    eighth_seconds = eighth_seconds.reshape(-1, 2)
    
    quarter_seconds = np.array(coords0_25)
    quarter_seconds = quarter_seconds.reshape(-1, 2)
    
    half_seconds = np.array(coords0_5)
    half_seconds = half_seconds.reshape(-1, 2)
    
    print('eighth_seconds:', eighth_seconds)
    print('quarter_seconds:', quarter_seconds)
    print('half_seconds:', half_seconds)
    
    
    #plot!
    plt.plot(half_seconds[:,0], half_seconds[:,1], label='half second steps')
    plt.plot(quarter_seconds[:,0], quarter_seconds[:,1], 'r+',label='quarter second steps')
    plt.plot(eighth_seconds[:,0], eighth_seconds[:,1],'bo', label='eighth second steps')
    plt.xlabel('x (world)')
    plt.ylabel('y (world)')
    plt.axis('equal') 
    
   
   
    plt.legend()
    plt.show()
    
if __name__ == '__main__':
    main()