// Khai báo thư viện
#include <WiFi.h>
#include <ESPAsyncWebServer.h>
#include <SimpleKalmanFilter.h>
#include <MPU6050_tockn.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
// Địa chỉ Wifi Server
const char* ssid = "ESP32-Access-Point";
const char* password = "123456789";
// Mở máy chủ Web HTPP trên cổng 80-cổng mặc định
AsyncWebServer server(80);
// Giao tiếp MPU6050 qua I2C
MPU6050 mpu6050(Wire);
// Giao tiếp OLED qua I2C
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define GOCX 110
#define GOCY 50
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
// Loc nhieu KalmanFilter
SimpleKalmanFilter locnhieu(2, 2, 0.01);

// Khai báo biến
const int joyX = 12;
const int joyY = 14;
const int button = 17;
const int top_limit = 2050;
const int bottom_limit = 2046;
boolean status = 0;
boolean prestatus = 1;
int count = 0;
int nhan = 0;
int gocX;
int gocY;
int gocZ;
float TEMP;
// Hình của con rồng lửa cuti
static const uint8_t image_data_Image[14336] = {
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xef, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xe1, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xfe, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfb, 0xef, 0x3f, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xcf, 0xdf, 0xff, 0xff, 0xff, 0xff, 0x59, 0xcf, 0x5e, 0x1f, 0xff, 0xff, 0xf6, 0xbf, 
	0x83, 0x33, 0xef, 0xff, 0xff, 0xe1, 0xbf, 0x93, 0xbc, 0x1f, 0xff, 0xff, 0x9f, 0x7f, 0xf7, 0xb0, 
	0x03, 0xff, 0xff, 0x43, 0x7f, 0xf7, 0xc0, 0x01, 0xff, 0xfe, 0x81, 0xb9, 0xff, 0xc0, 0x00, 0xff, 
	0xf9, 0x00, 0xbd, 0xdf, 0xc0, 0x00, 0x7f, 0xfa, 0x00, 0x3e, 0x3f, 0xc0, 0x00, 0x5f, 0xf8, 0x00, 
	0x3f, 0x1f, 0xc0, 0x00, 0x3f, 0xf8, 0x00, 0x1f, 0xaf, 0x80, 0x00, 0x3f, 0xf0, 0x00, 0x1f, 0x87, 
	0xa0, 0x00, 0x3f, 0xf0, 0x00, 0x0f, 0xc1, 0x80, 0x00, 0x1f, 0xe0, 0x41, 0x0f, 0xe4, 0x40, 0x00, 
	0x1f, 0xc0, 0x02, 0x03, 0xf0, 0x00, 0x00, 0x17, 0xc0, 0x14, 0x01, 0xf8, 0x00, 0x00, 0x0b, 0xc0, 
	0x0d, 0x80, 0xf6, 0x00, 0x00, 0x03, 0xe0, 0xbb, 0xc0, 0x74, 0x00, 0x06, 0x03, 0xe1, 0xf9, 0x1f, 
	0x00, 0x00, 0x0f, 0x03, 0xf1, 0xfc, 0x3f, 0xc0, 0x0a, 0x0f, 0x83, 0xf3, 0xff, 0xbf, 0xf0, 0x1d, 
	0x8f, 0x83, 0xf7, 0xcf, 0x47, 0xf8, 0x3e, 0x13, 0xc3, 0xe5, 0xbf, 0x31, 0xf8, 0x3f, 0x09, 0xc3, 
	0xf1, 0x3b, 0x60, 0x7f, 0xdf, 0x84, 0xc7, 0xf8, 0x31, 0x40, 0x7f, 0xe7, 0x80, 0xc7, 0xff, 0xe0, 
	0x00, 0xff, 0xfc, 0xc0, 0x6f, 0xf7, 0xc0, 0x01, 0xff, 0x86, 0xc0, 0x6f, 0xf7, 0xa0, 0x07, 0xfe, 
	0x18, 0xe8, 0x3f, 0xeb, 0x60, 0xcf, 0xff, 0xe8, 0x68, 0xbf, 0xe4, 0x60, 0xdf, 0xff, 0xf2, 0x62, 
	0x3f, 0xf9, 0x11, 0x9f, 0xff, 0xc6, 0x5f, 0x1f, 0xff, 0x01, 0xbf, 0xff, 0x9e, 0x3f, 0x5f, 0xfe, 
	0x41, 0xbf, 0xff, 0x97, 0x3f, 0x5f, 0xff, 0x41, 0x7f, 0xff, 0xc6, 0x9e, 0x5f, 0xff, 0x9f, 0x7f, 
	0xff, 0xf4, 0x18, 0x1f, 0xff, 0xce, 0xff, 0xff, 0xf0, 0x30, 0xbf, 0xff, 0xfd, 0xbf, 0xff, 0x7c, 
	0x21, 0xbf, 0xff, 0xff, 0xbf, 0xff, 0xff, 0x03, 0x7f, 0xff, 0xff, 0xdf, 0xff, 0x7f, 0x8e, 0xff, 
	0xff, 0xfb, 0xdf, 0xff, 0x7f, 0x9d, 0xff, 0xff, 0xfb, 0xe7, 0xfe, 0x7f, 0x13, 0xff, 0xff, 0xfb, 
	0xc3, 0xfe, 0x7f, 0x0f, 0xff, 0xff, 0xfb, 0x89, 0xfe, 0x5e, 0x1f, 0xff, 0xff, 0xf8, 0x00, 0x3e, 
	0x00, 0x1f, 0xff, 0xff, 0xfc, 0x87, 0xe3, 0x10, 0x5f, 0xff, 0xff, 0xfe, 0x23, 0xf8, 0x68, 0x4f, 
	0xff, 0xff, 0xfe, 0x53, 0xff, 0xe4, 0xef, 0xff, 0xff, 0xfd, 0xf3, 0xff, 0xf4, 0xaf, 0xff, 0xff, 
	0xfc, 0x07, 0xff, 0xf2, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf3, 0x77, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xfe, 0xe7, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff  };

