#include <Servo.h>

// 방향 enum (Arduino 커뮤니티 권장 방식)
enum Direction
{
    LEFT,
    MIDDLE,
    RIGHT
};

// CDS 센서 핀 설정 (3개 센서: 왼쪽 A0, 가운데 A1, 오른쪽 A2)
const int CDS_PINS[3] = {A0, A1, A2};
const int CDS_COUNT = 3;

// 서보모터 핀 설정
const int SERVO_PIN = 9;

// 서보모터 각도 설정 (각 방향에 대응: 왼쪽 0°, 가운데 90°, 오른쪽 180°)
const int ANGLES[3] = {0, 90, 180};

// 방향 문자열 (Arduino 권장 방식: const char 배열)
const char DIRECTION_STRINGS[3][10] = {"왼쪽", "중간", "오른쪽"};

// 센서 값 비교 임계값 (±5)
const int TOLERANCE = 5;

// 서보모터 흔들림 제스처 설정
const int SHAKE_INTERVAL = 200;      // 200ms마다 좌우 전환 (2개 센서 같을 때)
const int SHAKE_INTERVAL_SLOW = 700; // 700ms마다 좌우 전환 (3개 센서 모두 같을 때)

// 서보모터 상태 저장 (흔들림 제스처용)
int shake_angle_1 = 0;
int shake_angle_2 = 0;
int current_shake_state = 0;
int current_shake_interval = 200; // 현재 사용 중인 흔들림 간격

// 직렬 통신 설정
const int SERIAL_BAUD = 9600;
const int DEBUG_INTERVAL = 500; // 500ms마다 디버깅 정보 출력

Servo servo;

void setup()
{
    // 서보모터 초기화
    servo.attach(SERVO_PIN);

    // CDS 핀 설정 (입력)
    for (int i = 0; i < CDS_COUNT; i++)
    {
        pinMode(CDS_PINS[i], INPUT);
    }

    // 시리얼 통신 초기화 (디버깅용)
    Serial.begin(SERIAL_BAUD);
    Serial.println("CDS Servo Tracker started!");
    Serial.println("Tracking brightest light with 3 CDS sensors.");
}

