import logging
import sys
import keyboard
import numpy as np
import time
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from matplotlib import style

import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.positioning.motion_commander import MotionCommander

from cflib.utils.multiranger import Multiranger
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.syncLogger import SyncLogger
from cflib.crazyflie.commander import Commander


URI = 'radio://0/80/2M'
desiredHeight =0.5
desiredSpeed =0
heightOld =0
kp =10
kv =10
maxThrust = 42*9.8/1000
maxThrustRatio =0.8

hs =[]
vs =[]
fig = plt.figure()
ax1 = fig.add_subplot(1,1,1)

def animate(i):
    ax1.clear()
    ax1.plot(hs)
    

if len(sys.argv) > 1:
    URI = sys.argv[1]
#I =(10**-6)*np.array([16.572, 0.831,0.718],[0.831, 16.656,1.8],[0.718,1.8,29.262])
I =1
crazyflie ={'mass':0.03,
            #'I':I,
            #'invI':np.linalg.inv(I),
            }

if __name__ == '__main__':
    anim = animation.FuncAnimation(fig, animate, interval=1000)
    # Initialize the low-level drivers (don't list the debug drivers)
    lg = LogConfig(name='Stabilizer', period_in_ms=10)
    lg.add_variable('stateEstimate.vz','float')
    lg.add_variable('stateEstimate.z','float')

    cflib.crtp.init_drivers(enable_debug_driver=False)

    cf = Crazyflie(rw_cache='./cache')
    with SyncCrazyflie(URI, cf=cf) as scf:
        scf.cf.param.set_value('kalman.resetEstimation', '1')
        time.sleep(0.1)
        scf.cf.param.set_value('kalman.resetEstimation', '0')


        with SyncLogger(scf, lg) as logger:
            with Multiranger(scf) as mr:
                for log_entry in logger:
                    data = log_entry[1]
                    #print(data)
                    #detect the current height of the quadcopter
                    #height= data['stateEstimate.z']                    
                    height = mr.down
                    if(height is None):
                        height =0
                    velocity = data['stateEstimate.vz']
                    hs.append(height)
                    vs.append(velocity)
                    
                    #find the error of the current and desired
                    error = desiredHeight-height
                    errorV = desiredSpeed -velocity
                    #use the error to compute the gain from PD controller
                    F = crazyflie['mass']*(kp*error +kv*errorV+9.81)
                    print([height], [F])
                    FB =F*maxThrustRatio*65535/maxThrust
                    if FB > maxThrustRatio*65535:
                        FB = maxThrustRatio*65535
                    if FB<0:
                        FB =0
                    print(FB)
                    #send the thrust computed. (Assume yaw pitch & roll required are 0)
                    scf.cf.commander.send_setpoint(0,0,0,int(FB))
    
                    if(keyboard.is_pressed('q')):
                        scf.cf.commander.send_stop_setpoint()
                        break
                        
    print('Demo terminated!')