#include "Arduino.h"
#include "U8g2lib.h"
//U8G2_SSD1306_128X32_UNIVISION_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE); // initialization for the 128x32px OLED display, [full framebuffer, size = 512 bytes]
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, U8X8_PIN_NONE);

// Включить проводную библиотеку Arduino для I2C
#include "Wire.h"
const int MPU_addr = 0x68; // адрес датчика
// массив данных
// [accX, accY, accZ, temp, gyrX, gyrY, gyrZ]
// acc - ускорение, gyr - угловая скорость, temp - температура (raw)
int16_t data[7];  




double X3[8][3];
double Ax[3][3];
double Ay[3][3];
double Az[3][3];
double R[3][3];
//double L = 10, H = 6, W = 8;
//double dL = 0.1, dH = 0.1, dW = 0.1;
double fx = 0, fy = 0, fz = 0;
double ClientSize_Width = 128;
double ClientSize_Height = 64;
float xCorrect = 0;
float yCorrect = 0;
float zCorrect = 0;
float xMPU = 0;
float yMPU = 0;
float zMPU = 0;
int8_t goX = 0;
int8_t goY = 0;




void setup() {
  // инициализация
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  u8g2.begin(); // Инифиализация дисплея


  Serial1.begin(115200);
  pinMode(16, INPUT);
  delay(500);







  X3[0][0] = -1; X3[0][1] = -1; X3[0][2] = -1;
  X3[1][0] = -1; X3[1][1] = -1; X3[1][2] = 1;
  X3[2][0] = -1; X3[2][1] = 1; X3[2][2] = -1;
  X3[3][0] = -1; X3[3][1] = 1; X3[3][2] = 1;
  X3[4][0] = 1; X3[4][1] = -1; X3[4][2] = -1;
  X3[5][0] = 1; X3[5][1] = -1; X3[5][2] = 1;
  X3[6][0] = 1; X3[6][1] = 1; X3[6][2] = -1;
  X3[7][0] = 1; X3[7][1] = 1; X3[7][2] = 1;
}








