#      ----------------------------------------------------------------------------
#      "THE TSCHUNK-WARE LICENSE" (Revision 23):
#      Niklas Roy wrote this file. As long as you retain this notice you
#      can do whatever you want with this stuff. If we meet some day, and you think
#      this stuff is worth it, you can buy me a tschunk in return
#      ----------------------------------------------------------------------------

import display
import bhi160
import math
import light_sensor

WIDTH = 160
HEIGHT = 80

display_brightness = 30
target_brightness  = 30

mini_alphabet=(
    ((0,0,0,0,0,0,0), # 32:' '
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0)),
    
    ((0,0,1,0,0,0,0), #!
     (0,0,1,0,0,0,0),
     (0,0,1,0,0,0,0),
     (0,0,1,0,0,0,0),
     (0,0,1,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,1,0,0,0,0)),
    
    ((0,0,1,0,1,0,0), #"
     (0,0,1,0,1,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0)),
    
    ((0,0,1,0,1,0,0), ##
     (0,0,1,0,1,0,0),
     (1,1,1,1,1,1,1),
     (0,0,1,0,1,0,0),
     (1,1,1,1,1,1,1),
     (0,0,1,0,1,0,0),
     (0,0,1,0,1,0,0)),
    
    ((0,0,0,1,0,0,0), #$
     (1,1,1,1,1,1,1),
     (1,0,0,1,0,0,0),
     (1,1,1,1,1,1,1),
     (0,0,0,1,0,0,1),
     (1,1,1,1,1,1,1),
     (0,0,0,1,0,0,0)),
    
    ((0,0,0,0,0,0,1), #%
     (0,1,0,0,0,1,0),
     (0,0,0,0,1,0,0),
     (0,0,0,1,0,0,0),
     (0,0,1,0,0,0,0),
     (0,1,0,0,0,1,0),
     (1,0,0,0,0,0,0)),
    
    ((0,0,1,1,0,0,0), #&
     (0,1,0,0,1,0,0),
     (0,0,1,0,1,0,0),
     (0,1,1,1,1,0,1),
     (1,0,0,0,1,1,0),
     (1,0,0,0,1,1,0),
     (0,1,1,1,0,0,1)),
    
    ((0,0,0,1,0,0,0), #'
     (0,0,0,1,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0)),
    
    ((0,0,0,0,1,0,0), #(
     (0,0,0,1,0,0,0),
     (0,0,1,0,0,0,0),
     (0,0,1,0,0,0,0),
     (0,0,1,0,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,0,1,0,0)),
    
    ((0,0,1,0,0,0,0), #)
     (0,0,0,1,0,0,0),
     (0,0,0,0,1,0,0),
     (0,0,0,0,1,0,0),
     (0,0,0,0,1,0,0),
     (0,0,0,1,0,0,0),
     (0,0,1,0,0,0,0)),
    
    ((0,0,0,1,0,0,0), #*
     (0,1,0,1,0,1,0),
     (0,0,1,1,1,0,0),
     (1,1,1,1,1,1,1),
     (0,0,1,1,1,0,0),
     (0,1,0,1,0,1,0),
     (0,0,0,1,0,0,0)),
    
    ((0,0,0,1,0,0,0), #+
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0),
     (1,1,1,1,1,1,1),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0)),
    
    ((0,0,0,0,0,0,0), #,
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0)),
    
    ((0,0,0,0,0,0,0), #-
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (1,1,1,1,1,1,1),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0)),
    
    ((0,0,0,0,0,0,0), #.
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (1,0,0,0,0,0,0)),
    
    ((0,0,0,0,0,0,1), #/
     (0,0,0,0,0,1,0),
     (0,0,0,0,1,0,0),
     (0,0,0,1,0,0,0),
     (0,0,1,0,0,0,0),
     (0,1,0,0,0,0,0),
     (1,0,0,0,0,0,0)),
    
    ((1,1,1,1,1,1,1), #0
     (1,0,0,0,0,1,1),
     (1,0,0,0,1,0,1),
     (1,0,0,1,0,0,1),
     (1,0,1,0,0,0,1),
     (1,1,0,0,0,0,1),
     (1,1,1,1,1,1,1)),
    
    ((0,1,1,1,0,0,0), #1
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0),
     (1,1,1,1,1,1,1)),
    
    ((1,1,1,1,1,1,1), #2
     (0,0,0,0,0,0,1),
     (0,0,0,0,0,0,1),
     (1,1,1,1,1,1,1),
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0),
     (1,1,1,1,1,1,1)),
    
    ((1,1,1,1,1,1,1), #3
     (0,0,0,0,0,0,1),
     (0,0,0,0,0,0,1),
     (0,0,1,1,1,1,1),
     (0,0,0,0,0,0,1),
     (0,0,0,0,0,0,1),
     (1,1,1,1,1,1,1)),
    
    ((1,0,0,0,0,0,0), #4
     (1,0,0,1,0,0,0),
     (1,0,0,1,0,0,0),
     (1,1,1,1,1,1,1),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0)),
    
    ((1,1,1,1,1,1,1), #5
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0),
     (1,1,1,1,1,1,1),
     (0,0,0,0,0,0,1),
     (0,0,0,0,0,0,1),
     (1,1,1,1,1,1,1)),
    
    ((1,1,1,1,1,1,1), #6
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0),
     (1,1,1,1,1,1,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,1,1,1,1,1,1)),
    
    ((1,1,1,1,1,1,1), #7
     (0,0,0,0,0,1,0),
     (0,0,0,0,1,0,0),
     (0,1,1,1,1,1,0),
     (0,0,1,0,0,0,0),
     (0,1,0,0,0,0,0),
     (1,0,0,0,0,0,0)),
    
    ((1,1,1,1,1,1,1), #8
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,1,1,1,1,1,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,1,1,1,1,1,1)),
    
    ((1,1,1,1,1,1,1), #9
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,1,1,1,1,1,1),
     (0,0,0,0,0,0,1),
     (0,0,0,0,0,0,1),
     (1,1,1,1,1,1,1)),
    
    ((0,0,0,0,0,0,0), #:
     (0,0,0,1,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,0,0,0,0)),
    
    ((0,0,0,0,0,0,0), #;
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0)),
    
    ((0,0,0,0,1,0,0), #<
     (0,0,0,1,0,0,0),
     (0,0,1,0,0,0,0),
     (0,1,0,0,0,0,0),
     (0,0,1,0,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,0,1,0,0)),
    
    ((0,0,0,0,0,0,0), #=
     (0,0,0,0,0,0,0),
     (1,1,1,1,1,1,1),
     (0,0,0,0,0,0,0),
     (1,1,1,1,1,1,1),
     (0,0,0,0,0,0,0),
     (0,0,0,0,0,0,0)),
    
    ((0,0,1,0,0,0,0), #>
     (0,0,0,1,0,0,0),
     (0,0,0,0,1,0,0),
     (0,0,0,0,0,1,0),
     (0,0,0,0,1,0,0),
     (0,0,0,1,0,0,0),
     (0,0,1,0,0,0,0)),
    
    ((1,1,1,1,1,1,1), #?
     (0,0,0,0,0,0,1),
     (0,0,0,0,0,0,1),
     (0,0,0,1,1,1,1),
     (0,0,0,1,0,0,0),
     (0,0,0,0,0,0,0),
     (0,0,0,1,0,0,0)),
    
    ((1,1,1,1,1,1,1), #@
     (1,0,0,0,0,0,1),
     (1,0,1,1,1,0,1),
     (1,0,1,0,1,0,1),
     (1,0,1,1,1,1,1),
     (1,0,0,0,0,0,0),
     (1,1,1,1,1,1,1)),
    
    ((0,0,0,1,0,0,0), #A
     (0,0,1,0,1,0,0),
     (0,1,0,0,0,1,0),
     (1,1,1,1,1,1,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1)),
    
    ((1,1,1,1,1,0,0), #B
     (1,0,0,0,1,0,0),
     (1,0,0,0,1,0,0),
     (1,1,1,1,1,1,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,1,1,1,1,1,1)),
    
    ((0,0,1,1,1,1,1), #C
     (0,1,0,0,0,0,0),
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0),
     (0,1,0,0,0,0,0),
     (0,0,1,1,1,1,1)),
    
    ((1,1,1,1,1,0,0), #D
     (1,0,0,0,0,1,0),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,1,0),
     (1,1,1,1,1,0,0)),
    
    ((1,1,1,1,1,1,1), #E
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0),
     (1,1,1,1,1,0,0),
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0),
     (1,1,1,1,1,1,1)),
    
    ((1,1,1,1,1,1,1), #F
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0),
     (1,1,1,1,1,0,0),
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0)),
    
    ((1,1,1,1,1,1,1), #G
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0),
     (1,0,0,1,1,1,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,1,1,1,1,1,1)),
    
    ((1,0,0,0,0,0,1), #H
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,1,1,1,1,1,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1)),
    
    ((1,1,1,1,1,1,1), #I
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0),
     (1,1,1,1,1,1,1)),
    
    ((1,1,1,1,1,1,1), #J
     (0,0,0,0,0,0,1),
     (0,0,0,0,0,0,1),
     (0,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (0,1,0,0,0,1,0),
     (0,0,1,1,1,0,0)),
    
    ((1,0,0,0,0,0,1), #K
     (1,0,0,0,0,1,0),
     (1,0,0,0,1,0,0),
     (1,1,1,1,1,1,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1)),
    
    ((1,0,0,0,0,0,0), #L
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0),
     (1,1,1,1,1,1,1)),
    
    ((1,0,0,0,0,0,1), #M
     (1,1,0,0,0,1,1),
     (1,0,1,0,1,0,1),
     (1,0,0,1,0,0,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1)),
    
    ((1,0,0,0,0,0,1), #N
     (1,1,0,0,0,0,1),
     (1,0,1,0,0,0,1),
     (1,0,0,1,0,0,1),
     (1,0,0,0,1,0,1),
     (1,0,0,0,0,1,1),
     (1,0,0,0,0,0,1)),
    
    ((1,1,1,1,1,1,1), #O
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,1,1,1,1,1,1)),
    
    ((1,1,1,1,1,1,1), #P
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,1,1,1,1,1,1),
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0)),
    
    ((1,1,1,1,1,1,1), #Q
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,1,0,1),
     (1,0,0,0,0,1,0),
     (1,1,1,1,1,0,1)),
    
    ((1,1,1,1,1,1,1), #R
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,1,1,1,1,1,1),
     (1,0,0,0,1,0,0),
     (1,0,0,0,0,1,0),
     (1,0,0,0,0,0,1)),
    
    ((1,1,1,1,1,1,1), #S
     (1,0,0,0,0,0,0),
     (1,0,0,0,0,0,0),
     (1,1,1,1,1,1,1),
     (0,0,0,0,0,0,1),
     (0,0,0,0,0,0,1),
     (1,1,1,1,1,1,1)),
    
    ((1,1,1,1,1,1,1), #T
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0)),
    
    ((1,0,0,0,0,0,1), #U
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,1,1,1,1,1,1)),
    
    ((1,0,0,0,0,0,1), #V
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (0,1,0,0,0,1,0),
     (0,0,1,0,1,0,0),
     (0,0,0,1,0,0,0)),
    
    ((1,0,0,0,0,0,1), #W
     (1,0,0,0,0,0,1),
     (1,0,0,0,0,0,1),
     (1,0,0,1,0,0,1),
     (1,0,1,0,1,0,1),
     (1,1,0,0,0,1,1),
     (1,0,0,0,0,0,1)),
    
    ((1,0,0,0,0,0,1), #X
     (0,1,0,0,0,1,0),
     (0,0,1,0,1,0,0),
     (0,0,0,1,0,0,0),
     (0,0,1,0,1,0,0),
     (0,1,0,0,0,1,0),
     (1,0,0,0,0,0,1)),
    
    ((1,0,0,0,0,0,1), #Y
     (0,1,0,0,0,1,0),
     (0,0,1,0,1,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0),
     (0,0,0,1,0,0,0)),
    
    ((1,1,1,1,1,1,1), #Z
     (0,0,0,0,0,1,0),
     (0,0,0,0,1,0,0),
     (0,0,0,1,0,0,0),
     (0,0,1,0,0,0,0),
     (0,1,0,0,0,0,0),
     (1,1,1,1,1,1,1)))
    


