#include "Shared/RobotInfo.h"
#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Motion/MotionPtr.h"
#include "Behaviors/StateMachine.h"

$nodeclass DepthDemo : StateNode {
	
	$nodeclass PrintOutColor : VisualRoutinesStateNode : doStart {
		NEW_SKETCH(camFrame, yuv, sketchFromYUV());

		cout << "Created array" << endl;

		Sketch<usint> depthSketch(camSkS, "depthSketch");
		depthSketch->setColorMap(jetMapScaled);
		usint* depthpixels = depthSketch->getRawPixels();

		for (int y = 0; y < camFrame.height; ++y) {
			for (int x = 0; x < camFrame.width; ++x) {
				yuv pixel = camFrame(x, y);

//				cout << "(y,u,v): " << pixel << endl;

				double b = 1.164 * (pixel.y - 16) + 2.018 * (pixel.u - 128);
				double g = 1.164 * (pixel.y - 16) - 0.813 * (pixel.v - 128) - 0.391 * (pixel.u - 128);
				double r = 1.164 * (pixel.y - 16) + 1.596 * (pixel.v - 128);

				float red = min(max(r, 0.0), 255.0);
				float green = min(max(g, 0.0), 255.0);
				float blue = min(max(b, 0.0), 255.0);

				int d1 = (int) (red / 255.0 * 32 + 0.5);
				int d2 = (int) (green / 255.0 * 32 + 0.5);
				int d3 = (int) (blue / 255.0 * 32 + 0.5);

				usint depth = (d3 << 10) | (d2 << 5) | d1;

				*depthpixels++ = depth;

//			cout << "(r,g,b): (" << red << "," << green << "," << blue << ")" << endl;
//			cout << "Depth: " << depth << endl << endl;	
			}
		}

		NEW_SKETCH(depthFrame, usint, depthSketch);
	}

	$nodeclass PrintOutDepth : VisualRoutinesStateNode : doStart {
		NEW_SKETCH(camFrame, usint, sketchFromDepth());
		int midWidth = camFrame.width / 2;
		int midHeight = camFrame.height / 2;
		usint pixel = camFrame(midWidth, midHeight);
		
		cout << "Depth: " << pixel << endl;
	}

  virtual void setup() {
		$statemachine{
			PrintOutColor
    }
  }

}

REGISTER_BEHAVIOR(DepthDemo);