/*


*/
#include <Arduino.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>

#define BTN_PIN 5
#define TFT_DC 2
#define TFT_CS 15
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

#include "img_lib.h"
#include "d3.h"
#include "qsort.h"
#include "object.h"


#ifndef NDEBUG
#define DBG(fmt, ...) Serial.printf("[%9lu %s:%d] " fmt "\n", millis(), __func__, __LINE__, ##__VA_ARGS__)
#define TRACE()       Serial.printf("[%9lu %s:%d]\n", millis(), __func__, __LINE__)
#else
#define DBG(fmt, ...)
#define TRACE()
#endif

#define WARN(fmt, ...)  Serial.printf("\033[1;33m[%9lu %s:%d] " fmt "\e[0m\n", millis(), __func__, __LINE__, ##__VA_ARGS__)
#define ERR(fmt, ...)   Serial.printf("\033[1;31m[%9lu %s:%d] " fmt "\e[0m\n", millis(), __func__, __LINE__, ##__VA_ARGS__)

void setup() {
  pinMode(BTN_PIN, INPUT_PULLUP);

  tft.begin();
  tft.setRotation(1);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.print("Start");

  Serial.begin(115200);
  Serial.printf("Start !\n");
  Serial.printf("Application " __FILE__ " compiled " __DATE__ " at " __TIME__ "\n");

  DBG("This is a debug statement.");
  WARN("This is a warning.");
  ERR("This is an error.");
  DBG("max alloc %u B.\n", ESP.getMaxAllocHeap());
  DBG("Object has %u points, %u lines, %u triangles.", NPOINTS, NLINES, NTRIANGLES);
}

void loop() {
  const int height = 200; // tft.height();
  const int width = height; //tft.width()/2;
  STATIC_IMG(img, width, height, IMG_TYPE_RGB565);
  if (!img) {
    while (true) {
      ERR("alloc failed.");
      delay(10);
    }
  }

  uint32_t t0 = millis();

  // Draw a 3D object
  draw_obj(img);

  uint32_t t1 = millis();

  tft.drawRGBBitmap(0, 0, (uint16_t *)img->data, width, height);

  uint32_t t2 = millis();

  DBG("Draw in %u ms, blit in %u ms, max alloc %u B.\n", t1 - t0, t2 - t1, ESP.getMaxAllocHeap());

  delay(10);
}



void draw_obj(img_t *img) {
  DBG("draw_obj");
  /* Draw a 3D object */
  STATIC_MAT4X4(homogeneous_transform);
  STATIC_MAT4X4(projection);
  STATIC_MAT4X4(transf_proj);
  STATIC_VEC4(translation);
  translation->z = -5.0;

  float alpha, beta, gamma;
  int now_ms = millis();
  beta =  radians(now_ms / 6.);
  gamma = beta / 10;
  alpha = gamma / 30;

  /* Define a homogeneous transform, from 3 rotations and a 3-axes translation */
  //homogeneous_transform = ht_transform(homogeneous_transform, alpha, beta, gamma, translation);
  homogeneous_transform = ht_transform_axis_angle(homogeneous_transform,
  (vec4_t *) & ((const float []) {
    0.0, 1.0, 0.0, 0.0
  }),
  radians(now_ms / 150.),
  translation);
  /* Define the projection matrix */
  projection = ht_projection(projection, radians(45), img->w * 1.0 / img->h, 0.1, 1000.);
  /* The projected transform matrix is composed of both */
  transf_proj = mat4x4_prod(transf_proj, projection, homogeneous_transform);

  static int points2d[NPOINTS][3];
  static float points3d_transformed[NPOINTS][4];
  STATIC_VEC4(v_proj);

  DBG("transform vertices");
  /* Transform and project points */
  for (int i = 0; i < NPOINTS; i++) {
    mat4x4_vec4_prod(v_proj, transf_proj, (vec4_t *)&points3d[i]);

    if (abs(v_proj->w) < 1.0e-8) {
      ERR("Point %d projection error.", i);
      v_proj->w = 1.0;
    }
    points2d[i][0] = img->w * (0.5 + 1.0 * v_proj->x / v_proj -> w);
    points2d[i][1] = img->h * (0.5 + 1.0 * v_proj->y / v_proj -> w);
    points2d[i][2] = img->w * (0.5 + 1.0 * v_proj->z / v_proj -> w);  // for triangles z-sort

    //    DBG("Point #%u: (%d,%d,%d)", i, points2d[i][0], points2d[i][1], points2d[i][2]);

    // store points 3D coordinates (hacky...)
    points3d_transformed[i][0] = v_proj->x;
    points3d_transformed[i][1] = v_proj->y;
    points3d_transformed[i][2] = v_proj->z;
  }

  /* Sort triangles by Z coordinate */
  static int triangle_z[NTRIANGLES];
  static int z_sort[NTRIANGLES];
  static float lighting[NTRIANGLES];

  /* Set light direction */
  STATIC_VEC4(light_direction);
  light_direction->x = cos(radians(now_ms / 6.));
  light_direction->y = sin(radians(now_ms / 6.));
  light_direction->z = 1.0;
  vec4_normalize(light_direction);

  DBG("compute triangle Z and normal");
  /* Compute triangle Z, and normal-based lighting */
  STATIC_VEC4(normal);
  for (int i = 0; i < NTRIANGLES; i++) {
    int p = triangles[i][0];
    int q = triangles[i][1];
    int r = triangles[i][2];

    /* Compute triangle Z coordinate */
    triangle_z[i] = -(points2d[p][2] + points2d[q][2] + points2d[r][2]) / 3;

    /* Compute face orientation */
    triangle_normal((float *)normal, (float *)points3d_transformed[p], (float *)points3d_transformed[q], (float *)points3d_transformed[r]);
    lighting[i] = vec4_dot_prod(light_direction, normal);
  }

  /* sort triangles by average Z depth */
  quicksort(triangle_z, z_sort, NTRIANGLES);

  DBG("draw");
  /* Clear picture */
  img_fill(img, RGB565(0, 0, 0));

  /* Draw triangles */
  for (int i = 0; i < NTRIANGLES; i++) {
    int t = z_sort[i];

    int p = triangles[t][0];
    int q = triangles[t][1];
    int r = triangles[t][2];

    int px = points2d[p][0], py = points2d[p][1];
    int qx = points2d[q][0], qy = points2d[q][1];
    int rx = points2d[r][0], ry = points2d[r][1];

    // Each triangle a color
    //uint16_t color = HSV2RGB565(256 * t / NTRIANGLES, 196, 128);
    // Yellow, with Lambert lighting
    uint16_t color = HSV2RGB565(32, 196, (uint8_t)(32 + 128 + 96 * lighting[t]));

    img_fill_triangle(img, px, py, qx, qy, rx, ry, color);
  }

  /* Draw lines */
  for (int i = 0; i < NLINES; i++) {
    int p = lines[i][0];
    int q = lines[i][1];
    int px = points2d[p][0], py = points2d[p][1];
    int qx = points2d[q][0], qy = points2d[q][1];
    //img_draw_line(img, px, py, qx, qy, HSV2RGB565(256 * i / NLINES, 255, 255));
  }

  /* Draw points */
  for (int i = 0; i < NPOINTS; i++) {
    //img_draw_circle(img, points2d[i][0], points2d[i][1], 4, HSV2RGB565(256 * i / NPOINTS, 255, 255));
    //img_putpixel(img, points2d[i][0], points2d[i][1], HSV2RGB565(256 * i / NPOINTS, 255, 255));
  }
}