#include "ofApp.h"

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

void ofApp::setupKinect()
{
  ofLogNotice(__FUNCTION__) << "Found " << ofxAzureKinect::Device::getInstalledCount() << " installed devices.";

  auto kinectSettings = ofxAzureKinect::DeviceSettings();
  kinectSettings.updateIr = false;
  kinectSettings.updateColor = true;
  kinectSettings.colorResolution = K4A_COLOR_RESOLUTION_1080P;
  kinectSettings.updateVbo = false;

  auto deviceSettings = ofxAzureKinect::DeviceSettings();
  deviceSettings.syncImages = false;
  deviceSettings.depthMode = K4A_DEPTH_MODE_NFOV_UNBINNED;
  deviceSettings.updateIr = false;
  deviceSettings.updateColor = false;
  //deviceSettings.colorResolution = K4A_COLOR_RESOLUTION_1080P;
  deviceSettings.updateWorld = true;
  deviceSettings.updateVbo = false;
  auto bodyTrackingSettings = ofxAzureKinect::BodyTrackingSettings();
  //bodyTrackingSettings.processingMode = K4ABT_TRACKER_PROCESSING_MODE_CPU;
  bodyTrackingSettings.updateBodies = true;
  if (kinectDevice.open())
  {
    kinectDevice.startCameras(kinectSettings, bodyTrackingSettings);
  }
}

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_RGB);

  v4l2Pixels.allocate(camWidth, camHeight, OF_PIXELS_RGB);
}

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, 4);

  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);

  // initial velocities
  particles.zeroDataTexture(ofxGpuParticles::VELOCITY);
  for (unsigned y = 0; y < h; ++y)
  {
    for (unsigned x = 0; x < w; ++x)
    {
      unsigned idx = y * w + x;
      particlePosns[idx * 4] = 0;               //
      particlePosns[idx * 4 + 1] = ofRandomf(); // age
      particlePosns[idx * 4 + 2] = 0.f;         //
      particlePosns[idx * 4 + 3] = 0.f;         //
    }
  }
  particles.loadDataTexture(ofxGpuParticles::MISC, particlePosns);
  particles.zeroDataTexture(3);

  delete[] particlePosns;

  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())
  {
    auto &body = kinectDevice.getBodyIndexPix();
    hotspots.clear();
    for (int i = 0; i < kinectDevice.getNumBodies(); i++)
    {
      hotspots.push_back(std::vector<ofVec3f>());
    }
    int count = 0;
    for (int i = 0; i < camHeight; i++)
    {
      for (int j = 0; j < camWidth; j++)
      {
        int a = v4l2Cam.getPixels()[count / 3];
        v4l2Pixels.setColor(count++, a);
        v4l2Pixels.setColor(count++, a);
        v4l2Pixels.setColor(count++, a);
        if (i % 4 == 0 && j % 4 == 0 && i > 80)
        {
          auto c = body.getColor(j, i);
          if (c.r < hotspots.size())
          {
            hotspots.at(c.r).push_back(ofVec3f(j, i, a));
          }
        }
      }
    }
    v4l2Tex.allocate(v4l2Pixels);
    struct
    {
      bool operator()(ofVec3f a, ofVec3f b) const
      {
        return a.z > b.z;
      }
    } compareZThermal;
    for (int i = 0; i < hotspots.size(); i++)
    {
      std::sort(hotspots.at(i).begin(), hotspots.at(i).end(), compareZThermal);
    }
  }
}

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());

  if (hotspots.size() > 0 && hotspots.at(0).size() > 0)
  {
    shader.setUniform3f("uHottest0", hotspots.at(0).at(0));
  }
  if (hotspots.size() > 1 && hotspots.at(1).size() > 0)
  {
    shader.setUniform3f("uHottest1", hotspots.at(1).at(0));
  }
  else if (hotspots.size() > 0 && hotspots.at(0).size() > 0) {
    shader.setUniform3f("uHottest1", hotspots.at(0).at(1));

  }
}

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();
  kinectDevice.getBodyIndexTex().draw(0, 0, 360, 360);
}

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