#include"TFT_eSPI.h"        // ผนวกไลบรารีของชิปควบคุมจอแสดงผล LCD
TFT_eSPI tft;               // สร้างออบเจ็กต์ชื่อ tft จากคลาส TFT_eSPI สำหรับควบคุมการแสดงผล LCD

#include "PCA9685.h"
#include "DHT.h"
#define DHTPIN BCM26 // GPIO 26
#define DHTTYPE DHT11 // DHT 11 
DHT dht(DHTPIN, DHTTYPE);
PCA9685 pwmController;
PCA9685_ServoEval pwmServo;

unsigned long StartTime = 0;
int menu = 0;   //0:MENU 1:MENU1 2:MENU2 3:MENU3
int Speed = 0;  //-100% 0% +100% step 20%
int Angle = 0;  //-90 +90 step 15
String TitleString[] = {"PLEASE SELECT THE BUTTON ",
                        "TB6612 Control   ", 
                        "PCA9685 Control  ",
                        "DHT11 READ       "};
String MenuString[] = { "KEY C To Control TB6612 ", 
                        "KEY B To Control PCA9685 ",
                        "KEY A To Read DHT11   "};
String Page1String[] = {"SPEED PERCENTAGE   ",
                        "   ",
                        "PRESS WIO5S BUTTON T0 EXIT "};
String Page2String[] = {"SERVO DEGREE     ",
                        "   ",
                        "PRESS WIO5S BUTTON T0 EXIT "}; 
String Page3String[] = {"TEMPERATURE : ", 
                        "HUMIDITY    : ",
                        "PRESS WIO5S BUTTON T0 EXIT "};                                                
const int AIN1 = BCM4;
const int AIN2 = BCM5;
const int PWMA = BCM27;
const int servoPin = BCM6;

void displayMenu() {          //Display Title
  tft.drawString(TitleString[0], 20, 40);
  tft.drawString(MenuString[0], 20, 80); 
  tft.drawString(MenuString[1], 20, 120); //
  tft.drawString(MenuString[2], 20, 160); //

}
void displayMenu1() {
  String Direction = " ";
  tft.drawString(TitleString[1], 20, 40); //Display Title
  tft.drawString(Page1String[0], 20, 80);
  tft.drawString("SPEED : " + String(Speed) + " %  ", 40, 120); //
  if (Speed > 0 ) {
    Direction = "CW ";
  } else if (Speed < 0) {
    Direction = "CCW";
  } else {
    Direction = "---";
  }
  //tft.drawString(Direction, 40, 120); //
  tft.drawString(Page1String[2], 10, 180);
}
void displayMenu2() {
  tft.drawString(TitleString[2], 20, 40); //Display Title
  tft.drawString(Page2String[0], 20, 80);

  tft.drawString("ANGLE : " + String(Angle) + "  ", 40, 120); //
  tft.drawString(Page2String[2], 10, 180);
}
void displayMenu3() {
  tft.drawString(TitleString[3], 20, 40); //Display Title
  tft.drawString(Page3String[0], 20, 80);

  tft.drawString(Page3String[1], 20, 120);

  if (millis() - StartTime >= 1000) {
    float temp = dht.readTemperature();
    float humi = dht.readHumidity();
    tft.drawString(String(temp) + String(" C    "), 200,80);
    tft.drawString(String(humi) + String(" %RH    "), 200,120);     
    //tft.drawString("Temp: " + String(temp) + String(" C    "), 50, 100);
    //tft.drawString("Humi: " + String(humi) + String(" %RH    "), 50, 150);
    StartTime = millis();
  }
    tft.drawString(Page3String[2], 10, 180);
}
void controlDcMotor() {
  if (Speed >= 0) {
    digitalWrite(AIN1, HIGH);
    digitalWrite(AIN2, LOW);
    int speeds = map(Speed,0,100,0,255);
    analogWrite(PWMA, speeds);
    delay(1000);
  }
  if(Speed < 0) {
    digitalWrite(AIN1, LOW);
    digitalWrite(AIN2, HIGH);
    int speeds = map((Speed * (-1)),1,100,1,255);   
    analogWrite(PWMA,speeds);
    delay(1000);
  }
}
void controlServoMotor() {
  Serial.println(Angle);
  pwmController.setChannelPWM(0, pwmServo.pwmForAngle(Angle)); 
  delay(500);
}
void alarm(int s) {
  sound(1000, s * 1000); // Generate a s วินาที beep on the PIEZO
}
void beep() {
  sound(1000, 100); // Generate a 100ms beep on the PIEZO
}