#-------------------------------------------------------------- mini text
def mini_text(text,x_text,y_text,text_col, fast_update = False):

    for i in range(len(text)):
        c = ord(text[i])
        if c>96 and c<123: # a->A ... z->Z
            c-=32
        c-=32
        if c<0 or c>58:
            c=0
        if x_text < WIDTH:
            if c==0:
                x_text += 10 # increment start position of next character
            else:    
                for x in range(7): # show character
                    for y in range(7):
                        if (mini_alphabet[c][y][x]==1):
                            x_px =  x  + x_text 
                            y_px =  y  + y_text
                            if x_px>=0 and x_px<WIDTH and y_px>=0 and y_px<HEIGHT:
                                disp.pixel(x_px, y_px,col=text_col)

                if c==14:
                    x_text += 4 # increment start position of next character after '.' 
                else:
                    x_text += 10 # increment start position of next character

            
            if i%4 == 0 and i>0 and fast_update == True:
                disp.update()
        




#-------------------------------------------------------------- robust line (coordinates can be floats and outside screen - line will still be displayed)
def r_line(x1,y1,x2,y2,color=(255,255,255)):

    if x1>=0 and x1<= WIDTH and  x2>=0 and x2<= WIDTH and y1>=0 and y1<= HEIGHT and y2>=0 and y2<= HEIGHT: # line is on screen
        disp.line(int(x1),int(y1),int(x2),int(y2),col=color)
    else:
        
        if x1>WIDTH:
            dxa = x1  - x2     # x-distance
            dxb = WIDTH - x2

            r=1
            if dxa>0:
                r   = dxb / dxa
            
            dy  = y1 - y2      # y-distance
            dy  = dy * r
            y1  = y2 + dy
            x1  = WIDTH

        elif x1<0:
            dxa = x2  - x1     # x-distance
            dxb = 0 - x2
            r=1
            if dxa>0:
                r   = dxb / dxa
            dy  = y2 - y1      # y-distance
            dy  = dy * r
            y1  = y2 + dy
            x1  = 0

        if x2>WIDTH:
            dxa = x2  - x1    # x-distance
            dxb = WIDTH - x1
            r=1
            if dxa>0:
                r   = dxb / dxa
            dy  = y2 - y1      # y-distance
            dy  = dy * r
            y2  = y1 + dy
            x2  = WIDTH

        elif x2<0:
            dxa = x1  - x2     # x-distance
            dxb = 0 - x1
            r=1
            if dxa>0:
                r   = dxb / dxa
            dy  = y1 - y2      # y-distance
            dy  = dy * r
            y2  = y1 + dy
            x2  = 0
            
        if y1>HEIGHT:
            dya = y1  - y2     # y-distance
            dyb = HEIGHT - y2
            r=1
            if dya>0:
                r   = dyb / dya
            dx  = x1 - x2      # x-distance
            dx  = dx * r
            x1  = x2 + dx
            y1  = HEIGHT

        elif y1<0:
            dya = y2  - y1     # y-distance 
            dyb = 0 - y2
            r=1
            if dya>0:
                r   = dyb / dya
            dx  = x2 - x1      # x-distance 
            dx  = dx * r
            x1  = x2 + dx
            y1  = 0

        if y2>HEIGHT:
            dya = y2  - y1    # y-distance
            dyb = HEIGHT - y1
            r=1
            if dya>0:
                r   = dyb / dya
            dx  = x2 - x1      # x-distance
            dx  = dx * r
            x2  = x1 + dx
            y2  = HEIGHT

        elif y2<0:
            dya = y1  - y2     # y-distance
            dyb = 0 - y1
            r=1
            if dya>0:
                r   = dyb / dya
            dx  = x1 - x2      # x-distance
            dx  = dx * r
            x2  = x1 + dx
            y2  = 0
        
        disp.line(constrain(int(x1),0,WIDTH),constrain(int(y1),0,HEIGHT),constrain(int(x2),0,WIDTH),constrain(int(y2),0,HEIGHT),col=color)

    return
