// display the difference between adjacent video frames (press any key to exit)
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d/features2d.hpp>

#include <GL/gl.h>
#include <GL/freeglut.h>
//#include <GL/glm.h>
//#include <freeglut.h>
//#include <opencv2/nonfree/features2d.hpp>
#define KEY_ESCAPE 27
using namespace cv;

typedef Mat Image ;
typedef Mat View;

typedef struct {
  float x;
  float y;
  float z;
} P;

typedef struct {
  float r;
  float b;
  float g;
} Color;

typedef struct {
  P a;
  P b;
  P c;
  Color color;
} Triangle;

typedef struct {
  Triangle *members;
  int size;
} Model;

typedef struct {
  Model *members;
  int size;
} Models;

Model start_model;
Model current_model;
View current_view;
Color red,blue,green;

void printModel(Model m) {
  int i;
  printf("model:\n");
  for (i=0;i<m.size;i++) {
    Triangle t = m.members[i];
    printf("\ttriangle:\n");
    printf(  "\t\ta =     (%f %f %f)"
           "\n\t\tb =     (%f %f %f)"
           "\n\t\tc =     (%f %f %f)"
           "\n\t\tcolor = [%f %f %f]\n",
           t.a.x, t.a.y, t.a.z,
           t.b.x, t.b.y, t.b.z,
           t.c.x, t.c.y, t.c.z,
           t.color.r, t.color.b, t.color.g
          );
  }
}

void initStartModel(){
  //&start_model = malloc(sizeof(Model))
  static Color r = {1.0f,0.0f,0.0f};
  red = r;
  static Color b = {0.0f,1.0f,0.0f};
  blue = b;
  static Color g = {0.0f,0.0f,1.0f};
  green = g;
  static P p1 = {0.0f,0.0f,0.0f};
  static P p2 = {0.0f,1.0f,0.0f};
  static P p3 = {1.0f,0.0f,0.0f};
  static Triangle t0 = {p1,p2,p3,red};
  static P p1_ = {0.0f,1.0f,1.0f};
  static P p2_ = {0.0f,1.0f,0.0f};
  static P p3_ = {0.0f,0.0f,0.0f};
  static Triangle t1 = {p1_,p2_,p3_,blue};
  static Triangle* ts = (Triangle*)malloc(2*sizeof(Triangle));
  ts[0] = t0;
  ts[1] = t1;
  start_model.members = ts;
  start_model.size = 2;
  printModel(start_model);
  current_model = start_model;
}

typedef struct {
  int width;
  int height;
  char* title;
  float field_of_view_angle;
  float z_near;
  float z_far;
} glutWindow;

glutWindow win;

Image glutTakeCVImage() {
  // take a picture within glut and return formatted
  // for use in openCV
  int width = win.width;
  int height = win.height;
  unsigned char* buffer = new unsigned char[width*height*3];
  glReadPixels(0, 0, width, height, GL_RGB, GL_UNSIGNED_BYTE, buffer);
  Image img(height, width, CV_8UC3, buffer);
  Image flipped_img;
  flip(img,flipped_img,0);
  Image BGR_img;
  cvtColor(flipped_img,BGR_img, COLOR_RGB2BGR);
  return BGR_img;
}

void drawModel(Model m){
  int i;
  for (i=0;i<m.size;i++) {
    Triangle t = m.members[i];
    glPushMatrix();
      glColor3f(t.color.r,t.color.b,t.color.g);
      glBegin(GL_TRIANGLE_STRIP);
      glVertex3f(t.a.x,t.a.y,t.a.z);
      glVertex3f(t.b.x,t.b.y,t.b.z);
      glVertex3f(t.c.x,t.c.y,t.c.z);
      glEnd();
    glPopMatrix();
  }
}

typedef P L;

L line (P a, P b) {
  L *r = (L*)malloc(sizeof(L));
  r->x = b.x - a.x;
  r->y = b.y - a.y;
  r->z = b.z - a.z;
  return *r;
}

