import serial, math,time
from visual import *

ser = serial.Serial('COM5', 115200)

data1=0;
data2=0;
data3=0;
data4=0;
data5=0;
data6=0;
data7=0;

X=0;
Y=0;
Z=0;
signo=0;

scene.title='ECE 31289 UPB'

mybox = box(pos=(50,10,57/2), length=100, height=20, width=57)
pointer = arrow(pos=(0,0,0),axis=(0,100,0),shaftwidth=1)
pointer1 = arrow(pos=(0,0,0),axis=(0,0,100),shaftwidth=1)
pointer2 = arrow(pos=(0,0,0),axis=(100,0,0),shaftwidth=1)

mybox.opacity=0.5
mybox.color= color.blue
pointer.color= color.cyan
pointer1.color= color.red
pointer2.color= color.yellow
picth=0
roll=0
yaw=0

scene.autoscale =0

theta = 0 # initial polar angle of shaft (from vertical)
thetadot = 0 # initial rate of change of polar angle
alpha = 0 # initial spin angle
alphadot = 0.0 # initial rate of change of spin angle (spin ang. velocity)
phi = -pi/2. # initial azimuthal angle
phidot = 0 # initial rate of change of azimuthal angle
dt = 0.0001

while 1:
    
    keys1=ord(ser.read(1))
    
    if(keys1==7):
        data1=ord(ser.read(1))
        data2=ord(ser.read(1))
        data3=ord(ser.read(1))
        data4=ord(ser.read(1))
        data5=ord(ser.read(1))
        data6=ord(ser.read(1))
        data7=ord(ser.read(1))
        X=(data1 & 1)*128 | (((data1>>1)&1)<<16) | (data2 & 127)| ((data3 & 127)<<8)
        if(X & 4096):# paso a signed short 16 bits
            
            X=-0x1000 + (X & 4095)
        
            
        
        Y=(data1>>2 & 1)*128 | (((data1>>3)&1)<<16) |(data4 & 127) | ((data5 & 127)<<8)
        if(Y & 4096):# paso a signed short 16 bits
               
            Y=-0x1000 + (Y & 4095)
        
        
        Z=(data1>>4 & 1)*128 | (((data1>>5)&1)<<16) |(data6 & 127) | ((data7 & 127)<<8)
        if(Z & 4096):# paso a signed short 16 bits
               
            Z=-0x1000 + (Z & 4095)
            
        print 'X='+ str(X) + ' Y=' + str(Y) + ' Z=' + str(Z)
        #pitch=math.atan((X/(math.sqrt(Y**2 +Z**2)+0.01)))
        #roll=math.atan((Y/(math.sqrt(X**2 +Z**2)+0.01)))
        #yaw=math.atan((math.sqrt(X**2 +Y**2)/(Z+0.01)))

        mybox.axis= X,Y,Z
        #mybox.axis= pitch,roll,yaw
        mybox.length=30 
        mybox.width=100
        mybox.height=57