#!/usr/bin/env python

"""
   Nexus controller


"""

import rospy
import socket
from math import radians,sin,cos,pi
from std_msgs.msg import Float64

from diagnostic_msgs.msg import *
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from tf.broadcaster import TransformBroadcaster

import carcomm

class NexusController():

    def __init__(self, device, name):

        self.status = "OK"
        self.level = DiagnosticStatus.OK
        #        self.name = socket.gethostname()

        self.dirty = False                      # newly updated position?
        self.enabled = True                     # can we take commands?

        self.last_cmd = rospy.Time.now()
        self.rate = rospy.get_param("~nexus/rate", 50.0)
        self.timeout = rospy.get_param('~nexus/timeout',1.0)
        self.t_delta = rospy.Duration(1.0/self.rate)
        self.t_next = rospy.Time.now() + self.t_delta
        self.fake = rospy.get_param("~sim", False)
        port = rospy.get_param("~port", "/dev/ttyUSB0")

        # parameters?
#        self.base_frame_id = rospy.get_param('~controllers/'+name+'/base_frame_id', ''+self.name+'/base_footprint')
        self.base_frame_id = rospy.get_param('~controllers/'+name+'/base_frame_id', name+'base_footprint')
        self.odom_frame_id = rospy.get_param('~controllers/'+name+'/odom_frame_id', 'odom')

        self.x = 0.0                    # position in xy plane
        self.y = 0.0
        self.th = 0                     # orientation (theta)

        self.dx = 0                     # x-speed in meter/sec
        self.dy = 0                     # y-speed in meter/sec
        self.dr = 0                     # rotation in radians/sec
        self.dx_last = 0                # previous x-speed
        self.dy_last = 0                # previous y-speed
        self.dr_last = 0                # previous rotation
        self.then = rospy.Time.now()    # time for determining dx/dy

        # ROS interfaces
        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
        self.odomPub = rospy.Publisher("odom", Odometry, queue_size=5)
        self.odomBroadcaster = TransformBroadcaster()

        rospy.loginfo("Started Controller ("+name+")")

        # connect to the nexus robot
        if not self.fake:
            # connect to Nexus
            baudrate = 28800; parity = 'N'; rtscts = False; xonxoff = False; echo = False
            convert_outgoing = carcomm.CONVERT_CRLF; repr_mode = 0
            self.carcon = carcomm.CarControl(port, baudrate, parity, rtscts, xonxoff, echo, convert_outgoing, repr_mode )
            # to be sure that the car is ready
            rospy.sleep(2.0)
            rospy.loginfo("Started Nexus connection on port " + port + ".")

            self.carcon.sendpacket(20,0)
            self.carcon.getinfopacket()

        else:
            rospy.loginfo("Nexus  being simulated.")


    def startup(self):
        print "startup called"

    def shutdown(self):
        # close connection with the nexus
        if not self.fake:
            console.cleanup_console()
            carcon.stop()
        print "shutdown called"

    def cmdVelCb(self,req):
        """ Handle movement requests. """
        self.last_cmd = rospy.Time.now()
        if self.fake:
            self.dx = req.linear.x        # m/s
            self.dy = req.linear.y        # m/s            
            self.dr = req.angular.z       # rad/s
        else:
            # same as fake
            self.dx = req.linear.x        # m/s
            self.dy = req.linear.y        # m/s                        
            self.dr = req.angular.z       # rad/s

    def getDiagnostics(self):
        """ Get a diagnostics status. """
        msg = DiagnosticStatus()
        msg.name = self.name
        return msg

    def update(self):
        """ Do step in nexus controller. """
        now = rospy.Time.now()
        if now > self.t_next:
            elapsed = now - self.then
            self.then = now
            elapsed = elapsed.to_sec()

            if self.fake:
                self.x += cos(self.th)*self.dx*elapsed - sin(self.th)*self.dy*elapsed
                self.y += sin(self.th)*self.dx*elapsed + cos(self.th)*self.dy*elapsed
                self.th += self.dr*elapsed
            else:
                # we use read reackoning for the moment
                self.x += cos(self.th)*self.dx*elapsed - sin(self.th)*self.dy*elapsed
                self.y += sin(self.th)*self.dx*elapsed + cos(self.th)*self.dy*elapsed
                self.th += self.dr*elapsed


            # publish or perish
            quaternion = Quaternion()
            quaternion.x = 0.0 
            quaternion.y = 0.0
            quaternion.z = sin(self.th/2)
            quaternion.w = cos(self.th/2)
            self.odomBroadcaster.sendTransform(
                (self.x, self.y, 0), 
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
                rospy.Time.now(),
                self.base_frame_id,
                self.odom_frame_id
                )

            odom = Odometry()
            odom.header.stamp = now
            odom.header.frame_id = self.odom_frame_id
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.child_frame_id = self.base_frame_id
            odom.twist.twist.linear.x = self.dx
            odom.twist.twist.linear.y = self.dy
            odom.twist.twist.angular.z = self.dr
            self.odomPub.publish(odom)

            # stop on timeout. nu niet zinnig, er is toch geen terugkoppeling.
#            if now > (self.last_cmd + rospy.Duration(self.timeout)):
                #                self.dx = 0; self.dr = 0;
                #                print "timeout reached"

            # update motors
            if not self.fake:
                # only send if it really is different!
                if self.dx <> self.dx_last or self.dy <> self.dy_last   or self.dr <> self.dr_last:
# voor of achteruit
#                    self.carcon.setspeed(-self.dx, self.dy, self.dr)
                    self.carcon.setspeed(self.dx, self.dy, self.dr)
                    self.dx_last = self.dx; self.dy_last = self.dy; self.dr_last = self.dr

            self.t_next = now + self.t_delta



    def getDiagnostics(self):
        """ Update status of servos (voltages, temperatures). """
        print "diagnostics"
        """
        if self.iter % 5 == 0 and not self.fake:
            if self.device.use_sync_read:
                # arbotix/servostik/wifi board sync_read
                synclist = list()
                for joint in self.dynamixels:
                    if joint.readable:
                        synclist.append(joint.id)
                if len(synclist) > 0:
                    val = self.device.syncRead(synclist, P_PRESENT_VOLTAGE, 2)
                    if val:
                        for joint in self.dynamixels:
                            try:
                                i = synclist.index(joint.id)*2
                                if val[i] < 250:
                                    joint.voltage = val[i]/10.0
                                if val[i+1] < 100:
                                    joint.temperature = val[i+1]
                            except:
                                # not a readable servo
                                continue 
            else:
                # direct connection, or other hardware with no sync_read capability
                # hack to also use slave_id
                for joint in self.dynamixels:
                    if joint.readable:
                        val  = self.device.read(joint.id, P_PRESENT_VOLTAGE, 2)
                        vals = val
                        if joint.slave_id > 0:
                            vals  = self.device.read(joint.slave_id, P_PRESENT_VOLTAGE, 2)
                        try:
                            joint.voltage = val[0]
                            # use maximum
                            if val[1] > vals[1]:
                                joint.temperature = val[1]
                            else:
                                joint.temperature = vals[1]
                        except:
                            continue
        self.iter += 1
        return None
        """

