#!/usr/bin/env python
import serial

import roslib; roslib.load_manifest('yurt_base')
import rospy
from geometry_msgs.msg import Twist, Vector3
from std_msgs.msg import String

class MotorController():

    #
    def __init__(self, port='/dev/ttyUSB0', baud=115200):

        # Instructions
        self.INST_READ = 1
        self.INST_WRITE = 2
        self.INST_RESET = 3

        # Parameters
        self.SETPOINT1 = 0
        self.SETPOINT2 = 1
        self.SETPOINT3 = 2
        self.SETPOINT4 = 3

        self.PROCESS_VAL1 = 4
        self.PROCESS_VAL2 = 5
        self.PROCESS_VAL3 = 6
        self.PROCESS_VAL4 = 7

        self.DIRECTION1 = 8
        self.DIRECTION2 = 9
        self.DIRECTION3 = 10
        self.DIRECTION4 = 11

        self.WATCHDOG_PARAM = 13

        # Serial Port
        self.Serial = serial.Serial()
        self.Serial.port = port
        self.Serial.baudrate = baud

        # Buffer
        self.Buffer = []
        self.BufferStart = 0
        self.BufferEnd = 0

    #
    def ParamSet(self, parameters):
        checksum = 0
        id = self.NextID()
        self.TX(0xFF)
        self.TX(0xFF)
        self.TX(id)
        self.TX(self.INST_WRITE)
        self.TX(len(parameters))
        for param in parameters:
            self.TX(param)
            checksum += param
        checksum = 255 - ((id + self.INST_WRITE + len(parameters) + checksum) % 256)
        self.TX(checksum)
        print '0xFF', '0xFF', id, self.INST_WRITE, len(parameters), parameters, checksum

    #
    def ParamGet(self, parameters):
        checksum = 0
        id = self.NextID()
        self.TX(0xFF)
        self.TX(0xFF)
        self.TX(id)
        self.TX(self.INST_READ)
        self.TX(len(parameters))
        for param in parameters:
            self.TX(param)
            checksum += param
        checksum = 255 - ((id + self.INST_READ + len(parameters) + checksum) % 256)
        self.TX(checksum)
        print '0xFF', '0xFF', id, self.INST_READ, len(parameters), parameters, checksum

    #
    def NextID(self):
        return 2

    #
    def RX(self):
        c = self.Serial.read()
        self.BufferWrite(c)

    #
    def TX(self, c):
        self.Serial.write(chr(c))

    #
    def BufferRead(self):
        c = self.Buffer[:1]
        self.Buffer = self.Buffer[1:]
        return c

    #
    def BufferWrite(self, c):
        self.Buffer.append(c)

import Wrapper

class Rover:

    def __init__(self):
        rospy.init_node('yurt_base')
        self.cmd_vel = rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_handler)
        self.cmd_msg = Twist()

        self.controller = MotorController(port='/dev/ttyUSB0', baud=115200)
        self.controller.Serial.open()
        self.wrapper = Wrapper.Wrapper()
        self.run()


    def cmd_vel_handler(self, msg):
        """ Received command from ROS. Signal the main thread to send it to robot. """
        self.cmd_msg = msg
        # rospy.loginfo(self.cmd_msg.linear.x)
        # rospy.loginfo(self.cmd_msg.angular.z)


    def run(self):
        while not rospy.is_shutdown():
            try:
                # params = [self.controller.SETPOINT1, 0xFE,0xFE,self.controller.DIRECTION1, 0,1]
                # self.controller.ParamSet(params)
                decimal = self.wrapper.convertToHalf(self.cmd_msg.linear.x)
                self.wrapper.appendList(self.wrapper.setSpeed(2,decimal))
                self.wrapper.appendList(self.wrapper.setSpeed(1,decimal))
                # self.wrapper.appendList(self.wrapper.setSpeed(3,decimal))
                # self.wrapper.appendList(self.wrapper.setSpeed(4,decimal))
                # self.wrapper.appendList(self.wrapper.setSpeed(2,decimal))
                # print 'message',wrapper._finalMessage
                self.wrapper.sendMessages()
                self.wrapper.clearList()
                rospy.sleep(0.1)
            except (KeyboardInterrupt, SystemExit):
                print "\nUser interrupted\n"
                self.controller.Serial.close()
                break



if __name__ == '__main__':
    rover = Rover()
    # self.controller = MotorController(port='/dev/ttyUSB0', baud=115200)
    # self.controller.Serial.open()
    # params = [self.controller.SETPOINT1, 0xFE,0xFE,self.controller.DIRECTION1, 0,1]
    # self.controller.ParamSet(params)

    # self.controller.Serial.close()

    # rospy.sleep(0.1)