float magnitude (L line) {
  return sqrt(pow(line.x,2)+pow(line.y,2)+pow(line.z,2));
}

float dot_product (L a, L b) {
  return a.x*b.x +
         a.y*b.y +
         a.z*b.z;
}

float angle (L AB, L AC) {
  return acos(dot_product(AB,AC)
              /(magnitude(AB) * magnitude(AC)));
}

float area_triangle(Triangle t) {
  L AB = line(t.a,t.b);
  L AC = line(t.a,t.c);
  return .5 * magnitude(AB)*magnitude(AC)*sin(angle(AB,AC));
}

float area_model (Model m) {
  float total = 0;
  int i = 0;
  for (;i<m.size;i++) {
    total += area_triangle(m.members[i]);
  }
  return total;
}

float model_compare_with(Model m1, Model m2, float f(Model)) {
  return f(m2)/f(m1);
}

float model_compare(Model m1, Model m2) {
  return model_compare_with(m1,m2,area_model);
}

//Model
void reconstruct(Image i1, View v1, Image i2, View v2) {
  vector<cv::KeyPoint> kpts1,kpts2;
  Ptr<AKAZE> akazze = AKAZE::create();
  ORB detector();
  //GoodFeaturesToTrackDetector::Params paramsGTFF;
  //detector.detect(i0,keypoints0);
  //detector.detect(i1,keypoints1);
  //surf.detect(sceneMat,keypoints1);
  //surf.detect(objectMat,keypoints0);
  //SurfDescriptorExtractor extractor;
  //extractor.compute(sceneMat,keypointsS,descriptors_scene);
  //extractor.compute(objectMat,keypoints0,descriptors_objject);
  //ClannBasedMatcher matcher;
  //BFMatcher matcher(NORM_L1);
  //vector<vector<DMatch>> matches;
  //matcher.knnMatch(descriptors_object,descriptors_scene,matches,2);
  //vector<DMatch> good_matches;
  //size_t i = 0;
  //for (;i<matches.size();++i) {
    //if (matches[i].size()<2) continue;
    //const DMatch &m1 = matches[i][0];
    //const DMatch &m2 = matche[i][1];
    //if (m1.distance <= nndrRatio * m2.distance) good_matches.push_back(m1);
  //}
  //triangulatePoints(v1,v2,i1,i2,reconstruction);
  //return reconstruction;
  //triangulatePoints(InputArray projMatr1,   //cam0 3×4 projection mat
                    //InputArray projMatr2,   //cam1 3×4 projection mat
                    //InputArray projPoints1, //cam0pnts feature points
                    //InputArray ProjPoints2, //cam1pnts feature points
                    //OutputArray points4D)   //pnts3D 4×N array reconstructed points
                                                //homogeneous coordinates?
  //Model m = mesh_from_points(points4D);
  //return m;
}

/*

vector<Model> experiment(View v1, View v2, int max_model) {
  vector<Model> model_series;
  model_series.push(start_model);
  int model_index = 1;
  for (;model_index<max_model;model_index++) {
    img1 = get_img(model_series[model_index-1],v1);
    img2 = get_img(model_series[model_index-1],v2);
    model_series.push(reconstruct(img1,v1,img2,v2));
  }
  return model_series;
}
*/

void showModelView() {
  GLfloat*mv = (GLfloat*)malloc(16*sizeof(GLfloat));
  glGetFloatv(GL_MODELVIEW_MATRIX,mv);
  int i,j,n;
  printf("MODEL VIEW\n");
  for (n=0,i=0;i<4;i++) {
    for (j=0;j<4;j++,n++)
      printf("%f ", mv[n]);
    putchar('\n');
  }
  putchar('\n');
}

