# encoding=utf-8

"""程序缺陷:
    1. 没有进行相机标定： 没有测量不同角度对应不同分辨率的像素情况
    2. 没有添加测距模块: 太远太近对于程序的参数都有所不同，采集的情况也会不一样
    3. 相机比较便宜，采集图像精度不足
    4. 定位的算法性能不足，Orin无法实现实时监测，只能跳帧，并添加额外的编程思路进行稳定性调整
    5. 没有实现预期目标，判断正脸以及侧脸
    6. 没有实现上位机，没有实现图片采集归档模块
"""


import time
import cv2
from Drivers.Servo import SERVO_HOR, SERVO_VER, HorServo, VerServo
from Service.Detect import Detect
from Service.Trace import Trace

# camera
default_size = (1024, 768)

# performance optimize
actually_size = (default_size[0] // 2, default_size[1] // 2)
detect = Detect(cap_size=actually_size, fps=30, buffer_size=30)

# servo
default_h_angle, default_v_angle = 72.5, 75
now_h_angle, now_v_angle = 72.5, 75
# best param: 40, 105
h_s = HorServo(SERVO_HOR, default_h_angle, 10, 170)
# best param 30, 120
v_s = VerServo(SERVO_VER, default_v_angle, 10, 170)
time.sleep(1)

# trace
trace = Trace(actually_size[0], actually_size[1], tolerance=(0.05, 0.05))

# frame count
frame_count = 0
# about 0.5s for nx to optimize servo
# fps=30, so 15 frame do step
# really optimize
frame_step = 5  # about (1 / 30) * 5 = 0.167s
lost_count = 0
lost_step = int(2 / frame_step * 30)  # 2s

if __name__ == '__main__':
    try:
        while True:
            buffer = detect.capture_wrapper.out_stack()
            if buffer is not None:
                frame_count = (frame_count + 1) % frame_step
                cv2.imshow("ori", buffer)
                cv2.waitKey(1)

            if frame_count == 0:
                buffer, face, marks = detect.get_detect()

                if (buffer is not None) and (face is None):
                    lost_count = (lost_count + 1) % lost_step
                    if lost_count == 0:
                        print("rollback")
                        ret = h_s.to_angle(default_h_angle)
                        if ret != -1:
                            now_h_angle = ret
                        ret = v_s.to_angle(default_v_angle)
                        if ret != -1:
                            now_v_angle = ret

                if face is not None:
                    # re-count
                    lost_count = 0

                    # center
                    # (top, right, bottom, left)
                    face = face[0]
                    top, right, bottom, left = face
                    now_x = (left + right) // 2
                    now_y = (top + bottom) // 2

                    # optimize servo location
                    delta_x, delta_angle = trace.get_next_position(now_x, now_y, (16, 12))
                    delta_angle_x, delta_angle_y = delta_angle
                    print("---------------Servo Info Start-----------------------")
                    if delta_angle_x != -1:
                        new_h_angle = now_h_angle + delta_angle_x
                        ret = h_s.to_angle(new_h_angle)
                        if ret != -1:
                            now_h_angle = ret
                            print("now h_angle is: ", now_h_angle)
                        else:
                            print("Horizontal location {} is over".format(new_h_angle))

                    if delta_angle_y != -1:
                        new_v_angle = now_v_angle + delta_angle_y
                        ret = v_s.to_angle(new_v_angle)
                        if ret != -1:
                            now_v_angle = ret
                            print("now v_angle is: ", now_v_angle)
                        else:
                            print("Vertical location {} is over".format(new_v_angle))
                    print("-------------------------------------------------------")

                    print("----------------Location Debug---------------------")
                    # landmarks
                    for each_key in marks[0]:
                        points = marks[0][each_key]
                        for point in points:
                            cv2.circle(buffer, point, 1, (0, 0, 255), -1)
                    # face
                    cv2.rectangle(buffer, (face[3], face[0]), (face[1], face[2]), (0, 255, 0), 2)
                    # face center point
                    cv2.circle(buffer, (now_x, now_y), 5, (0, 255, 0), -1)
                    # buffer center point
                    cv2.circle(buffer, (actually_size[0] // 2, actually_size[1] // 2), 5, (255, 0, 0), -1)
                    # show
                    cv2.imshow("debug", buffer)
                    cv2.waitKey(1)
                    # debug info
                    print("center_x", actually_size[0] // 2, "center_y", actually_size[1] // 2)
                    print("now_x", now_x, "now_y", now_y)
                    # print("delta_x", delta_x[0], "delta_y", delta_x[1])
                    print("angle_x", delta_angle[0], "angle_y", delta_angle[1])
                    print("----------------------------------------------------")

    finally:
        h_s.clean_up()
        v_s.clean_up()