void setup()
{
  // Mở cổng Serial cho dễ fix
  Serial.begin(115200);
  // Khởi động I2C-Master
  Wire.begin();
  // Khởi động MPU6050
  mpu6050.begin();
  // Cân bằng MPU6050
  mpu6050.calcGyroOffsets(true);
  // Khởi tạo điểm truy cập WIFI
  WiFi.softAP(ssid, password);
  IPAddress IP = WiFi.softAPIP();
  // In IP ra Serial Moniter 
  Serial.println();
  Serial.print("Setting AP (Access Point)…");
  Serial.print("AP IP address: ");
  Serial.println(IP);
  // Khai báo các chân I/O
  pinMode(joyX, INPUT_PULLUP);
  pinMode(joyY, INPUT_PULLUP);
  pinMode(button, INPUT_PULLUP);
  // Phát tí hiệu OLED
  delay(150);
  oled.clearDisplay();
  oled.setTextSize(1); 
  oled.setTextColor(WHITE); 

  oled.setCursor(10, 10);        
  oled.println("AUTHOR: Tony Huynh");  

  oled.setCursor(25, 28);        
  oled.println("DEMONSTRATION");  

  oled.setCursor(GOCX, GOCY);        
  oled.println(".");  


  oled.setCursor(GOCX, GOCY-4);        
  oled.println(".");  

  oled.setCursor(GOCX+2, GOCY-3);        
  oled.println(".");  

  oled.setCursor(GOCX-2, GOCY-3);        
  oled.println(".");  


  oled.setCursor(GOCX, GOCY-8);        
  oled.println(".");  

  oled.setCursor(GOCX+2, GOCY-7);        
  oled.println(".");  

  oled.setCursor(GOCX-2, GOCY-7);        
  oled.println(".");  

  oled.setCursor(GOCX+4, GOCY-6);        
  oled.println(".");  

  oled.setCursor(GOCX-4, GOCY-6);        
  oled.println(".");  

  oled.setCursor(GOCX, GOCY-12);        
  oled.println(".");  

  oled.setCursor(GOCX+2, GOCY-11);        
  oled.println(".");  

  oled.setCursor(GOCX-2, GOCY-11);        
  oled.println(".");  

  oled.setCursor(GOCX+4, GOCY-10);        
  oled.println(".");  

  oled.setCursor(GOCX-4, GOCY-10);        
  oled.println(".");  

  oled.setCursor(GOCX+6, GOCY-9);        
  oled.println(".");  

  oled.setCursor(GOCX-6, GOCY-9);        
  oled.println(".");  

  oled.setCursor(GOCX-82, GOCY-5);        
  oled.println("WIFI STATION");  
  oled.display();
  delay(1500);
  // Gởi góc lên Wifi Server
  server.on("/trucX", HTTP_GET, [](AsyncWebServerRequest *request)
  {
    request->send_P(200, "text/plain", readX().c_str());
  });
  
  server.on("/trucY", HTTP_GET, [](AsyncWebServerRequest *request)
  {
    request->send_P(200, "text/plain", readY().c_str());
  });

  server.on("/trucZ", HTTP_GET, [](AsyncWebServerRequest *request)
  {
    request->send_P(200, "text/plain", readZ().c_str());
  });

  server.begin();
}