#-------------------------------------------------------------- rotation matrices

pi = 3.14159265359

def rot_y(p=(1,1,1),a=0):
    x =  math.cos(a) * p[0] + math.sin(a) * p[2]
    y =  p[1]
    z = -math.sin(a) * p[0] + math.cos(a) * p[2]
    return (x,y,z)

def rot_x(p=(1,1,1),a=0):
    x =  p[0]
    y =  math.cos(a) * p[1] + math.sin(a) * p[2]
    z = -math.sin(a) * p[1] + math.cos(a) * p[2]
    return (x,y,z)

def rot_z(p=(1,1,1),a=0):
    a=-a
    
    x =  math.cos(a) * p[0] + math.sin(a) * p[1]
    y = -math.sin(a) * p[0] + math.cos(a) * p[1]
    z =  p[2]
    return (x,y,z)

#-------------------------------------------------------------- constrain
def constrain(value, minimum, maximum):
    if value<minimum:
        value=minimum
    if value>maximum:
        value=maximum
    return value


#-------------------------------------------------------------- read bhi10
magnet = bhi160.BHI160Magnetometer(sample_rate=20)

mx=0
my=0
mz=0
ms=0

def read_magnetometer():
    global mx
    global my
    global mz
    global ms
    #.......................................................... read magnetometer

    samples = magnet.read()
    
    if len(samples) > 0:                     
        sample = samples[len(samples)-1]
          
        mx =   -sample.x 
        my =   sample.y 
        mz =   sample.z
        ms =   sample.status
        
    return 



