#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include <Servo.h>
#include <Wire.h>
#include <U8g2lib.h>

SoftwareSerial gpsSerial(10, 11); // RX, TX
TinyGPSPlus gps;
Servo servoMotor;

#define OLED_RESET 4
U8G2_SSD1306_128X64_NONAME_F_SW_I2C u8g2(U8G2_R0, /* clock=*/ SCL, /* data=*/ SDA, /* reset=*/ OLED_RESET);

void setup() {
  Serial.begin(9600);
  gpsSerial.begin(9600);
  servoMotor.attach(9);
  u8g2.begin();
  servoMotor.write(90);
  delay(3000);
  servoMotor.write(0);
  delay(3000);
}

void loop() {
  while (gpsSerial.available() > 0) {
    if (gps.encode(gpsSerial.read())) {
      if (gps.speed.isValid()) {
        float speedKmph = gps.speed.kmph();
        
        Serial.print("Speed: ");
        Serial.print(speedKmph);
        Serial.println(" km/h");

        u8g2.clearBuffer();

        if (speedKmph < 30) {
          servoMotor.write(0);
          u8g2.setFont(u8g2_font_ncenB14_tr);
          u8g2.setCursor(20, 30);
          u8g2.print("OFF");
          Serial.println("Servo Motor turned to 0 degrees.");
        } else if (speedKmph < 80) {
          servoMotor.write(90);
          u8g2.setFont(u8g2_font_ncenB14_tr);
          u8g2.setCursor(20, 30);
          u8g2.print("ON");
          Serial.println("Servo Motor turned to 90 degrees.");
        } else {
          // ทำอะไรตามความต้องการ เช่น โหมด Standby
          u8g2.setFont(u8g2_font_ncenB14_tr);
          u8g2.setCursor(20, 30);
          u8g2.print("Standby");
          Serial.println("Servo Motor in Standby Mode.");
        }

        u8g2.sendBuffer();
      }
    }
  }
}