void loop() {




  double w = ClientSize_Width / 2;
  double h = ClientSize_Height / 2;
  double a = w;
  double z0 = 10;
  double X2 [8][3];
  double X2T[3][8];

  double cx = cos(fx), sx = sin(fx);
  double cy = cos(fy), sy = sin(fy);
  double cz = cos(fz), sz = sin(fz);

  R[0][0] = a; R[0][1] = 0; R[0][2] = w;
  R[1][0] = 0; R[1][1] = a; R[1][2] = h;
  R[2][0] = 0; R[2][1] = 0; R[2][2] = 1;

  Ax[0][0] = 1; Ax[0][1] = 0; Ax[0][2] = 0;
  Ax[1][0] = 0; Ax[1][1] = cx; Ax[1][2] = -sx;
  Ax[2][0] = 0; Ax[2][1] = sx; Ax[2][2] = cx;

  Ay[0][0] = cy; Ay[0][1] = 0; Ay[0][2] = -sy;
  Ay[1][0] = 0; Ay[1][1] = 1; Ay[1][2] = 0;
  Ay[2][0] = sy; Ay[2][1] = 0; Ay[2][2] = cy;

  Az[0][0] = cz; Az[0][1] = -sz; Az[0][2] = 0;
  Az[1][0] = sz; Az[1][1] = cx; Az[1][2] = 0;
  Az[2][0] = 0; Az[2][1] = 0; Az[2][2] = 1;

  double Axy [3][3];
  double Axyz [3][3];
  double RAxyz [3][3];
  double X3T [3][8];
  double AxyzX3T [3][8];

/////////////////////////////////////////////////////////////////////////////
        
  for (int i = 0; i < 3; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      Axy[i][j] = 0;
      for (int k = 0; k < 3; k++)
      {
        Axy[i][j] += Ax[i][k] * Ay[k][j];
      }
    } 
  } 
  for (int i = 0; i < 3; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      Axyz[i][j] = 0;
      for (int k = 0; k < 3; k++)
      {
        Axyz[i][j] += Axy[i][k] * Az[k][j];
      } 
    } 
  }        
  for (int i = 0; i < 8; i++)
    {
    for (int j = 0; j < 3; j++)
    {
      X3T[j][i] = X3[i][j];
    }
  } 
  for (int i = 0; i < 3; i++)
  {
    for (int j = 0; j < 8; j++)
    {
      AxyzX3T[i][j] = 0;
      for (int k = 0; k < 3; k++)
      {
        AxyzX3T[i][j] += Axyz[i][k] * X3T[k][j];
      } 
    } 
  } 
  for (int i = 0; i < 8; i++)
  {
    AxyzX3T[2][i] = AxyzX3T[2][i] + z0;
  } 
  for (int i = 0; i < 3; i++)
  {
    for (int j = 0; j < 8; j++)
    {
      X2T[i][j] = 0;
      for (int k = 0; k < 3; k++)
      {
        X2T[i][j] += R[i][k] * AxyzX3T[k][j];
      } 
    } 
  } 
  for (int i = 0; i < 3; i++)
  {
    for (int j = 0; j < 8; j++)
    {
      X2[j][i] = X2T[i][j];
    } 
  } 

  for (int i = 0; i < 8; i++)
  {
    X2[i][0] = X2[i][0] / X2[i][2];
    X2[i][1] = X2[i][1] / X2[i][2];
  } // for
            
            
  u8g2.clearBuffer();
  drawLine((int)X2[1][0] + goX, (int)X2[1][1] + goY, (int)X2[3][0] + goX, (int)X2[3][1] + goY);
  drawLine((int)X2[1][0] + goX, (int)X2[1][1] + goY, (int)X2[5][0] + goX, (int)X2[5][1] + goY);
  drawLine((int)X2[5][0] + goX, (int)X2[5][1] + goY, (int)X2[7][0] + goX, (int)X2[7][1] + goY);
  drawLine((int)X2[7][0] + goX, (int)X2[7][1] + goY, (int)X2[3][0] + goX, (int)X2[3][1] + goY);
  drawLine((int)X2[0][0] + goX, (int)X2[0][1] + goY, (int)X2[2][0] + goX, (int)X2[2][1] + goY);
  drawLine((int)X2[0][0] + goX, (int)X2[0][1] + goY, (int)X2[4][0] + goX, (int)X2[4][1] + goY);
  drawLine((int)X2[4][0] + goX, (int)X2[4][1] + goY, (int)X2[6][0] + goX, (int)X2[6][1] + goY);
  drawLine((int)X2[6][0] + goX, (int)X2[6][1] + goY, (int)X2[2][0] + goX, (int)X2[2][1] + goY);
  drawLine((int)X2[0][0] + goX, (int)X2[0][1] + goY, (int)X2[1][0] + goX, (int)X2[1][1] + goY);
  drawLine((int)X2[2][0] + goX, (int)X2[2][1] + goY, (int)X2[3][0] + goX, (int)X2[3][1] + goY);
  drawLine((int)X2[4][0] + goX, (int)X2[4][1] + goY, (int)X2[5][0] + goX, (int)X2[5][1] + goY);
  drawLine((int)X2[6][0] + goX, (int)X2[6][1] + goY, (int)X2[7][0] + goX, (int)X2[7][1] + goY);
  u8g2.sendBuffer();





  getData();  // получаем

  if (data[3] < -12420){
    Serial1.println("Температура гироскопа: ERROR");
  }else if (data[3] > 7980) {
    Serial1.println("Температура гироскопа: ERROR");
  }else {
    Serial1.print("Температура гироскопа: ");
    Serial1.print(map(data[3], -26020, 16480, -40.00, 85.00));
    Serial1.println("°C");
  }
  
  if (analogRead(26) > 600){
    Serial1.println(" Вверх ");
    xCorrect -= 0.05;

  }else if (analogRead(26) < 400){
    Serial1.println(" Вниз ");
    xCorrect += 0.05;
  }
  if (analogRead(27) > 600){
    Serial1.println(" Лево ");
    yCorrect -= 0.05;

  }else if (analogRead(27) < 400){
    Serial1.println(" Право ");
    yCorrect += 0.05;
  }
  
  goX = map (data[1], -32768, 32768 , -50, 50);
  goY = -(map (data[0], -32768, 32768 , -20, 20));
  goZ (map (data[2], -32768, 32768 , 0, 2000));
  xMPU = map(data[4], -32750, 32750, -4361, 4361);
  yMPU = map(data[5], -32750, 32750, -4361, 4361);
  zMPU = map(data[6], -32750, 32750, -4361, 4361);



  Serial1.print("Кнопка: ");
  if(digitalRead(16)){
    Serial1.println("Сброс!!!");
    xCorrect = 0;
    yCorrect = 0;
    zCorrect = 0;
  } else{
    Serial1.println("OK");
  }

  fx = xCorrect + (xMPU/1000);
  fy = yCorrect + (yMPU/1000);
  fz = zCorrect + (zMPU/1000);



  delay(100);
}


void getData() {//Получение данных с MPU
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // Адрес MPU
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true); // Чтение с 14 регистра
  for (byte i = 0; i < 7; i++) {
    data[i] = Wire.read() << 8 | Wire.read();
  }
}

void goZ (float goZ){
  goZ /= 500;
  X3[0][0] = -goZ; X3[0][1] = -goZ; X3[0][2] = -goZ;
  X3[1][0] = -goZ; X3[1][1] = -goZ; X3[1][2] = goZ;
  X3[2][0] = -goZ; X3[2][1] = goZ; X3[2][2] = -goZ;
  X3[3][0] = -goZ; X3[3][1] = goZ; X3[3][2] = goZ;
  X3[4][0] = goZ; X3[4][1] = -goZ; X3[4][2] = -goZ;
  X3[5][0] = goZ; X3[5][1] = -goZ; X3[5][2] = goZ;
  X3[6][0] = goZ; X3[6][1] = goZ; X3[6][2] = -goZ;
  X3[7][0] = goZ; X3[7][1] = goZ; X3[7][2] = goZ;
}


void drawLine(int16_t x1, int16_t x2, int16_t y1, int16_t y2){
  if (x1 < 0){
    x1 = 0;
  } else if (x1 > 127){
    x1 = 127;
  }
  if (x2 < 0){
    x2 = 0;
  } else if (x2>63){
    x2 = 63;
  }
if (y1 < 0){
    y1 = 0;
  } else if (y1>127){
    y1 = 127;
  }
if (y2 < 0){
    y2 = 0;
  } else if (y2>63){
    y2 = 63;
  }

  u8g2.drawLine(x1, x2, y1, y2);  
}