#-------------------------------------------------------------- draw bargraph

def bargraph_log(y_pos,value,label):
    
    value_string = '%i' % value
    while len(value_string)<4:
        value_string =' '+value_string
        
    color = (255,255,255)
    if abs(value)>=950:
        color=(255,64,64)
        value_string='----'
            
    mini_text(label+':'+value_string,94,y_pos,grey)
    y_pos +=10
    value = constrain( int(  math.sqrt(abs(int(value)))/31.62*35 ),0,35 )
        
    disp.rect(85,y_pos,159,6+y_pos,col=dk_grey,filled=False)     # frame
    if mx<0:
        disp.rect(122-value,2+y_pos,122,4+y_pos,col=color,filled=True) # -bar
    else:
        disp.rect(122,2+y_pos,122+value,4+y_pos,col=color,filled=True) # +bar
        
    disp.line(122,y_pos,122,6+y_pos,col=dk_grey)                 # 0-line


#-------------------------------------------------------------- 3d model magnet

red  =  (255,64,64) # virtual magnet colors
grn  =  (64,255,64)
wht  =  (255,255,255)
grey =  (180,180,180)
dk_grey=(120,120,120)

lines = ((( -2, -1,  1 ),(-2, -1,-1), red),  
         (( -2,  1,  1 ),(-2,  1,-1), red),
         (( -2, -1,  1 ),(-2,  1, 1), red),
         (( -2, -1, -1 ),(-2,  1,-1), red),
        
         (( -2,  1,  1 ),( 0,  1, 1), red),
         (( -2, -1,  1 ),( 0, -1, 1), red),
         (( -2,  1, -1 ),( 0,  1,-1), red),
         (( -2, -1, -1 ),( 0, -1,-1), red),

         ((  2, -1,  1 ),( 2, -1,-1), grn),
         ((  2,  1,  1 ),( 2,  1,-1), grn),
         ((  2, -1,  1 ),( 2,  1, 1), grn),
         ((  2, -1, -1 ),( 2,  1,-1), grn),
        
         ((  2,  1,  1 ),( 0,  1, 1), grn),
         ((  2, -1,  1 ),( 0, -1, 1), grn),
         ((  2,  1, -1 ),( 0,  1,-1), grn),
         ((  2, -1, -1 ),( 0, -1,-1), grn),
        
         ((  0, -1,  1 ),( 0, -1,-1), wht),
         ((  0,  1,  1 ),( 0,  1,-1), wht),
         ((  0, -1,  1 ),( 0,  1, 1), wht),
         ((  0, -1, -1 ),( 0,  1,-1), wht),

         (( -2, .5,-.5 ),(-2,-.5,-.5), red), # N
         (( -2,-.5,-.5 ),(-2, .5, .5), red),
         (( -2, .5, .5 ),(-2,-.5, .5), red),
         
         ((  2, .5, .5 ),( 2, .5,-.5), grn), # S
         ((  2, .5,-.5 ),( 2,  0,-.5), grn),
         ((  2,  0,-.5 ),( 2,  0, .5), grn), 
         ((  2,  0, .5 ),( 2,-.5, .5), grn), 
         ((  2,-.5, .5 ),( 2,-.5,-.5), grn))

        

        
