from crazy import CrazyDragon

import time

from cflib.crazyflie.log        import LogConfig
from cflib.crazyflie.syncLogger import SyncLogger



orientation_std_dev = 8.0e-3


def adjust_orientation_sensitivity( cf: CrazyDragon ):
    cf.param.set_value('locSrv.extQuatStdDev', orientation_std_dev)


def activate_kalman_estimator( cf: CrazyDragon ):
    cf.param.set_value('stabilizer.estimator', '2')

    # Set the std deviation for the quaternion data pushed into the
    # kalman filter. The default value seems to be a bit too low.
    cf.param.set_value('locSrv.extQuatStdDev', 0.06)


def reset_estimator( cf: CrazyDragon ):
    cf.param.set_value('kalman.resetEstimation', '1')
    time.sleep(0.1)
    cf.param.set_value('kalman.resetEstimation', '0')

    # time.sleep(1)
    wait_for_position_estimator( cf )


def wait_for_position_estimator( cf: CrazyDragon ):
    print('Waiting for estimator to find position...')

    log_config = LogConfig(name='Kalman Variance', period_in_ms=10)
    log_config.add_variable('kalman.varPX', 'FP16')
    log_config.add_variable('kalman.varPY', 'FP16')
    log_config.add_variable('kalman.varPZ', 'FP16')

    

    var_y_history = [1000] * 10
    var_x_history = [1000] * 10
    var_z_history = [1000] * 10

    threshold = 0.001

    with SyncLogger( cf, log_config ) as logger:
        for log_entry in logger:
            data = log_entry[1]

            var_x_history.append( data['kalman.varPX'] )
            var_x_history.pop( 0 )
            var_y_history.append( data['kalman.varPY'] )
            var_y_history.pop( 0 )
            var_z_history.append( data['kalman.varPZ'] )
            var_z_history.pop( 0 )

            min_x = min( var_x_history )
            max_x = max( var_x_history )
            min_y = min( var_y_history )
            max_y = max( var_y_history )
            min_z = min( var_z_history )
            max_z = max( var_z_history )

            if ( max_x - min_x ) < threshold and (
                    max_y - min_y ) < threshold and (
                    max_z - min_z ) < threshold:
                break