class ZumyROS:	
  def __init__(self):
    self.zumy = Zumy()
    rospy.init_node('zumy_ros')
    self.cmd = (0,0)
    rospy.Subscriber('cmd_vel', Twist, self.cmd_callback_test, queue_size=1)
    self.lock = Condition()
    self.rate = rospy.Rate(30)
    self.name = socket.gethostname()
    self.vl = 0
    self.vr = 0
    self.feedback = True
    self.alpha = 0.8
    self.heartBeat = rospy.Publisher('/' + self.name + '/heartBeat', String, queue_size=5)
    self.imu_pub = rospy.Publisher('/' + self.name + '/imu', Imu, queue_size = 1)
    self.r_enc_pub = rospy.Publisher('/' + self.name + '/r_enc', Int32, queue_size = 5)
    self.l_enc_pub = rospy.Publisher('/' + self.name + '/l_enc', Int32, queue_size = 5)
    self.l_spd_pub = rospy.Publisher('/'+ self.name + '/l_spd_pub', Float32, queue_size = 5)
    self.r_spd_pub = rospy.Publisher('/' + self.name + '/r_spd_pub', Float32, queue_size = 5)
    self.l_denc_pub = rospy.Publisher('/' + self.name + '/l_denc_pub', Float32, queue_size = 5)
    self.r_denc_pub = rospy.Publisher('/' + self.name + '/r_denc_pub', Float32, queue_size = 5)
    self.duration_pub = rospy.Publisher('/'+ self.name + '/Duration', Float32, queue_size = 5)
    self.imu_count = 0
    #self.timestamp = rospy.Time.now()
    #self.prevtimestamp = rospy.Time.now()
    #self.duration = (self.timestamp - self.prevtimestamp).to_sec()
    #self.pl = PID(0.0008,0.000055,0.00005)
    #self.pr = PID(0.0009,0.000065,0.00006)
    self.l_enc_count = 0
    self.l_enc_prev_count = 0
    self.l_enc_count_diff = 0
    self.prev_l_enc_count_diff = 0
    self.r_enc_count = 0
    self.r_ecn_prev_count = 0
    self.r_enc_count_diff = 0
    self.prev_r_enc_count_diff = 0
    self.l_vel_list = []
    self.r_vel_list = []

  def cmd_callback(self, msg):
    lv = 1
    la = 1
    v = msg.linear.x
    a = msg.angular.z
    self.vr = lv*v - la*a
    self.vl = lv*v + la*a
    self.zumy.PID_l.setPoint(self.vl)
    self.zumy.PID_r.setPoint(self.vr)

  def cmd_callback_test(self,msg):
    self.vl = msg.linear.x
    self.vr = msg.angular.z
    self.zumy.PID_l.setPoint(self.vl)
    self.zumy.PID_r.setPoint(self.vr)





  def run(self):
    while not rospy.is_shutdown():
      self.zumy.update_enc_denc()
      self.zumy.update_time()
      # self.prevtimestamp = self.timestamp
      # self.timestamp = rospy.Time.now()
      # self.duration = (self.timestamp - self.prevtimestamp).to_sec()
      # self.duration_pub.publish(self.duration)
      
   #   if time_now - self.timestamp > .5:
   #       self.cmd = (0,0)

      # self.lock.acquire()
      # imu_data = self.zumy.read_imu()
      # enc_data = self.zumy.read_enc()
      # self.lock.release()
      
      # imu_msg = Imu()
      # imu_msg.header = Header(self.imu_count,rospy.Time.now(),self.name)
      # imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
      # imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
      # imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
      # imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
      # imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
      # imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
      # self.imu_pub.publish(imu_msg)
      
      # enc_msg = Int32()
      # enc_msg.data = enc_data[1]
      # self.r_enc_pub.publish(enc_msg)
      # enc_msg.data = enc_data[0]
      # self.l_enc_pub.publish(enc_msg)
      # self.l_enc_prev_count = self.l_enc_count
      # self.r_enc_prev_count = self.r_enc_count
      # self.l_enc_count = enc_data[0]
      # self.r_enc_count = enc_data[1]
      # self.prev_l_enc_count_diff = self.l_enc_count_diff
      # self.l_enc_count_diff = self.alpha*\
      #           (self.l_enc_count - self.l_enc_prev_count)/\
      #           self.duration + \
      #           self.alpha*\
      #           self.prev_l_enc_count_diff
      # self.prev_r_enc_count_diff = self.r_enc_count_diff
      # self.r_enc_count_diff = self.alpha*\
      #           (self.r_enc_count - self.r_enc_prev_count)/\
      #           self.duration + \
      #           self.alpha*\
      #           self.prev_r_enc_count_diff
      pid_l = self.zumy.PID_l.update(self.zumy.denc[0]/self.zumy.duration)
      pid_r = self.zumy.PID_r.update(self.zumy.denc[1]/self.zumy.duration)
      #pid_l = confined(pid_l, 0.2)
      #pid_r = confined(pid_r, 0.2)
      # pid_l_pub = Float32()
      # pid_r_pub = Float32()
      # l_set_point_pub = Float32()
      # r_set_point_pub = Float32()
      # l_set_point_pub.data = self.l_enc_count_diff
      # r_set_point_pub.data = self.r_enc_count_diff
      # pid_l_pub.data = pid_l
      # pid_r_pub.data = pid_r
      # self.l_spd_pub.publish(pid_l_pub)
      # self.r_spd_pub.publish(pid_r_pub)
      self.l_denc_pub.publish(self.zumy.denc[0]/self.zumy.duration)
      self.r_denc_pub.publish(self.zumy.denc[1]/self.zumy.duration)
      self.l_vel_list.append(self.zumy.denc[0]/self.zumy.duration)
      self.r_vel_list.append(self.zumy.denc[1]/self.zumy.duration)

      self.heartBeat.publish("I am alive from Glew!!")
      
      self.lock.acquire()
      if self.feedback:
        self.zumy.cmd(pid_l,pid_r)
      else:
        self.zumy.cmd(self.vl, self.vr)
      self.lock.release()

      self.rate.sleep()

    # If shutdown, turn off motors
    self.zumy.cmd(0,0)
    f = open('/home/glew/coop_slam_workspace/src/ros_zumy/src/test.txt', 'a')
    for ii in range(len(self.l_vel_list)):
      f.write(("%.2f\t" % self.l_vel_list[ii]))
      f.write(("%.2f\n" % self.r_vel_list[ii]))
    f.close()
