import cv2
import mediapipe as mp
import time
import math
import serial

# シリアルポートの設定（Arduinoと接続されているポートを指定）
ser = serial.Serial('COM6', 9600)  # ポート名は随時変更確認！

# Mediapipeの初期化
mp_pose = mp.solutions.pose
pose = mp_pose.Pose()
mp_drawing = mp.solutions.drawing_utils

# カメラのキャプチャ（パソコンの内蔵カメラ”0”）
cap = cv2.VideoCapture(0)


#map_value関数
def map_value(value, min_input, max_input, min_output, max_output):
    # valueをmin_inputからmax_inputの範囲からmin_outputからmax_outputの範囲にマッピング
    return min_output + (max_output - min_output) * ((value - min_input) / (max_input - min_input))
#角度計算
def calculate_angle(x1, y1, x2, y2, x3, y3):
    # ベクトルABとBCを計算
    vec_AB = (x2 - x1, y2 - y1)
    vec_BC = (x3 - x2, y3 - y2)

    # ベクトルの内積と大きさを計算
    dot_product = vec_AB[0] * vec_BC[0] + vec_AB[1] * vec_BC[1]
    magnitude_AB = math.sqrt(vec_AB[0]**2 + vec_AB[1]**2)
    magnitude_BC = math.sqrt(vec_BC[0]**2 + vec_BC[1]**2)

    # 角度を計算（アークコサイン）
    angle_rad = math.acos(dot_product / (magnitude_AB * magnitude_BC))
    angle_deg = math.degrees(angle_rad)
    return angle_deg

while cap.isOpened():
    success, image = cap.read()
    if not success:
        print("カメラからフレームを取得できませんでした")
        break

    # Mediapipe用に画像をRGBに変換
    image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
    results = pose.process(image_rgb)

    if results.pose_landmarks:
        # ポーズランドマークを描画
        mp_drawing.draw_landmarks(image, results.pose_landmarks, mp_pose.POSE_CONNECTIONS)

        # ランドマークの位置を取得
        landmarks = results.pose_landmarks.landmark

        # ランドマーク15, 13, 11の間の角度を計算
        lm_15 = landmarks[mp_pose.PoseLandmark.LEFT_WRIST.value]
        lm_13 = landmarks[mp_pose.PoseLandmark.LEFT_ELBOW.value]
        lm_11 = landmarks[mp_pose.PoseLandmark.LEFT_SHOULDER.value]
        angle_15_13_11 = calculate_angle(lm_15.x, lm_15.y, lm_13.x, lm_13.y, lm_11.x, lm_11.y)
        
        # 角度をサーボモーターの角度に変換（30度から130度の範囲にマッピング）
        servo_angle = map_value(angle_15_13_11, 0, 180, 180, 20)

        # Arduinoに角度を送信
        ser.write(f"{int(servo_angle)}\n".encode())

        # ランドマークと対応するサーボの角度を表示
        cv2.putText(image, f'Servo: {int(servo_angle)}', (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2, cv2.LINE_AA)

    # 画像を表示
    cv2.imshow('Mediapipe Pose', image)

    # 'q'キーで終了
    if cv2.waitKey(5) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()
ser.close()

#角度計算
def calculate_angle(x1, y1, x2, y2, x3, y3):
    # ベクトルABとBCを計算
    vec_AB = (x2 - x1, y2 - y1)
    vec_BC = (x3 - x2, y3 - y2)

    # ベクトルの内積と大きさを計算　dot_productドット積は内積の意
    dot_product = vec_AB[0] * vec_BC[0] + vec_AB[1] * vec_BC[1]

def calculate_angle(x1, y1, x2, y2, x3, y3):
    # ベクトルABとBCを計算
    vec_AB = (x2 - x1, y2 - y1)
    vec_BC = (x3 - x2, y3 - y2)

    # ベクトルの内積と大きさを計算
    dot_product = vec_AB[0] * vec_BC[0] + vec_AB[1] * vec_BC[1]
    magnitude_AB = math.sqrt(vec_AB[0]**2 + vec_AB[1]**2)
    magnitude_BC = math.sqrt(vec_BC[0]**2 + vec_BC[1]**2)

   # 角度を計算（アークコサイン）
    angle_rad = math.acos(dot_product / (magnitude_AB * magnitude_BC))
    angle_deg = math.degrees(angle_rad)

angle_deg = math.degrees(angle_rad)

//************Arduino IDE*************
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

// PCA9685の初期化
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

// サーボの最小・最大パルス幅
#define SERVOMIN  150 // パルス幅最小値
#define SERVOMAX  600 // パルス幅最大値

// サーボのチャンネル
#define SERVO_0_CHANNEL 0

void setup() {
  Serial.begin(9600);
  pwm.begin();
  pwm.setPWMFreq(60);  // サーボ周波数を60Hzに設定
  delay(10);
}

void loop() {
  if (Serial.available()) {
    int angle = Serial.parseInt();

    // サーボ角度をPWM信号に変換して設定
    int pulse_0 = map(angle, 0, 180, SERVOMIN, SERVOMAX);
    pwm.setPWM(SERVO_0_CHANNEL, 0, pulse_0);
  }
}