#include "ofApp.h"

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

void ofApp::setupKinect()
{
  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);
  }
}

void ofApp::setupThermal()
{
  // 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::setup()
{
  // ofDisableArbTex();
  // ofSetVerticalSync(false);
  //ofSetLogLevel(OF_LOG_VERBOSE);

  setupKinect();
  setupThermal();

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

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

  gradient.load("gradient.png");

  // 1,000,000 particles
  unsigned w = 512;
  unsigned h = 512;

  particles.init(w, h, OF_PRIMITIVE_POINTS, false, 3);

  particles.loadShaders("shaders/particles/update", "shaders/particles/draw");

  float *particlePosns = new float[w * h * 4];
  for (unsigned y = 0; y < h; ++y)
  {
    for (unsigned x = 0; x < w; ++x)
    {
      unsigned idx = y * w + x;
      particlePosns[idx * 4] = 400.f * x / (float)w - 200.f;     // particle x
      particlePosns[idx * 4 + 1] = 400.f * y / (float)h - 200.f; // particle y
      particlePosns[idx * 4 + 2] = 0.f;                          // particle z
      particlePosns[idx * 4 + 3] = 0.f;                          // dummy
    }
  }
  particles.loadDataTexture(ofxGpuParticles::POSITION, particlePosns);
  delete[] particlePosns;

  // initial velocities
  particles.zeroDataTexture(ofxGpuParticles::VELOCITY);
  particles.zeroDataTexture(ofxGpuParticles::MISC);

  particles.whateverImages.insert({"u_depth", kinectDevice.getDepthTex()});
  particles.whateverImages.insert({"u_world", kinectDevice.getDepthToWorldTex()});
  particles.whateverImages.insert({"u_v4l2cam", v4l2Tex});
  particles.whateverImages.insert({"imageTexture", gradient.getTexture()});

  // listen for update event to set additonal update uniforms
  ofAddListener(particles.updateEvent, this, &ofApp::onParticlesUpdate);
  ofAddListener(particles.drawEvent, this, &ofApp::onParticlesDraw);
}

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

void ofApp::updateThermal()
{
  v4l2Cam.grabFrame();
  if (v4l2Cam.isNewFrame())
  {
    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);
  }
}

void ofApp::update()
{
  updateThermal();

  particles.update();
}

void ofApp::onParticlesUpdate(ofxShader &shader)
{
  ofVec3f mouse(ofGetMouseX() - .5f * ofGetWidth(), .5f * ofGetHeight() - ofGetMouseY(), 0.f);
  shader.setUniform3fv("mouse", mouse.getPtr());
  shader.setUniform1f("elapsed", ofGetLastFrameTime());
  shader.setUniform1f("radiusSquared", 200.f * 200.f);

  shader.setUniform2i("uFrameSize", kinectDevice.getDepthTex().getWidth(), kinectDevice.getDepthTex().getHeight());
  shader.setUniform2i("uDepthFrameSize", kinectDevice.getDepthTex().getWidth(), kinectDevice.getDepthTex().getHeight());
}

void ofApp::onParticlesDraw(ofxShader &shader)
{
}

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

void ofApp::drawDebug()
{
  ofDrawBitmapStringHighlight(ofToString(ofGetFrameRate(), 2) + " FPS", 10, 20);
}

void ofApp::draw()
{
  ofBackground(0);
  if (kinectDevice.isStreaming())
  {
    particles.whateverImages.at("u_depth") = kinectDevice.getDepthTex();
    particles.whateverImages.at("u_world") = kinectDevice.getDepthToWorldTex();
    particles.whateverImages.at("u_v4l2cam") = v4l2Tex;

    cam.begin();
    particles.draw();
    cam.end();
  }

  drawDebug();
}

void ofApp::keyPressed(int key)
{
}

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)
{
}