from PyQt5 import QtWidgets
from PyQt5.QtGui import QImage,QPixmap
from PyQt5.QtCore import QThread,pyqtSignal as Signal,pyqtSlot as Slot
import cv2,imutils
import sys
class MyThread(QThread):
    frame_signal = Signal(QImage)
    def run(self):
        self.cap = cv2.VideoCapture(0)
        while self.cap.isOpened():
            _,frame = self.cap.read()
            frame = self.cvimage_to_label(frame)
            self.frame_signal.emit(frame)
    def cvimage_to_label(self,image):
        image = imutils.resize(image,width = 640)
        image = cv2.cvtColor(image,cv2.COLOR_BGR2RGB)
        image = QImage(image,
                       image.shape[1],
                       image.shape[0],
                       QImage.Format_RGB888)
        return image
class MainApp(QtWidgets.QMainWindow):
    def __init__(self):
        super().__init__()
        self.init_ui()
        self.show()
    def init_ui(self):
        self.setFixedSize(640,640)
        self.setWindowTitle("Camera FeedBack")
        widget = QtWidgets.QWidget(self)
        layout = QtWidgets.QVBoxLayout()
        widget.setLayout(layout)
        self.label = QtWidgets.QLabel()
        layout.addWidget(self.label)
        self.open_btn = QtWidgets.QPushButton("Open The Camera", clicked=self.open_camera)
        layout.addWidget(self.open_btn)
        self.camera_thread = MyThread()
        self.camera_thread.frame_signal.connect(self.setImage)
        self.setCentralWidget(widget)
    def open_camera(self):        
        self.camera_thread.start()
        print(self.camera_thread.isRunning())
    @Slot(QImage)
    def setImage(self,image):
        self.label.setPixmap(QPixmap.fromImage(image))
if __name__ == "__main__":
    app = QtWidgets.QApplication([])
    main_window = MainApp()
    sys.exit(app.exec())

#----------------------------------------------------------------------------------------
Kp = 0.02  # Proportional gain
Ki = 0.001  # Integral gain
Kd = 0.05 # Derivative gain

# Initialize PID state variables
integral_pan = 0
integral_tilt = 0
previous_error_pan = 0
previous_error_tilt = 0

def track_face_pid(face_center_x, face_center_y):
    global pan_angle, tilt_angle
    global integral_pan, integral_tilt, previous_error_pan, previous_error_tilt
    
    # Calculate the error from the center positions
    pan_error = abs(center_x - face_center_x)
    tilt_error = abs(center_y - face_center_y)

    # Calculate the integral term
    integral_pan += pan_error
    integral_pan = max(-10, min(10, integral_pan))
    integral_tilt += tilt_error
    integral_tilt = max(-10, min(10, integral_tilt))

    # Calculate the derivative term
    derivative_pan = pan_error - previous_error_pan
    derivative_tilt = tilt_error - previous_error_tilt

    # Calculate PID output
    pan_output = Kp * pan_error + Ki * integral_pan + Kd * derivative_pan
    tilt_output = Kp * tilt_error + Ki * integral_tilt + Kd * derivative_tilt

    # Update previous error
    previous_error_pan = pan_error
    previous_error_tilt = tilt_error

    if face_center_x < center_x - tolerance:
        # pan_angle -= horizontal_step  # Move left min 0
        pan_angle = max(pan_min, min(pan_max, int(pan_angle-pan_output)))
    elif face_center_x > center_x + tolerance:
        # pan_angle += horizontal_step  # Move right max 180
        pan_angle = max(pan_min, min(pan_max, int(pan_angle+pan_output)))

    # Tilt servo control (vertical)
    if face_center_y < center_y - tolerance:
        # tilt_angle -= vertical_step  # Move up min 0
        tilt_angle = max(tilt_min, min(tilt_max, int(tilt_angle-tilt_output)))
    elif face_center_y > center_y + tolerance:
        # tilt_angle += vertical_step  # Move down max 180
        tilt_angle = max(tilt_min, min(tilt_max, int(tilt_angle+tilt_output)))

    # Send angles to Arduino
    move_servos(pan_angle, tilt_angle)
#--------------------------------------------------------------------------------