Matx34f getProjection() {
  // 0 4 8 12
  // 1 5 9 13
  // 2 6 10 14
  // 3 7 11 15
  GLfloat*p = (GLfloat*)malloc(16*sizeof(GLfloat));
  glGetFloatv(GL_PROJECTION,p);
  Matx34f m(p[0], p[4], p[8], p[12],
            p[1], p[5], p[9], p[13],
            p[2], p[6], p[10], p[14]);
            //p[3], p[7], p[11], p[15] cut this off?
  return m;
}

void display() {
  showModelView();
  // Clear Screen and Depth Buffer
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glLoadIdentity();
  // Define a viewing transformation
  // need to use View matrix
  gluLookAt(4,2,0, 0,0,0, 0,1,0);
  glPushMatrix();
    drawModel(current_model);
  glPopMatrix();
  glutSwapBuffers();
  //Image img = glutTakeCVImage();
  //GlutleaveMainloop();
  //return img;
}

Image get_img (Model m, View v) {
  current_model = m;
  current_view = v;
  glutMainLoop();
  Image img = glutTakeCVImage();
  glutLeaveMainLoop();
  return img;
}


void initializeGlut () {
   // select projection matrix
  glMatrixMode(GL_PROJECTION);
  // set the viewport
  glViewport(0, 0, win.width, win.height);
  // set matrix mode
  glMatrixMode(GL_PROJECTION);
  // reset projection matrix
  glLoadIdentity();
  GLfloat aspect = (GLfloat) win.width / win.height;
  // set up a perspective projection matrix
  gluPerspective(win.field_of_view_angle, aspect, win.z_near, win.z_far);
  // specify which matrix is the current matrix
  glMatrixMode(GL_MODELVIEW);
  glShadeModel(GL_SMOOTH);
  // specify the clear value for the depth buffer
  glClearDepth( 1.0f );
  glEnable(GL_DEPTH_TEST);
  glDepthFunc(GL_LEQUAL);
  // specify implementation-specific hints
  glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
  GLfloat amb_light[] = { 0.1, 0.1, 0.1, 1.0 };
  GLfloat diffuse[] = { 0.6, 0.6, 0.6, 1 };
  GLfloat specular[] = { 0.7, 0.7, 0.3, 1 };
  glLightModelfv( GL_LIGHT_MODEL_AMBIENT, amb_light);
  glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuse);
  glLightfv(GL_LIGHT0, GL_SPECULAR, specular);
  glEnable(GL_LIGHT0);
  glEnable(GL_COLOR_MATERIAL);
  glShadeModel(GL_SMOOTH);
  glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, GL_FALSE);
  glDepthFunc(GL_LEQUAL);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_LIGHTING);
  glEnable(GL_LIGHT0);
  glClearColor(0.0, 0.0, 0.0, 1.0);
}

Image snap(){
  VideoCapture capture(0); // connect to a camera
  Image img;
  capture >> img;
  return img;
}

void keyboard (unsigned char key, int mousePositionX, int mousePositionY) {
  Image img;
  switch (key)
  {
    case KEY_ESCAPE:
      exit(0);
      break;
    case 'c':
      printf("hello\n");
      glutLeaveMainLoop();
      img = glutTakeCVImage();
      while (waitKey(10)<0) {
        imshow("CV image", img);
      }
      glutMainLoop();
      break;
    default:
      break;
  }
}

int main(int argc, char **argv) {
  // set window values
  win.width = 640;
  win.height = 480;
  win.title = const_cast<char*>("OpenGL/GLUT");
  win.field_of_view_angle = 45;
  win.z_near = 1.0f;
  win.z_far = 500.0f;
  initStartModel();
  printf("area model = %f\n", area_model(start_model));
  // initialize and run program
  glutInit(&argc, argv);                                     // GLUT initialization
  glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);  // Display Mode
  glutInitWindowSize(win.width,win.height);                  // set window size
  glutCreateWindow(win.title);                               // create Window
  glutDisplayFunc(display);                                  // register Display Function
  glutIdleFunc(display);                                     // register Idle Function
  glutKeyboardFunc(keyboard);                                // register Keyboard Handler
  initializeGlut();
  glutMainLoop();

  return 0;
}