def step(self, dt):
    'step once by dt seconds'
    self.time_elapsed += dt
    D = cdist(self.state[:, :3], self.state[:, 3:6], 'euclidean')
    (ind, din) = np.where((D > 122))
    uniqua = (ind == din)
    ind = ind[uniqua]
    for i in zip(ind):
        v = self.state[(i, 8)]
        v_avg = v
        a_ver = self.acc_vert
        a_ver_eff = self.acc_vert_eff
        height = (self.max_height - self.state[(i, 2)])
        print(height)
        if (height > 0):
            n = 1
            if (v > 0):
                n = (v / abs(v))
            stop = ((n * (v ** 2)) / (2 * a_ver))
            t_end = abs((v / a_ver))
            b1 = (((v ** 2) + (t_end ** 2)) ** 0.5)
            b2 = ((((v + ((n * a_ver) * dt)) ** 2) + ((t_end + dt) ** 2)) ** 0.5)
            s1 = ((((a_ver * dt) ** 2) + (dt ** 2)) ** 0.5)
            s2 = (dt * 2)
            P = (((b2 - b1) + s1) + s2)
            t = (((((P / 2) * ((P / 2) - s1)) * ((P / 2) - s2)) * (((P / 2) - b2) + b1)) ** 0.5)
            h = ((2 * t) / (b2 - b1))
            area = (n * (t + ((b2 - b1) * h)))
            if ((t_end <= dt) and (stop > (height - area))):
                v_avg = 0
                self.state[(i, 8)] = 0
                self.state[(i, 2)] = self.max_height
            elif (stop > (height - area)):
                t_max = 0
                if (stop < height):
                    a = (2 * (a_ver ** 2))
                    b = ((4 * a_ver) * v)
                    c = ((v ** 2) - ((2 * a_ver) * height))
                    t_max = (((- b) + (((b ** 2) - ((4 * a) * c)) ** 0.5)) / (2 * a))
                v_max = (v + (a_ver * (t_max / dt)))
                v_end = (((2 * v_max) - v) - (a_ver * dt))
                v_avg = ((((v_max + v) / 2) * (t_max / dt)) + (((v_max + v_end) / 2) * ((dt - t_max) / dt)))
                self.state[(i, 8)] = v_end
            else:
                v_avg = (v + ((a_ver * dt) / 2))
                self.state[(i, 8)] += (a_ver * dt)
        elif (height < 0):
            n = (v / abs(v))
            stop = ((n * (v ** 2)) / (2 * a_ver_eff))
            t_end = abs((v / a_ver_eff))
            b1 = (((v ** 2) + (t_end ** 2)) ** 0.5)
            b2 = ((((v + ((n * a_ver_eff) * dt)) ** 2) + ((t_end + dt) ** 2)) ** 0.5)
            s1 = ((((a_ver_eff * dt) ** 2) + (dt ** 2)) ** 0.5)
            s2 = (dt * 2)
            P = (((b2 - b1) + s1) + s2)
            t = (((((P / 2) * ((P / 2) - s1)) * ((P / 2) - s2)) * (((P / 2) - b2) + b1)) ** 0.5)
            h = ((2 * t) / (b2 - b1))
            area = (n * (t + ((b2 - b1) * h)))
            if ((t_end <= dt) and (abs(stop) <= abs(height))):
                v_avg = ((v / 2) * (t_end / dt))
                self.state[(i, 8)] = (v + (a_ver_eff * t_end))
            elif (stop < (height - area)):
                v_max = ((height * (2 * a_ver_eff)) ** 0.5)
                t_max = ((v_max - v) / a_ver_eff)
                v_end = (((2 * v_max) - v) - (a_ver_eff * dt))
                v_avg = ((((v_max + v) / 2) * (t_max / dt)) + (((v_max + v_end) / 2) * ((dt - t_max) / dt)))
                self.state[(i, 8)] = v_end
            else:
                v_avg = (v - ((a_ver_eff * dt) / 2))
                self.state[(i, 8)] = (v - (a_ver_eff * dt))
        else:
            self.state[(i, 8)] += (0 * dt)
        self.state[(i, 2)] += (v_avg * dt)
        r = (self.state[i, 3:5] - self.state[i, :2])
        m = np.linalg.norm(r)
        u = (r / m)
        a_hor = self.acc_hor
        v_hor = self.state[i, 6:8]
        h = np.linalg.norm(v_hor)
        stop = ((h ** 2) / (2 * a_hor))
        t_end = (h / a_hor)
        b1 = (((h ** 2) + (t_end ** 2)) ** 0.5)
        b2 = ((((h + (a_hor * dt)) ** 2) + ((t_end + dt) ** 2)) ** 0.5)
        s1 = ((((a_hor * dt) ** 2) + (dt ** 2)) ** 0.5)
        s2 = (dt * 2)
        P = (((b2 - b1) + s1) + s2)
        t = (((((P / 2) * ((P / 2) - s1)) * ((P / 2) - s2)) * (((P / 2) - b2) + b1)) ** 0.5)
        s = ((2 * t) / (b2 - b1))
        area = (t + ((b2 - b1) * s))
        if ((t_end <= dt) and (stop < area)):
            v_hor = ((h / 2) * (t_end / dt))
            self.state[i, 6:8] = ((h - (a_hor * t_end)) * u)
        elif (stop > (m - area)):
            v_max = ((m * (2 * a_hor)) ** 0.5)
            t_max = ((v_max - h) / a_hor)
            v_end = (((2 * v_max) - h) - (a_hor * dt))
            v_hor = ((((v_max + h) / 2) * (t_max / dt)) + (((v_max + v_end) / 2) * ((dt - t_max) / dt)))
            self.state[i, 6:8] = (v_end * u)
        else:
            v_hor = (h + ((a_hor * dt) / 2))
            self.state[i, 6:8] = ((h + (a_hor * dt)) * u)
        self.state[i, :2] += ((v_hor * dt) * u)
    (done, fund) = np.where((D <= 122))
    uniquo = (done == fund)
    done = done[uniquo]
    for d in zip(done):
        print('here')
        v = self.state[(i, 8)]
        v_avg = v
        a_ver_eff = self.acc_vert_eff
        n = (- 1)
        if (v < 0):
            n = (v / abs(v))
        stop = ((n * (v ** 2)) / (2 * a_ver_eff))
        t_end = abs((v / a_ver_eff))
        b1 = (((v ** 2) + (t_end ** 2)) ** 0.5)
        b2 = ((((v + ((n * a_ver_eff) * dt)) ** 2) + ((t_end + dt) ** 2)) ** 0.5)
        s1 = ((((a_ver_eff * dt) ** 2) + (dt ** 2)) ** 0.5)
        s2 = (dt * 2)
        P = (((b2 - b1) + s1) + s2)
        t = (((((P / 2) * ((P / 2) - s1)) * ((P / 2) - s2)) * (((P / 2) - b2) + b1)) ** 0.5)
        h = ((2 * t) / (b2 - b1))
        area = (n * (t + ((b2 - b1) * h)))
        if ((t_end <= dt) and (stop > area)):
            v_avg = ((v / 2) * (t_end / dt))
            self.state[(i, 8)] = (v + (a_ver_eff * t_end))
            self.state[(i, 9)] = self.time_elapsed
        elif (stop < ((- self.state[(i, 2)]) - area)):
            v_max = (((- self.state[(i, 2)]) * (2 * a_ver_eff)) ** 0.5)
            t_max = ((v_max - v) / a_ver_eff)
            v_end = (((2 * v_max) - v) - (a_ver_eff * dt))
            v_avg = ((((v_max + v) / 2) * (t_max / dt)) + (((v_max + v_end) / 2) * ((dt - t_max) / dt)))
            self.state[(i, 8)] = v_end
        else:
            v_avg = (v - ((a_ver_eff * dt) / 2))
            self.state[(i, 8)] = (v - (a_ver_eff * dt))
        self.state[(i, 2)] += (v_avg * dt)
    E = squareform(pdist(self.state[:, :3], 'euclidean'))
    (ind1, ind2) = np.where((E < (2 * self.size)))
    unique = (ind1 < ind2)
    ind1 = ind1[unique]
    ind2 = ind2[unique]
    for (i1, i2) in zip(ind1, ind2):
        if (self.state[(i1, 2)] > self.state[(i2, 2)]):
            self.state[(i1, 8)] += (self.acc_vert * dt)
            self.state[(i2, 8)] -= (self.acc_vert_eff * dt)
        else:
            self.state[(i1, 8)] -= (self.acc_vert * dt)
            self.state[(i2, 8)] += (self.acc_vert_eff * dt)
    if (self.obstacles > 0):
        DO = np.vstack([self.state[:, :3].copy(), self.obs_state.copy()])
        F = squareform(pdist(DO, 'euclidean'))
        (d_rone, obs) = np.where((F < (2 * self.obstacles_size)))
        unique = ((d_rone < obs) and (obs >= self.drones))
        d_rone = d_rone[unique]
        obs = obs[unique]
        for (d, o) in zip(d_rone, obs):
            if ((self.obs_state[((o - self.drones), 2)] < 110) and (self.state[(d, 2)] < self.obs_state[((o - self.drones), 2)])):
                self.state[(d, 8)] += (self.acc_vert * dt)
            else:
                r = (self.state[d, 3:5] - self.state[d, :2])
                ro = (self.obs_state[(o - self.drones), :2] - self.state[d, :2])
                r_rel = np.cross(r, ro)
                if (r_rel[2] > 0):
                    self.state[(d, 6)] += (self.acc_hor * dt)
                    self.state[(d, 7)] += (self.acc_hor * dt)
                else:
                    self.state[(d, 6)] -= (self.acc_hor * dt)
                    self.state[(d, 7)] -= (self.acc_hor * dt)
    np.clip(self.state[:, 6], ((- self.max_speed) + self.wind[0]), (self.max_speed + self.wind[0]))
    np.clip(self.state[:, 7], ((- self.max_speed) + self.wind[1]), (self.max_speed + self.wind[1]))
