# SPDX-FileCopyrightText: 2021 ladyada for Adafruit Industries
# SPDX-License-Identifier: MIT

import array
import time
import board
import busio
import adafruit_mpu6050
import pwmio
import math
import digitalio
import sys

cycleTime = 0.1
jumpTime = 0.007
idleFrequency = 100

firstLight = digitalio.DigitalInOut(board.GP16)
firstLight.direction = digitalio.Direction.OUTPUT

secondLight = digitalio.DigitalInOut(board.GP17)
secondLight.direction = digitalio.Direction.OUTPUT

thirdLight = digitalio.DigitalInOut(board.GP18)
thirdLight.direction = digitalio.Direction.OUTPUT

fourthLight = digitalio.DigitalInOut(board.GP19)
fourthLight.direction = digitalio.Direction.OUTPUT

fifthLight = digitalio.DigitalInOut(board.GP20)
fifthLight.direction = digitalio.Direction.OUTPUT

sixthLight = digitalio.DigitalInOut(board.GP21)
sixthLight.direction = digitalio.Direction.OUTPUT

seventhLight = digitalio.DigitalInOut(board.GP22)
seventhLight.direction = digitalio.Direction.OUTPUT

eigthLight = digitalio.DigitalInOut(board.GP26)
eigthLight.direction = digitalio.Direction.OUTPUT

ninthLight = digitalio.DigitalInOut(board.GP27)
ninthLight.direction = digitalio.Direction.OUTPUT

tenthLight = digitalio.DigitalInOut(board.GP28)
tenthLight.direction = digitalio.Direction.OUTPUT

leds = [firstLight, secondLight, thirdLight, fourthLight, fifthLight, sixthLight, seventhLight, eigthLight, ninthLight, tenthLight]

pwm = pwmio.PWMOut(board.GP0, duty_cycle=2 ** 15, frequency=idleFrequency, variable_frequency=True)
i2c = busio.I2C(board.GP5, board.GP4)
mpu = adafruit_mpu6050.MPU6050(i2c)

speedMeter = 0

def jump():
    jumpArray = []
    freqVal = 1000
    for i in range(50):
        freqVal = freqVal + 50
        pwm.frequency = freqVal
        time.sleep(0.007)

def throttle(status):
    if (status == "up"):
        pwm.frequency = 100 * (int)(math.fabs((mpu.acceleration[0] + 1)))
    else:
        pwm.frequency = idleFrequency

while True:
    print("Acceleration: X:%.2f, Y: %.2f, Z: %.2f m/s^2" % (mpu.acceleration))
    print("Gyro X:%.2f, Y: %.2f, Z: %.2f rad/s" % (mpu.gyro))
    #print("Temperature: %.2f C" % mpu.temperature)
    print("")

    speedMeter = int(math.fabs(math.ceil(mpu.acceleration[0] / 2)))
    slicedOnLeds = leds[:speedMeter]
    slicedOffLeds = leds[speedMeter:]
    for speedMeter in slicedOnLeds:
        speedMeter.value = True
    for speedMeter in slicedOffLeds:
        speedMeter.value = False

    xAccelerationDelta = int(math.fabs(math.ceil(mpu.acceleration[0] / 2)))
    zAccelerationDelta = (mpu.acceleration[2]) - 9.81
    
    if (xAccelerationDelta > 0):
        throttle("up")
    elif (xAccelerationDelta < 0):
        throttle("idle")
    
    if (zAccelerationDelta > 1):
        jump()

    time.sleep(cycleTime)