# Reference 
# http://planning.cs.uiuc.edu/node659.html
# Author : C3MX <tutorgaming@gmail.com>

# Global Variables 
odom_x = 0.0
odom_y = 0.0
odom_theta = 0.0

# Wheel&Encoder Config
wheel_radius = 0.4  # Metres
wheel_separation = 0.5 # Metres
tick_per_revolute = 1000 # Tick in 1 Round Spinning

# Memorize the prev_tick
prev_left_tick = 0
prev_right_tick = 0

def calculate_odom_full_kinematic(left_enc_tick, right_enc_tick):
  """
  Odometry Calculation in the very small timestep
  """
  # [Calculate Per Wheel TICK-Meter] (Meter unit)
  tick_per_meter = (2* math.pi * wheel_radius)/tick_per_revolute
  
  # [Convert Distance each wheel] (Meter unit)
  left_distance = (left_enc_tick - prev_left_tick) * tick_per_meter
  right_distance = (right_enc_tick - prev_right_tick) * tick_per_meter
  
  # [Calculate dx dy dtheta]
  # Theta Calculation (right - left because we use right-handed rule) 
  small_theta = (right_distance - left_distance)/wheel_separation
  # Distance in robot frame in this small time step
  small_robot_distance =  (right_distance + left_distance)/2.00
  
  # If Robot is Rotate (prevent icc radius divide by zero)
  if small_theta != 0:
    # Radius of curvature for whole robot movement
    radius = deltaTravel / deltaTheta
    # Find the instantaneous center of curvature (ICC).
    iccX = odom_x - radius*sin(odom_theta)
    iccY = odom_y + radius*cos(odom_theta)

    small_x = cos(small_theta)*(odom_x - iccX) - sin(small_theta)*(odom_y - iccY) + iccX - odom_y
    small_y = sin(small_theta)*(odom_x - iccX) + cos(small_theta)*(odom_y - iccY) + iccY - odom_y
  else: 
    #Robot is not Rotate
    small_x = small_robot_distance*cos(odom_theta)
    small_y = small_robot_distance*sin(odom_theta)
   
  # [Integrate into the result]
  odom_x += small_x
  odom_y += small_y
  odom_theta += small_theta
  odom_theta = odom_theta % (2*pi)
  
  return odom_x, odom_y, odom_theta