void loop()
{
    // 각 CDS 센서 값 읽기
    int cds_values[3];
    for (int i = 0; i < CDS_COUNT; i++)
    {
        cds_values[i] = analogRead(CDS_PINS[i]);
    }

    // 가장 낮은 CDS 값 (빛이 가장 강한 센서) 찾기
    int min_index = 0;
    int min_value = cds_values[0];
    for (int i = 1; i < CDS_COUNT; i++)
    {
        if (cds_values[i] < min_value)
        {
            min_value = cds_values[i];
            min_index = i;
        }
    }

    // 가장 높은 CDS 값 (빛이 가장 약한 센서) 찾기
    int max_index = 0;
    int max_value = cds_values[0];
    for (int i = 1; i < CDS_COUNT; i++)
    {
        if (cds_values[i] > max_value)
        {
            max_value = cds_values[i];
            max_index = i;
        }
    }

    // 센서 값 범위 계산
    int value_range = max_value - min_value;

    // 판단 로직
    if (value_range <= TOLERANCE)
    {
        // 모든 센서 값이 거의 같음 (3개 모두 같음) - 0도와 180도를 왔다갔다 (0.7초 간격)
        shake_angle_1 = ANGLES[0];                    // 0도
        shake_angle_2 = ANGLES[2];                    // 180도
        current_shake_interval = SHAKE_INTERVAL_SLOW; // 0.7초 간격
        moveServoBidirectional();
    }
    else if (min_value >= 1018)
    {
        // 가장 강한 빛이 1023 근처 (±5 범위) - 혼란 제스처 (0.7초 간격)
        shake_angle_1 = ANGLES[0];                    // 0도
        shake_angle_2 = ANGLES[2];                    // 180도
        current_shake_interval = SHAKE_INTERVAL_SLOW; // 0.7초 간격
        moveServoBidirectional();
    }
    else if (max_value <= 10)
    {
        // 가장 약한 빛이 거의 0 근처 (±5 범위) - 혼란 제스처 (0.7초 간격)
        shake_angle_1 = ANGLES[0];                    // 0도
        shake_angle_2 = ANGLES[2];                    // 180도
        current_shake_interval = SHAKE_INTERVAL_SLOW; // 0.7초 간격
        moveServoBidirectional();
    }
    else
    {
        // 2개 센서가 같은 경우 판단
        if (abs(cds_values[0] - cds_values[1]) <= TOLERANCE)
        {
            // 왼쪽(0도)과 중간(90도)이 같음 → 오른쪽과 비교하여 더 강한 쪽으로 이동
            if (cds_values[2] < cds_values[0])
            {
                // 오른쪽이 더 강함 → 180도로 이동
                servo.write(ANGLES[2]);
            }
            else if (cds_values[2] > cds_values[0])
            {
                // 왼쪽/중간이 더 강함 → 0도와 90도 사이 흔들림
                shake_angle_1 = ANGLES[0];               // 0도
                shake_angle_2 = ANGLES[1];               // 90도
                current_shake_interval = SHAKE_INTERVAL; // 200ms 간격
                moveServoBidirectional();
            }
            else
            {
                // 3개 모두 같음
                shake_angle_1 = ANGLES[0];                    // 0도
                shake_angle_2 = ANGLES[2];                    // 180도
                current_shake_interval = SHAKE_INTERVAL_SLOW; // 0.7초 간격
                moveServoBidirectional();
            }
        }
        else if (abs(cds_values[1] - cds_values[2]) <= TOLERANCE)
        {
            // 중간(90도)과 오른쪽(180도)이 같음 → 왼쪽과 비교하여 더 강한 쪽으로 이동
            if (cds_values[0] < cds_values[1])
            {
                // 왼쪽이 더 강함 → 0도로 이동
                servo.write(ANGLES[0]);
            }
            else if (cds_values[0] > cds_values[1])
            {
                // 중간/오른쪽이 더 강함 → 90도와 180도 사이 흔들림
                shake_angle_1 = ANGLES[1];               // 90도
                shake_angle_2 = ANGLES[2];               // 180도
                current_shake_interval = SHAKE_INTERVAL; // 200ms 간격
                moveServoBidirectional();
            }
            else
            {
                // 3개 모두 같음
                shake_angle_1 = ANGLES[0];                    // 0도
                shake_angle_2 = ANGLES[2];                    // 180도
                current_shake_interval = SHAKE_INTERVAL_SLOW; // 0.7초 간격
                moveServoBidirectional();
            }
        }
        else if (abs(cds_values[0] - cds_values[2]) <= TOLERANCE)
        {
            // 왼쪽(0도)과 오른쪽(180도)이 같음 → 중간과 비교하여 더 강한 쪽으로 이동
            if (cds_values[1] < cds_values[0])
            {
                // 중간이 더 강함 → 90도로 이동
                servo.write(ANGLES[1]);
            }
            else if (cds_values[1] > cds_values[0])
            {
                // 왼쪽/오른쪽이 더 강함 → 0도와 180도 사이 흔들림
                shake_angle_1 = ANGLES[0];               // 0도
                shake_angle_2 = ANGLES[2];               // 180도
                current_shake_interval = SHAKE_INTERVAL; // 200ms 간격
                moveServoBidirectional();
            }
            else
            {
                // 3개 모두 같음
                shake_angle_1 = ANGLES[0];                    // 0도
                shake_angle_2 = ANGLES[2];                    // 180도
                current_shake_interval = SHAKE_INTERVAL_SLOW; // 0.7초 간격
                moveServoBidirectional();
            }
        }
        else
        {
            // 명확한 빛 방향 감지 - 해당 방향으로 이동
            servo.write(ANGLES[min_index]);
        }
    }

    // 디버깅: 500ms마다 센서 값과 상태 출력
    static unsigned long last_print_time = 0;
    if (millis() - last_print_time >= DEBUG_INTERVAL)
    {
        Serial.print("CDS값: ");
        Serial.print(cds_values[0]);
        Serial.print(", ");
        Serial.print(cds_values[1]);
        Serial.print(", ");
        Serial.print(cds_values[2]);
        Serial.print(" | ");

        // 범위 또는 비슷한 센서 정보 출력
        if (value_range <= TOLERANCE)
        {
            Serial.println("범위: 3개 모든 조도센서 값이 비슷합니다");
        }
        else if (min_value >= 1018)
        {
            Serial.println("범위: 빛이 너무 강해서 판단할 수 없습니다 (혼란 제스처)");
        }
        else if (max_value <= 10)
        {
            Serial.println("범위: 빛이 너무 약해서 판단할 수 없습니다 (혼란 제스처)");
        }
        else if (abs(cds_values[0] - cds_values[1]) <= TOLERANCE)
        {
            // 왼쪽과 중간이 같음
            if (cds_values[2] < cds_values[0])
            {
                Serial.println("범위: 오른쪽 조도센서 값이 강합니다");
            }
            else
            {
                Serial.println("범위: 왼쪽과 중간 조도센서 값이 비슷하고 강합니다");
            }
        }
        else if (abs(cds_values[1] - cds_values[2]) <= TOLERANCE)
        {
            // 중간과 오른쪽이 같음
            if (cds_values[0] < cds_values[1])
            {
                Serial.println("범위: 왼쪽 조도센서 값이 강합니다");
            }
            else
            {
                Serial.println("범위: 중간과 오른쪽 조도센서 값이 비슷하고 강합니다");
            }
        }
        else if (abs(cds_values[0] - cds_values[2]) <= TOLERANCE)
        {
            // 왼쪽과 오른쪽이 같음
            if (cds_values[1] < cds_values[0])
            {
                Serial.println("범위: 중간 조도센서 값이 강합니다");
            }
            else
            {
                Serial.println("범위: 왼쪽과 오른쪽 조도센서 값이 비슷하고 강합니다");
            }
        }
        else
        {
            // 3개 모두 다름
            if (min_index == 0)
            {
                Serial.println("범위: 왼쪽 조도센서 값이 가장 강합니다");
            }
            else if (min_index == 1)
            {
                Serial.println("범위: 중간 조도센서 값이 가장 강합니다");
            }
            else
            {
                Serial.println("범위: 오른쪽 조도센서 값이 가장 강합니다");
            }
        }
        last_print_time = millis();
    }
}

// 서보모터 좌우 흔들림 제스처 (설정된 두 각도 사이를 왔다갔다)
void moveServoBidirectional()
{
    static unsigned long last_shake_time = 0;
    static int shake_direction = 0; // 0: angle_1, 1: angle_2

    if (millis() - last_shake_time >= current_shake_interval)
    {
        if (shake_direction == 0)
        {
            servo.write(shake_angle_1);
            shake_direction = 1;
        }
        else
        {
            servo.write(shake_angle_2);
            shake_direction = 0;
        }
        last_shake_time = millis();
    }
}