#include <Esplora.h>
#include <SPI.h>
#include <TFT.h>
#include <math.h>

int r = EsploraTFT.height()/2;  // Radius der Libelle
int mx = EsploraTFT.width()/2;  // Mittelpunkt x
int my = EsploraTFT.height()/2; // Mittelpunkt y
int x_old = mx;
int y_old = my;
int x_new = x_old;
int y_new = y_old;
int sl_range = 255;  // Enpfindlichkeit für Libelle
int ang_range = 100;
int x_cal = -15;  // Kalibrierung 
int y_cal = -15;  // Kalibrierung
unsigned long interval = 50;
unsigned long prev_millis = 0;

String phi_x_old;
String phi_y_old;
String x_axis_old;
String y_axis_old;
String test_old;
double phi_x = 0; // Winkel in x-Richtung in °
double phi_y = 0; // Winkel in y-Richtung in °

void setup() {
  EsploraTFT.begin();
  displaySpiritLevel();
}

void loop() {
  double x_axis = Esplora.readAccelerometer(X_AXIS) + x_cal;
  double y_axis = Esplora.readAccelerometer(Y_AXIS) + y_cal;
  
  // Display rotieren
  /*if((-10.00 < phi_x && phi_x < 10.00) && (-10.00 < phi_y && phi_y < 10.00)){
     printString("Test 1",&test_old,1,21);
     EsploraTFT.setRotation(1);
  }else if ((-100 < phi_x < 30) && (-10 < phi_y < 10)){
    printString("Test 0",&test_old,1,21);
     EsploraTFT.setRotation(0);
  }else if ((30 < phi_x < 100) && (-10 < phi_y < 10)){
    printString("Test 2",&test_old,1,21);
     EsploraTFT.setRotation(2);
  //}else if (-10 < phi_x < 10 && -10 < phi_y < 10){
  
    
  //}else if (-10 < phi_x < 10 && -10 < phi_y < 10){
    
  }*/
  
  EsploraTFT.setRotation(2);
  mx = EsploraTFT.width()/2;
  my = EsploraTFT.height()/2;
  
  String s = "Test 1";  
  printString(s,&test_old,1,21);
     
  if(millis() - prev_millis > interval){
     displayAngel(x_axis,y_axis,ang_range); // Neigungswinkel anzeigen
     prev_millis = millis();
  }
   
  displayBubble(x_axis,y_axis,sl_range); // Libelle anzeigen
}

void displayAngel(double x_axis,double y_axis,int range){
  double a_x = map(x_axis,-range,range,-9.81,9.81); //Skalierung auf Beschleunigunswerte
  a_x = constrain(a_x,-9.81,9.81);
  double a_y = map(y_axis,-range,range,-9.81,9.81); //Skalierung auf Beschleunigunswerte
  a_y = constrain(a_y,-9.81,9.81);
  phi_x = asin(a_x/9.81) * 180/3.14; // Winkel in x-Richtung in °
  phi_y = asin(a_y/9.81) * 180/3.14; // Winkel in y-Richtung in °
  
  char c = (char)247; // Grad-Zeichen
  String phi_x_s = "Phi X: " + String(phi_x) + String(c);
  printString(phi_x_s,&phi_x_old,1,1);
  String phi_y_s = "Phi Y: " + String(phi_y) + String(c);
  printString(phi_y_s,&phi_y_old,1,11);
  
  }

void printString(String txt,String * txt_ptr_old,int x_pos,int y_pos){
  
  // alten Text löschen
  String txt_old = *txt_ptr_old;
  char txt_out_old[txt_old.length() + 1];
  txt_old.toCharArray(txt_out_old,txt_old.length() + 1);
  EsploraTFT.stroke(255,255,255);
  EsploraTFT.text(txt_out_old,x_pos,y_pos);
  
  /*EsploraTFT.noStroke();
  EsploraTFT.fill(255,255,255);
  EsploraTFT.rect(x_pos,y_pos,96,11);*/
  
  
  // neuen Text schreiben
  char txt_out[txt.length() + 1];
  txt.toCharArray(txt_out,txt.length() + 1);
  EsploraTFT.stroke(0,0,0);
  EsploraTFT.text(txt_out,x_pos,y_pos);
  
  *txt_ptr_old = txt;
  }
  
void displayBubble(double x_axis,double y_axis,int range){
  x_axis = map(x_axis,-range,range,-r,r);
  y_axis = map(y_axis,-range,range,-r,r);
  
  // Umrechnung in Polarkoordinaten
  int r_1 = sqrt((x_axis * x_axis) + (y_axis * y_axis));
  double phi = atan2(y_axis,x_axis);
  
  //Beschränkung des Abstandes
  r_1 = min(r_1,r-10);
  
  //Zurückrechnung in kartesische Koordinaten
  x_axis = r_1 * cos(phi);
  y_axis = r_1 * sin(phi);
  
  x_new = x_axis  + mx;
  y_new = -y_axis  + my;

  EsploraTFT.noStroke();
  EsploraTFT.fill(255,255,0);
  EsploraTFT.circle(x_old,y_old,8);
  
  EsploraTFT.stroke(128,128,128);
  EsploraTFT.fill(255,255,255);
  EsploraTFT.circle(x_new,y_new,8);
  
  EsploraTFT.stroke(0, 0, 0);
  EsploraTFT.noFill();
  EsploraTFT.circle(mx,my,10);
  
  x_old = x_new;
  y_old = y_new;
  }
  
void displaySpiritLevel(){
  EsploraTFT.background(255,255,255);
  EsploraTFT.stroke(0, 0, 0);
  EsploraTFT.fill(255,255,0);
  EsploraTFT.circle(mx,my,r);
  EsploraTFT.noFill();
  EsploraTFT.circle(mx,my,10);
  }