from comm import *
import sys
import time
from struct import *

class AppTest:
    def __init__(self, adr_module, adr_sensor):
        self.serial_port = SerialComm('/dev/ttyUSB0')
        self.module = adr_module
        self.sensor = adr_sensor

    def finish(self):
        self.serial_port.close()

    def cmd_version(self):
        resp = self.serial_port.request(self.module, 0, CMD_VERSION,[])
        version = chr(resp[3])+chr(resp[4])+chr(resp[5])
        print("Version: "+version)

    def cmd_echo(self, msg):        
        resp = self.serial_port.request(self.module, 0, CMD_ECHO,msg)
        echo = ""
        for r in range(len(msg)):
            echo = echo + chr(resp[3+r])
        print("Echo:"+echo)

    def cmd_set_param(self, param):        
        resp = self.serial_port.request(self.module, self.srensor, (SET+CMD_PARAM),[])     

    def cmd_get_param(self, param):        
        resp = self.serial_port.request(self.module, self.sensor, (CMD_PARAM),[])     

    def cmd_sensor_cnt(self):
        print("SENSOR CNT")
        resp = self.serial_port.request(self.module, 0, CMD_SENSOR_CNT,[])        
        print(resp)
        return resp[3]

    def cmd_sensor_type(self, index):
        resp = self.serial_port.request(self.module, index, CMD_SENSOR_TYPE,[])
        return resp[3]

    def cmd_sensors_type(self, pocet):
        resp = self.serial_port.request(self.module, 0, CMD_SENSOR_TYPE,[])
        typy = [];
        for i in range(pocet):
            typy.append([resp[3+2*i],resp[4+2*i]])
        return typy

    def cmd_sensor_get_params(self, index):
        resp = self.serial_port.request(self.module, index, CMD_PARAM,[])
        return resp

    def cmd_sensor_get_data(self, index):
        resp = self.serial_port.request(self.module, index, CMD_DATA,[])
        if len(resp)>1:
            h = resp[5] * 256 + resp[4]
        # print(hex(h))        
            return h/4096*3.3
        return 0

    def cmd_sensor_get_data_IMU(self, sensor_index):
        resp = self.serial_port.request(self.module, sensor_index, CMD_DATA,[])
        #print(resp)
        x = 0
        y = 0
        z = 0
        if len(resp)>6:
            x = resp[5]*256 + resp[4]
            y = resp[7]*256 + resp[6]
            z = resp[9]*256 + resp[8]
            if x > 32768:
                x = x - 65535
            if y > 32768:
                y = y - 65535                
            if z > 32768:
                z = z - 65535                
        return [x,y,z]

    def cmd_sensor_get_data_FSR(self, cnt):
        resp = self.serial_port.request(self.module, 0, CMD_DATA,[])
        print(resp)
        if len(resp)>1:
            for i in range(cnt):
                sen = resp[3+3*i]
                h = resp[5+3*i] * 256 + resp[4+3*i]
                print("FSR",sen,":\t", h/4096.0*3.3)
        
        

    def cmd_module_info(self, param):
        resp = self.serial_port.request(self.module, 0, CMD_INFO,[param])
        if param == 0xE3:
            data=0
            for i in range(4):
                data=data+resp[i+3]*pow(256,i)
            return hex(data)
        if param == 0xE6:
            data=0
            for i in range(8):
                data=data+resp[i+3]*pow(256,7-i)
            return hex(data)
        l = len(resp)
        data=""
        for i in range(l-4):
            data=data+chr(resp[i+3])
        return data

    def cmd_module_start(self):
        print("MODULE START")
        self.serial_port.requestBroadcast(CMD_START,[])

    def cmd_module_stop(self):
        print("MODULE STOP")
        self.serial_port.requestBroadcast(CMD_STOP,[])

if __name__ == "__main__":
    
    app = AppTest(0x05, 0x0)
    #app.cmd_version()
    #app.cmd_version()
    # app.cmd_module_stop()
    app.cmd_echo([65,66,67,68,69,70])
    app.cmd_echo([97,98,99,100])
    #sys.exit()
    #time.sleep(0.5)
    #app.cmd_module_stop()
    time.sleep(0.5)
    app.cmd_module_start()
    
    #app.cmd_module_stop()
    #sys.exit()
    pocet = app.cmd_sensor_cnt()
    #sys.exit()
    print("pocet senzorov=", pocet)
    #app.cmd_sensor_get_data_FSR(pocet)    
    for i in range(6):
        acc = app.cmd_sensor_get_data_IMU(1)
        gyr = app.cmd_sensor_get_data_IMU(2)
        print(acc)
        print(gyr)

    sys.exit()
    
    for i in range(pocet):
        d=app.cmd_sensor_get_data(i+1)
        print("Data", i+1, ":\t", d)
    

    time.sleep(1)
    app.cmd_module_stop()
    
    
    
    print("Sensor type")
    for i in range(pocet):
        print(app.cmd_sensor_type(i+1))
    print("Sensors type")
    print(app.cmd_sensors_type(pocet))
    par = app.cmd_sensor_get_params(1)
    print(par)

    
    r=app.cmd_module_info(0xE1)
    print('NAME:' , r)    
    r=app.cmd_module_info(0xE2)
    print('TYPE', r)
        
    r=app.cmd_module_info(0xE4)
    print('FW', r)

    r=app.cmd_module_info(0xE5) 
    print('HW', r)
    
    r=app.cmd_module_info(0xE3)    
    print("UUID", r)

    r=app.cmd_module_info(0xE6)    
    print("MEM ID", r)
    app.finish()