package com.haxademic.core.hardware.depthcamera.cameras;

import org.intel.rs.device.Device;

import com.haxademic.core.app.P;
import com.haxademic.core.draw.context.PG;
import com.haxademic.core.draw.image.ImageUtil;
import com.haxademic.core.hardware.depthcamera.DepthCameraSize;

import ch.bildspur.realsense.RealSenseCamera;
import ch.bildspur.realsense.type.ColorScheme;
import processing.core.PApplet;
import processing.core.PGraphics;
import processing.core.PImage;

public class RealSenseWrapper 
implements IDepthCamera {
	
	protected RealSenseCamera camera;
	public static int CAMERA_W = 1280;
	public static int CAMERA_H = 720;
	protected boolean DEPTH_ACTIVE = true;
	protected boolean RGB_ACTIVE = true;
	protected boolean MIRROR = true;
	protected boolean threaded = true;
	protected Boolean threadBusy = false;
	protected boolean hasUpdated = false;
	protected PGraphics mirrorRGB;
	protected PGraphics mirrorDepth;
	protected short[][] data;
	public static float METERS_FAR_THRESH = 15;
	public static ColorScheme COLOR_SCHEME = ColorScheme.Cold;

	public RealSenseWrapper(PApplet p, boolean initRGB, boolean initDepthImage) {
		this(p, initRGB, initDepthImage, CAMERA_W, CAMERA_H, null);
	}
	
	public RealSenseWrapper(PApplet p, boolean initRGB, boolean initDepthImage, String serialNumber) {
		this(p, initRGB, initDepthImage, CAMERA_W, CAMERA_H, serialNumber);
	}
	
	public RealSenseWrapper(PApplet p, boolean initRGB, boolean initDepthImage, int width, int height) {
		this(p, initRGB, initDepthImage, width, height, null);
	}
	
	public RealSenseWrapper(PApplet p, boolean initRGB, boolean initDepthImage, int width, int height, String serialNumber) {
		RGB_ACTIVE = initRGB;
		DEPTH_ACTIVE = initDepthImage;
		CAMERA_W = width;
		CAMERA_H = height;

		data = new short[CAMERA_H][CAMERA_W];
		DepthCameraSize.setSize(CAMERA_W, CAMERA_H);
		
		camera = new RealSenseCamera(p);
		camera.enableColorStream(CAMERA_W, CAMERA_H);
		camera.enableDepthStream(CAMERA_W, CAMERA_H);
		if(COLOR_SCHEME != null) camera.enableColorizer(COLOR_SCHEME);
//		camera.enableIRStream(CAMERA_W, CAMERA_H, 30);
		camera.enableAlign();
		camera.addThresholdFilter(0.0f, METERS_FAR_THRESH);
//		camera.addSpatialFilter(1, 0.75f, 50, 1);
//		camera.addDecimationFilter(2);
//		camera.addDisparityTransform(true);
//		camera.addHoleFillingFilter(HoleFillingType.FarestFromAround);
//		camera.addTemporalFilter(0.5f, 30, PersistencyIndex.ValidIn1_Last2);
		if(serialNumber == null) {
			camera.start();
		} else {
			camera.start(serialNumber);
		}
		
		mirrorRGB = PG.newPG(CAMERA_W, CAMERA_H);
		mirrorDepth = PG.newPG(CAMERA_W, CAMERA_H);
	}
	
	public static void listConnectedCameras() {
		P.out("getDeviceCount", RealSenseCamera.getDeviceCount());
		Device devices[] = RealSenseCamera.getDevices();
		for (int i = 0; i < devices.length; i++) {
			Device device = devices[i];
			P.out("Device["+i+"] SerialNumber:", device.getSerialNumber());
		}
	}
	
//	public void setNearFar(float near, float far) {
//		METERS_NEAR_QUEUED = near;
//		METERS_FAR_QUEUED = far;
//	}
	
	public RealSenseCamera camera() {
		return camera;
	}
	
	public void stop() {
	}
	
	///////////////////////////
	// THREADED UPDATING
	///////////////////////////

	public void update() {
		if(threaded) {
			if(threadBusy == false) {
				new Thread(new Runnable() { public void run() {
					threadBusy = true;
					try {
						camera.readFrames();
					} catch (NullPointerException e) {
						P.out("RealSenseWrapper failed to update");
					}
					if(DEPTH_ACTIVE) data = camera.getDepthData();
					threadBusy = false;
					hasUpdated = true;
				}}).start();
			}
		} else {
			camera.readFrames();
			hasUpdated = true;
		}
		
		// copy image to buffers
		// make sure camera has updated once before reading images & data
		if(hasUpdated) {
			if(DEPTH_ACTIVE) {
				if(MIRROR) ImageUtil.copyImageFlipH(camera.getDepthImage(), mirrorDepth);
			}
			if(RGB_ACTIVE) {
				if(MIRROR) ImageUtil.copyImageFlipH(camera.getColorImage(), mirrorRGB);
			}
		}
	}
	
	///////////////////////////
	// GETTERS
	///////////////////////////
	
	public PImage getDepthImage() {
		return (MIRROR) ? mirrorDepth : camera.getDepthImage();
	}
	
	public PImage getIRImage() {
		return null;
	}
	
	public PImage getRgbImage() {
		return (MIRROR) ? mirrorRGB : camera.getColorImage();
	}
	
	public int rgbWidth() {return CAMERA_W;};
	public int rgbHeight() {return CAMERA_H;};
	
	public int[] getDepthData() {
		return null;
	}
	
	public boolean isActive() {
		return camera.isRunning();
	}
	
	public void setMirror( boolean mirrored ) {
		MIRROR = mirrored;
	}
	
	public boolean isMirrored() {
		return MIRROR;
	}
	
	public int getDepthAt( int x, int y ) {
		return (MIRROR) ? data[y][CAMERA_W - 1 - x] : data[y][x];
//		return Math.round(camera.getDistance((MIRROR) ? CAMERA_W - x : x, y) * 1000f);
	}
}

@@ -1055,16 +1055,96 @@
 me.Cold;
+%0A%09%0A%09public static void setSmallStream() %7B%0A%09%09CAMERA_W = 640;%0A%09%09CAMERA_H = 480;%0A%09%7D
 %0A%0A%09publi

Add RealSenseWrapper.setSmallStream() helper