// originally by Andy Sloane for the Arduboy
// https://www.a1k0n.net/2017/02/01/arduboy-teapot.html

#include <Arduino.h>
#include <U8g2lib.h>
#include <Wire.h>

#include <avr/pgmspace.h>
#include "draw.h"
#include "sincos.h"
#include "vec.h"
#include "teapot.h"

U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);

const int MPU_addr = 0x68;                       // I2C address of the MPU-6050
uint8_t *screen_;
uint16_t frame_ = 0;

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

  // Initialize the MPU-6050 and test if it is connected.
  Wire.beginTransmission( MPU_addr);
  Wire.write( 0x6B);                           // PWR_MGMT_1 register
  Wire.write( 0);                              // set to zero (wakes up the MPU-6050)
  auto error = Wire.endTransmission();
  if( error != 0)
  {
    Serial.println(F( "Error, MPU-6050 not found"));
    for(;;);
  }
}

class Stars {
  static const int kNumStars = 30;
  uint8_t ypos_[kNumStars];
  uint8_t xpos_[kNumStars];
  uint8_t xspeed_[kNumStars];

 public:
  Stars() {
    for (uint8_t i = 0; i < kNumStars; i++) {
      ypos_[i] = rand() & 63;
      xpos_[i] = rand() & 255;
      xspeed_[i] = 1 + (rand() & 7);
    }
  }

  void Draw() {
    for (uint8_t i = 0; i < kNumStars; i++) {
      uint16_t page = (ypos_[i] >> 3) << 7;
      uint8_t mask = 1 << (ypos_[i] & 7);
      uint8_t x = xpos_[i] >> 1;
      screen_[page + x] |= mask;
      if (xpos_[i] < xspeed_[i]) {
        xpos_[i] = 255;
        ypos_[i] = rand() & 63;
        xspeed_[i] = 1 + (rand() & 7);
      } else {
        xpos_[i] -= xspeed_[i];
      }
    }
  }
};

int32_t angle_A_ = 0, angle_B_ = 0, angle_C_ = 0;
int16_t scale_ = 1024 + 200;

void ReadInput(int16_t gx, int16_t gy, int16_t gz) {
//  static int32_t A_target = 100000, B_target = 50000;
  static int32_t A_target = 0, B_target = 0, C_target = 0;
  static const int32_t kTurnSpeed = 1000;
  static bool manual_control = false;

  if (gx || gy || gz) {
    manual_control = true;
    A_target += gx / 4;
    B_target += gy / 4;
    C_target += gz / 4;
  }

  if (!manual_control) {
    A_target += 499;
    B_target += 331;
    C_target += 229;
  }

  angle_A_ += (A_target - angle_A_) >> 6;
  angle_B_ += (B_target - angle_B_) >> 6;
  angle_C_ += (C_target - angle_C_) >> 6;
}

static Vec216 verts[mesh_NVERTS];  // rotated, projected screen space vertices

// down-convert signed 1.10 precision to signed 0.7 precision
// (scaling down from -1024..1024 to -127..127)
static int8_t RescaleR(int16_t x) {
  return ((uint32_t) x*127 + 512) >> 10;
}

