#!/usr/bin/env python
import roslib
import rospy
import math
import sys, select, termios, tty
import time

from std_msgs.msg import Int32
from phidgets.msg import motor_encoder

millis = lambda: int(round(time.time() * 1000))

R = 0.0365
D = 0.274
N = 360.0

class EncoderDeadRec:
    def __init__(self):

        self.firstrun = True
        self.left_count_init = 0 # initial count of left wheel encoder
        self.right_count_init = 0 # initial count of right wheel encoder

        self.left_count = 0
        self.left_count_change = 0
        self.right_count = 0
        self.right_count_change = 0

        rospy.Subscriber("/left/encoder", motor_encoder, self.sub1callback)
        rospy.Subscriber("/right/encoder", motor_encoder, self.sub2callback)

        self.calculate()

    def sub1callback(self, ros_data):
        self.left_count = ros_data.count
        self.left_count_change = ros_data.count_change

    def sub2callback(self, ros_data):
        self.right_count = ros_data.count
        self.right_count_change = ros_data.count_change

    def calculate(self):
        rate = rospy.Rate(10) # 10hz

        while not rospy.is_shutdown():

            if self.firstrun == True:
                self.left_count_init = self.left_count
                self.right_count_init = self.right_count
            self.firstrun == False
            self.left_count -= self.left_count_init
            self.right_count -= self.right_count_init

            eta_k_l = (self.left_count*2.0*math.pi/N)
            eta_k_r = (self.right_count*2.0*math.pi/N)
            a_k_l = R*eta_k_l
            a_k_r = R*eta_k_r
            a_k = (a_k_l + a_k_r)/2.0
            DeltPhi_k = (a_k_l + a_k_r)/D
            r_k = a_k/DeltPhi_k
            taylor = 0.0
            for i in range(2): # set depth of taylor expansion here
                taylor+= ( (-1**(i+2))*(DeltPhi_k**(2*i))/math.factorial(2*(i+1)) )
            DeltLambda = math.sqrt((2*a_k**2)*taylor)

            print "\n"
            print self.left_count
            print self.left_count_change
            print self.right_count
            print self.right_count_change
            print eta_k_r

        rate.sleep()

if __name__=="__main__":
    rospy.init_node('EncoderDeadRec')


    try:
        est = EncoderDeadRec()
    except rospy.ROSInterruptException: pass