class Arduino():
    def __init__(self,port):
        self.SET_ACQUISITION = 100
        self.STOP_ACQUISITION = 101
        self.TAILLE_BLOC_INT16 = 180
        self.TAILLE_BLOC_INT8 = self.TAILLE_BLOC_INT16*2
        self.TAILLE_BLOC_ACCEL = int(self.TAILLE_BLOC_INT16/3)
        self.TAILLE_BLOC_ACCEL_GYRO = int(self.TAILLE_BLOC_INT16/6)
        self.ACCEL_FS_2 = 0
        self.ACCEL_FS_4 = 1
        self.ACCEL_FS_8 = 2
        self.ACCEL_FS_16 = 3
        self.accel_sensitivity = [16384.0,8192.0,4096.0,2048.0]
        self.GYRO_FS_250 = 0
        self.GYRO_FS_500 = 1
        self.GYRO_FS_1000 = 2
        self.GYRO_FS_2000 = 3
        self.gyro_sensitivity = [131,65.5,32.8,16.4]
        self.GYRO_NONE = 4
        self.DLPF_BW_260 = 0
        self.DLPF_BW_184 = 1
        self.DLPF_BW_94 = 2
        self.DLPF_BW_44 = 3
        self.DLPF_BW_21 = 4
        self.DLPF_BW_10 = 5
        self.DLPF_BW_5 = 6
        self.gyro = 0
        self.ser = serial.Serial(port,baudrate=19200)

    def close(self):
        self.ser.close()
    		 
    def write_int8(self,v):
    	char = int(v&0xFF) # nécessaire pour les nombres négatifs
    	self.ser.write((char).to_bytes(1,byteorder='big'))
    	
    def write_int16(self,v):
        v = numpy.int16(v)
        char1 = int((v & 0xFF00) >> 8)
        char2 = int((v & 0x00FF))
        self.ser.write((char1).to_bytes(1,byteorder='big'))
        self.ser.write((char2).to_bytes(1,byteorder='big'))
        
    def write_int32(self,v):
        v = numpy.int32(v)
        char1 = int((v & 0xFF000000) >> 24)
        char2 = int((v & 0x00FF0000) >> 16)
        char3 = int((v & 0x0000FF00) >> 8)
        char4 = int((v & 0x000000FF))
        self.ser.write((char1).to_bytes(1,byteorder='big'))
        self.ser.write((char2).to_bytes(1,byteorder='big'))
        self.ser.write((char3).to_bytes(1,byteorder='big'))
        self.ser.write((char4).to_bytes(1,byteorder='big'))    		 
    		 
    def lancer_acquisition(self,accel_range,gyro_range,rate_div,dlpf_mode):
        if gyro_range<4: self.gyro = 1
        else : self.gyro = 0
        self.ser.write((self.SET_ACQUISITION).to_bytes(1,byteorder='big'))
        self.ser.write((accel_range).to_bytes(1,byteorder='big'))
        self.ser.write((gyro_range).to_bytes(1,byteorder='big'))
        self.ser.write((rate_div.to_bytes(1,byteorder='big')))
        self.ser.write((dlpf_mode).to_bytes(1,byteorder='big'))
        self.ser.write((self.TAILLE_BLOC_INT16).to_bytes(1,byteorder='big'))
    		 
    def fechant(self,rate_div,dlpf_mode):
        if dlpf_mode==self.DLPF_BW_260:
            fechant = 8000/(1+rate_div)
        else:
            fechant = 1000/(1+rate_div)
        return fechant    		 
    		 
    def echelle_accel(self,accel_range):
        return self.accel_sensitivity[accel_range] 		  
    		  
    def echelle_gyro(self,gyro_range):
        return self.gyro_sensitivity[gyro_range]
    		   
    def stopper_acquisition(self):
     	self.write_int8(self.STOP_ACQUISITION)
    		   
    def lecture(self):
        buf = self.ser.read(self.TAILLE_BLOC_INT8)
        if self.gyro :
            ndata = 6
            N = self.TAILLE_BLOC_ACCEL_GYRO
        else :
            ndata = 3
            N = self.TAILLE_BLOC_ACCEL
        data = numpy.zeros((ndata,N),dtype=numpy.int16)
        j = 0
        for i in range(N):
            for k in range(ndata):
                data[k,i] = buf[j]+0x100*buf[j+1]
                j += 2
            
        return data 		    
    		    
class AcquisitionThread(threading.Thread):
    def __init__(self,arduino,accel_range,gyro_range,rate,dlpf_mode,nblocs):
        threading.Thread.__init__(self)
        self.arduino = arduino
        self.accel_range = accel_range
        self.gyro_range = gyro_range
        self.rate = rate
        self.dlpf_mode = dlpf_mode
        self.nblocs = nblocs # nombre de blocs maximal
        self.running = False
        if gyro_range<4:
            ndata = 6
            self.taille_bloc = self.arduino.TAILLE_BLOC_ACCEL_GYRO
        else:
            ndata = 3
            self.taille_bloc = self.arduino.TAILLE_BLOC_ACCEL
        self.data = numpy.zeros((ndata,self.taille_bloc*self.nblocs),dtype=numpy.float32)  		    
    		    
    def run(self):
        self.arduino.lancer_acquisition(self.accel_range,self.gyro_range,self.rate,self.dlpf_mode)
        self.fechant = self.arduino.fechant(self.rate,self.dlpf_mode)
        self.running = True
        self.indice_bloc = 0
        while self.running:
            i = self.indice_bloc*self.taille_bloc
            j = i+self.taille_bloc
            self.data[:,i:j] = self.arduino.lecture()
            self.indice_bloc += 1
            if self.indice_bloc==self.nblocs:
                self.stop()	      
    		      
    def paquet(self,longueur):
        j = self.indice_bloc*self.taille_bloc
        i = j-longueur
        if i<0: i=0
        if j-i<=0: return (-1,-1,-1)
        temps = numpy.arange(j-i)/self.fechant
        tmax = longueur/self.fechant
        return (temps,self.data[:,i:j],tmax) 		      
    		      
    def stop(self):
        self.running = False
        self.join()
        self.arduino.stopper_acquisition()