#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#include <Adafruit_NeoPixel.h>

// 初始化OLED显示屏 (128x64像素)
Adafruit_SSD1306 display(128, 64);

// 初始化NeoPixel LED (使用引脚6，1个LED)
#define LED_PIN   6
#define LED_COUNT 1
Adafruit_NeoPixel neoPixel(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);

// MPU-6050 I2C地址
const int mpuAddress = 0x68;

// 陀螺仪旋转角度变量
float xByGyro, yByGyro, zByGyro;

// 显示中心坐标
const int xOrigin = 64;
const int yOrigin = 32;

// 视距参数 (值越大透视效果越弱)
const float viewDistance = 150.0;

// 立方体顶点坐标 (8个顶点，每个顶点有x,y,z三个值)
#define NUM_VERTICES 8
const int cube_vertex[NUM_VERTICES][3] = 
{
  { -20, -20,  20 },  // 顶点0: x, y, z
  {  20, -20,  20 },  // 顶点1
  {  20,  20,  20 },  // 顶点2
  { -20,  20,  20 },  // 顶点3
  { -20, -20, -20 },  // 顶点4
  {  20, -20, -20 },  // 顶点5
  {  20,  20, -20 },  // 顶点6
  { -20,  20, -20 }   // 顶点7
};

// 线框数组，存储转换后的2D坐标
int wireframe[NUM_VERTICES][2];

void setup()
{
  Serial.begin(115200);
  Wire.begin();

  // 初始化OLED显示屏
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) 
  {
    Serial.println(F("SSD1306分配失败")); // OLED初始化失败
    for(;;); // 停机
  }

  // 初始化MPU-6050
  Wire.beginTransmission(mpuAddress);
  Wire.write(0x6B); // PWR_MGMT_1寄存器
  Wire.write(0);    // 唤醒MPU-6050
  auto error = Wire.endTransmission();
  if(error != 0)
  {
    Serial.println(F("错误: 未找到MPU-6050")); // MPU-6050未找到
    for(;;); // 停机
  }

  // 初始化NeoPixel
  neoPixel.begin();
}

void loop()
{
  // 读取MPU-6050数据
  Wire.beginTransmission(mpuAddress);
  Wire.write(0x3B); // 从ACCEL_XOUT_H寄存器开始
  Wire.endTransmission(false); // 重复启动条件

  // 定义传感器变量 (16位有符号整数)
  int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;

  // 请求14字节数据 (7个传感器值)
  Wire.requestFrom(mpuAddress, 14);
  AcX = Wire.read()<<8 | Wire.read(); // 加速度X
  AcY = Wire.read()<<8 | Wire.read(); // 加速度Y
  AcZ = Wire.read()<<8 | Wire.read(); // 加速度Z
  Tmp = Wire.read()<<8 | Wire.read(); // 温度
  GyX = Wire.read()<<8 | Wire.read(); // 陀螺X
  GyY = Wire.read()<<8 | Wire.read(); // 陀螺Y
  GyZ = Wire.read()<<8 | Wire.read(); // 陀螺Z

  // 计算旋转角度 (简化处理)
  // 通过加速度计获取静态角度
  float xByAccel = (float)AcX * 0.0001;
  float yByAccel = (float)AcY * 0.0001;
  float zByAccel = (float)AcZ * 0.0001;

  // 通过陀螺仪获取动态角度变化
  xByGyro += (float)GyX * 0.00001;
  yByGyro += (float)GyY * 0.00001;
  zByGyro += (float)GyZ * 0.00001;

  // 合并两种角度测量
  float x = xByAccel + xByGyro;
  float y = yByAccel + yByGyro;
  float z = zByAccel + zByGyro;

  // 保持角度在0-2π范围内
  if(x < 0.0) x += 2.0 * M_PI; else if(x > 2.0 * M_PI) x -= 2.0 * M_PI;
  if(y < 0.0) y += 2.0 * M_PI; else if(y > 2.0 * M_PI) y -= 2.0 * M_PI;
  if(z < 0.0) z += 2.0 * M_PI; else if(z > 2.0 * M_PI) z -= 2.0 * M_PI;

  // 3D变换和投影
  for(int i = 0; i < NUM_VERTICES; i++) 
  {
    // 绕Y轴旋转
    float rotx = cube_vertex[i][2] * sin(y) + cube_vertex[i][0] * cos(y);
    float roty = cube_vertex[i][1];
    float rotz = cube_vertex[i][2] * cos(y) - cube_vertex[i][0] * sin(y);

    // 绕X轴旋转
    float rotxx = rotx;
    float rotyy = roty * cos(x) - rotz * sin(x);
    float rotzz = roty * sin(x) + rotz * cos(x);

    // 绕Z轴旋转
    float rotxxx = rotxx * cos(z) - rotyy * sin(z);
    float rotyyy = rotxx * sin(z) + rotyy * cos(z);
    float rotzzz = rotzz;

    // 添加透视效果
    rotxxx *= viewDistance / (viewDistance + rotzzz);
    rotyyy *= viewDistance / (viewDistance + rotzzz);

    // 移动到屏幕中心
    rotxxx += (float)xOrigin;
    rotyyy += (float)yOrigin;

    // 存储转换后的坐标
    wireframe[i][0] = (int)rotxxx;
    wireframe[i][1] = (int)rotyyy;
  }
  
  // 绘制线框
  draw_wireframe();

  // 根据温度设置NeoPixel颜色
  float Celsius = ((float)Tmp / 340.00) + 36.53;
  float hue = (Celsius + 40.0) / 125.0 * 65535.0;
  uint32_t rgbcolor = neoPixel.ColorHSV((uint16_t)hue);
  neoPixel.setPixelColor(0, rgbcolor);
  neoPixel.show(); // 更新LED
}

