import math
import logging
from time import *
import numpy as np
from vpython import *
import paho.mqtt.client as mqtt

logging.basicConfig(level=logging.INFO)
logging.info('vpython setup')

scene.range = 5
toRad = 2 * np.pi/360
toDeg = 1 / toRad
scene.forward = vector(-1, -1, -1)

scene.width = 600
scene.height = 600

xarrow = arrow(length=2, shaftwidth=.1, color=color.red, axis=vector(1, 0, 0))
yarrow = arrow(length=2, shaftwidth=.1, color=color.green, axis=vector(0, 1, 0))
zarrow = arrow(length=4, shaftwidth=.1, color=color.blue, axis=vector(0, 0, 1))

frontArrow = arrow(length=4, shaftwidth=.1, color=color.purple, axis=vector(1, 0, 0))
upArrow = arrow(length=1, shaftwidth=.1, color=color.magenta, axis=vector(0, 1, 0))
sideArrow = arrow(length=2, shaftwidth=.1, color=color.orange, axis=vector(0, 0, 1))

bBoard = box(length=6, width=2, height=.2, opacity=.8, pos=vector(0, 0, 0,))
bn = box(length=1, width=.75, height=.1, pos=vector(-.5, .1+.05, 0), color=color.blue)
nano = box(length=1.75, width=.6, height=.1, pos=vector(-2, .1+.05, 0), color=color.green)
myObj = compound([bBoard,bn,nano])


def on_message(client, userdata, message):
    msg = message.payload.decode("utf-8")
    splitPacket = msg.split(";")
    roll = float(splitPacket[0])*toRad
    pitch = float(splitPacket[1])*toRad
    yaw = float(splitPacket[2])*toRad+np.pi
    print("Roll = ", roll*toDeg, " Pitch = ", pitch*toDeg, "Yaw = ", yaw*toDeg)
    rate(50)
    k = vector(cos(yaw)*cos(pitch), sin(pitch), sin(yaw)*cos(pitch))
    y = vector(0, 1, 0)
    s = cross(k, y)
    v = cross(s, k)

    frontArrow.axis = k
    sideArrow.axis = s
    upArrow.axis = v
    myObj.axis = k
    myObj.up = v
    sideArrow.length = 2
    frontArrow.length = 4
    upArrow.length = 1


client = mqtt.Client("P1")
client.connect("localhost")
client.subscribe("bno055")
client.on_message = on_message
client.loop_start()
