#include "ofApp.h"

// some v4l2 global settings
int camWidth = 640;
int camHeight = 480;

//--------------------------------------------------------------
void ofApp::setup()
{
  ofDisableArbTex();
  ofSetVerticalSync(false);
  //ofSetLogLevel(OF_LOG_VERBOSE);

  ofLogNotice(__FUNCTION__) << "Found " << ofxAzureKinect::Device::getInstalledCount() << " installed devices.";

  // Open Kinect.
  if (kinectDevice.open())
  {
    auto kinectSettings = ofxAzureKinect::DeviceSettings();
    kinectSettings.updateIr = false;
    kinectSettings.updateColor = true;
    kinectSettings.colorResolution = K4A_COLOR_RESOLUTION_1080P;
    kinectSettings.updateVbo = false;
    kinectDevice.startCameras(kinectSettings);
  }

  // Load shader.
  // auto shaderSettings = ofShaderSettings();
  // shaderSettings.shaderFiles[GL_VERTEX_SHADER] = "shaders/render.vert";
  // shaderSettings.shaderFiles[GL_GEOMETRY_SHADER] = "shaders/render.geom";
  // shaderSettings.shaderFiles[GL_FRAGMENT_SHADER] = "shaders/render.frag";
  // shaderSettings.bindDefaults = true;
  // if (shader.setup(shaderSettings))
  // {
  //   ofLogNotice(__FUNCTION__) << "Success loading shader!";
  // }
  shader.load("shaders/render.vert", "shaders/render.frag", "shaders/render.geom");

  // Setup vbo.
  std::vector<glm::vec3> verts(1);
  vbo.setVertexData(verts.data(), verts.size(), GL_STATIC_DRAW);

  pointSize = 2.0f;
  useColorSpace = false;

  boundShader.allocate(ofGetWidth(), ofGetHeight());
  boundShader.load("shaders/bound.frag");

  fbos.insert({"ofcam", ofFbo()});
  fbos.at("ofcam").allocate(ofGetWidth(), ofGetHeight(), GL_RGB32F_ARB);

  // this must be called before init (otherwise fprintf will tell you so)
  // note that high framerates will only function properly if the usb has enough bandwidth
  // for example, a ps3 eye cam at 60 fps will only function when it has full USB 2.0 bandwidth available
  v4l2Cam.setDesiredFramerate(60);

  // use this to set appropriate device and capture method
  v4l2Cam.initGrabber("/dev/video2", IO_METHOD_MMAP, camWidth, camHeight);

  // some initial settings
  int set_gain = 2.0;
  bool set_autogain = true;

  // rudimentary settings implementation: each settings needs a seperate call to the settings method
  v4l2Cam.settings(ofxV4L2_AUTOGAIN, set_autogain);
  v4l2Cam.settings(ofxV4L2_GAIN, set_gain);

  // we use a texture because the ofxV4L2 class has no draw method (yet)
  // we use GL_LUMINANCE because the ofxV4L2 class supports only grayscale (for now)
  v4l2Tex.allocate(camWidth, camHeight, GL_LUMINANCE);
}

//--------------------------------------------------------------
void ofApp::exit()
{
  kinectDevice.close();
}

//--------------------------------------------------------------
void ofApp::update()
{
  v4l2Cam.grabFrame();
  if (v4l2Cam.isNewFrame())
  {
    // v4l2Tex.loadData(v4l2Cam.getPixels(), camWidth, camHeight, GL_RED);
    ofPixels p;
    p.allocate(camWidth, camHeight, OF_PIXELS_RGB);
    for (int i = 0; i < camHeight * camWidth; i++)
      p.setColor(i, v4l2Cam.getPixels()[i]);
    v4l2Tex.allocate(p);
    // v4l2Tex.setRGToRGBASwizzles(true);
    // ofLogError() << v4l2Cam.getPixels();
  }
}