void __delay_sound(uint16_t ms) { //delay for create sound
  unsigned int i;
  for (i = 0; i < ms; i++)
    delayMicroseconds(10);
}
void sound(uint16_t freq, uint32_t time) { //create sound
  uint32_t dt = 0, m = 0;
  uint32_t tempTime;
  pinMode(WIO_BUZZER, OUTPUT);
  digitalWrite(WIO_BUZZER, LOW);
  tempTime = time * 50;
  dt = 50000 / freq;
  time = (tempTime) / dt;
  for (m = 0; m < time; m++) {
    digitalWrite(WIO_BUZZER, HIGH);
    __delay_sound(dt);
    digitalWrite(WIO_BUZZER, LOW);
    __delay_sound(dt);
  }
}
void setup() {
  Serial.begin(115200);
  Wire.begin();                     // ใช้งาน I2C
  tft.begin();                      // เริ่มต้นการทำงานของจอแสดงผล LCD
  tft.setRotation(3);               // หมุนตำแหน่งการแสดงผลเป็นโหมด 3
  tft.fillScreen(TFT_BLACK);        // กำหนดให้แสดงสีแดงเต็มหน้าจอแสดงผล
  tft.setTextColor(TFT_WHITE,TFT_BLACK);
  tft.setTextSize(2);
  /******************ตั้งค่าควบคุม SERVO MOTOR*****************/
  pwmController.resetDevices();       // Resets all PCA9685 devices on i2c line
  pwmController.init();               // Initializes module using default totem-pole driver mode, and default disabled phase balancer
  pwmController.setPWMFrequency(60); // Set PWM freq to 100Hz (default is 200Hz, supports 24Hz to 1526Hz)
  /********************************************************/
  pinMode(WIO_KEY_A, INPUT_PULLUP);
  pinMode(WIO_KEY_B, INPUT_PULLUP);
  pinMode(WIO_KEY_C, INPUT_PULLUP);
  pinMode(WIO_5S_UP, INPUT_PULLUP);
  pinMode(WIO_5S_DOWN, INPUT_PULLUP);
  pinMode(WIO_5S_LEFT, INPUT_PULLUP);
  pinMode(WIO_5S_RIGHT, INPUT_PULLUP);
  pinMode(WIO_5S_PRESS, INPUT_PULLUP);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(servoPin, OUTPUT);
  dht.begin();
  pwmController.setChannelPWM(0, pwmServo.pwmForAngle(-90)); //ตำแหน่งเริ่มต้นของ SERVO
  delay(500);
}

void loop() {
/*************SCAN KEY*************************/
if (!digitalRead(WIO_5S_PRESS)) {
    tft.fillScreen(TFT_BLACK);  // ล้างหน้าจอ
    menu = 0;
    delay(200);       // Debounce Switch
  }
if(menu == 0){
  if (!digitalRead(WIO_KEY_A)) {
    tft.fillScreen(TFT_BLACK);  // ล้างหน้าจอ
    menu = 1;
    delay(200);       // Debounce Switch
  }
  if (!digitalRead(WIO_KEY_B)) {
    tft.fillScreen(TFT_BLACK);  // ล้างหน้าจอ
    menu = 2;
    delay(200);       // Debounce Switch
  }
  if (!digitalRead(WIO_KEY_C)) {
    tft.fillScreen(TFT_BLACK);  // ล้างหน้าจอ
    menu = 3;
    delay(200);       // Debounce Switch
  }
}
  /***************เลือกเมนู*******************/
  if (menu == 0) {        //เลือกหน้าเมนูหลัก
    displayMenu();        //แสดงหน้าเมนู
  }
  if (menu == 1) {        //เลือกเมนู 1

    if (!digitalRead(WIO_5S_UP)) {  //กดขึ้น
      Speed = Speed + 20;           //เพิ่มค่าความเร็วขึ้น 20%
      if (Speed > 100) {            //ถ้าความเร็วมากกว่า 100%
        Speed = 100;                //ให้ความเร็วเท่ากับ 100%
        alarm(3);                   //แจ้งเตือน 3 วินาที
      }
      delay(200);       // Debounce Switch
    }
    if (!digitalRead(WIO_5S_DOWN)) {//กดลง
      Speed = Speed - 20;           //ลคค่าความเร็วลง 20%
      if (Speed < -100) {           //ถ้าความเร็วน้อยกว่า -100%
        Speed = -100;               //ให้ความเร็วเท่ากับ -100%
        alarm(3);                   //แจ้งเตือน 3 วินาที
      }
      delay(200);       // Debounce Switch
    }
    displayMenu1();                 //แสดงหน้าจอที่ 1
    controlDcMotor();               //ควบคุม DC Motor
  }
  if (menu == 2) {        //เลือกเมนู 2
    if (!digitalRead(WIO_5S_UP)) {  //กดขึ้น
      Angle = Angle + 15;           //เพิ่มค่ามุม 15 องศา
      if (Angle > 90) {             //ถ้าค่ามุมมากกว่า 90 องศา
        Angle = 90;                 //ให้ค่ามุมเท่ากับ 90 องศา
        alarm(3);                   //แจ้งเตือน 3 วินาที
      }
      delay(200);       // Debounce Switch
    }
    if (!digitalRead(WIO_5S_DOWN)) {  //กดลง
      Angle = Angle - 15;             //ลดค่ามุมลง 15 องศา
      if (Angle < -90) {              //ถ้ามุมน้อยกว่า -90 องศา
        Angle = -90;                  //ให้ค่ามุมเท่ากับ -90 องศา
        alarm(3);                     //แจ้งเตือน 3 วินาที
      }
      delay(200);         // Debounce Switch
    }
    displayMenu2();                   //แสดงหน้าจอที่ 2
    controlServoMotor();              //ควบคุม Servo Motor
  }
  if (menu == 3) {                    //เลือกเมนู 3
    displayMenu3();                   //แสดงหน้าจอที่ 3
  }
}