delete old code

from serial_msg import SerialWriter

# TODO: (x, y) -> (yaw, pitch)

#

# def get_converter(weight: int, height: int):

#     def f(n: float): # an odd function

#         return math.log(n + 1) if n >= 0 else math.log(1 - n)

#     def converter(x: int, y: int) -> tuple: # TODO

#         dx = x - weight // 2

#         dy = y - height // 2

#         return int(f(dx) * 8), int(f(dy) * 6)

#     return converter

#

# class Tracer:

#     """

#     (x, y) -> (delta_yaw, delta_pitch)

#     and keep movement smooth

#     """

#     def __init__(self, width, height):

#         self.step = 5

#         self.central_x = width >> 1

#         self.central_y = height >> 1

#

#     def convert(self, target: tuple) -> tuple:

#         if target is not None:

#             self.step = (self.step - 1) if self.step > 5 else 5

#             x, y = target

#             dx = x - self.central_x

#             dy = y - self.central_y

#             eta = self.step / math.sqrt(dx * dx + dy * dy)

#             return (-int(dx * eta), int(dy * eta)) if eta < 1 else target

#         else:

#             self.step = self.step + 1 if self.step < 50 else 50

#             return 0, 0

class Smoother:

    def __init__(self, shape):

from random import randint

# class BgrColor(Enum):

#     RED = (0, 0, 255)

#     BLUE = (255, 0, 0)

#     GREEN = (0, 255, 0)

#     YELLOW = (0, 255, 255)

#     PURPLE = (255, 0, 255)

BGR_RED = (0, 0, 255)

BGR_BLUE = (255, 0, 0)

BGR_GREEN = (0, 255, 0)

BGR_YELLOW = (0, 255, 255)

BGR_PURPLE = (255, 0, 255)

