# Gantry
import matrix
import LCD
import math
import time
from machine import ADC


def GantryDynamics(X, U):
    x = X[0]
    q = X[1]
    dx = X[2]
    dq = X[3]
    g  = 9.8
    M = [[3, math.cos(q)],[math.cos(q),1]]
    C = [[dq*dq*math.sin(q)],[-g*math.sin(q)]]
    F = [[U],[0]]
    Mi = matrix.inv(M)
    MC = matrix.mult(Mi,C)
    MF = matrix.mult(Mi,F)
    ddX = matrix.add(MC,MF)
    dX = [dx,dq,ddX[0][0],ddX[1][0]]
    return(dX)

def GantryDisplay(X, Color):
    x = X[0]
    q = X[1]
    x0 = 240 + x*100
    y0 = 100
    x1 = x0 + 100*math.sin(q)
    y1 = y0 + 100*math.cos(q)
    
    Navy = LCD.RGB(0,0,5)
    White = LCD.RGB(150,150,150)
    LCD.Line(0,y0,479,y0,White)
    LCD.Line(240,y0-5,240,y0+5,White)
    LCD.Box(x0-20,y0,x0+20,y0-20,Color)
    LCD.Line(x0,y0,x1,y1,Color)
    LCD.Circle(x1,y1,5,Color)

def Integrate(X, dX, dt):
    for i in range(0,3):
        Y[i] = X[i] + dX[i] * dt
    return(Y)


a2d0 = machine.ADC(0)
F0 = a2d0.read_u16() 

White = LCD.RGB(150,150,150)
Navy = LCD.RGB(0,0,5)

LCD.Init()
LCD.Clear(Navy)

k = 50 / 32000

X = [-2,0,0,0]
dt = 0.02
t = 0
while(t < 10):
    F = ( a2d0.read_u16() - F0 ) * k
    dX = GantryDynamics(X, F)
    GantryDisplay(X,Navy)
    X = Integrate(X, dX, dt)
    t = t + dt
    GantryDisplay(X,White)
    time.sleep(0.01)