#include <Arduino.h>
#include <U8g2lib.h>
#include <Wire.h>
//#include <Adafruit_Sensor.h>
//#include <SoftwareSerial.h>
#include <math.h>
#include <MPU6050_tockn.h>

//usefull constants
#define m_to_feet 3.28084 //convert m to feet
#define knots_to_kmph 1.852 // convert knots to kmph
#define refresh_rate 1000 // refresh rate in ms
unsigned long lastTime;

// display
// used lib https://github.com/olikraus/u8g2
U8G2_SSD1306_128X64_NONAME_F_HW_I2C bigScr(U8G2_R0, 5, 4, U8X8_PIN_NONE);
#define cx 64
#define cy 32
#define vx 46
#define vy 20

MPU6050 gyroSens(Wire);
bool gyroSensorAvailable = false;
struct gyroDataStruct {
  float pitch,yaw,roll; 
  };
gyroDataStruct gyroData;

void setup() {
  int line = 2;
  Serial.begin(9600);
  Wire.begin();
  Serial.println("######## Init display");
  initDisplay();

  Serial.println("######## Init sensor");
  //initGyro(); 

  //if(gyroSensorAvailable) { bigScr.drawStr(0,line*8,"Gyro/Accelerometer OK"); } else { bigScr.drawStr(0,line*8,"No Gyro/Accelerometer"); } 
  bigScr.sendBuffer();
  line ++;
  bigScr.drawStr(0,line*8,"######## Init completed");

  delay(2000);
}

void loop() {
  lastTime = millis();
  bigScr.clear();
  //readGyro();
  
  drawHorizon();
  drawHorizonBaseline();
  drawPlane();

  bigScr.sendBuffer();
  unsigned long elapsed = millis() - lastTime;
  delay(refresh_rate-elapsed);   // update by refresh 
}

//display
void initDisplay(){
  bigScr.begin();
  bigScr.setFont(u8g_font_helvR08);
  bigScr.clear();
  bigScr.drawStr(0,8,"Big screen init..."); 
  bigScr.sendBuffer(); 
}

void drawPlane(){
  //draw plane in center
  bigScr.drawHLine(49,cy,10); //left
  bigScr.drawHLine(69,cy,10); //right
  bigScr.drawLine(59,cy,cx,37); //mid
  bigScr.drawLine(69,cy,cx,37); //mid
  bigScr.drawPixel(cx,cy); //mid point
}

void drawHorizonBaseline(){
  //zero line
  bigScr.drawTriangle(cx-vx+3,cy,cx-vx,cy+3,cx-vx,cy-3);
  bigScr.drawHLine(cx-vx+3,cy,4); //left
  bigScr.drawTriangle(cx+vx-3,cy,cx+vx,cy+3,cx+vx,cy-3);
  bigScr.drawHLine(cx+vx-7,cy,4); //left
}

void drawHorizon(){
  //draw horizon
 
  // bigScr.drawFrame(cx-vx,cy-vy,2*vx,2*vy);  //debug frame

  // adjust roll
  int dy = - vx * tan(PI*gyroData.roll/180) - vy*sin(PI*gyroData.pitch/180);
  int dx = vx;
  int hy = abs(dy);

 // frame roll in view
  if(hy > vy) {
    int offset = (hy - vy) / abs(tan(PI*gyroData.roll/180));
    dx -= offset; 
    if(dy < 0) dy = -vy; else dy = vy;
  }
  bigScr.drawLine(cx - dx,cy - dy,cx + dx,cy + dy); //horizon line 
  //fill land
}


//gyro
void initGyro(){
  gyroSens.begin();
  gyroSens.calcGyroOffsets(true);
  gyroSensorAvailable = true;
  Serial.println("Gyro sensor initialized");
}

void readGyro(){	
  gyroSens.update();
  
  // needs adjusting accordinh to sensor positioning
  gyroData.pitch = gyroSens.getAngleY();
  gyroData.yaw = gyroSens.getAngleZ();
  gyroData.roll = gyroSens.getAngleX(); 
  
  //debug output
  Serial.println("Gyro sensor");
  Serial.print("Pitch: ");	Serial.print(gyroData.pitch);	Serial.println(" deg");
  Serial.print("Yaw: "); Serial.print(gyroData.yaw); Serial.println(" deg");
  Serial.print("Roll: "); Serial.print(gyroData.roll); Serial.println(" deg");
}