void DrawObject() {
  // construct rotation matrix
  int16_t cA, sA, cB, sB, cC, sC;

  GetSinCos((angle_A_ >> 6) & 1023, &sA, &cA);
  GetSinCos((angle_B_ >> 6) & 1023, &sB, &cB);
  GetSinCos((angle_C_ >> 6) & 1023, &sC, &cC);

  // rotate about X axis by C, then Y axis by B, then Z axis by A
  //     [            cA*cB,             cB*sA,    sB]
  // R = [-cA*sB*sC - cC*sA,  cA*cC - sA*sB*sC, cB*sC]
  //     [-cA*cC*sB + sA*sC, -cA*sC - cC*sA*sB, cB*cC]

  // local coordinate frame given rotation values
  // spend some time up front to get an accurate rotation matrix before
  // rounding to 0.7 fixed point
  // the 32-bit math is only done once per frame, and then we can do
  // all the per-vertex stuff in 8-bit math 
  Mat338 rotation_matrix(Vec38(
        RescaleR((int32_t) cA*cB >> 10),
        RescaleR((int32_t) cB*sA >> 10),
        RescaleR(sB)),
      Vec38(
        RescaleR(((int32_t) -cA*sB*sC >> 10) - (int32_t) cC*sA >> 10),
        RescaleR((int32_t) cA*cC - ((int32_t) sA*sB*sC >> 10) >> 10),
        RescaleR((int32_t) cB*sC >> 10)),
      Vec38(
        RescaleR(((int32_t) -cA*cC*sB >> 10) + (int32_t) sA*sC >> 10),
        RescaleR((int32_t) -cA*sC - ((int32_t) cC*sA*sB >> 10) >> 10),
        RescaleR((int32_t) cB*cC >> 10)));

  int8_t sortaxis = 0, sortaxisz = rotation_matrix.z.x;
  if (abs(rotation_matrix.z.y) > abs(sortaxisz)) {
    sortaxis = 1;
    sortaxisz = rotation_matrix.z.y;
  }
  if (abs(rotation_matrix.z.z) > abs(sortaxisz)) {
    sortaxis = 2;
    sortaxisz = rotation_matrix.z.z;
  }

  rotation_matrix.RotateAndProject(mesh_vertices, mesh_NVERTS, scale_, verts);

  // rotate and project all vertices
  /*
  {
    Vec216 *vertptr = verts;
    for (uint16_t j = 0; j < 3*mesh_NVERTS; j += 3) {
      Vec38 obj_vert(
          pgm_read_byte_near(mesh_vertices + j),
          pgm_read_byte_near(mesh_vertices + j + 1),
          pgm_read_byte_near(mesh_vertices + j + 2));
      Vec38 world_vert(
          Fx.dot(obj_vert),
          Fy.dot(obj_vert),
          Fz.dot(obj_vert));
      world_vert.project(scale_, vertptr++);
    }
  }
  */

  // back-face cull and sort faces
  for (uint16_t i = 0; i < mesh_NFACES; i++) {
    uint16_t jf = i;
    // use face sort order depending on which axis is most facing toward camera
    if (sortaxisz < 0) {
      jf = mesh_NFACES - 1 - i;
    }
    if (sortaxis == 1) {
      jf = pgm_read_byte_near(mesh_ysort_faces + jf);
    } else if (sortaxis == 2) {
      jf = pgm_read_byte_near(mesh_zsort_faces + jf);
    }
    jf *= 3;
    uint8_t fa = pgm_read_byte_near(mesh_faces + jf),
            fb = pgm_read_byte_near(mesh_faces + jf + 1),
            fc = pgm_read_byte_near(mesh_faces + jf + 2);
    Vec216 sa = verts[fb] - verts[fa];
    Vec216 sb = verts[fc] - verts[fa];
    if ((int32_t) sa.x * sb.y > (int32_t) sa.y * sb.x) {  // check winding order
      continue;  // back-facing
    }

    int8_t illum = rotation_matrix.CalcIllumination(mesh_normals + jf);
    uint8_t pat[4];
    GetDitherPattern(illum, pat);
    // GetDitherPattern(illum, frame_, pat);
    // u8g2.drawTriangle(
    //     verts[fa].x >> 4, verts[fa].y >> 4,
    //     verts[fb].x >> 4, verts[fb].y >> 4,
    //     verts[fc].x >> 4, verts[fc].y >> 4);
    FillTriangle(
        verts[fa].x, verts[fa].y,
        verts[fb].x, verts[fb].y,
        verts[fc].x, verts[fc].y,
        pat, screen_);
  }
}

Stars stars_;

void loop(void) {
  Wire.beginTransmission( MPU_addr);
  Wire.write( 0x43);  // starting with register 0x43 (GYRO_XOUT_H)
  Wire.endTransmission( false);
  Wire.requestFrom( MPU_addr, 6);  // request a total of 6 bytes
  int16_t GyX = Wire.read()<<8 | Wire.read();  // 0x43 (GYRO_XOUT_H)  & 0x44 (GYRO_XOUT_L)
  int16_t GyY = Wire.read()<<8 | Wire.read();  // 0x45 (GYRO_YOUT_H)  & 0x46 (GYRO_YOUT_L)
  int16_t GyZ = Wire.read()<<8 | Wire.read();  // 0x47 (GYRO_ZOUT_H)  & 0x48 (GYRO_ZOUT_L)

  u8g2.clearBuffer();
  screen_ = u8g2.getBufferPtr();
  stars_.Draw();
  ReadInput(GyX, GyY, GyZ);
  DrawObject();
  u8g2.sendBuffer();
  frame_++;
}