#!/usr/bin/python

import sys

import time
startbyte = 0x0F
TREX_ADDRESS = 0x07


class TrexDatacmd:
    """
    create a data package and send it
    """

    def __init__(self):
        '''
        Reset the trex controller data command
        '''

        self.sv = [0,0,0,0,0,0] #[1500, 1500, 1500, 1500, 0, 0]
        self.sd = [0,0,0,0,0,0] #[5, 10, -5, -15, 20, -20]
        self.lmspeed = 0
        self.rmspeed = 255
        self.ldir = 5
        self.rdir = 5
        self.lmbrake = 0
        self.rmbrake = 0
        self.devibrate = 50
        self.sensitivity = 50
        self.lowbat = 550
        self.i2caddr = 0x07
        self.i2cfreq = 0
        self.PWMfreq = 6


def sndDataCmd(self):
    """Sends cmd to trx after sanity check"""

    if not (self.PWMfreq > 0 and self.PWMfreq < 8):
        print ("PWM value not set correctly", self.PWMfreq)
        return
    elif not(self.lmspeed > -256 and self.lmspeed < 256):
        print ("left motor speed not set correctly", self.lmspeed)
        return
    elif not(self.lmbrake > -256 and self.lmbrake < 256):
        print ("left break calue not set correctly", self.lmbrake)
        return
    elif not(self.rmspeed > -256 and self.rmspeed < 256):
        print ("Right motor speed not set correctly", self.rmspeed)
        return
    elif not(self.rmbrake > -256 and self.rmbrake < 256):
        print ("Right motor speed not set correctly", self.rmbrake)
        return
    elif (abs(self.sv[0])>2400):
        print ("Servo 0 value out of range")
        return
    elif (abs(self.sv[1])>2400):
        print ("Servo 0 value out of range")
        return
    elif (abs(self.sv[2])>2400):
        print ("Servo 0 value out of range")
        return
    elif (abs(self.sv[3])>2400):
        print ("Servo 0 value out of range")
        return
    elif (abs(self.sv[4]) > 2400):
        print ("Servo 0 value out of range")
        return
    elif (abs(self.sv[5]) > 2400):
        print ("Servo 0 value out of range")
        return
    elif not (self.sensitivity > -1 and self.sensitivity < 1024):
        print("Sensisity is outof range")
        return
    elif not (self.lowbat > 549 and self.lowbat < 3001):
        print("Low Battary is out of range")
        return
    elif not (self.i2caddr == 7):
        print("Do not udpate the i2c address",self.i2caddr)
        return
    elif not (self.i2cfreq < 2):
        print("I2C frequency is out of range",self.i2cfreq)



    tx = [startbyte, \
        self.PWMfreq, \
        __hight_byte(self.lmspeed), \
        __low_byte(self.lmspeed), \
        self.lmbrake, \
        __hight_byte(self.rmspeed), \
        __low_byte(self.rmspeed), \
        self.rmbrake, \
        __hight_byte(self.sv[0]), \
        __low_byte(self.sv[0]), \
        __hight_byte(self.sv[1]), \
        __low_byte(self.sv[1]), \
        __hight_byte(self.sv[2]), \
        __low_byte(self.sv[2]), \
        __hight_byte(self.sv[3]), \
        __low_byte(self.sv[3]), \
        __hight_byte(self.sv[4]), \
        __low_byte(self.sv[4]), \
        __hight_byte(self.sv[5]), \
        __low_byte(self.sv[5]), \
        self.devibrate, \
        __hight_byte(self.sensitivity), \
        __low_byte(self.sensitivity), \
        __hight_byte(self.lowbat), \
        __low_byte(self.lowbat), \
        self.i2caddr, \
        self.i2cfreq \
        ]

    btx = bytes(tx)

    print (btx)




class TrexStatus:

    def __init__(self):
        self.pr_cmd_st = 0
        self.bat_vlt = 0
        self.lft_mtr_cur = 0
        self.lft_mtr_cnt = 0
        self.rht_mtr_cur = 0
        self.rht_mtr_cnt = 0
        self.acc_x = 0
        self.acc_y = 0
        self.acc_z = 0
        self.imp_x = 0
        self.imp_y = 0
        self.imp_z = 0

    def pntTrexSt(self):
        print("Previous data command status", end=" ")
        if (self.pr_cmd_st & 0x00):
            print("has update sucessfully.")
        else:
            print("has an Error -", end = " ")

        if (self.pr_cmd_st & 0x01):
            print("Start byte not received or incorrect data packet size")

        if (self.pr_cmd_st & 0x02):
            print("PWM frequency was not 1-7")

        if (self.pr_cmd_st & 0x04):
            print("Left or right motor speed was not -255 to +255")

        if (self.pr_cmd_st & 0x08):
            print("One or more servo positions was not -2400 to +2400")

        if (self.pr_cmd_st & 0x10):
            print("Impact sensitivity not 0-1023")

        if (self.pr_cmd_st & 0x20):
            print("Low battery was not 550 to 3000 i.e., 5.5V to 30V")

        if (self.pr_cmd_st & 0x40):
            print("I2C slave address was not 0-127")

        if (self.pr_cmd_st & 0x80):
            print("I2C speed not 0 which means 100kHz or 1 which means 400kHz")

        print("Battery voltage: " + str(self.bat_vlt))
        print("Left motor current: " + str(self.lft_mtr_cnt))
        print("Right motor current: " + str(self.rht_mtr_cnt))
        print("Accelerometer X-axis: " + str(self.acc_x))
        print("Accelerometer Y-axis : " + str(self.acc_y))
        print("Accelerometer Z-axis : " + str(self.acc_z))
        print("Impact X-axis: " + str(self.imp_x))
        print("Impact Y-axis: " + str(self.imp_y))
        print("Impact Z-axis: " + str(self.imp_z))


