#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <avr/wdt.h>  // برای تایمر سگ نگهبان

// مشخصات نمایشگر OLED
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 32
#define OLED_RESET    -1  // از -1 استفاده می‌کنیم چون پین ریست فیزیکی نیاز نیست

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// پین‌های استفاده شده
const int potMotorPin = A0;  // پتانسیومتر اول (موقعیت موتور)
const int potTargetPin = A1; // پتانسیومتر دوم (موقعیت هدف)
const int leftPin = 9;       // پین برای فعال‌سازی چپ‌گرد
const int rightPin = 10;     // پین برای فعال‌سازی راست‌گرد
const int leftSwitchPin = 2; // ورودی برای چپ‌گرد
const int rightSwitchPin = 3;// ورودی برای راست‌گرد
const int potControlSwitchPin = 4; // ورودی برای فعال‌سازی کنترل با پتانسیومتر

const int ledPin = 13;       // پین LED داخلی

int motorPosition = 0;       // مقدار پتانسیومتر موتور
int targetPosition = 0;      // مقدار پتانسیومتر هدف
int deadZone = 5;            // ناحیه حاشیه برای خاموشی موتور
bool motorControlledByPot = false; // وضعیت کنترل توسط پتانسیومتر

unsigned long previousMillis = 0;   // زمان قبلی برای چشمک زدن LED
const long interval = 100;          // فاصله زمانی بین خاموش/روشن شدن LED (100 میلی‌ثانیه)
bool ledState = LOW;                // وضعیت فعلی LED (خاموش یا روشن)

