#!/usr/bin/python
#
# visualize phidget accelerometer 3d readouts
# Author: Tamas Decsi

from visual import *
from time import sleep
import _phidget

def main():
    # open accelerometer
    acm = _phidget.AcceleroMeter()
    acm.open()
    acm.waitForAttachment(1000)
    # visualize
    acv1 = box( pos=(0,0,0), axis=(0,1,0), size=(0.1,1,1), up=(0,1,0), color=color.green )
    whatsup = arrow( pos=(0,0,0), axis=(0,1,0), length = 1, color=color.white)
    textual = label( pos=(0,0,0), opacity = 0.80, border = 5, height = 20, xoffset = 10, yoffset = 5, text = "0\n0\n0")
    while True:
        accel = (
            acm.getAcceleration(0),
            acm.getAcceleration(1),
            acm.getAcceleration(2),
        )
        d = vector(-accel[0], accel[2], -accel[1])

        textual.text = "x:%-.3f\ny:%-.3f\nz:%-.3f" % accel
        textual.pos = d

        whatsup.axis = d
        whatsup.length =  mag(d)

        # FIXME: angles signedness
        xyangle = vector(0,1,0).diff_angle(vector(d[0],d[1],0)) * [1,-1][accel[0]<0]
        zyangle = vector(0,1,0).diff_angle(vector(0,d[1],d[2])) * [1,-1][accel[1]*accel[2]>=0]
        acv1.up = (0,1,0)
        acv1.axis = (0,1,0)
        acv1.size  = (0.1,1,1)
        acv1.rotate(angle = zyangle, axis=(1,0,0), origin=(0,0,0))
        acv1.rotate(angle = xyangle, axis=(0,0,1), origin=(0,0,0))

        sleep(0.1) # 100 ms

if __name__ == "__main__":
    main()