class BarColor(Enum):

            self.add_debug_img("Red to Blue", cv2.cvtColor(hsv_frame, cv2.COLOR_HSV2BGR))

        # TODO: decide the range of lightness based on histogram

        # lower_light = np.array([90, 0, 215])

        lower_light = np.array([90, 0, 210])

        upper_light = np.array([120, 100, 255])

        # lower_light = np.array([90, 0, 180])

        lower_light = np.array([90, 0, 200])

        upper_light = np.array([120, 130, 255])

        lower_halo  = np.array([90, 100, 185])

        upper_halo  = np.array([120, 255, 255])

        halo_area = cv2.inRange(hsv_frame, lower_halo, upper_halo)

        return light_area, halo_area

    @staticmethod

    def get_connected_components_info(frame):

        # cpn: component

        cpn_count, label_map, cpn_info, centroids = \

            cv2.connectedComponentsWithStats(image=frame, connectivity=4, ltype=cv2.CV_32S)

        def pack(label, info, centroid):

            northwest_x, northwest_y, width, height, pixel_number = info

            x, y = centroid

            return {

                "label": label,

                "size": (width, height),

                "centroid": (int(round(x)), int(round(y))),

                "northwest": (northwest_x, northwest_y),

                "southeast": (northwest_x + width, northwest_y + height),

                "pixels": pixel_number

            }

        # background removed

        return sorted(map(pack, range(0, cpn_count), cpn_info, centroids), key=lambda x: -x["pixels"])[1:], label_map

    @staticmethod

    def zoom_and_shift(area):

        nw_x, nw_y = area["northwest"]

        se_x, se_y = area["southeast"]

        nw_x -= area["width"] // 4

        se_x += area["width"] // 4

        nw_y -= area["height"] // 2

        return {

            "northwest": (nw_x, nw_y), "southeast": (se_x, se_y),

            "width": se_x - nw_x, "height": se_y - se_x

        }

    # @staticmethod

    # def select_lights_in_halo(light, halo):

    #     light_components, light_map = Detector.get_connected_components_info(light)

    #     halo_components= Detector.get_connected_components_info(halo)[0]

    #     selected_halo = list(filter(lambda c: c["pixels"] > 20, halo_components)) # TODO: eliminate magic number

    #

    #     # find halo area

    #     try:

    #         northwest_x = min([x["northwest"][0] for x in selected_halo])

    #         northwest_y = min([x["northwest"][1] for x in selected_halo])

    #         southeast_x = max([x["southeast"][0] for x in selected_halo])

    #         southeast_y = max([x["southeast"][1] for x in selected_halo])

    #     except ValueError: # selected_halo is []

    #         return [], None

    #     width = southeast_x - northwest_x

    #     height = southeast_y - northwest_y

    #

    #     # zoom and shift

    #     northwest_x -= width // 4

    #     southeast_x += width // 4

    #     northwest_y -= height // 2

    #

    #     # select inside ones

    #     def is_fully_inside(c):

    #         c_nw_x, c_nw_y = c["northwest"]

    #         c_se_x, c_se_y = c["southeast"]

    #         return northwest_x <= c_nw_x and northwest_y <= c_nw_y and c_se_x <= southeast_x and c_se_y <= southeast_y

    #     insides = list(filter(is_fully_inside, light_components)) # FIXME: why doesn't work if don't convert it to a list?

    #

    #     # add angle info

    #     for i in insides:

    #         nw_x, nw_y = i["northwest"]

    #         se_x, se_y = i["southeast"]

    #         top_line, bottom_line = light_map[nw_y, nw_x:se_x], light_map[se_y-1, nw_x:se_x]

    #         tx, ty = nw_x + np.argmax(top_line), nw_y           # top peak

    #         bx, by = nw_x + np.argmax(bottom_line), se_y-1      # bottom peak

    #         angle = math.atan2(by-ty, tx-bx) / math.pi          # unit: pi rad

    #         i["angle"] = angle

    #

    #     return insides, {"northwest": (northwest_x, northwest_y), "southeast": (southeast_x, southeast_y)}

    @staticmethod

    def select_valid_bars(lights):

        if lights == []:

            return []

        # eliminate small ones

        max_pixels = max([x["pixels"] for x in lights])

        big_enough = list(filter(lambda x: (x["pixels"] / max_pixels) > 0.06, lights))

        def is_bar_near(bar):

            width, height = bar["size"]

            square_ratio = height / width

            return 1.5 <= square_ratio < 8

        def is_bar_far(bar):

            width, height = bar["size"]

            square_ratio = height / width

            return 0.8 < square_ratio < 2

        selected = list(filter(is_bar_near, big_enough))

        if len(selected) < 2:

            selected = list(filter(is_bar_far, big_enough))

        vertical_bars = filter(lambda x: abs(x["angle"] - 0.5) < 0.2, selected)

        # filled_ratio test for removing components in strange shape

        def filled_ratio(bar):

            width, height = bar["size"]

            return -bar["pixels"] / (width * height)

        return sorted(vertical_bars, key=filled_ratio)[0:6]

    @staticmethod

    def select_pair(bars):

        length = len(bars)

        if length < 2:

            return ()

        pairs = [(i, j) for i in range(0, length-1) for j in range(i+1, length)]

        def y_dis(bar1, bar2):

            y1 = bar1["centroid"][1]

            y2 = bar2["centroid"][1]

            height_base = min((bar1["size"][1], bar2["size"][1]))

            return abs(y1 - y2) / height_base

        def square_ratio(bar1, bar2): # TODO

            pass

        def area(bar1, bar2):

            a1 = bar1["pixels"]

            a2 = bar2["pixels"]

            return abs(a1 - a2) / min((a1, a2))

        def parallel(bar1, bar2):

            theta1, theta2 = bar1["angle"], bar2["angle"]

            return abs(theta1 - theta2) * 5

        def judge(pair):

            index1, index2 = pair

            bar1, bar2 = bars[index1], bars[index2]

            policy = [(y_dis, 0.6), (area, 0.3), (parallel, 1.2)]

            # # debug

            # print(pair, 'y_dis:', y_dis(bar1, bar2), '\tarea:', area(bar1, bar2), '\tparallel:', parallel(bar1, bar2))

            return sum([func(bar1, bar2) * coefficient for func, coefficient in policy])

        judging_result = [{"indices": pair, "score": judge(pair)} for pair in pairs]

        winner = min(judging_result, key=lambda x: x["score"])

        # # debug

        # print("winner:", winner)

        if winner["score"] > 1.2:

            return ()

        i1, i2 = winner["indices"]

        return bars[i1], bars[i2]

    def get_lights(self, binary_img: np.ndarray):

        _, width = binary_img.shape

        # https://docs.opencv.org/2.4/modules/imgproc/doc/structural_analysis_and_shape_descriptors.html?highlight=findcontours#findcontours

        img, contours, hierarchy = cv2.findContours(binary_img, mode=cv2.RETR_EXTERNAL, method=cv2.CHAIN_APPROX_SIMPLE)

        rectangles_info = [cv2.minAreaRect(i) for i in contours if len(i) * 60 > width]  # filter (omit small ones) and map

        # rectangles_info = [cv2.fitEllipse(i) for i in contours if len(i) * 60 > width]

        min_width = width // 60

        rectangles_info = [cv2.minAreaRect(i) for i in contours if len(i) > min_width]  # filter (omit small ones) and map

        # rectangles_info = [cv2.fitEllipse(i) for i in contours if len(i) > min_width]

        return rectangles_info

    def halo_circle(self, binary_img: np.ndarray):

        height, width = binary_img.shape

        img, contours, hierarchy = cv2.findContours(binary_img, mode=cv2.RETR_EXTERNAL, method=cv2.CHAIN_APPROX_SIMPLE)

        selected_contours = [i for i in contours if len(i) * 60 > width]  # small contours omitted

        min_width = width // 60

        selected_contours = [i for i in contours if len(i) > min_width]  # small contours omitted

        if len(selected_contours) > 0:

            combined_halo_points = np.concatenate(selected_contours)

            original_center, original_radius = cv2.minEnclosingCircle(combined_halo_points)

            x, y = original_center

            center = round(x), round(y - original_radius * 0.6)  # shift up

            radius = round(original_radius * 1.8 if original_radius < (width / 4) else width * 0.375)  # zoom

            # zoom

            if original_radius > width / 4:

                radius = round(width * 0.375)

            elif width//15 < original_radius < width//4:

                radius = round(original_radius * 1.8)

            else:

                radius = round(width / 15)

        else: # no valid halo, consider the central area

            center = width // 2, height // 2

            radius = width // 3

        def judge(pair):

            i1, i2 = pair

            bar1, bar2 = lights[i1], lights[i2]

            policy = [(square_ratio, 5), (y_dis, 15), (size_similarity, 1), (parallel, 1)]  # [(func, coefficient),...]

            policy = [(square_ratio, 5), (y_dis, 18), (size_similarity, 1), (parallel, 1.2)]  # [(func, coefficient),...]

            return sum((func(bar1, bar2) * coefficient for func, coefficient in policy))  # weighted sum

        judge_results = [(pair, judge(pair)) for pair in pairs]

        def get_img_selected_pair():

            selected_pair_show = np.copy(self.frame)

            l1, l2 = lights[i1], lights[i2]

            self.debug_print("l1:", l1)

            self.debug_print("l2:", l2)

            self.debug_print("score:", winner_score)

            self.debug_print("parallel:", parallel(l1, l2))

            self.debug_print("size_similarity:", size_similarity(l1, l2))

            self.debug_print("square_ratio:", 5 * square_ratio(l1, l2))

            self.debug_print("y_dis:", 15 * y_dis(l1, l2))

            # self.debug_print("l1:", l1)

            # self.debug_print("l2:", l2)

            # self.debug_print("score:", winner_score)

            # self.debug_print("parallel:", 1.2 * parallel(l1, l2))

            # self.debug_print("size_similarity:", size_similarity(l1, l2))

            # self.debug_print("square_ratio:", 5 * square_ratio(l1, l2))

            # self.debug_print("y_dis:", 18 * y_dis(l1, l2))

            c1, s1, a1 = l1

            c2, s2, a2 = l2

            cv2.drawMarker(selected_pair_show, position=round_point(c1), color=(0, 255, 255), markerSize=15, thickness=2)

            cv2.drawMarker(selected_pair_show, position=round_point(c2), color=(0, 255, 255), markerSize=15, thickness=2)

            cv2.drawMarker(selected_pair_show, position=round_point(c1), color=BGR_YELLOW, markerSize=15, thickness=2)

            cv2.drawMarker(selected_pair_show, position=round_point(c2), color=BGR_YELLOW, markerSize=15, thickness=2)

            return selected_pair_show

        self.add_debug_img("Selected Pair", get_img_selected_pair)

        frame = cv2.pyrUp(cv2.pyrDown(frame)) # down-sample

        light, halo = self.hsv(frame)

        # lights_in_halo, selected_area = Detector.select_lights_in_halo(light, halo)

        halo_center, halo_radius = self.halo_circle(halo)

        lights_info = self.get_lights(light)

        vertical_lights = self.select_vertical_lights(lights_inside)

        target = self.get_armor(vertical_lights)

        # selected_bars = Detector.select_valid_bars(lights_in_halo)

        # selected_pair = Detector.select_pair(selected_bars)

        # if selected_pair != ():

        #     bar1, bar2 = selected_pair

        #     target = mid_point(bar1["centroid"], bar2["centroid"])

        # else:

        #     target = None

        #

        # # def get_img_selected_halo():

        # #     cv2.rectangle(halo, pt1=selected_area["northwest"], pt2=selected_area["southeast"], color=255, thickness=1)

        # #     return halo

        # # def get_img_selected_light():

        # #     cv2.rectangle(light, pt1=selected_area["northwest"], pt2=selected_area["southeast"], color=255, thickness=1)

        # #     return light

        # # if selected_area is not None:

        # #     self.add_debug_img("Halo", get_img_selected_halo)

        # #     self.add_debug_img("Light", get_img_selected_light)

        # def get_img_selected():

        #     selected = np.copy(self.frame)

        #     for each in selected_bars:

        #         cv2.drawMarker(img=selected, position=each["centroid"], color=(0, 255, 255), markerSize=25, thickness=2)

        #     for each in selected_pair:

        #         cv2.circle(img=selected, center=each["centroid"], radius=5, color=(0, 0, 255), thickness=-1)

        #     return selected

        # self.add_debug_img("Selected", get_img_selected)

        return target