############################################################### main

with display.open() as disp:

    while True:
        disp.clear()
        read_magnetometer()

        # calculate magnetic field magnitude and rotation angles 
        z_angle = 0
        if mx!=0:
            z_angle  = math.atan(my/mx)
        if mx<0:
            z_angle += pi
        y_angle = 0
        xy_magnitude = math.sqrt(pow(mx,2)+pow(my,2))
        if xy_magnitude!=0:
            y_angle = math.atan(mz/xy_magnitude)
        xyz_magnitude=math.sqrt(pow(xy_magnitude,2)+pow(mz,2))

        # create list of lines which are rotated in 3d and calculate depth of center of line
        z_lines=[[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0],[0,0,0,0,[0,0,0],0]]
        x_shift = -41
        persp = 5
        zoom=2.3
        count = 0
        for l in lines:
            color=l[2]
            if abs(mx)>=950 or abs(my)>=950 or abs(mz)>=950:
                color=grey

            l_start = rot_y(l[0], y_angle)
            l_end   = rot_y(l[1], y_angle)
            
            l_start = rot_z(l_start, z_angle)
            l_end   = rot_z(l_end  , z_angle)
            
            screen_s_x = int( ( l_start[0] * zoom * ( l_start[2] + persp ) ) + 80 + x_shift)
            screen_s_y = int( ( l_start[1] * zoom * ( l_start[2] + persp ) ) + 40)
            screen_e_x = int( ( l_end[0]   * zoom * ( l_end[2]   + persp ) ) + 80 + x_shift)
            screen_e_y = int( ( l_end[1]   * zoom * ( l_end[2]   + persp ) ) + 40)
            
            depth = constrain(((l_start[2]+l_end[2])/2 + 2)/4,0,1)*.6+.6
            color_out=(constrain(int(color[0]*depth),0,255),constrain(int(color[1]*depth),0,255),constrain(int(color[2]*depth),0,255))

            z_lines[count]=[screen_s_x,screen_s_y,screen_e_x,screen_e_y,color_out,depth]

            count += 1

        # z-sorting
        for i in range(len(z_lines)):
            max_depth = 100
            max_depth_index = 0
            count =-1
            # find deepest line
            for l in z_lines: 
                count +=1
                if l[5]<max_depth:
                    max_depth = l[5]
                    max_depth_index=count
            # draw current deepest line 
            r_line(z_lines[max_depth_index][0],z_lines[max_depth_index][1],z_lines[max_depth_index][2],z_lines[max_depth_index][3],z_lines[max_depth_index][4])
            # clear current deepest line in original list
            z_lines[max_depth_index][5]=100


        for i in range(0,360,30):
            
            r_line(39+math.sin(i/180*pi-z_angle)*39,40+math.cos(i/180*pi-z_angle)*39,39+math.sin((i+15)/180*pi-z_angle)*39,40+math.cos((i+15)/180*pi-z_angle)*39,(120,120,120))

        bargraph_log(8,mx,'X')
        bargraph_log(29,my,'Y')
        bargraph_log(50,mz,'Z')

        #...................................................... adjust display brightness to environment

        target_brightness =(target_brightness *50 + (light_sensor.read()*17 - 180))/51

        if target_brightness < display_brightness-2:
            display_brightness -= 1
        if target_brightness > display_brightness+2:
            display_brightness = int(target_brightness+1)
        display_brightness = constrain(display_brightness,5,100)

        #...................................................... update screen           
        disp.backlight(display_brightness).update()