# Path: python/py_quad_control/controller/sparc.py
# class SparcController:
# class DataCloud:
#     def __init__(self, control_range, ref_range, input_size, init_input, init_ref, init_y, monotonicity=1,
#                  dc_radius_const=0.5):
#     def update_reference_range(self, refmin, refmax):
#     def update(self, curr_x, curr_y, curr_ref, prev_u):
#     def get_global_density(self, z):
#     def __init__(self, z, initial_variance, radius_update_const=0.5):
#     def update_focal_point(self, z):
#     def __update_radius__(self):
#     def __update_variance_and_centroid__(self, curr_z):
#     def add_point(self, curr_z):
#     def get_local_density(self, x):
#     def update_consequent(self, prev_md, prev_ref, curr_y, prev_u, c, umin, umax):
#     def set_consequent(self, new_consequent):
#     def get_consequent(self):


import numpy as np
import math
from ...controller import sparc
        curr_e = 0
    else:
        curr_e = ref - y

    # Calculate previous error
    prev_e = refprev - yprev

    # Generate Inputs
    x = np.array([curr_e, (curr_e - prev_e)])
    return x


class NavigationController:

    def __init__(self, initial_longitude_input, initial_latitude_input, initial_position_reference, current_position,
                 controller_lat_init=None, controller_long_init=None):
        
        self.longitude_u = 0.0
        self.latitude_u = 0.0
        self.previous_position = np.array([0, 0])
        self.previous_position_reference = np.array([0, 0])

        if controller_long_init is None:
            self.longitudeController = sparc.SparcController((UMIN_LON, UMAX_LON), (REFMIN_LON, REFMAX_LON), X_SIZE,
                                                             initial_longitude_input, initial_position_reference[0],
                                                             current_position[0])
        else:
            self.longitudeController = controller_long_init

        if controller_long_init is None:

# Path: python/py_quad_control/controller/sparc.py
# class SparcController:
# class DataCloud:
#     def __init__(self, control_range, ref_range, input_size, init_input, init_ref, init_y, monotonicity=1,
#                  dc_radius_const=0.5):
#     def update_reference_range(self, refmin, refmax):
#     def update(self, curr_x, curr_y, curr_ref, prev_u):
#     def get_global_density(self, z):
#     def __init__(self, z, initial_variance, radius_update_const=0.5):
#     def update_focal_point(self, z):
#     def __update_radius__(self):
#     def __update_variance_and_centroid__(self, curr_z):
#     def add_point(self, curr_z):
#     def get_local_density(self, x):
#     def update_consequent(self, prev_md, prev_ref, curr_y, prev_u, c, umin, umax):
#     def set_consequent(self, new_consequent):
#     def get_consequent(self):


# Path: python/py_quad_control/vrep_sim/sparc/navigationcontroller.py
import numpy as np
import math
from ...controller import sparc

        curr_e = 0
    else:
        curr_e = ref - y

    # Calculate previous error
    prev_e = refprev - yprev

    # Generate Inputs
    x = np.array([curr_e, (curr_e - prev_e)])
    return x


class NavigationController:

    def __init__(self, initial_longitude_input, initial_latitude_input, initial_position_reference, current_position,
                 controller_lat_init=None, controller_long_init=None):
        
        self.longitude_u = 0.0
        self.latitude_u = 0.0
        self.previous_position = np.array([0, 0])
        self.previous_position_reference = np.array([0, 0])

        if controller_long_init is None:
            self.longitudeController = sparc.SparcController((UMIN_LON, UMAX_LON), (REFMIN_LON, REFMAX_LON), X_SIZE,
                                                             initial_longitude_input, initial_position_reference[0],
                                                             current_position[0])
        else:
            self.longitudeController = controller_long_init

        if controller_long_init is None:

            self.latitudeController = sparc.SparcController((UMIN_LAT, UMAX_LAT), (REFMIN_LAT, REFMAX_LAT), X_SIZE,