def getTrexSts(self):
    '''
    Get status from trex
        - Battery voltage:   An integer that is 100x the actual voltage
        - Motor current:  Current drawn by the motor in mA
        - Accelerometer
        - Impact
    update the object with trex status
    '''

    b = __trex_status()
    self.pr_cmd_st = b[0][1]
    self.bat_vlt = __hight_low_int(b[0][2], b[0][3])
    self.lft_mtr_cur = __hight_low_int(b[0][4], b[0][5])
    self.lft_mtr_cnt = __hight_low_int(b[0][6], b[0][7])
    self.rht_mtr_cur = __hight_low_int(b[0][8], b[0][9])
    self.rht_mtr_cnt = __hight_low_int(b[0][10], b[0][11])
    self.acc_x = __hight_low_int(b[0][12], b[0][13])
    self.acc_y = __hight_low_int(b[0][14], b[0][15])
    self.acc_z = __hight_low_int(b[0][16], b[0][17])
    self.imp_x = __hight_low_int(b[0][18], b[0][19])
    self.imp_y = __hight_low_int(b[0][20], b[0][21])
    self.imp_z = __hight_low_int(b[0][22], b[0][23])
    return


trex_st = TrexStatus()
trexCmd = TrexDatacmd()


def __trex_reset(self):
    '''
    Reset the trex controller data command
    '''
    self.sv[0] = 1500
    self.sv[1] = 1500
    self.sv[2] = 1500
    self.sv[3] = 1500
    self.sv[4] = 0
    self.sv[5] = 0

    self.sd[0] = 5
    self.sd[1] = 10
    self.sd[2] = -5
    self.sd[3] = -15
    self.sd[4] = 20
    self.sd[5] = -20

    self.lmspeed = 0  # left motor speed from -255 to +255 (negative value = reverse)
    self.rmspeed = 0  # right motor speed from -255 to +255 (negative value = reverse)
    self.ldir = 5  # how much to change left  motor speed each loop (use for motor testing)
    self.rdir = 5  # how much to change right motor speed each loop (use for motor testing)
    self.lmbrake = 0  # left motor brake (non zero value = brake)
    self.rmbrake = 0  # right motor brake (non zero value = brake)
    self.devibrate = 50  # time delay after impact to prevent false re-triggering due to chassis vibration
    self.sensitivity = 50  # threshold of acceleration / deceleration required to register as an impact
    self.lowbat = 550  # adjust to suit your battery: 550 = 5.50V
    self.i2caddr = 7  # default I2C address of T'REX is 7. If this is changed, the T'REX will automatically store new address in EEPROM
    self.i2cfreq = 0  # I2C clock frequency. Default is 0=100kHz. Set to 1 for 400kHz


def printb(string):
    '''
    Print bold text to screen
    '''
    print('\033[1m' + string + '\033[0m')


def __trex_status():
    '''
    Read status from trex
    Return as a byte array
    '''
    #answer = i2c.read_i2c_block_data(TREX_ADDRESS, 240, 24)

    return b'\x0f\x01\x04\xb6\x9f\xad\xff\xff\xff\xd0\xff\xff\x02\x86\x02\x86\x02\x86\x00\x00\x00\x00\x00\x00'
    #return answer
    #return [b'\x0f\x00\x04\xb6\x9f\xad\xff\xff\xff\xd0\xff\xff\x02\x88\x02\x88\x02\x88\x00\x00\x00\x00\x00\x00']


def __hight_byte(integer):
    '''
    Get the hight byte from a int
    '''
    return integer >> 8


def __low_byte(integer):
    '''
    Get the low byte from a int
    '''
    return integer & 0xFF


def __hight_low_int(hight_byte, low_byte):
    '''
    Convert low and low and hight byte to int
    '''
    return (int(hight_byte) << 8) + int(low_byte)



if __name__ == '__main__':
    #print("-------------------")
    getTrexSts(trex_st)
    #trex_st.pntTrexSt()
    sndDataCmd(trexCmd)  # initial
    print()
    #todo: Create a thread to read the status periodically
    #todo: ROS message to update the Trex status continously
    #todo: ROS service to set the trex speeds/break and direction or \
    # drive it using differential steering [http://wiki.ros.org/diff_drive_controller]
    #todo: GUI to view the trex status and plot it

    getTrexSts(trex_st)
    if trex_st.pr_cmd_st == 0:
        sndDataCmd(trexCmd)  # initial
        time.sleep(1)
    else:
        while (1):
            print(".", end = " ")
            getTrexSts(trex_st)
            trexCmd.rmspeed -= 10
            trexCmd.lmspeed += 10
            sndDataCmd(trexCmd)  # initial
            #time.sleep(1)

            #print(__hight_byte(trexCmd.lmspeed),__low_byte(trexCmd.lmspeed))

            if trexCmd.lmspeed > 200:
                break