# -*- coding: utf-8 -*-
import serial
import numpy
import time

class Arduino():
    def __init__(self,port):
        self.ser = serial.Serial(port,baudrate=115200)
        c_recu = self.ser.read(1)
        while ord(c_recu)!=0:
            c_recu = self.ser.read(1)
        c_recu = self.ser.read(1)
        while ord(c_recu)!=255:
            c_recu = self.ser.read(1)
        c_recu = self.ser.read(1)
        while ord(c_recu)!=0:
            c_recu = self.ser.read(1)
        self.MVT_PERIODIQUE = 100

    def close(self):
        self.ser.close()

    def write_int16(self,v):
        v = numpy.int16(v)
        char1 = (v & 0xFF00) >> 8
        char2 = (v & 0x00FF)
        self.ser.write(chr(char1))
        self.ser.write(chr(char2))
        
    def write_int32(self,v):
        v = numpy.int32(v)
        char1 = (v & 0xFF000000) >> 24
        char2 = (v & 0x00FF0000) >> 16
        char3 = (v & 0x0000FF00) >> 8
        char4 = (v & 0x000000FF)
        self.ser.write(chr(char1))
        self.ser.write(chr(char2))
        self.ser.write(chr(char3))
        self.ser.write(chr(char4))


    def mvt_periodique(self,N,delais,T,num_periodes):
        self.ser.write(chr(self.MVT_PERIODIQUE))
        self.write_int16(N)
        P = len(delais)
        self.ser.write(chr(P))
        for p in range(P):
            self.write_int16(delais[p])
        self.write_int16(T);
        self.write_int16(num_periodes)

    def mvt_sinus(self,N,P,A,T,num_periodes):
        dtheta = numpy.pi/2/P
        theta = 0
        delais = []
        for p in range(P):
            num_pas = A*(numpy.sin(theta+dtheta)-numpy.sin(theta))
            theta += dtheta
            delais.append(int(N*1.0/num_pas))
        self.mvt_periodique(N,delais,T,num_periodes)
        
    def wait(self):
        self.ser.read(1)