void loop()
{
  // update giá trị của MPU sau mỗi lần loop
  mpu6050.update();

  // in số liệu ra OLED
  oled.clearDisplay(); 
  oled.setTextSize(1);           
  oled.setCursor(2, 2);        
  oled.println("SERVO-X: ");  
  oled.setCursor(52, 2);        
  oled.println(readX());  
  oled.setCursor(2, 18);        
  oled.println("SERVO-Y: ");  
  oled.setCursor(52, 18);        
  oled.println(readY());  
  oled.setCursor(2, 34);        
  oled.println("TEMP: ");  
  oled.setCursor(34, 34);        
  oled.println(readTEMP());  

  oled.setCursor(10, 52);        
  oled.println("WIFI-SVR");  

  oled.drawBitmap(71, 0, image_data_Image, 56, 64, 1);
  oled.drawRect(2, 48, 65, 15, WHITE);
  oled.display();

  // in thôi chắc ko có bug ở đây đâu
  Serial.print("trucX: ");
  Serial.print(mpu6050.getAngleX());
  Serial.print("  ||  ");
  Serial.print("trucY: ");
  Serial.print(mpu6050.getAngleY());
  Serial.print("  ||  ");
  Serial.print("trucZ: ");
  Serial.print(mpu6050.getAngleZ());
  Serial.println("  ||  ");
}

// Hàm xử lý góc
String readX() 
{
  gocX = locnhieu.updateEstimate(mpu6050.getAngleX());
  return String(gocX);
}

String readY() 
{
  if (nutnhan()==0)
  {
    gocY = locnhieu.updateEstimate(mpu6050.getAngleY());
    if (gocY >= 100 || gocY<=-170)
      gocY = -180;
    if (gocY>=70 && gocY<100)
      gocY = 20;
    gocY = map(gocY, 20, -180, 0, 140);
  }
  else if (nutnhan()==1)
  {
    if(locnhieu.updateEstimate(analogRead(joyY)) > top_limit)
      gocY++;
    else if(locnhieu.updateEstimate(analogRead(joyY)) < bottom_limit)
      gocY--;    
  }  
  gioihan(gocY);
  return String(gocY);
}

String readZ() 
{
  if (nutnhan()==0)
  {
    gocZ = locnhieu.updateEstimate(mpu6050.getAngleZ());
    if (gocZ<=-12)
      gocZ = -12;
    if (gocZ>=12)
      gocZ = 12;
    gocZ = map(gocZ, -12, 12, 0, 180);
  }
  else if (nutnhan()==1)
  {
    if(locnhieu.updateEstimate(analogRead(joyX)) > top_limit)
      gocZ++;
    else if(locnhieu.updateEstimate(analogRead(joyX)) < bottom_limit)
      gocZ--;    
  }  
  gioihan(gocZ);
  return String(gocZ);
}

String readTEMP()
{
  TEMP = locnhieu.updateEstimate(mpu6050.getTemp());
  TEMP = round(TEMP*10)/10;
  return String(TEMP);
}
int nutnhan()
{
  status = digitalRead(button);
  if (status != prestatus)
    count++;
  if((count/2)%2==0)
    nhan = 0;
  if((count/2)%2!=0)
    nhan = 1;
  prestatus = status;
  return nhan;
}

int gioihan(int a)
{
  if (a >= 180)
    a = 180;
  if (a <= 0)
    a = 0;
  return a;
}