/*
 MPU6050 conectat SDA la GPIO21 SCK la GPIO22
 trimite unghiuri reale si temp prin webserial
 afiseaza pe oled adresa IP unghiuri si temp
 Oled conectat: SDA la GPIO21 SCK la GPIO22
 ambele sunt I2C dar identificate prib adresele lor din cod
 genereaza server cu IP 192.168.4.1
  
  dupa incarcare, pe Oled si Serial Monitor 
  apare adresa IP 192.168.4.1
  Se dexchide pe Android Chrome
  Se introdice 192.168.4.1/webserial
  apare pagina web
  tot ce se editeaza  pe telefon apare pe Oled
 Se poate conecta inca un telefon cu acelasi IP
*/

#include <Arduino.h>
#include <WiFi.h>
#include <AsyncTCP.h>
#include <ESPAsyncWebServer.h>
#include <WebSerial.h>

AsyncWebServer server(80);
const char* ssid = "ESP32-Soft-AP";          
const char* password = "12345678"; 

#include<Wire.h>

#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#define OLED_ADDR   0x3C
Adafruit_SSD1306 display(-1);

const int MPU_addr=0x68;
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
 
int minVal=265;
int maxVal=402;
 
double x;
double y;
double z;
double t;

void recvMsg(uint8_t *data, size_t len){
  WebSerial.println("Received Data...");
  String d = "";
  for(int i=0; i < len; i++){
    d += char(data[i]);
    Serial.println(d);
    display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print("Primit=");
  display.print(d);//afiseaza text tranmmsis pe oled
  display.display();
  }
  WebSerial.println(d);
  
}


void Connect_WiFi()  //reconecteaza rapid
{
WiFi.begin(ssid, password);
while(WiFi.status() != WL_CONNECTED)
{
delay(100);
}
}

void setup(){

  Serial.begin(115200);
  
  Serial.print("Setting soft access point mode");

WiFi.softAP(ssid, password);
IPAddress IP = WiFi.softAPIP();
Serial.print("AP IP address: ");
Serial.println(IP);
server.begin();
  Serial.print("IP Address: ");
  Serial.println(IP);
  
  WebSerial.msgCallback(recvMsg);
  WebSerial.begin(&server);
   
  
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);

display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);

display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print("MPU Angles");
  display.print("webserial IP=");
  display.print(IP);
  display.print("/webserial"); 
  display.display();
  delay(5000);
 
}
void loop(){

Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true);
AcX=Wire.read()<<8|Wire.read();
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read();
Tmp=Wire.read()<<8|Wire.read();

int xAng = map(AcX,minVal,maxVal,-90,90);
int yAng = map(AcY,minVal,maxVal,-90,90);
int zAng = map(AcZ,minVal,maxVal,-90,90);
 
x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);
 t=Tmp/340.00+36.53;


 WebSerial.print(" X=");
 WebSerial.print(x);
 WebSerial.print(" Y=");
 WebSerial.print(y);
 WebSerial.print(" Z=");
 WebSerial.print(z);
 WebSerial.print(" T=");
 WebSerial.print(t);


Serial.print("AngleX= ");
Serial.println(x);

Serial.print("AngleY= ");
Serial.println(y);
 
Serial.print("AngleZ= ");
Serial.println(z);

Serial.print("T= ");
Serial.println(t);
Serial.println("-----------------------------------------");
 
display.clearDisplay();
display.setTextSize(2);
display.setCursor(0,0);
display.print("X: ");
display.println(x);

display.setTextSize(2);
display.setCursor(0,20);
display.print("Y: ");
display.println(y);
 
display.setTextSize(2);
display.setCursor(0,40);
display.print("Z: ");
display.println(z);

display.setCursor(0,60);
display.print("T: ");
display.println(t);
display.display();
 
delay(1000);
}