step once by dt seconds
def step(self, dt):
    
    self.time_elapsed += dt
    D = cdist(self.state[:, :3], self.state[:, 3:6], 'euclidean')
    (ind, din) = np.where((D > 122))
    uniqua = (ind == din)
    ind = ind[uniqua]
    for i in zip(ind):
        v = self.state[(i, 8)]
        v_avg = v
        a_ver = self.acc_vert
        a_ver_eff = self.acc_vert_eff
        height = (self.max_height - self.state[(i, 2)])
        print(height)
        if (height > 0):
            n = 1
            if (v > 0):
                n = (v / abs(v))
            stop = ((n * (v ** 2)) / (2 * a_ver))
            t_end = abs((v / a_ver))
            b1 = (((v ** 2) + (t_end ** 2)) ** 0.5)
            b2 = ((((v + ((n * a_ver) * dt)) ** 2) + ((t_end + dt) ** 2)) ** 0.5)
            s1 = ((((a_ver * dt) ** 2) + (dt ** 2)) ** 0.5)
            s2 = (dt * 2)
            P = (((b2 - b1) + s1) + s2)
            t = (((((P / 2) * ((P / 2) - s1)) * ((P / 2) - s2)) * (((P / 2) - b2) + b1)) ** 0.5)
            h = ((2 * t) / (b2 - b1))
            area = (n * (t + ((b2 - b1) * h)))
            if ((t_end <= dt) and (stop > (height - area))):
                v_avg = 0
                self.state[(i, 8)] = 0
                self.state[(i, 2)] = self.max_height
            elif (stop > (height - area)):
                t_max = 0
                if (stop < height):
                    a = (2 * (a_ver ** 2))
                    b = ((4 * a_ver) * v)
                    c = ((v ** 2) - ((2 * a_ver) * height))
                    t_max = (((- b) + (((b ** 2) - ((4 * a) * c)) ** 0.5)) / (2 * a))
                v_max = (v + (a_ver * (t_max / dt)))
                v_end = (((2 * v_max) - v) - (a_ver * dt))
                v_avg = ((((v_max + v) / 2) * (t_max / dt)) + (((v_max + v_end) / 2) * ((dt - t_max) / dt)))
                self.state[(i, 8)] = v_end
            else:
                v_avg = (v + ((a_ver * dt) / 2))
                self.state[(i, 8)] += (a_ver * dt)
        elif (height < 0):
            n = (v / abs(v))
            stop = ((n * (v ** 2)) / (2 * a_ver_eff))
            t_end = abs((v / a_ver_eff))
            b1 = (((v ** 2) + (t_end ** 2)) ** 0.5)
            b2 = ((((v + ((n * a_ver_eff) * dt)) ** 2) + ((t_end + dt) ** 2)) ** 0.5)
            s1 = ((((a_ver_eff * dt) ** 2) + (dt ** 2)) ** 0.5)
            s2 = (dt * 2)
            P = (((b2 - b1) + s1) + s2)
            t = (((((P / 2) * ((P / 2) - s1)) * ((P / 2) - s2)) * (((P / 2) - b2) + b1)) ** 0.5)
            h = ((2 * t) / (b2 - b1))
            area = (n * (t + ((b2 - b1) * h)))
            if ((t_end <= dt) and (abs(stop) <= abs(height))):
                v_avg = ((v / 2) * (t_end / dt))
                self.state[(i, 8)] = (v + (a_ver_eff * t_end))
            elif (stop < (height - area)):
                v_max = ((height * (2 * a_ver_eff)) ** 0.5)
                t_max = ((v_max - v) / a_ver_eff)
                v_end = (((2 * v_max) - v) - (a_ver_eff * dt))
                v_avg = ((((v_max + v) / 2) * (t_max / dt)) + (((v_max + v_end) / 2) * ((dt - t_max) / dt)))
                self.state[(i, 8)] = v_end
            else:
                v_avg = (v - ((a_ver_eff * dt) / 2))
                self.state[(i, 8)] = (v - (a_ver_eff * dt))
        else:
            self.state[(i, 8)] += (0 * dt)
        self.state[(i, 2)] += (v_avg * dt)
        r = (self.state[i, 3:5] - self.state[i, :2])
        m = np.linalg.norm(r)
        u = (r / m)
        a_hor = self.acc_hor
        v_hor = self.state[i, 6:8]
        h = np.linalg.norm(v_hor)
        stop = ((h ** 2) / (2 * a_hor))
        t_end = (h / a_hor)
        b1 = (((h ** 2) + (t_end ** 2)) ** 0.5)
        b2 = ((((h + (a_hor * dt)) ** 2) + ((t_end + dt) ** 2)) ** 0.5)
        s1 = ((((a_hor * dt) ** 2) + (dt ** 2)) ** 0.5)
        s2 = (dt * 2)
        P = (((b2 - b1) + s1) + s2)
        t = (((((P / 2) * ((P / 2) - s1)) * ((P / 2) - s2)) * (((P / 2) - b2) + b1)) ** 0.5)
        s = ((2 * t) / (b2 - b1))
        area = (t + ((b2 - b1) * s))
        if ((t_end <= dt) and (stop < area)):
            v_hor = ((h / 2) * (t_end / dt))
            self.state[i, 6:8] = ((h - (a_hor * t_end)) * u)
        elif (stop > (m - area)):
            v_max = ((m * (2 * a_hor)) ** 0.5)
            t_max = ((v_max - h) / a_hor)
            v_end = (((2 * v_max) - h) - (a_hor * dt))
            v_hor = ((((v_max + h) / 2) * (t_max / dt)) + (((v_max + v_end) / 2) * ((dt - t_max) / dt)))
            self.state[i, 6:8] = (v_end * u)
        else:
            v_hor = (h + ((a_hor * dt) / 2))
            self.state[i, 6:8] = ((h + (a_hor * dt)) * u)
        self.state[i, :2] += ((v_hor * dt) * u)
    (done, fund) = np.where((D <= 122))
    uniquo = (done == fund)
    done = done[uniquo]
    for d in zip(done):
        print('here')
        v = self.state[(i, 8)]
        v_avg = v
        a_ver_eff = self.acc_vert_eff
        n = (- 1)
        if (v < 0):
            n = (v / abs(v))
        stop = ((n * (v ** 2)) / (2 * a_ver_eff))
        t_end = abs((v / a_ver_eff))
        b1 = (((v ** 2) + (t_end ** 2)) ** 0.5)
        b2 = ((((v + ((n * a_ver_eff) * dt)) ** 2) + ((t_end + dt) ** 2)) ** 0.5)
        s1 = ((((a_ver_eff * dt) ** 2) + (dt ** 2)) ** 0.5)
        s2 = (dt * 2)
        P = (((b2 - b1) + s1) + s2)
        t = (((((P / 2) * ((P / 2) - s1)) * ((P / 2) - s2)) * (((P / 2) - b2) + b1)) ** 0.5)
        h = ((2 * t) / (b2 - b1))
        area = (n * (t + ((b2 - b1) * h)))
        if ((t_end <= dt) and (stop > area)):
            v_avg = ((v / 2) * (t_end / dt))
            self.state[(i, 8)] = (v + (a_ver_eff * t_end))
            self.state[(i, 9)] = self.time_elapsed
        elif (stop < ((- self.state[(i, 2)]) - area)):
            v_max = (((- self.state[(i, 2)]) * (2 * a_ver_eff)) ** 0.5)
            t_max = ((v_max - v) / a_ver_eff)
            v_end = (((2 * v_max) - v) - (a_ver_eff * dt))
            v_avg = ((((v_max + v) / 2) * (t_max / dt)) + (((v_max + v_end) / 2) * ((dt - t_max) / dt)))
            self.state[(i, 8)] = v_end
        else:
            v_avg = (v - ((a_ver_eff * dt) / 2))
            self.state[(i, 8)] = (v - (a_ver_eff * dt))
        self.state[(i, 2)] += (v_avg * dt)
    E = squareform(pdist(self.state[:, :3], 'euclidean'))
    (ind1, ind2) = np.where((E < (2 * self.size)))
    unique = (ind1 < ind2)
    ind1 = ind1[unique]
    ind2 = ind2[unique]
    for (i1, i2) in zip(ind1, ind2):
        if (self.state[(i1, 2)] > self.state[(i2, 2)]):
            self.state[(i1, 8)] += (self.acc_vert * dt)
            self.state[(i2, 8)] -= (self.acc_vert_eff * dt)
        else:
            self.state[(i1, 8)] -= (self.acc_vert * dt)
            self.state[(i2, 8)] += (self.acc_vert_eff * dt)
    if (self.obstacles > 0):
        DO = np.vstack([self.state[:, :3].copy(), self.obs_state.copy()])
        F = squareform(pdist(DO, 'euclidean'))
        (d_rone, obs) = np.where((F < (2 * self.obstacles_size)))
        unique = ((d_rone < obs) and (obs >= self.drones))
        d_rone = d_rone[unique]
        obs = obs[unique]
        for (d, o) in zip(d_rone, obs):
            if ((self.obs_state[((o - self.drones), 2)] < 110) and (self.state[(d, 2)] < self.obs_state[((o - self.drones), 2)])):
                self.state[(d, 8)] += (self.acc_vert * dt)
            else:
                r = (self.state[d, 3:5] - self.state[d, :2])
                ro = (self.obs_state[(o - self.drones), :2] - self.state[d, :2])
                r_rel = np.cross(r, ro)
                if (r_rel[2] > 0):
                    self.state[(d, 6)] += (self.acc_hor * dt)
                    self.state[(d, 7)] += (self.acc_hor * dt)
                else:
                    self.state[(d, 6)] -= (self.acc_hor * dt)
                    self.state[(d, 7)] -= (self.acc_hor * dt)
    np.clip(self.state[:, 6], ((- self.max_speed) + self.wind[0]), (self.max_speed + self.wind[0]))
    np.clip(self.state[:, 7], ((- self.max_speed) + self.wind[1]), (self.max_speed + self.wind[1]))