//--------------------------------------------------------------
void ofApp::draw()
{
  ofBackground(0);

  if (kinectDevice.isStreaming())
  {
    fbos.at("ofcam").begin();
    ofClear(0);
    cam.begin();
    ofEnableDepthTest();

    // ofDrawAxis(100.0f);

    ofPushMatrix();
    ofRotateXDeg(180);

    shader.begin();
    shader.setUniform1f("uSpriteSize", pointSize);

    int numPoints;

    if (useColorSpace)
    {
      shader.setUniformTexture("uDepthTex", kinectDevice.getDepthInColorTex(), 1);
      shader.setUniformTexture("uWorldTex", kinectDevice.getColorToWorldTex(), 2);
      shader.setUniformTexture("uColorTex", kinectDevice.getColorTex(), 3);
      shader.setUniformTexture("u_v4l2cam", v4l2Tex, 4);
      shader.setUniform2i("uFrameSize", kinectDevice.getColorTex().getWidth(), kinectDevice.getColorTex().getHeight());
      shader.setUniform2i("uDepthFrameSize", kinectDevice.getDepthInColorTex().getWidth(), kinectDevice.getDepthInColorTex().getHeight());

      numPoints = kinectDevice.getColorTex().getWidth() * kinectDevice.getColorTex().getHeight();
    }
    else
    {
      shader.setUniformTexture("uDepthTex", kinectDevice.getDepthTex(), 1);
      shader.setUniformTexture("uWorldTex", kinectDevice.getDepthToWorldTex(), 2);
      shader.setUniformTexture("uColorTex", kinectDevice.getColorInDepthTex(), 3);
      shader.setUniformTexture("u_v4l2cam", v4l2Tex, 4);
      shader.setUniform2i("uFrameSize", kinectDevice.getDepthTex().getWidth(), kinectDevice.getDepthTex().getHeight());
      shader.setUniform2i("uDepthFrameSize", kinectDevice.getDepthTex().getWidth(), kinectDevice.getDepthTex().getHeight());

      numPoints = kinectDevice.getDepthTex().getWidth() * kinectDevice.getDepthTex().getHeight();
    }

    vbo.drawInstanced(GL_POINTS, 0, 1, numPoints);
    shader.end();
    ofPopMatrix();
    cam.end();
    fbos.at("ofcam").end();

    ofDisableDepthTest();
    auto tex = kinectDevice.getDepthTex();
    boundShader.setUniformTexture("u_depth", tex);
    boundShader.setUniformTexture("u_ofcam", fbos.at("ofcam"));
    boundShader.setUniformTexture("u_v4l2cam", v4l2Tex);
    boundShader.setUniform1i("u_init", 1);
    boundShader.render();
    boundShader.setUniform1i("u_init", 0);
    for (int i = 0; i < 60; i++)
    {
      boundShader.render();
    }
    boundShader.draw(0, 0);
  }
  // v4l2Tex.draw(0, 0);

  ofDrawBitmapStringHighlight(ofToString(ofGetFrameRate(), 2) + " FPS", 10, 20);
}

//--------------------------------------------------------------
void ofApp::keyPressed(int key)
{
  if (key == 'w')
  {
    pointSize *= 2;
  }
  else if (key == 's')
  {
    pointSize /= 2;
  }
  else if (key == ' ')
  {
    useColorSpace ^= 1;
  }
}

//--------------------------------------------------------------
void ofApp::keyReleased(int key)
{
}

//--------------------------------------------------------------
void ofApp::mouseMoved(int x, int y)
{
}

//--------------------------------------------------------------
void ofApp::mouseDragged(int x, int y, int button)
{
}

//--------------------------------------------------------------
void ofApp::mousePressed(int x, int y, int button)
{
}

//--------------------------------------------------------------
void ofApp::mouseReleased(int x, int y, int button)
{
}

//--------------------------------------------------------------
void ofApp::mouseEntered(int x, int y)
{
}

//--------------------------------------------------------------
void ofApp::mouseExited(int x, int y)
{
}

//--------------------------------------------------------------
void ofApp::windowResized(int w, int h)
{
}

//--------------------------------------------------------------
void ofApp::gotMessage(ofMessage msg)
{
}

//--------------------------------------------------------------
void ofApp::dragEvent(ofDragInfo dragInfo)
{
}