// 绘制立方体线框
void draw_wireframe(void)
{
  display.clearDisplay(); // 清屏

  // 绘制立方体的12条边
  display.drawLine(wireframe[0][0], wireframe[0][1], wireframe[1][0], wireframe[1][1], SSD1306_WHITE);
  display.drawLine(wireframe[1][0], wireframe[1][1], wireframe[2][0], wireframe[2][1], SSD1306_WHITE);
  display.drawLine(wireframe[2][0], wireframe[2][1], wireframe[3][0], wireframe[3][1], SSD1306_WHITE);
  display.drawLine(wireframe[3][0], wireframe[3][1], wireframe[0][0], wireframe[0][1], SSD1306_WHITE);

  display.drawLine(wireframe[4][0], wireframe[4][1], wireframe[5][0], wireframe[5][1], SSD1306_WHITE);
  display.drawLine(wireframe[5][0], wireframe[5][1], wireframe[6][0], wireframe[6][1], SSD1306_WHITE);
  display.drawLine(wireframe[6][0], wireframe[6][1], wireframe[7][0], wireframe[7][1], SSD1306_WHITE);
  display.drawLine(wireframe[7][0], wireframe[7][1], wireframe[4][0], wireframe[4][1], SSD1306_WHITE);

  display.drawLine(wireframe[0][0], wireframe[0][1], wireframe[4][0], wireframe[4][1], SSD1306_WHITE);
  display.drawLine(wireframe[1][0], wireframe[1][1], wireframe[5][0], wireframe[5][1], SSD1306_WHITE);
  display.drawLine(wireframe[2][0], wireframe[2][1], wireframe[6][0], wireframe[6][1], SSD1306_WHITE);
  display.drawLine(wireframe[3][0], wireframe[3][1], wireframe[7][0], wireframe[7][1], SSD1306_WHITE);

  // 在一个面上添加交叉线
  display.drawLine(wireframe[1][0], wireframe[1][1], wireframe[3][0], wireframe[3][1], SSD1306_WHITE);
  display.drawLine(wireframe[0][0], wireframe[0][1], wireframe[2][0], wireframe[2][1], SSD1306_WHITE);

  display.display(); // 更新显示
}