void setup() {
  // تنظیم پین‌ها
    pinMode(leftPin, OUTPUT);
      pinMode(rightPin, OUTPUT);
        pinMode(leftSwitchPin, INPUT_PULLUP);    // ورودی برای چپ‌گرد
          pinMode(rightSwitchPin, INPUT_PULLUP);   // ورودی برای راست‌گرد
            pinMode(potControlSwitchPin, INPUT_PULLUP); // ورودی برای کنترل با پتانسیومتر
              pinMode(ledPin, OUTPUT);  // تنظیم پین LED داخلی به عنوان خروجی
                
                  Serial.begin(9600);

                    // فعال‌سازی تایمر سگ نگهبان با زمان 1 ثانیه
                      wdt_enable(WDTO_1S);

                        // راه‌اندازی نمایشگر OLED
                          if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
                              Serial.println(F("Failed to initialize OLED display"));
                                  for (;;); // در صورت خطا متوقف می‌شود
                                    }

                                      // پاک کردن نمایشگر و تنظیمات اولیه
                                        display.clearDisplay();
                                          display.setTextSize(2);      // تنظیم سایز فونت به بزرگ
                                            display.setTextColor(SSD1306_WHITE);  // متن سفید روی زمینه مشکی
                                              display.setCursor(0, 10);    // تنظیم مکان شروع نمایش متن
                                              }

                                              void loop() {
                                                // بازنشانی تایمر سگ نگهبان
                                                  wdt_reset();

                                                    // چشمک زدن LED داخلی بدون استفاده از delay
                                                      blinkLEDWithoutDelay();

                                                        // خواندن وضعیت سوئیچ کنترل پتانسیومتر
                                                          motorControlledByPot = digitalRead(potControlSwitchPin) == LOW;

                                                            // اگر کنترل توسط پتانسیومتر فعال است
                                                              if (motorControlledByPot) {
                                                                  controlMotorWithPotentiometer();
                                                                    } else {
                                                                        // در غیر این صورت، با سوئیچ‌های چپ‌گرد و راست‌گرد کنترل شود
                                                                            controlMotorWithSwitches();
                                                                              }

                                                                                // نمایش زاویه موتور روی OLED
                                                                                  displayMotorAngleOnOLED();

                                                                                    delay(10); // تاخیر کوتاه برای پایداری بیشتر
                                                                                    }

                                                                                    void controlMotorWithPotentiometer() {
                                                                                      // خواندن موقعیت پتانسیومترها با 5 بار نمونه‌گیری و محاسبه میانگین
                                                                                        motorPosition = readAnalogAverage(potMotorPin);
                                                                                          targetPosition = readAnalogAverage(potTargetPin);

                                                                                            // محاسبه اختلاف بین موقعیت فعلی و هدف
                                                                                              int error = targetPosition - motorPosition;

                                                                                                // در صورتی که اختلاف در محدوده حاشیه است، موتور را خاموش کن
                                                                                                  if (abs(error) <= deadZone) {
                                                                                                      stopMotor();
                                                                                                        } else if (error > 0) {
                                                                                                            // اگر هدف بالاتر از موقعیت فعلی است، به سمت راست بچرخد
                                                                                                                turnRight();
                                                                                                                  } else {
                                                                                                                      // اگر هدف پایین‌تر از موقعیت فعلی است، به سمت چپ بچرخد
                                                                                                                          turnLeft();
                                                                                                                            }

                                                                                                                              // نمایش اطلاعات در سریال مانیتور (اختیاری)
                                                                                                                                Serial.print("Motor Position: ");
                                                                                                                                  Serial.print(motorPosition);
                                                                                                                                    Serial.print(" Target Position: ");
                                                                                                                                      Serial.print(targetPosition);
                                                                                                                                        Serial.print(" Error: ");
                                                                                                                                          Serial.println(error);
                                                                                                                                          }

                                                                                                                                          void controlMotorWithSwitches() {
                                                                                                                                            // اگر سوئیچ چپ‌گرد فعال شده باشد
                                                                                                                                              if (digitalRead(leftSwitchPin) == LOW) {
                                                                                                                                                  turnLeft();
                                                                                                                                                    } 
                                                                                                                                                      // اگر سوئیچ راست‌گرد فعال شده باشد
                                                                                                                                                        else if (digitalRead(rightSwitchPin) == LOW) {
                                                                                                                                                            turnRight();
                                                                                                                                                              } 
                                                                                                                                                                // اگر هیچ کدام فعال نیستند، موتور خاموش شود
                                                                                                                                                                  else {
                                                                                                                                                                      stopMotor();
                                                                                                                                                                        }
                                                                                                                                                                        }

                                                                                                                                                                        void turnLeft() {
                                                                                                                                                                          // فعال کردن چپ‌گرد و غیرفعال کردن راست‌گرد
                                                                                                                                                                            digitalWrite(leftPin, HIGH);
                                                                                                                                                                              digitalWrite(rightPin, LOW);
                                                                                                                                                                              }

                                                                                                                                                                              void turnRight() {
                                                                                                                                                                                // فعال کردن راست‌گرد و غیرفعال کردن چپ‌گرد
                                                                                                                                                                                  digitalWrite(leftPin, LOW);
                                                                                                                                                                                    digitalWrite(rightPin, HIGH);
                                                                                                                                                                                    }

                                                                                                                                                                                    void stopMotor() {
                                                                                                                                                                                      // خاموش کردن هر دو جهت
                                                                                                                                                                                        digitalWrite(leftPin, LOW);
                                                                                                                                                                                          digitalWrite(rightPin, LOW);
                                                                                                                                                                                          }

                                                                                                                                                                                          // تابع برای چشمک زدن LED داخلی بدون استفاده از delay
                                                                                                                                                                                          void blinkLEDWithoutDelay() {
                                                                                                                                                                                            // زمان فعلی را می‌خوانیم
                                                                                                                                                                                              unsigned long currentMillis = millis();

                                                                                                                                                                                                // بررسی می‌کنیم که آیا فاصله زمانی موردنظر گذشته است یا نه
                                                                                                                                                                                                  if (currentMillis - previousMillis >= interval) {
                                                                                                                                                                                                      // ذخیره‌سازی زمان فعلی به عنوان زمان قبلی
                                                                                                                                                                                                          previousMillis = currentMillis;

                                                                                                                                                                                                              // تغییر وضعیت LED
                                                                                                                                                                                                                  ledState = !ledState;

                                                                                                                                                                                                                      // تنظیم وضعیت جدید LED
                                                                                                                                                                                                                          digitalWrite(ledPin, ledState);
                                                                                                                                                                                                                            }
                                                                                                                                                                                                                            }

                                                                                                                                                                                                                            // تابع برای خواندن 5 بار از ورودی آنالوگ و محاسبه میانگین
                                                                                                                                                                                                                            int readAnalogAverage(int pin) {
                                                                                                                                                                                                                              int total = 0;
                                                                                                                                                                                                                                
                                                                                                                                                                                                                                  // جمع‌کردن 5 بار نمونه‌گیری از ورودی آنالوگ
                                                                                                                                                                                                                                    for (int i = 0; i < 5; i++) {
                                                                                                                                                                                                                                        total += analogRead(pin);
                                                                                                                                                                                                                                          }
                                                                                                                                                                                                                                            
                                                                                                                                                                                                                                              // محاسبه میانگین
                                                                                                                                                                                                                                                return total / 5;
                                                                                                                                                                                                                                                }

                                                                                                                                                                                                                                                // تابع برای محاسبه و نمایش زاویه موتور روی نمایشگر OLED
                                                                                                                                                                                                                                                void displayMotorAngleOnOLED() {
                                                                                                                                                                                                                                                  float motorAngle = calculateMotorAngle();  // محاسبه زاویه موتور

                                                                                                                                                                                                                                                    // تبدیل مقدار زاویه به رشته برای نمایش با یک رقم اعشار
                                                                                                                                                                                                                                                      char angleString[10];
                                                                                                                                                                                                                                                        dtostrf(motorAngle, 5, 1, angleString); // تبدیل به رشته با دقت 1 رقم اعشار

                                                                                                                                                                                                                                                          // نمایش زاویه روی OLED
                                                                                                                                                                                                                                                            display.clearDisplay();         // پاک کردن صفحه نمایش
                                                                                                                                                                                                                                                              display.setCursor(0, 10);       // قرار دادن مکان شروع نمایش
                                                                                                                                                                                                                                                                //display.print("Angle: ");       // نمایش متن "Angle:"
                                                                                                                                                                                                                                                                  display.print(angleString);     // نمایش مقدار زاویه
                                                                                                                                                                                                                                                                    display.print(" deg");          // نمایش واحد درجه
                                                                                                                                                                                                                                                                      display.display();              // به‌روزرسانی نمایشگر
                                                                                                                                                                                                                                                                      }

                                                                                                                                                                                                                                                                      // تابع محاسبه موقعیت زاویه موتور (0 تا 360 درجه)
                                                                                                                                                                                                                                                                      float calculateMotorAngle() {
                                                                                                                                                                                                                                                                        int potValue = readAnalogAverage(potMotorPin); // خواندن مقدار آنالوگ
                                                                                                                                                                                                                                                                          float angle = (potValue / 1023.0) * 360.0; // تبدیل به زاویه (0 تا 360 درجه)
                                                                                                                                                                                                                                                                            return angle;
                                                                                                                                                                                                                                                                            }