#include <Servo.h>

Servo myServo;

const int closeDoor = 0;      // 문 닫힐 각도
const int halfOpenDoor = 90;  // 문 중간 각도
const int fullOpenDoor = 180; // 문 열릴 각도

const int switchPin = 2; // 스위치 핀

const int redLedPin = 3;   // 빨강 LED 핀
const int blueLedPin = 4;  // 파랑 LED 핀
const int greenLedPin = 5; // 초록 LED 핀

const int servoPin = 6; // 모터 핀

const int trig1Pin = 7;  // 초음파센서 1의 에코 핀
const int echo1Pin = 8;  // 초음파센서 1의 트리거 핀
const int trig2Pin = 9;  // 초음파센서 2의 에코 핀
const int echo2Pin = 10; // 초음파센서 2의 트리거 핀

const int pirSensorPin = 11; // PIR 센서 핀


void setup()
{
    Serial.begin(9600);
    myServo.attach(servoPin);
    myServo.write(0);
    delay(2000);

    pinMode(switchPin, INPUT_PULLUP); // 스위치핀 , 풀업모드로 설정 , 안누를때 HIGH, 누를때 LOW

    pinMode(redLedPin, OUTPUT);   // 빨강 LED 핀 , 출력 모드로 설정
    pinMode(blueLedPin, OUTPUT);  // 파랑 LED 핀 , 출력 모드로 설정
    pinMode(greenLedPin, OUTPUT); // 초록 LED 핀 , 출력 모드로 설정

    pinMode(trig1Pin, OUTPUT); // 초음파센서 1의 트리거 핀 , 출력 모드로 설정
    pinMode(echo1Pin, INPUT);  // 초음파센서 1의 에코 핀 , 입력 모드로 설정
    pinMode(trig2Pin, OUTPUT); // 초음파센서 2의 트리거 핀 , 출력 모드로 설정
    pinMode(echo2Pin, INPUT);  // 초음파센서 2의 에코 핀 , 입력 모드로 설정

    pinMode(pirSensorPin, INPUT); // PIR 센서 핀 , 입력 모드로 설정

    digitalWrite(redLedPin, LOW);   // 빨강 LED 초기 LOW로 설정
    digitalWrite(blueLedPin, LOW);  // 파랑 LED 초기 LOW로 설정
    digitalWrite(greenLedPin, LOW); // 초록 LED 초기 LOW로 설정
}


void loop()
{
    bool pirState = digitalRead(pirSensorPin); // PIR 센서 값 읽기
    bool switchState = digitalRead(switchPin); // 스위치 값 읽기

    static int forDebugMode = 0;
    if (pirState == HIGH) // PIR 센서 값이 HIGH일때(물체 또는 인간 감지 상태)
    {
        static int distance1 = getDistance(trig1Pin, echo1Pin); // 초음파 1 계산 함수
        static int distance2 = getDistance(trig2Pin, echo2Pin); // 초음파 2 계산 함수
        
        if (distance1 <= 50 && distance2 <= 50) // 초음파 1, 2 의 거리값이  50cm 이하일때는(장애물과의 거리)
        {
            if (switchState == HIGH) // 스위치 값이 HIGH일때( 눌렸을때)
            {
                // 파랑LED 켜지고 , 서보모터 180도로 이동한다.
                digitalWrite(blueLedPin, HIGH);
                digitalWrite(redLedPin, LOW);
                digitalWrite(greenLedPin, LOW);
                myServo.write(fullOpenDoor);
                forDebugMode = 1;                
            }
            else
            {
                // 빨강LED 켜지고 , 서보모터 90도로 이동한다.
                digitalWrite(redLedPin, HIGH);
                digitalWrite(blueLedPin, LOW);
                digitalWrite(greenLedPin, LOW);
                myServo.write(halfOpenDoor);
                forDebugMode = 2;       
            }
        }
        else // 첫번째 조건을 뺀 나머지 모든 경우는
        {
            // 파랑LED 켜지고 , 서보모터 0도로 이동한다.
            digitalWrite(blueLedPin, HIGH);
            digitalWrite(redLedPin, LOW);
            digitalWrite(greenLedPin, LOW);
            myServo.write(closeDoor);
            forDebugMode = 3;       
        }
    }
    else // PIR 센서 값이 LOW일때
    {
        // 초롱LED 켜지고 , 서보모터 180도로 이동한다.
        digitalWrite(greenLedPin, HIGH);
        digitalWrite(blueLedPin, LOW);
        digitalWrite(redLedPin, LOW);
        myServo.write(fullOpenDoor);
        forDebugMode = 4;       
    }

    // 디버깅용
    Serial.print("Mode :");
    Serial.print(forDebugMode);
    Serial.print(" , ");
    Serial.print("Distance1:");
    Serial.print(getDistance(trig1Pin, echo1Pin));
    Serial.print("  , Distance2:");
    Serial.print(getDistance(trig2Pin, echo2Pin));
    Serial.println(switchState);
    delay(1);
}


int getDistance(int trigPin, int echoPin)
{
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    long duration = pulseIn(echoPin, HIGH);
    int distance = duration * 0.034 / 2; // cm 변환
    return distance;
}