class ZumyROS:	
  def __init__(self):
    self.zumy = Zumy()
    rospy.init_node('zumy_ros')
    self.cmd = (0,0)
    rospy.Subscriber('cmd_vel', Twist, self.cmd_callback_test, queue_size=1)
    self.lock = Condition()
    self.rate = rospy.Rate(30)
    self.name = socket.gethostname()
    self.vl_enc = 0
    self.vr_enc = 0
    self.feedback = True
    self.heartBeat = rospy.Publisher('/' + self.name + '/heartBeat', String, queue_size=5)
    self.imu_pub = rospy.Publisher('/' + self.name + '/imu', Imu, queue_size = 1)
    self.r_enc_pub = rospy.Publisher('/' + self.name + '/r_enc', Int32, queue_size = 5)
    self.l_enc_pub = rospy.Publisher('/' + self.name + '/l_enc', Int32, queue_size = 5)
    self.l_spd_pub = rospy.Publisher('/'+ self.name + '/l_spd_pub', Float32, queue_size = 5)
    self.r_spd_pub = rospy.Publisher('/' + self.name + '/r_spd_pub', Float32, queue_size = 5)
    self.l_denc_pub = rospy.Publisher('/' + self.name + '/l_denc_pub', String, queue_size = 5)
    self.r_denc_pub = rospy.Publisher('/' + self.name + '/r_denc_pub', String, queue_size = 5)
    self.duration_pub = rospy.Publisher('/'+ self.name + '/Duration', Float32, queue_size = 5)
    self.imu_count = 0
    self.timestamp = rospy.Time.now()
    #self.prevtimestamp = rospy.Time.now()
    #self.duration = (self.timestamp - self.prevtimestamp).to_sec()
    #self.pl = PID(0.0008,0.000055,0.00005)
    #self.pr = PID(0.0009,0.000065,0.00006)
    self.l_enc_count = 0
    self.l_enc_prev_count = 0
    self.l_enc_count_diff = 0
    self.prev_l_enc_count_diff = 0
    self.r_enc_count = 0
    self.r_ecn_prev_count = 0
    self.r_enc_count_diff = 0
    self.prev_r_enc_count_diff = 0
    self.l_vel_list = []
    self.r_vel_list = []

  def cmd_callback(self, msg):
    v_tr = msg.linear.x
    omega_rot = msg.angular.z
    zumy_width = 0.086*8/4 # distance between the center of two front wheels of zumy in meter
    v_l = v_tr - omega_rot*zumy_width/2
    v_r = v_tr + omega_rot*zumy_width/2
    enc_cnt = 600 #encoder count per rev
    wheel_radius = 0.02 #Wheel radius in meter
    self.vl_enc = v_l*enc_cnt/(2*math.pi*wheel_radius)
    self.vr_enc = v_r*enc_cnt/(2*math.pi*wheel_radius)
    self.zumy.PID_l.setPoint(self.vl_enc)
    self.zumy.PID_r.setPoint(self.vr_enc)

  def cmd_callback_test(self,msg):
    self.vl = msg.linear.x
    self.vr = msg.angular.z
    self.zumy.PID_l.setPoint(self.vl)
    self.zumy.PID_r.setPoint(self.vr)

  def run(self):
    # i=0
    # msg = Twist()
    # msg.linear.x = 0.01
    # msg.angular.z = 0
    # self.cmd_callback(msg)
    # time1 = time.time()
    while not rospy.is_shutdown():# and i<900:
      i = i+1

      #Get the feedback and update the feecback control
      self.zumy.update_enc_denc()
      self.zumy.update_time()
      pid_l = self.zumy.PID_l.update(self.zumy.denc[0]/self.zumy.duration)
      pid_r = self.zumy.PID_r.update(self.zumy.denc[1]/self.zumy.duration)

      # self.prevtimestamp = self.timestamp
      # self.timestamp = rospy.Time.now()
      # self.duration = (self.timestamp - self.prevtimestamp).to_sec()
      # self.duration_pub.publish(self.duration)
      
      #if time_now - self.timestamp > .5:
      #    self.cmd = (0,0)

      # self.lock.acquire()
      # imu_data = self.zumy.read_imu()
      # enc_data = self.zumy.read_enc()
      # self.lock.release()
      
      # imu_msg = Imu()
      # imu_msg.header = Header(self.imu_count,rospy.Time.now(),self.name)
      # imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
      # imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
      # imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
      # imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
      # imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
      # imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
      # self.imu_pub.publish(imu_msg)
      
      #Publish current encoder value
      enc_msg = Int32()
      enc_msg.data = self.zumy.enc[1]
      self.r_enc_pub.publish(enc_msg)
      enc_msg.data = self.zumy.enc[0]
      self.l_enc_pub.publish(enc_msg)
      #Publish current encoder difference value

      l_denc_pub = String()
      r_denc_pub = String()
      l_denc_pub.data = "%.2f" % (self.zumy.denc[0]/self.zumy.duration)
      r_denc_pub.data = "%.2f" % (self.zumy.denc[1]/self.zumy.duration)
      self.l_denc_pub.publish(l_denc_pub)
      self.r_denc_pub.publish(r_denc_pub)

      #Append current encoder speed to save to txt file
      self.l_vel_list.append(self.zumy.denc[0]/self.zumy.duration)
      self.r_vel_list.append(self.zumy.denc[1]/self.zumy.duration)

      self.heartBeat.publish("I am alive from Glew!!")
      print "Hello"
      
      self.lock.acquire()
      if self.feedback:
        self.zumy.cmd(pid_l,pid_r)
      else:
        self.zumy.cmd(0,0)
      self.lock.release()

      self.rate.sleep()

    time2 = time.time()
    duration = time2-time1
    # If shutdown, turn off motors
    self.zumy.cmd(0,0)