#!/usr/bin/env python
#encoding: utf8
import sys, rospy, math
from pimouse_ros.msg import MotorFreqs
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponse

class Motor():
	def __init__(self):
		if not self.set_power(False): sys.exit(1)

		rospy.on_shutdown(self.set_power)
		self.sub_raw = rospy.Subscriber('motor_raw', MotorFreqs, self.callback_raw_freq)
		self.sub_cmd_vel = rospy.Subscriber('cmd_vel', Twist, self.callback_cmd_vel)
		self.srv_on = rospy.Service('motor_on', Trigger, self.callback_on)
		self.srv_off = rospy.Service('motor_off', Trigger, self.callback_off)
		self.last_time = rospy.Time.now()
		self.using_cmd_vel = False

	def set_power(self, onoff = False):
		en = "/dev/rtmotoren0"
		try:
			with open(en, 'w') as f:
				f.write("1\n" if onoff else "0\n")
			self.is_on = onoff
			return True
		except:
			rospy.logerr("cannot write to " + en)

		return False

	def set_raw_freq(self, left_hz, right_hz):
		if not self.is_on:
			rospy.logerr("not enpowered")
			return

		try:
			with open("/dev/rtmotor_raw_l0", 'w') as lf, open("/dev/rtmotor_raw_r0", 'w') as rf:
				lf.write(str(int(round(left_hz))) + "\n")
				rf.write(str(int(round(right_hz))) + "\n")
		except:
			rospy.logerr("cannot write to rtmotor_raw_*")

	def callback_raw_freq(self, message):
		self.set_raw_freq(message.left_hz, message.right_hz)

	def callback_cmd_vel(self, message):
		forward_hz = 80000.0*message.linear.x/(9*math.pi)
		rot_hz = 400.0*message.angular.z/math.pi
		self.set_raw_freq(forward_hz-rot_hz, forward_hz+rot_hz)
		self.using_cmd_vel = True
		self.last_time = rospy.Time.now()

	def onoff_response(self, onoff):
		d = TriggerResponse()
		d.success = self.set_power(onoff)
		d.message = "ON" if self.is_on else "OFF"
		return d
	def callback_on(self, message):	return self.onoff_response(True)
	def callback_off(self, message): return self.onoff_response(False)

if __name__ == '__main__':
	rospy.init_node('motors')
	m = Motor()

	rate = rospy.Rate(10)
	while not rospy.is_shutdown():
		if m.using_cmd_vel and rospy.Time.now().to_sec() - m.last_time.to_sec() >= 1.0:
			m.set_raw_freq(0, 0)
			m.using_cmd_vel = False
		rate.sleep()
