public class Preview extends ViewGroup implements SurfaceHolder.Callback, PreviewCallback  {
    private final String TAG = "Camera::Preview";

    SurfaceView mSurfaceView;
    SurfaceHolder mHolder;
    Size mPreviewSize;
    List<Size> mSupportedPreviewSizes;
    Camera mCamera = null; 
    
    ReentrantLock curPreviewImageLock = new ReentrantLock();
	
	Mat curPreviewImage = null;
	
	boolean savePreviewToFile = false, singlePreviewToFile = false;
	long startSavePreviewTime = 0, singlePreviewCounter = 0 ;
	int previewWidth = 640, previewHeight = 480;
	int poseId = 0;


    public Preview(Context context, SurfaceView sv) {
        super(context);

        mSurfaceView = sv;
       
        mHolder = mSurfaceView.getHolder();
        mHolder.addCallback(this);
        mHolder.setType(SurfaceHolder.SURFACE_TYPE_PUSH_BUFFERS);
    }
    
    // Just test
    public Preview(SurfaceView sv, int previewWidthVal, int previewHeightVal) {
        super(sv.getContext());

        mSurfaceView = sv;
       
        mHolder = mSurfaceView.getHolder();
        mHolder.addCallback(this);
        mHolder.setType(SurfaceHolder.SURFACE_TYPE_PUSH_BUFFERS);
        
        previewWidth = previewWidthVal;
        previewHeight = previewHeightVal;
        
    }
    
    @Override
    public boolean onTouchEvent(MotionEvent event) {
    	Log.d(TAG, "Tap detected");
    	return true;
    }

    public void setCamera(Camera camera) {
//    	Log.d(TAG, String.format("Preview::setCamera"));
    	mCamera = camera;
    	if (mCamera != null) {
//        	Log.d(TAG, String.format("Preview::setCamera, mCamera != null"));
    		mSupportedPreviewSizes = mCamera.getParameters().getSupportedPreviewSizes();
    		requestLayout();

    		// get Camera parameters
    		Camera.Parameters params = mCamera.getParameters();

    		//List<String> focusModes = params.getSupportedFocusModes();
    		/*if (focusModes.contains(Camera.Parameters.FOCUS_MODE_AUTO)) {
    			// set the focus mode
    			params.setFocusMode(Camera.Parameters.FOCUS_MODE_AUTO);
    			// set Camera parameters
    			mCamera.setParameters(params);
    		}*/
//    		params.setPictureSize(640, 480);
    		params.setFocusMode(Camera.Parameters.FOCUS_MODE_INFINITY);
//    		params.setFocusMode(Camera.Parameters.FOCUS_MODE_EDOF);
    		
    		//mCamera.setDisplayOrientation(90);
    		mCamera.setParameters(params);
    	}
    }

    @Override
    protected void onMeasure(int widthMeasureSpec, int heightMeasureSpec) {
//    	Log.d(TAG, "onMeasure");
        // We purposely disregard child measurements because act as a
        // wrapper to a SurfaceView that centers the camera preview instead
        // of stretching it.
        final int width = resolveSize(getSuggestedMinimumWidth(), widthMeasureSpec);
        final int height = resolveSize(getSuggestedMinimumHeight(), heightMeasureSpec);
        setMeasuredDimension(width, height);

        if (mSupportedPreviewSizes != null) {
            mPreviewSize = getOptimalPreviewSize(mSupportedPreviewSizes, width, height);
            
            for (Size s : mSupportedPreviewSizes) {
            	Log.d(TAG, String.format("previewSize possibility = (%d, %d)", s.width, s.height));
            }
            
        	Log.d(TAG, String.format("mPreviewSize = (%d, %d)", mPreviewSize.width, mPreviewSize.height));
        }
    }

    @Override
    protected void onLayout(boolean changed, int l, int t, int r, int b) {
        if (changed && getChildCount() > 0) {
            final View child = getChildAt(0);

            final int width = r - l;
            final int height = b - t;

            int previewWidth = width;
            int previewHeight = height;
            if (mPreviewSize != null) {
                previewWidth = mPreviewSize.width;
                previewHeight = mPreviewSize.height;
            }

            // Center the child SurfaceView within the parent.
            if (width * previewHeight > height * previewWidth) {
                final int scaledChildWidth = previewWidth * height / previewHeight;
                child.layout((width - scaledChildWidth) / 2, 0,
                        (width + scaledChildWidth) / 2, height);
            } else {
                final int scaledChildHeight = previewHeight * width / previewWidth;
                child.layout(0, (height - scaledChildHeight) / 2,
                        width, (height + scaledChildHeight) / 2);
            }
        }
    }

    public void surfaceCreated(SurfaceHolder holder) {
        // The Surface has been created, acquire the camera and tell it where
        // to draw.
//    	Log.d(TAG, "surfaceCreated");
        try {
            if (mCamera != null) {
//            	Log.d(TAG, "mCamera != null");
            	
//        		Camera.Parameters parameters = mCamera.getParameters();
//        		parameters.setPreviewSize(306, 163);
            	
                mCamera.setPreviewDisplay(holder);
                mCamera.startPreview();
            }
        } catch (IOException exception) {
            Log.e(TAG, "IOException caused by setPreviewDisplay()", exception);
        }
    }

    public void surfaceDestroyed(SurfaceHolder holder) {
        // Surface will be destroyed when we return, so stop the preview.
        if (mCamera != null) {
            mCamera.stopPreview();
        }
    }


    public void startSavingPreviewToFile() {
    	startSavePreviewTime = System.currentTimeMillis();
    	savePreviewToFile = true;
    }
    
    public void stopSavingPreviewToFile() {
    	savePreviewToFile = false;
    }
    
    public boolean getSavingPreviewState() {
    	return savePreviewToFile;
    }
    
    public void saveSinglePreviewToFile(Integer id) {
    	Log.d(TAG, "saveSinglePreviewToFile " + id);
    	if ( poseId == -1 || poseId != id) {
    		poseId = id;
    		singlePreviewCounter = 0;
    	}
    	singlePreviewToFile = true;
    }
    
    private Size getOptimalPreviewSize(List<Size> sizes, int w, int h) {
        final double ASPECT_TOLERANCE = 0.3;
        double targetRatio = (double) w / h;
        if (sizes == null) return null;

        Size optimalSize = null;
        double minDiff = Double.MAX_VALUE;

        int targetHeight = h;

        // Try to find an size match aspect ratio and size
        for (Size size : sizes) {
            double ratio = (double) size.width / size.height;
            if (Math.abs(ratio - targetRatio) > ASPECT_TOLERANCE) continue;
            if (Math.abs(size.height - targetHeight) < minDiff) {
                optimalSize = size;
                minDiff = Math.abs(size.height - targetHeight);
            }
        }

        // Cannot find the one match the aspect ratio, ignore the requirement
        if (optimalSize == null) {
            minDiff = Double.MAX_VALUE;
            for (Size size : sizes) {
                if (Math.abs(size.height - targetHeight) < minDiff) {
                    optimalSize = size;
                    minDiff = Math.abs(size.height - targetHeight);
                }
            }
        }
        return optimalSize;
    }
    

    public void surfaceChanged(SurfaceHolder holder, int format, int w, int h) {
    	Log.d(TAG, "surfaceChanged");
    	if(mCamera != null) {
//        	Log.d(TAG, "mCamera != null");
        	
        	//need to stop - otherwise setParameters will fail when changing previewSize
            mCamera.stopPreview();
            
    		Camera.Parameters parameters = mCamera.getParameters();
    		//parameters.setPreviewSize(previewWidth, previewHeight);
    		parameters.setPreviewSize(previewWidth, previewHeight);
    		
    //		parameters.setPreviewSize(mPreviewSize.width, mPreviewSize.height);
//    		parameters.setPreviewSize(mPreviewSize.width, mPreviewSize.height);
    		requestLayout();

    		mCamera.setParameters(parameters);
    		mCamera.setPreviewCallback(this);
    		mCamera.startPreview();
    		
    	}
    }
    

    
    @Override
	public void onPreviewFrame(byte[] data, Camera camera) {
    	Log.d(TAG, "onPreviewFrame");
		// TODO Auto-generated method stub
    	Camera.Parameters parameters = mCamera.getParameters();
    	
		int imageFormat = parameters.getPreviewFormat(); 		   
		if (imageFormat == ImageFormat.NV21)
		{
//			Log.d(TAG, "imageFormat == ImageFormat.NV21");
//			Log.d(TAG, String.format("data.length = %d", data.length));
    	   
			Camera.Size prevSize = parameters.getPreviewSize();
			
//			Log.d(TAG, String.format("preview size = (%d, %d)", prevSize.width, prevSize.height));
			
			Mat imageBGRA = new Mat();
			Mat imageYUV = new Mat(prevSize.height + prevSize.height / 2, prevSize.width, CvType.CV_8UC1);
			imageYUV.put(0,  0, data);
			
			Imgproc.cvtColor(imageYUV, imageBGRA, Imgproc.COLOR_YUV420sp2BGR, 4);
    	   
			curPreviewImageLock.lock();
			
			try {

				curPreviewImage = imageBGRA;
				if ( savePreviewToFile )
				{
					Log.d(TAG, "if ( savePreviewToFile )");
					saveImageFromPreview(curPreviewImage, 0, System.currentTimeMillis() - startSavePreviewTime);
					singlePreviewToFile = false;
				}
				if ( singlePreviewToFile ) {
					Log.d(TAG, "if ( singlePreviewToFile ) " + poseId + " " + singlePreviewCounter);
					saveImageFromPreview(curPreviewImage, poseId, singlePreviewCounter);
					singlePreviewToFile = false;
					singlePreviewCounter++;
				}
				
				
			} finally {
			//						Log.d(TAG, "onPreviewFrame finally");
				curPreviewImageLock.unlock();
			}
    	   
       }

	}
    
    private void saveImageFromPreview(Mat image, int id, long value) {
    	Log.d(TAG, "saveImageFromPreview");
    	File folder = new File(Environment.getExternalStorageDirectory()
    			+ "/OpenAIL/rawData/Imgs");
		if (!folder.exists()) {
			folder.mkdirs();
		}

		String file = String.format(folder.getAbsolutePath() + "/%04d_%08d.png", id,
				value);
		Highgui.imwrite(file, image);
    
		//savePreviewToFile = false;
    }
    
    
    public Mat  getCurPreviewImage() {
		Mat ret = null;
		
		curPreviewImageLock.lock();
		
		try {
			if(curPreviewImage != null){
				ret = curPreviewImage.clone();
			}
		} finally {
//			Log.d(TAG, "getCurPreviewImage finally");
			curPreviewImageLock.unlock();
		}
		
		return ret;
    }
	

} public class QRCodeDecoderClass {
    private final String TAG = "QRCode";
    
    private Context mContext;
    
    private List<Pair<Integer,String>> recognizedMessages = null;
    
    private final Semaphore recognizedMessagesMtx = new Semaphore(1, true);
    
    
    // Creating camera matrix
    private Mat cameraMatrix640, cameraMatrix1920;

	// Creaing distCoefss
	private MatOfDouble distCoeffs640, distCoeffs1920;
    
    public QRCodeDecoderClass (Context context) {
    	mContext = context;
    	recognizedMessages = new LinkedList<Pair<Integer,String>>();        
    	
    }
	
	public void decode(Integer positionId, Mat image) {
		Log.d(TAG, "creating async task");
		new DecodeAsyncTask(positionId, mContext).execute(image);
	}
	
	public List<Pair<Integer,Point3>> getRecognizedQRCodes() {
		
		List<Pair<Integer,Point3>> returnList = new LinkedList<Pair<Integer,Point3>>();
		
		try {
			
			
			
			recognizedMessagesMtx.acquire();
			for (Pair<Integer, String> pair : recognizedMessages) {
				
				// Preparations to read positions
				Scanner positionScanner = null;
				positionScanner = new Scanner(pair.second);
				positionScanner.useLocale(Locale.US);
			
				
				positionScanner.next("Position");
				double X = positionScanner.nextDouble();
				double Y = positionScanner.nextDouble();
				double Z = positionScanner.nextDouble();
				
				Point3 pos = new Point3(X, Y, Z);
				returnList.add(new Pair<Integer,Point3>(pair.first, pos));
			}
			recognizedMessages.clear();
			
			recognizedMessagesMtx.release();
			
		} catch (Exception e) {
			e.printStackTrace();
		}
		
		
		
		return returnList;
		
	}
	
	private class DecodeAsyncTask extends AsyncTask<Mat, String, String> {
		
		Integer positionId = 0;
		
		Context context;

        /**
         * @param context
         */
        private DecodeAsyncTask(Integer _positionId, Context _context) {
        	context = _context;
        	positionId = _positionId;
        }
        
        @Override
		protected String doInBackground(Mat... image) {
        	  	
        	cameraMatrix640 = Mat.zeros(3, 3, CvType.CV_64F);
        	cameraMatrix640.put(0, 0, 573.61280);
        	cameraMatrix640.put(0, 2, 320.74271);
        	cameraMatrix640.put(1, 1, 575.86251);
        	cameraMatrix640.put(1, 2, 241.84638);
        	cameraMatrix640.put(2, 2, 1.0);
        	distCoeffs640 = new MatOfDouble(0.08371, -0.20442, 0.00016, -0.00077, 0.0000);
        	
        	cameraMatrix1920 = Mat.zeros(3, 3, CvType.CV_64F);
        	cameraMatrix1920.put(0, 0, 1728.16411);
        	cameraMatrix1920.put(0, 2, 962.62688);
        	cameraMatrix1920.put(1, 1, 1715.60377);
        	cameraMatrix1920.put(1, 2, 546.66063);
        	cameraMatrix1920.put(2, 2, 1.0);
        	distCoeffs1920 = new MatOfDouble(0.11443, -0.28006, 0.00070, -0.00019, 0.00000);
        	
//        	return decodeQRImage(image[0], cameraMatrix1920, distCoeffs1920);
        	
			File mainDir = new File(Environment.getExternalStorageDirectory()
					+ "/OpenAIL/QR/");
//        	File mainDir = new File(Environment.getExternalStorageDirectory()
//					+ "/OpenAIL/SPA_tests/6");

			FileOutputStream locStream = null;
			try {
				locStream = new FileOutputStream(
						mainDir.toString() + "/localization.log");
			} catch (FileNotFoundException e) {
				e.printStackTrace();
			}
			PrintStream localizationStream = new PrintStream(locStream);

			File[] dirs = mainDir.listFiles();
			for (File dir : dirs) {
				if (dir.isDirectory()) {

					File[] files = dir.listFiles();
					for (File file : files) {
						Log.d(TAG, "Possible file: " + file.getAbsolutePath());
						
//						String [] s = file.getName().split("_");
//						int val = Integer.parseInt(s[0]);
//						Log.d(TAG, "Pose number: " + val);
						int val = 0;
						
						Mat img = Highgui.imread(file.getAbsolutePath());
						
						String posOrient = "";
						if ( dir.getName().contains("640"))
							posOrient = decodeQRImage(img, cameraMatrix640, distCoeffs640);
						else
							posOrient = decodeQRImage(img, cameraMatrix1920, distCoeffs1920);
							
						
						

						publishProgress(dir.getName() + "/" + file.getName()
								+ " " + posOrient);

						localizationStream.println(dir.getName() + "/"
								+ file.getName() + " " + posOrient);
//						localizationStream.println("EDGE_SE2:STEP " + val + " X "
//								+ posOrient + " 1.0 0.0 1.0");
					}

				}
			}
			localizationStream.close();

			// return decodeQRImage(image[0]);
			return "FINISHED!";
		}
        
        protected void onProgressUpdate(String... progress) {
            Toast.makeText(context, progress[0], Toast.LENGTH_LONG).show();
        }
        

        @Override
        protected void onPostExecute(String result) {
        	Log.d(TAG, "onPostExecute : " + result);
        	Toast.makeText(context, result, Toast.LENGTH_LONG).show();
        	
        	if ( result != "Decode failed!") {
				try {
					recognizedMessagesMtx.acquire();
					
					recognizedMessages.add(new Pair<Integer,String>(positionId, result));
		        	
					recognizedMessagesMtx.release();
				} catch (InterruptedException e) {
					e.printStackTrace();
				}
        	}

        }
        
        private String decodeQRImage(Mat image, Mat cameraMatrix, MatOfDouble distCoeffs) {
        	Log.d(TAG, "decodeQRImage in async");
        	
        	Mat undist = new Mat();
        	Imgproc.undistort(image, undist, cameraMatrix, distCoeffs);
        	image = undist;
        	
        	
        	
        	// Creating bitmap from OpenCV::Mat
        	Bitmap bMap = Bitmap.createBitmap(image.width(), image.height(), Bitmap.Config.ARGB_8888);
        	Utils.matToBitmap(image, bMap);
    		
    		// Getting the int Array from bitmap
    		int[] intArray = new int[bMap.getWidth()*bMap.getHeight()]; 
    		bMap.getPixels(intArray, 0, bMap.getWidth(), 0, 0, bMap.getWidth(), bMap.getHeight());  

    		// Creating binary bitmap for QR code detection
    		LuminanceSource source = new RGBLuminanceSource(bMap.getWidth(), bMap.getHeight(),intArray);
    		BinaryBitmap bitmap = new BinaryBitmap(new HybridBinarizer(source));
    		
    		// QRreader - decoding the qr code
    		Result result = null;
    		try {
    			QRCodeReader reader = new QRCodeReader();  
    			result = reader.decode(bitmap);
    		}
    		catch (Exception e) {
    			return "Not detected";
//    			return "Decode failed!";
    		}
    		
    		
    		//return result.getText();
    		
    		

    		// Extracting the points detected on QR code
    		Point [] imgP = new Point[4];
    		int i =0;
    		ResultPoint[] points = result.getResultPoints();
    		for (ResultPoint p : points) {
				Log.d(TAG, "QR code point: " + p.getX() + " " + p.getY());

				imgP[i] = new Point();
				imgP[i].x = p.getX();
				imgP[i].y = p.getY();

				i++;
    		}
    		Log.d(TAG, "image type: " + image.type() ); 
    		
    		Mat grayImage = new Mat();
        	Imgproc.cvtColor(image, grayImage, Imgproc.COLOR_RGB2GRAY);
        	
        	image = new Mat();
        	Imgproc.adaptiveThreshold(grayImage, image, 255, Imgproc.ADAPTIVE_THRESH_MEAN_C, Imgproc.THRESH_BINARY, 15, 10);
    		
    		
    		// TESTING white region detector
    		BitMatrix bitImg = new BitMatrix(image.width(), image.height());
    		for ( i=0;i<image.width();i++)
    		{
    			for (int j=0;j<image.height();j++)
    			{
    				//Log.d(TAG, "TEST: " + image.get(j, i)[0]); 
    				
//    				int value = bMap.getPixel(i, j);
//    				int R = (value >> 16) & 0xff ;    
//    				int G = (value >> 8) & 0xff ; 
//    				int B = value & 0xff ; 
    				
//					if (0.299 * R + 0.587 * G + 0.114 * B > 150)
    				//Log.d(TAG, "TEST: " + image.get(j, i)[0]); 
    				if(image.get(j, i)[0] < 128)
						bitImg.set(i, j);
//					else
//						bitImg.unset(i, j);
    				
    			}
    		}
    		  		
    		WhiteRectangleDetector wrd;
			try {
				wrd = new WhiteRectangleDetector(bitImg, (int)(imgP[0].y - imgP[1].y), (int)((imgP[0].x + imgP[2].x)/2), (int)((imgP[0].y + imgP[2].y)/2));
				ResultPoint [] rps = wrd.detect();
				 i =0;
	    		for (ResultPoint p : rps) {
	    			Log.d(TAG, "WRD point: " + p.getX() + " " + p.getY() ); 
	    			Point cvPoint = new Point(p.getX(), p.getY());
            		Core.circle(image, cvPoint, 20, new Scalar(0, 0, 255));
            		
            		imgP[i] = new Point();
        			imgP[i].x = p.getX();
        			imgP[i].y = p.getY();
        			i++;
	    		}
			} catch (Exception e) {
				Log.d(TAG, "WRD failed: " + e.getMessage() ); 
				return "WRD failed";
			}
    		
			
			// Saving the image with marked points
			File folder = new File(Environment.getExternalStorageDirectory()
					+ "/OpenAIL/rawData/Imgs");
    		Highgui.imwrite(folder.getAbsolutePath() + "/testDet.png", image);
    		
    		
    		
			// Creating image points
    		MatOfPoint2f imagePoints = new MatOfPoint2f(imgP[0], imgP[1], imgP[2], imgP[3]);
    		Log.d(TAG, "Created image points " + imagePoints.rows() + " " + imagePoints.cols() ); 
    		
    		
    		
    		// 3D points -> inside QR CODE
//    		Point3 bottomLeft = new Point3(0, 64, 0);
//    		Point3 topLeft = new Point3(0, 0, 0);
//    		Point3 topRight = new Point3(64, 0, 0);
//    		Point3 middle = new Point3(64/2.0, 64/2.0, 0);
//    		Point3 bottomRight = new Point3(53.0, 53.0, 0);
    		
    		// 3D points -> outside QR CODE
    		Point3 topLeft = new Point3(0, 0, 0);
    		Point3 bottomLeft = new Point3(0, 0.180, 0);
    		Point3 topRight = new Point3(0.180, 0, 0);
    		Point3 bottomRight = new Point3(0.180, 0.180, 0);
    		
    		
    		Log.d(TAG, "PreProjected points: " + topLeft.x + " " + topLeft.y); 
    		Log.d(TAG, "PreProjected points: " + bottomLeft.x + " " + bottomLeft.y); 
    		Log.d(TAG, "PreProjected points: " + topRight.x + " " + topRight.y); 
    		Log.d(TAG, "PreProjected points: " + bottomRight.x + " " + bottomRight.y); 
    		
    		MatOfPoint3f objectPoints = new MatOfPoint3f(topLeft, bottomLeft, topRight, bottomRight);
    		
    		Log.d(TAG, "Created object points " + objectPoints.rows() + " " + objectPoints.cols()); 
    		
	    	
    		
    		
    		// Place to save results
    		Mat rvec = new Mat(3, 1, CvType.CV_64F), tvec = new Mat(3, 1, CvType.CV_64F);
   
    		// Calling solvePNP
    		try {
    			Calib3d.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, false, Calib3d.CV_ITERATIVE);
    			
    			Log.d(TAG, "tvec: " + tvec.cols() + " " + tvec.rows()); 
    			Log.d(TAG, "tvec: " + tvec.get(0, 0)[0]); 
    			Log.d(TAG, "tvec: " + tvec.get(1, 0)[0]); 
    			Log.d(TAG, "tvec: " + tvec.get(2, 0)[0]); 
    			
    			MatOfPoint2f projectedPoints = new MatOfPoint2f();
    			Calib3d.projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, projectedPoints);
    			
    			for (int i1=0;i1<projectedPoints.rows();i1++) {
    				Log.d(TAG, "Projected points: " + projectedPoints.get(i1, 0)[0] + " " + projectedPoints.get(i1, 0)[1]); 
    			}
    		}
    		catch (Exception e) {
    			Log.d(TAG, "PNP failed: " + e.getMessage() ); 
    			return "PNP failed";
    		}
    		
    		Mat rotationMatrix = new Mat();
    		Calib3d.Rodrigues(rvec, rotationMatrix);
    		
    		double m00 = rotationMatrix.get(0, 0)[0];
    		double m21 = rotationMatrix.get(2, 1)[0];
    		double m12 = rotationMatrix.get(1, 2)[0];
    		double m02 = rotationMatrix.get(0, 2)[0];
    		double m20 = rotationMatrix.get(2, 0)[0];
    		double m01 = rotationMatrix.get(0, 1)[0];
    		double m10 = rotationMatrix.get(1, 0)[0];
    		double m11 = rotationMatrix.get(1, 1)[0];
    		double m22 = rotationMatrix.get(2, 2)[0];
    		
    		double tr = m00 + m11 + m22;
    		double qw = 0, qx = 0, qy = 0, qz = 0;
			if (tr > 0) {
				double S = Math.sqrt(tr + 1.0) * 2; // S=4*qw
				qw = 0.25 * S;
				qx = (m21 - m12) / S;
				qy = (m02 - m20) / S;
				qz = (m10 - m01) / S;
			} else if ((m00 > m11) & (m00 > m22)) {
				double S =  Math.sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx
				qw = (m21 - m12) / S;
				qx = 0.25 * S;
				qy = (m01 + m10) / S;
				qz = (m02 + m20) / S;
			} else if (m11 > m22) {
				double S =  Math.sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy
				qw = (m02 - m20) / S;
				qx = (m01 + m10) / S;
				qy = 0.25 * S;
				qz = (m12 + m21) / S;
			} else {
				double S =  Math.sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz
				qw = (m10 - m01) / S;
				qx = (m02 + m20) / S;
				qy = (m12 + m21) / S;
				qz = 0.25 * S;
			}
    		
    		
    		Log.d(TAG, "SolvePNP success! XYZ: " + tvec.get(0, 0)[0] + " " + tvec.get(1, 0)[0] + " " + tvec.get(2, 0)[0] ); 
    		Log.d(TAG, "SolvePNP success! qx qy qz qw: " + qx + " " + qy + " " + qz + " " + qw ); 
    		
    		String posOrient = tvec.get(0, 0)[0] + " " + tvec.get(1, 0)[0] + " " + tvec.get(2, 0)[0] + " " + qx + " " + qy + " " + qz + " " + qw;
    		
    		
    		rotationMatrix = rotationMatrix.t();  // rotation of inverse
//    		tvec.put(1, 0, - tvec.get(1, 0)[0]);
//    		tvec.put(2, 0, - tvec.get(2, 0)[0]);
//    		tvec.put(3, 0, - tvec.get(3, 0)[0]);
    		//tvec = rotationMatrix * tvec; // translation of inverse
    		Mat tvec2 = new Mat();
    		Core.gemm(rotationMatrix, tvec, -1, new Mat(), 0, tvec2, 0);
    		Log.d(TAG, "Inverse! tvec2: " + tvec2.get(0, 0)[0] + " " + tvec2.get(1, 0)[0] + " " + tvec2.get(2, 0)[0] ); 
    		
    		double distanceToCamera = Math.sqrt(tvec2.get(0, 0)[0]*tvec2.get(0, 0)[0] + tvec2.get(2, 0)[0]*tvec2.get(2, 0)[0]);
    		Log.d(TAG, "Inverse! DIST: " + Math.sqrt(tvec2.get(0, 0)[0]*tvec2.get(0, 0)[0] + tvec2.get(2, 0)[0]*tvec2.get(2, 0)[0]) + " m" ); 
    		
    		// camera Z axis in XZ plane of QR
    		Mat Z = rotationMatrix.col(2);
    		Z.put(1, 0, 0);
    		// Z axis of QR
    		Mat Zqr = Mat.zeros(3, 1, CvType.CV_64F);
    		Zqr.put(2, 0, 1.0);
    		
    		
    		double dotProduct = Zqr.dot(Z);
    		Mat crossProduct = Zqr.cross(Z); 
    		double angleToCamera = Math.signum(crossProduct.get(1, 0)[0]) *Math.acos(dotProduct);
    		Log.d(TAG, "Inverse! ANGLE: " + Math.toDegrees(angleToCamera) + " deg"); 
    		
    		return distanceToCamera + " " + angleToCamera ;//+ "\r\n" + posOrient;
    		
    		//return result.toString();
    	}

      
    
	}
	
	

} public class VisualCompass implements Runnable  {
	final String moduleLogName = "VisualCompass";
	String TAG = "FastABLE";
	
	static {
		System.loadLibrary("VisualOdometryModule");
	}
	
	public native void NDKEstimateRotation(long matAddrImg1, long matAddrImg2);

	public native float[] NDKTestFASTABLE(String configFile,
			String[] trainImgsArray, int[] trainLengths,
			String[] testImgsArray, int testLength);
	public native void WTF();
	
	Thread visualCompassThread;
	boolean visualCompassThreadRun = false;
	
	public void startThread() {
		visualCompassThread = new Thread(this, "Visual compass");
		visualCompassThreadRun = true;
		visualCompassThread.start();
	}
	
	public void stopThread() {
		try {
			visualCompassThreadRun = false;
			visualCompassThread.join();
		} catch (InterruptedException e) {
			Log.e(moduleLogName, "Failed to join Visual Compass thread");
		}
	}
	
	public boolean isThreadRunning() {
		return visualCompassThreadRun;
	}
	
	
	@Override
	public void run() {
		System.loadLibrary("VisualOdometryModule");
		testFastABLE();
		//testRun();
		Log.d(moduleLogName, "Finished test run!");
		
		while ( visualCompassThreadRun ) {
			
			try {
				Log.d(moduleLogName, "Visual Compass thread -- idle ...");
				Thread.sleep(5000);
			} catch (InterruptedException e) {
				Log.e(moduleLogName, "Failed to sleep Visual Compass thread");
			}
			
		}
		Log.d(moduleLogName, "Finished Visual Compass thread");
	}
	
	private void testRun() {
		File root = Environment.getExternalStorageDirectory();
		File file1 = new File(root, "OpenAIL/VisualCompass/testImage1.png");
		File file2 = new File(root, "OpenAIL/VisualCompass/testImage2.png");
		
		// OpenCV 3.0.0: Mat img = Imgcodecs.imread(file.getAbsolutePath()), dst = new Mat();
		Mat img1 = Highgui.imread(file1.getAbsolutePath());
		Mat img2 = Highgui.imread(file2.getAbsolutePath());
		
		Log.d(moduleLogName, "Loaded images! Calling NDK");
		
		NDKEstimateRotation(img1.getNativeObjAddr(), img2.getNativeObjAddr());

	}
	
	private void testFastABLE() {
		// Config file
        String configFile = String.format(Locale.getDefault(), Environment
                .getExternalStorageDirectory().toString()
                + "/FastABLE"
                + "/config.txt");

        // Train data
        String[] trainImagesFiles;
        String[] testImagesFiles;

        // Lengths of segments
        int[] trainLengths;
        {
            String trainPath = String.format(Locale.getDefault(), Environment
                    .getExternalStorageDirectory().toString()
                    + "/FastABLE"
                    + "/train/");

            // Get segment dirs inside training dir
            File trainDir = new File(trainPath);
            File trainDirsArr[] = trainDir.listFiles();
            Log.d(TAG, "Train segments: " + trainDirsArr.length);

            // Read segment dirs and put into ArrayList
            ArrayList<String> trainImgsList = new ArrayList<String>();
            ArrayList<Integer> trainLengthsList = new ArrayList<Integer>();
            for (int d = 0; d < trainDirsArr.length; d++) {
                if (trainDirsArr[d].isDirectory()) {
                    File curTrainFiles[] = trainDirsArr[d].listFiles();
                    Log.d(TAG, "Train dir:" + trainDirsArr[d].getName());
                    Log.d(TAG, "Train size:" + curTrainFiles.length);
                    trainLengthsList.add(curTrainFiles.length);
                    for (int i = 0; i < curTrainFiles.length; ++i) {
                        trainImgsList.add(curTrainFiles[i].getAbsoluteFile().toString());
                    }
                }
            }
            // Move data to arrays
            trainLengths = new int[trainLengthsList.size()];
            for (int i = 0; i < trainLengthsList.size(); ++i) {
                trainLengths[i] = trainLengthsList.get(i);
            }
            trainImagesFiles = trainImgsList.toArray(new String[trainImgsList.size()]);
        }

        // Test data

        // Length of test data
        int testLength = 0;
        {
            String testPath = String.format(Locale.getDefault(), Environment
                    .getExternalStorageDirectory().toString()
                    + "/FastABLE"
                    + "/test/");

            File testDir = new File(testPath);
            File testFiles[] = testDir.listFiles();
            Log.d(TAG, "Test size: " + testFiles.length);

            testLength = testFiles.length;
            testImagesFiles = new String[testFiles.length];
            for (int i = 0; i < testFiles.length; ++i) {
                testImagesFiles[i] = testFiles[i].getAbsoluteFile().toString();
            }
        }

        // Run NDK function to compute results
        float[] res = NDKTestFASTABLE(configFile,
                                trainImagesFiles,
                                trainLengths,
                                testImagesFiles,
                                testLength);

        Log.d(TAG, "RESULT: oa = " + res[0] + "\t fa = " + res[1]);
	}

}
 public class VisualPlaceRecognition implements Runnable {
	
	private static final String moduleLogName = "VisualPlaceRecognitionJava";
	
	static {
		try {
		System.loadLibrary("VisualPlaceRecognitionModule");
		} catch (UnsatisfiedLinkError e)
		{
			Log.d(moduleLogName, "VPR Native code library failed to load.\n" + e);
		}
	}
	
	// Native methods
	private native long createAndLoadFabmapNDK(String settingsPath);
	private native long createAndTrainFabmapNDK(String settingsPath, int trainingSetSize);
	private native void addTestSetFabmapNDK(long addrFabmapEnv, int testSetSize);
	private native int testLocationFabmapNDK(long addrFabmapEnv, long addrImageToTest, boolean addToTest);
	private native void destroyFabmapNDK(long addrFabmap);
	
	/// Java code	
	
	// Fabmap object address
	long addrFabMapEnv;
	
	// Training images
	Mat[] trainImages;
	
	// Test images
	Mat[] testImages;
	
	// Processing thread
	Thread vprThread;
	boolean performVPR;
	
	// Preview to get current image
	Preview preview;
	
	// Add images to test
	ReentrantLock placesToAddLock = new ReentrantLock();
	ReentrantLock matchedListLock = new ReentrantLock();
	
	List<Mat> placesToAdd;
	List<Integer> fabmapIDs;
	
	List<org.dg.openAIL.IdPair<Integer,Integer>> vprMatchedList;
	
	int saveImageNextNum = 0;
	
	public VisualPlaceRecognition() {
		
		//Initialize FabMapEnv pointer to NULL
		addrFabMapEnv = 0;
	}

	public void start(Preview _preview) {
		
		placesToAdd = new ArrayList<Mat>();
		fabmapIDs = new ArrayList<Integer>();
		vprMatchedList = new ArrayList<org.dg.openAIL.IdPair<Integer, Integer>>();
		
		preview = _preview;
		performVPR = true;
		vprThread = new Thread(this, "VPR thread");
		vprThread.start();
	}
	
	public void stop() {
		performVPR = false;
		try {
			vprThread.join();
		} catch (InterruptedException e) {
			Log.d(moduleLogName, "Failed to join recognizePlacesThread");
		}
	}

	// TODO: Just for test
	public void callAndVerifyAllMethods() {
		trainVisualPlaceRecognition();
		
		{
			try{
				String resPath = String.format(Locale.getDefault(), Environment
						.getExternalStorageDirectory().toString()
						+ "/OpenAIL"
						+ "/VPR/res.log");
				PrintStream resStream = new PrintStream(new FileOutputStream(resPath));
				
				String fabmapTestRecPath = String.format(Locale.getDefault(), Environment
						.getExternalStorageDirectory().toString()
						+ "/OpenAIL"
						+ "/VPR/testRec/");
				
				File fTestRec = new File(fabmapTestRecPath);        
				File fileTestRec[] = fTestRec.listFiles();
				Log.d(moduleLogName, "Test rec size: " + fileTestRec.length);
				
				for (int i=0; i < fileTestRec.length; i++)
				{
				    Log.d(moduleLogName, "Test rec filename:" + fileTestRec[i].getName());
				    Mat testRecImage = Highgui.imread(fabmapTestRecPath + fileTestRec[i].getName(), 0);
				    
				    int match = recognizePlace(testRecImage);
				    
				    resStream.print(String.format("%d %d\n", i + 1, match + 1));
				}
				
				resStream.close();
			}
			catch(Exception e){
				e.printStackTrace();
			}
		}
		
		destroyFabmapNDK(addrFabMapEnv);
	}
	
	// Method used to run the training of FABMAP
	public void trainVisualPlaceRecognition() {
		Log.d(moduleLogName, "Called trainVisualPlaceRecognition()");
		
		if(addrFabMapEnv != 0){
			destroyFabmapNDK(addrFabMapEnv);
			addrFabMapEnv = 0;
		}
		
		// Read training images
		int trainingSetSize;
		{
			String fabmapTrainPath = String.format(Locale.getDefault(), Environment
					.getExternalStorageDirectory().toString()
					+ "/OpenAIL"
					+ "/VPR/train/");
			
			File fTrain = new File(fabmapTrainPath);        
			File fileTrain[] = fTrain.listFiles();
			Log.d(moduleLogName, "Train size: " + fileTrain.length);
			trainImages = new Mat[fileTrain.length];
			
			for (int i=0; i < fileTrain.length; i++)
			{
			    Log.d(moduleLogName, "Train filename:" + fileTrain[i].getName());
			    trainImages[i] = Highgui.imread(fabmapTrainPath + fileTrain[i].getName(), 0);
			}
			trainingSetSize = fileTrain.length;
		}

		Log.d(moduleLogName, "training FabMap");
		// Fabmap settings path
		{
			createAndLoadFabmap();
		}
		
		// Read testimages
		int testSetSize;

		String fabmapTestPath = String.format(Locale.getDefault(), Environment
				.getExternalStorageDirectory().toString()
				+ "/OpenAIL"
				+ "/VPR/test/");

		File fTest = new File(fabmapTestPath);
		File fileTest[] = fTest.listFiles();
		Log.d(moduleLogName, "Test size: " + fileTest.length);
		testImages = new Mat[fileTest.length];

		for (int i = 0; i < fileTest.length; i++) {
			Log.d(moduleLogName, "Test filename:" + fileTest[i].getName());
			testImages[i] = Highgui.imread(
					fabmapTestPath + fileTest[i].getName(), 0);
		}
		testSetSize = fileTest.length;

		Log.d(moduleLogName, "adding test images");
		addTestSetFabmapNDK(addrFabMapEnv, testSetSize);
	}
	/**
	 * 
	 */
	private void createAndLoadFabmap() {
		String fabmapSettingsPath = String.format(Locale.getDefault(), Environment
		.getExternalStorageDirectory().toString()
		+ "/OpenAIL"
		+ "/VPR/settings.yml");
		
		// Call training of fabmap library
		//addrFabMapEnv = createAndTrainFabmapNDK(fabmapSettingsPath, trainingSetSize);
		addrFabMapEnv = createAndLoadFabmapNDK(fabmapSettingsPath);
	}
	
	// Method used to try to match new image from camera to existing dataset of images
	public int recognizePlace(Mat imageToTest) {
		int match = testLocationFabmapNDK(addrFabMapEnv, imageToTest.getNativeObjAddr(), false);

		Log.d(moduleLogName, "Recognized place: " + match);
		
		return match;
	}
	
	public void savePlace(double x, double y, double z, Mat image){
		
		savePlace(x, y, z, image, saveImageNextNum);
				
		saveImageNextNum++;
	}
	
	public void savePlace(double x, double y, double z, Mat image, int id){
		
		String path = String.format(Locale.getDefault(), Environment
				.getExternalStorageDirectory().toString()
				+ "/OpenAIL"
				+ "/rawData"
				+ "/images/");
		
		String imagePath = path + String.format("img%04d.png", id);
		
		Log.d(moduleLogName, "Saving image to: " + imagePath);
		Log.d(moduleLogName, String.format("image size = (%d, %d)", image.cols(), image.rows()));
		
		Highgui.imwrite(imagePath, image);
		
		
	}
	
	
	public void addPlace(int id, Mat image) {
		placesToAddLock.lock();
		fabmapIDs.add(id);
		placesToAdd.add(image);
		placesToAddLock.unlock();
	}
	
	@Override
	public void run() {
		
		// Load train database (train has to been perform before)
		createAndLoadFabmap();
		
		// Load test images
		
		while(performVPR) {
			// Get new image
			Mat img = preview.getCurPreviewImage();
			
			// Try to recognize that image
			int fabmapID = recognizePlace(img);
			
			// Add to a list of possible matches
			if (fabmapID > 0) {
				matchedListLock.lock();
				// TODO: Jakie jest current ID?
				
				matchedListLock.unlock();
			}			
			
			// Add images to test (if any)
			placesToAddLock.lock();
			while(placesToAdd.size() != 0)
			{
				// Add place to recognition in NDK
				// ....
				
				placesToAdd.remove(0);
			}
			placesToAddLock.unlock();
		}
		
	}

	
	public List<org.dg.openAIL.IdPair<Integer, Integer>> getAndClearVPRMatchedList() {
		
		try {
			matchedListLock.lock();
			List<org.dg.openAIL.IdPair<Integer, Integer>> returnList = new ArrayList<org.dg.openAIL.IdPair<Integer, Integer>>(vprMatchedList.size());
			for(org.dg.openAIL.IdPair<Integer, Integer> item: vprMatchedList) 
				returnList.add( new org.dg.openAIL.IdPair<Integer, Integer>(item) );
			vprMatchedList.clear();
			
			matchedListLock.unlock();
			return returnList;
		} catch (Exception e) {
			Log.d(moduleLogName, "Failed to acquire recognizedList Mtx in getRecognizedPlaces");
		}
		return new ArrayList<org.dg.openAIL.IdPair<Integer, Integer>>();
		
		
		
		
	}
} public class GraphManager {
	// Name for logger
	static final String moduleLogName = "GraphManager.java";
	
	// Loading NDK graph module
	static {
		System.loadLibrary("GraphOptimizationModule");
	}
	
	/* Calls to the native part of the code
	 * - long NDKGraphCreate() 
	 * 		-> Creates graph 
	 * - void NDKGraphAddVertexEdge(long addrGraph, String g2oStream) 
	 * 		-> adds vertex/add to graph
	 * - double[] NDKGraphGetVertexPosition(long addrGraph, int id) 
	 * 		-> gets position of single vertex
	 * - double[] NDKGraphGetPositionOfAllVertices(long addrGraph) 
	 * 		-> gets positions of all vertices
	 * - double NDKGraphOptimize(long addrGraph, int iterationCount, String path) 
	 * 		-> optimizes graph for set of iterations and saves file, return chi2 (if <0 then error)
	 * - void NDKGraphDestroy(long addrGraph) 
	 * 		-> cleaning 
	 */
	public native long NDKGraphCreate();
	public native void NDKGraphAddVertexEdge(long addrGraph, String g2oStream);
	public native double[] NDKGraphGetVertexPosition(long addrGraph, int id);
	public native double[] NDKGraphGetPositionOfAllVertices(long addrGraph);
	public native double NDKGraphOptimize(long addrGraph, int iterationCount, String path);
	public native void NDKGraphDestroy(long addrGraph);
	
	// Address in memory of graph structure
	long addrGraph = 0;
	
	// Id of the last user vertex
	int currentPoseId = 0;
	
	// Is graph connected to map
	boolean mapConnected = false;
	
	// The starting ids of qr edges
	int qrCodeId = 20000;
	
	// Start/Stop information
	boolean optimizationInProgress = false;
	
	// If there is new information about current estimates
	public boolean changeInOptimizedData = false;
	
	// Thread used in optimization 
	Thread optimizationThread;
	
	// Parameters
	org.dg.openAIL.ConfigurationReader.Parameters.GraphManager parameters;
	
	// File to save current graph
	PrintStream graphStream = null;
	
	// Current estimate
	static final Object currentEstimateMtx = new Object();
	List<Vertex> currentEstimate = new ArrayList<Vertex>();
	
	// Timestamps
	long timestampStart = 0;
	List<Long> timestamps = new ArrayList<Long>();
	
	///
	/// Methods -- creation/start/destruction
	///
	
	/*
	 * Creates graph class and loads parameters
	 */
	public GraphManager(org.dg.openAIL.ConfigurationReader.Parameters.GraphManager _parameters) {
		parameters = _parameters;
	}
	
	/*
	 * Cleaning
	 */
	public void destroyGraph() {
		if (addrGraph != 0)
			NDKGraphDestroy(addrGraph);
		addrGraph = 0;
	}
	
	/*
	 * Prepares graph for processing - creates graph in NDK and start optimization thread
	 */
	public void start() {
		Log.d(moduleLogName, "start()");
		
		timestamps.clear();
		currentEstimate.clear();
		
		openCreatedGraphStream();
		
		mapConnected = false;
		startOptimizeOnlineThread();
	}
	/**
	 * 
	 */
	public void openCreatedGraphStream() {
		try {
			File folder = new File(Environment.getExternalStorageDirectory()
					+ "/OpenAIL");

			if (!folder.exists()) {
				folder.mkdir();
			}

			File dir = new File(String.format(
					Environment.getExternalStorageDirectory() + "/OpenAIL/GraphLog/"));
			if (!dir.exists()) {
				dir.mkdirs();
			}

			String fileName = dir.toString() + "/lastCreatedGraph.g2o";
			FileOutputStream fgraphStream;
			fgraphStream = new FileOutputStream(fileName);
			graphStream = new PrintStream(fgraphStream);
			
		} catch (FileNotFoundException e) {
			graphStream = null;
			e.printStackTrace();
		}
	}
	
	/*
	 * Can be used to check if processing has been started
	 */
	public boolean isOptimizationInProgress() {
		return optimizationInProgress;
	}
	
	/*
	 * We stop the optimization thread
	 */
	public void stopOptimizationThread() {
			
		optimizationInProgress = false;
		try {
			if ( optimizationThread != null )
				optimizationThread.join();
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
		getVerticesEstimates();
		
		saveEstimates2file();
		
		destroyGraph();
		
		if (graphStream != null)
		{
			graphStream.close();
			graphStream = null;
		}
		
	}
	
	/*
	 * Optimizes graph stored in a file provided as a parameter 
	 */
	public void optimizeGraphInFile(String fileName) {
		Log.d(moduleLogName, "Creating graph");
		addrGraph = NDKGraphCreate();

		Log.d(moduleLogName, "Calling test from file");
		String path2 = Environment.getExternalStorageDirectory().getAbsolutePath() + "/OpenAIL/GraphLog/";
		String filePath = path2 + fileName;
		
		
		String fileContent = "";
		try {
			File fl = new File(filePath);
			FileInputStream fin = new FileInputStream(fl);
		
		    BufferedReader reader = new BufferedReader(new InputStreamReader(fin));
		    StringBuilder sb = new StringBuilder();
		    String line = null;
		    while ((line = reader.readLine()) != null) {
		      sb.append(line).append("\n");
		    }
		    reader.close();
		    fileContent = sb.toString();
		} catch (Exception e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		Log.d(moduleLogName, "File test : " + fileContent);
	   
		NDKGraphAddVertexEdge(addrGraph, fileContent);
		
		optimizeLoadedGraph(parameters.optimizeFromFileIterationCount);
		
		destroyGraph();
	}
	
	public void optimizeGraph() {
		optimizeLoadedGraph(parameters.optimizeFromFileIterationCount);
		
		destroyGraph();	
	}
	
	public void setStartTime() {
		timestampStart = System.nanoTime();
		timestamps.add(Long.valueOf(0));
	}
	
	///
	/// Methods -- adding positions and measurements
	/// 
	
	/* 
	 * adds VertexSE2 with position (X,Y,angle), e.g. for map points
	 */
	public void addVertexSE2(int id, double X, double Y, double angle) {
		Log.d(moduleLogName, "addVertexSE2("+id + ", " + X + ", " + Y + ", " + angle + ")");
		
		checkGraphExistance();
		
		String g2oString = "VERTEX_SE2 " + id + " " + X + " " + Y + " " + angle +"\n";
		saveGraph2file(g2oString);
		NDKGraphAddVertexEdge(addrGraph, g2oString);
	}
	
	/* 
	 * adds VertexXYZ with position (X,Y,Z), e.g. for map points
	 */
	public void addVertexXYZ(int id, double X, double Y, double Z) {
		Log.d(moduleLogName, "addVertexXYZ("+id + ", " + X + ", " + Y + ", " + Z + ")");
		
		checkGraphExistance();
		
		String g2oString = "VERTEX_XYZ " + id + " " + X + " " + Y + " " + Z +"\n";
		saveGraph2file(g2oString);
		NDKGraphAddVertexEdge(addrGraph, g2oString);
	}
	
	/* 
	 * adds VertexXY with position (X,Y), e.g. for map points
	 */
	public void addVertexXY(int id, double X, double Y) {
		Log.d(moduleLogName, "addVertexXY("+id + ", " + X + ", " + Y + ")");
		
		checkGraphExistance();
		
		String g2oString = "VERTEX_XY " + id + " " + X + " " + Y +"\n";
		saveGraph2file(g2oString);
		NDKGraphAddVertexEdge(addrGraph, g2oString);
	}
	
	
	
	public void createNewPose() {
		currentPoseId++;
		timestamps.add(System.nanoTime() - timestampStart);
	}
	
	/*
	 * adds the stepometer measurement to the graph
	 */
	public void addStepometerMeasurement(double distance, double theta) {
		checkGraphExistance();
		
		
		// The information value of angle depends on angle
		float infValueTheta = (float) (Math.PI/2 - theta);
		if ( infValueTheta  < 0.1f )
			infValueTheta = 0.1f;
		
		String informatiomMatrixOfStep = "1.0 0.0 " + infValueTheta;
		String edgeStep ="EDGE_SE2:STEP " + (currentPoseId-1) + " " + currentPoseId + " " + distance + " " + theta + " " + informatiomMatrixOfStep + "\n";
		
		saveGraph2file(edgeStep);
		NDKGraphAddVertexEdge(addrGraph, edgeStep);
	}
	
	/*
	 * adds the orientation measurement to the graph
	 */
	public void addOrientationMeasurement(int id1, int id2, double theta) {
		checkGraphExistance();
		
		
		// The information value of angle depends on angle
		float infValueTheta = (float) (Math.PI/2 - theta);
		if ( infValueTheta  < 0.1f )
			infValueTheta = 0.1f;
		
		String edgeOrientation ="EDGE_SE2:ORIENT " + id1 + " " + id2 + " " + " " + theta + " 1.0 \n";
		
		saveGraph2file(edgeOrientation);
		NDKGraphAddVertexEdge(addrGraph, edgeOrientation);
	}
	
	/*
	 * adds WiFi fingerprint matches to the graph
	 */
	public void addMultipleWiFiFingerprints(List<org.dg.openAIL.IdPair<Integer, Integer>> foundWiFiFingerprintMatches) {
		checkGraphExistance();
		Log.d(moduleLogName, "addMultipleWiFiFingerprints - mapConnected");
		mapConnected = true;
		
		String g2oString = "";
		for (org.dg.openAIL.IdPair<Integer, Integer> placeIds : foundWiFiFingerprintMatches)
		{
			String edgeWiFiFingerprint = createWiFiFingerprintEdgeString(placeIds.getFirst(), placeIds.getSecond());
			g2oString = g2oString + edgeWiFiFingerprint;
		}
		saveGraph2file(g2oString);
		
		NDKGraphAddVertexEdge(addrGraph, g2oString);
	}
	
	/*
	 * adds VPR matches to the graph
	 */
	public void addMultipleVPRMatches(List<org.dg.openAIL.IdPair<Integer, Integer>> foundVPRMatches) {
		checkGraphExistance();
		Log.d(moduleLogName, "addMultipleVPRMatches - mapConnected");
		mapConnected = true;
		
		String g2oString = "";
		for (org.dg.openAIL.IdPair<Integer, Integer> placeIds : foundVPRMatches)
		{
			String edgeWiFiFingerprint = createVPRVicinityEdgeString(placeIds.getFirst(), placeIds.getSecond());
			g2oString = g2oString + edgeWiFiFingerprint;
		}
		saveGraph2file(g2oString);
		
		NDKGraphAddVertexEdge(addrGraph, g2oString);
	}
	
	/*
	 * adds QR matches to the graph
	 */
	public void addMultipleQRCodes(List<Pair<Integer, Point3>> listOfPositions) {
		checkGraphExistance();
		Log.d(moduleLogName, "addMultipleQRCodes - mapConnected");
		mapConnected = true;
		
		String g2oString = "";
		for (Pair<Integer, Point3> measurement: listOfPositions)
		{
			addVertexSE2(qrCodeId, measurement.second.x, measurement.second.y, measurement.second.z);
			String edgeQR = createQREdgeString(qrCodeId, measurement.first);
			
			g2oString = g2oString + edgeQR;
			qrCodeId++;
		}
		
		Log.d(moduleLogName, g2oString);
		
		saveGraph2file(g2oString);
		NDKGraphAddVertexEdge(addrGraph, g2oString);
	}
	
	/*
	 * adds WiFi measurements to the graph
	 */
	public void addMultipleWiFiMeasurements(List<DirectMeasurement> measurements) {
		checkGraphExistance();
		
		String g2oString = "";
		for (DirectMeasurement measurement: measurements)
		{
			//String edgeWiFi = createWiFi_SE2_XY_EdgeString(measurement.idAP, measurement.idPos, measurement.distance);
			String edgeWiFi = createWiFi_SE2_XYZ_EdgeString(measurement.idPos, measurement.idAP, measurement.distance, measurement.frequency);
			
			g2oString = g2oString + edgeWiFi;
		}
		
		
		saveGraph2file(g2oString);
		NDKGraphAddVertexEdge(addrGraph, g2oString);
	}
		
	///
	/// Methods -- getting ids and estiamtes
	///
	
	/*
	 * getting the id of the current user position
	 */
	public int getCurrentPoseId()
	{
		return currentPoseId;
	}
	
	/*
	 * returns the current estimates of vertices
	 */
	public List<Vertex> getVerticesEstimates() {
		Log.d(moduleLogName, "getVerticesEstimates()");
		if(changeInOptimizedData) {
			synchronized (currentEstimateMtx) {
				currentEstimate = getVerticesEstimatesFromNDK();
			}
		}
		return currentEstimate;
	}
	
	/*
	 * returns the estimate of the current vertex
	 */
	public Vertex getCurrentPoseEstimate() {
		Log.d(moduleLogName, "getCurrentPoseEstimate()");
		Vertex v = null;
		synchronized (currentEstimateMtx) {
			if (currentEstimate != null && !currentEstimate.isEmpty()) {
				v = currentEstimate.get(currentEstimate.size()-1);
			}
		}
		return v;
	}

	/*
	 * Checks if we can provide user position on a map
	 */
	public boolean isMapConnected() {
		Log.d(moduleLogName, "isMapConnected: " + mapConnected);
		return mapConnected;
	}
	
	///
	/// Methods -- private
	///
	
	/*
	 * Checks and creates graph if id does not exist
	 */
	private void checkGraphExistance() {
		Log.d(moduleLogName, "checkGraphExistance()");
		if ( addrGraph == 0)
			addrGraph = NDKGraphCreate();
	}
	
	/*
	 * Method used to optimize loaded graph in separate thread
	 */
	private void optimizeLoadedGraph(final int iterationCount) {
		checkGraphExistance();
		final String path = Environment.getExternalStorageDirectory()
				.getAbsolutePath() + "/OpenAIL/GraphLog/";
		optimizationInProgress = true;

		optimizationThread = new Thread() {
			public void run() {

				double chi2 = NDKGraphOptimize(addrGraph, iterationCount, path);

				synchronized (currentEstimateMtx) {
					currentEstimate = getVerticesEstimatesFromNDK();
				}

				Log.d(moduleLogName, "Optimization ended");

				destroyGraph();

			};
		};
		optimizationThread.start();

		try {
			Thread.sleep(100, 0);
			optimizationThread.join();
		} catch (InterruptedException e) {
			Log.d(moduleLogName, "optimizationThread.join() - failed");
		}
	}
	
	/*
	 * Method used to start online optimization
	 */
	private void startOptimizeOnlineThread() {
		Log.d(moduleLogName, "startOptimizeOnlineThread()");
		
		// Create graph if needed
		checkGraphExistance();
		
		final String path = Environment.getExternalStorageDirectory().getAbsolutePath() + "/OpenAIL/GraphLog/";
		
		// New thread
		optimizationThread = new Thread() {
		public void run() {
			final double chi2Threshold = 1.e-30;
			
			optimizationInProgress = true;
			
			int timesWithoutMarkingChange = 0;
			
			// While we did not get the stop condition
			while ( optimizationInProgress )
			{
				// Perform one iteration
				double chi2 = NDKGraphOptimize(addrGraph, 1, path);		
	
				Log.d(moduleLogName, "OptimizationOnline: iteration ended with chi2=" + chi2);
				
				// Should we sleep as the graph is probably empty
				if (chi2 < chi2Threshold && timesWithoutMarkingChange < 5)
				{					
					try {
						timesWithoutMarkingChange++;
						Thread.sleep(200);
					} catch (InterruptedException e) {
						e.printStackTrace();
					}
				}
				else
				{
					timesWithoutMarkingChange = 0;
					changeInOptimizedData = true;
				}			
				
			}
			Log.d(moduleLogName, "OptimizationOnline ended");
			} ;
		};
		
		// Start new optimization thread
		optimizationThread.start();
	}
	
	/*
	 * returns the list of estimates from NDK
	 */
	private List<Vertex> getVerticesEstimatesFromNDK() {
		Log.d(moduleLogName, "getVerticesEstimatesFromNDK()");
		
		// The list of all vertices
		List<Vertex> vertices = new ArrayList<Vertex>();
		
		// Getting the estimates
		double [] pos = NDKGraphGetPositionOfAllVertices(addrGraph);
		
		// For all estimates
		for (int i=0;i<pos.length;i+=4)
		{
			// Cast id to int
			Double idDouble = pos[i];
			Vertex vertex = new Vertex(idDouble.intValue(), pos[i+1], pos[i+2], pos[i+3]);
			// Add new vertex
			vertices.add(vertex);
		}
		// Return the list of vertices
		return vertices;
	}	
	
	/*
	 * save string to g2o file
	 */
	private void saveGraph2file(String g2oString)
	{
		if (graphStream!=null)
		{
			graphStream.print(g2oString);
		}
	}
	
	/*
	 * Saves the graph position estimates with timestamps to the file
	 */
	private void saveEstimates2file()
	{
		Log.d(moduleLogName, "saveEstimates2file");
		Log.d(moduleLogName, "currentEstimate.size()=" + currentEstimate.size() + " vs timestamps.size()=" + timestamps.size());
		
		File folder = new File(Environment.getExternalStorageDirectory()
				+ "/OpenAIL");

		if (!folder.exists()) {
			folder.mkdir();
		}

		File dirResult = new File(folder.getAbsolutePath() + "/result");
		if (!dirResult.exists()) {
			dirResult.mkdirs();
		}


		try {
			FileOutputStream faccStream = new FileOutputStream(dirResult
					+ "/positionEstimates.log");
			PrintStream positionEstimatesStream = new PrintStream(faccStream);
			
			for (int i=0, j=0;i<currentEstimate.size() && j<timestamps.size(); i++)
			{
				Vertex v = currentEstimate.get(i);
				if (v.id < 10000) {
					positionEstimatesStream.println(timestamps.get(j) + " " + v.id + " " + v.X + " " + v.Y + " " + v.Z);
					j++;
				}
			}
		
			positionEstimatesStream.close();
			
		} catch (FileNotFoundException e) {
			e.printStackTrace();
		}
		
	}

	/**
	 * Creates a WiFi measurement string from values
	 */
	private String createWiFi_SE2_XY_EdgeString(int id, int id2, double distance) {
		String edgeWiFi ="EDGE_SE2:WIFI " + id + " " + id2 + " " + distance + " " + parameters.informationMatrixOfWiFi + "\n";
		return edgeWiFi;
	}
	
	/**
	 * Creates a WiFi measurement string from values
	 */
	private String createWiFi_SE2_XYZ_EdgeString(int id, int id2, double distance, double freq) {
		String edgeWiFi ="EDGE_SE2:WIFI_SE2_XYZ " + id + " " + id2 + " " + freq + " " + distance + " 1.0 \n";
		return edgeWiFi;
	}
	
	/**
	 * Creates a QR measurement string from values
	 */
	private String createQREdgeString(int qrId, int posId) {
		String edgeWiFi ="EDGE_SE2:QR " + qrId + " " + posId + " 0.0 999999999.0\n";
		return edgeWiFi;
	}
	
	/**
	 * Creates a WiFi Fingerprint string from values
	 */
	private String createWiFiFingerprintEdgeString(int id, int id2) {
		String edgeWiFi ="EDGE_SE2:WIFI_FINGERPRINT " + id2 + " " + id + " " + parameters.wifiFingerprintDeadBandRadius +  " " + parameters.informationMatrixOfWiFiFingerprint + "\n";
		return edgeWiFi;
	}
	
	/**
	 * Creates a VPR measurement string from values
	 */
	private String createVPRVicinityEdgeString(int id, int id2) {
		String edgeWiFi ="EDGE_SE2:VPR_VICINITY " + id2 + " " + id + " " + parameters.vprVicinityDeadBandRadius +  " " + parameters.informationMatrixOfVPRVicinity + "\n";
		return edgeWiFi;
	}
	
} public class Vertex {
	public int id;
	public double X, Y, Z;
	
	public Vertex(int id, double X, double Y, double Z) {
		this.id = id;
		this.X = X;
		this.Y = Y;
		this.Z = Z;
	}

}
 public class WiFiMeasurement {
	public int id;
	public double distance;
	
	public WiFiMeasurement(int _id, double _distance){
		id = _id;
		distance = _distance;
	}
} public class InertialSensors {
	private static final String moduleLogName = "InertialSensors.java";
	
	// Main Android handler
	SensorManager sensorManager;

	// Streams to save data
	private enum ActiveStreamNames {
		ACCELEROMETER, GYROSCOPE, MAGNETOMETER, ACCELEROMETER_WITHOUT_GRAVITY, ORIENTATION_ANDROID, ORIENTATION_ANDROID_EULER, ORIENTATION_AEKF,
		ORIENTATION_AEKF_EULER, ORIENTATION_COMPLEMENTARY, ORIENTATION_COMPLEMENTARY_EULER, PRESSURE;
	}
	boolean save2file = false;
	boolean activeStreams[];
	PrintStream accStream, gyroStream, magStream, accwogStream, orientAndroidStream, orientAndroidEulerStream,
			myOrientAEKFStream, myOrientAEKFEulerStream, myOrientComplementaryStream, myOrientComplementaryEulerStream, pressureStream;

	// Starting timestamp and current timestamp
	long timestampStart = 0, currentTimestamp;
	boolean firstNewData = true;

	// Last estimates
	private final Semaphore orientMtx = new Semaphore(1, true);
	private final Semaphore orientCompMtx = new Semaphore(1, true);
	private final Semaphore orientAndroidMtx = new Semaphore(1, true);
	private final Semaphore yawMtx = new Semaphore(1, true);
	
	private final Semaphore gyroMtx = new Semaphore(1, true);
	private final Semaphore accMtx = new Semaphore(1, true);
	
	int id = 0;
	float acc[], mag[], accwog[], orient[], orientComp[], orientAndroid[], lastAndroidQuat[];

	// isRunning
	boolean isStarted;

	// AEKF
	boolean performOrientationEstimation = true;
	long lastOrientationEstimationTimestamp;
	AHRSModule orientationEstimator = null;
	
	// Complementary filter
	boolean runComplementaryFilter = false;
	ComplementaryFilter complementaryFilterEstimation = null;

	// Stepometer
	private LinkedList<Float> accStepometerWindow, accVarianceWindow, gyroVarianceWindow;
	static int gyroWindowSize = 100;
	boolean stepometerStarted = false;
	int stepometerRunningCounter = 0;
	float lastYawZ = 0.0f;
	boolean firstYawCall = true;
	Stepometer stepometer;
	
	// Testing new stepometer angle
	public enum DeviceOrientation {VERTICAL, HORIZONTAL_LEFT, HORIZONTAL_RIGHT, UNKNOWN};
	private int deviceOrientationPoll [] = new int [4];
	private int deviceOrientationIter = 0;
	private DeviceOrientation deviceOrientation = DeviceOrientation.VERTICAL;
	float stepometerAngle;
	
	
	// Acc variance
	int accVarianceRunningCounter = 0;
	final int accVarianceWindowSize = 100;
	
	// Magnetic recognition
	private final Semaphore magneticWindowMtx = new Semaphore(1, true);
	private List<Float> magneticWindow;
	int magneticRecognitionCounter = 0;
	final int magneticWindowSize = 512;
	MagneticRecognition magneticRecognition;
	
	// Barometer
	float lastBarometerValue = 0;
	BarometerProcessing barometer;
	
	// Parameters
	Parameters.InertialSensors parameters;

	public InertialSensors(SensorManager _sensorManager, Parameters.InertialSensors _parameters) {
		sensorManager = _sensorManager;
		parameters = _parameters;
		
		isStarted = false;
		acc = new float[3];
		mag = new float[3];
		accwog = new float[3];
		orient = new float[3];
		orientComp = new float[3];
		orientAndroid = new float[3];
		lastAndroidQuat = new float[4];
		performOrientationEstimation = true;
		
		accStepometerWindow = new LinkedList<Float>();
		accVarianceWindow = new LinkedList<Float>();
		gyroVarianceWindow = new LinkedList<Float>();
		stepometer = new Stepometer(parameters.stepometer);
		
		magneticWindow = new ArrayList<Float>();
		magneticRecognition = new MagneticRecognition();
		
		barometer = new BarometerProcessing();
		
		// Complementary filter
		complementaryFilterEstimation = new ComplementaryFilter(0.999325f);
		
		/// Choose which filters to save
		activeStreams = new boolean[11];
		
		activateStreamBasedOnParams();	
		
	}

	/**
	 * 
	 */
	private void activateStreamBasedOnParams() {
		// Basic sensors
		activeStreams[ActiveStreamNames.ACCELEROMETER.ordinal()] = parameters.record.accelerometer;
		activeStreams[ActiveStreamNames.GYROSCOPE.ordinal()] = parameters.record.gyroscope;
		activeStreams[ActiveStreamNames.MAGNETOMETER.ordinal()] = parameters.record.magnetometer;
		activeStreams[ActiveStreamNames.ACCELEROMETER_WITHOUT_GRAVITY.ordinal()] = parameters.record.accelerometerWithoutGravity;
		activeStreams[ActiveStreamNames.PRESSURE.ordinal()] = parameters.record.barometer;
		
		// Android estimation
		activeStreams[ActiveStreamNames.ORIENTATION_ANDROID.ordinal()] = parameters.record.orientationAndroid;
		activeStreams[ActiveStreamNames.ORIENTATION_ANDROID_EULER.ordinal()] = parameters.record.orientationAndroidEuler;
		
		// AEKF
		activeStreams[ActiveStreamNames.ORIENTATION_AEKF.ordinal()] = parameters.record.orientationAEKF;
		activeStreams[ActiveStreamNames.ORIENTATION_AEKF_EULER.ordinal()] = parameters.record.orientationAEKFEuler;
		
		// CF
		activeStreams[ActiveStreamNames.ORIENTATION_COMPLEMENTARY.ordinal()] = parameters.record.orientationCF;
		activeStreams[ActiveStreamNames.ORIENTATION_COMPLEMENTARY_EULER.ordinal()] = parameters.record.orientationCFEuler;
	}

	public InertialSensors save2file(boolean _save2file) {
		this.save2file = _save2file;
		return this;
	}

	public InertialSensors performOrientationEstimation(
			boolean _performOrientationEstimation) {
		this.performOrientationEstimation = _performOrientationEstimation;
		return this;
	}

	public InertialSensors id(int _id) {
		this.id = _id;
		return this;
	}

	public boolean getState() {
		return isStarted;
	}

	
	// Stepometer
	public void startStepometer() {
		stepometerStarted = true;

	}

	public void stopStepometer() {
		stepometerStarted = false;
	}
	
	public boolean isStepometerStarted() {
		return stepometerStarted;
	}

	public float getStepometerLastDetectedFrequency() {
		return stepometer.getLastFoundFreq();
	}

	public float getStepometerCoveredStepDistance() {
		return stepometer.getCoveredStepDistance();
	}
	
	public float getStepometerStepDistance() {
		return stepometer.getStepDistance();
	}

	public float getStepometerNumberOfSteps() {
		return stepometer.getDetectedNumberOfSteps();
	}
	
	
	public float getStepometerAngle() {
		return stepometerAngle;
	}
	
	/**
	 * Returns Yaw (Z-axis) in degrees
	 */
	public float getYawForStepometer() {
		try {
			yawMtx.acquire();
			
			float yawZ = 0.0f;
			float firstCallBias = 0.0f;
			if ( parameters.verticalOrientation == true ) {
				yawZ = stepometerAngle;
				firstCallBias = (float) parameters.priorMapStepometerBiasVertical;
			}
			else {
				yawZ = stepometerAngle;
				firstCallBias = (float) parameters.priorMapStepometerBiasHorizontal;
			}
			yawMtx.release();
			
			// In any case we compute the difference in orientations
			float deltaYaw = yawZ - lastYawZ;
			lastYawZ = yawZ;
			
			// In first case, the diff is equal to global orientation corrected by map bias
			if ( firstYawCall) {
				Log.d(moduleLogName, "deltaYaw = " + deltaYaw + " firstCallBias = " + firstCallBias);
				firstYawCall = false;
				return deltaYaw + firstCallBias;
			}
			return deltaYaw;
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		return 0.0f;
	}
	
	public float getGlobalYaw() {
		try {
			yawMtx.acquire();
			float yawZ = orientAndroid[2];
			yawMtx.release();
			return yawZ;
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		return 0.0f;
	}
	
	public float getAccVariance() {
		float val = 0.0f;
		try {
			accMtx.acquire();
			val = varList(accVarianceWindow);
			accMtx.release();
		} catch (Exception e) { }
		
		return val;
	}
	
	public float getGyroVariance() {
		
		float val = 0.0f;
		try {
			gyroMtx.acquire();
			val = varList(gyroVarianceWindow);
			gyroMtx.release();
		} catch (InterruptedException e) {
		}
		
		return val;
	}
		
	
	public int getDeviceOrientation() {
		return deviceOrientation.ordinal();
	}
	
	// Magnetic recognition
	public int recognizePlaceBasedOnMagneticScan()
	{
		// TODO!
		return -1;
		
//		try {
//			magneticWindowMtx.acquire();
//			List<Float> copy = new ArrayList<Float>(magneticWindow.subList(0, 512));
//			magneticWindowMtx.release();
//			
//			return magneticRecognition.recognizePlace(copy);
//		} catch (InterruptedException e) {
//			e.printStackTrace();
//			return -1;
//		}
	}
	
	public void addMagneticRecognitionPlace()
	{
		
		try {
			magneticWindowMtx.acquire();
			List<Float> copy = new ArrayList<Float>(magneticWindow.subList(0, 512));
			magneticWindowMtx.release();
			
			magneticRecognition.addPlace(copy);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		
	}
	
	public int getSizeOfPlaceDatabase()
	{
		return magneticRecognition.getSizeOfPlaceDatabase();
	}
	
	
	// Barometer
	public void startBarometerProcessing() {
		barometer.start(lastBarometerValue);
	}
	
	public void stopBarometerProcessing() {
		barometer.stop();
	}
	
	public boolean isBarometerProcessingStarted() {
		return barometer.isStarted();
	}
	
	public float getEstimatedHeight() {
		return barometer.getEstimateHeight();
	}
	
	public int getCurrentFloor() {
		barometer.setCurrentHeight(lastBarometerValue);
		return barometer.getCurrentFloor();
	}
	
	

	// Start
	public void start() {

		init();
		
		
		
		initPhysicalSensors();

	}
	
	public void startPlayback() {
		init();
	}

	/**
	 * 
	 */
	private void init() {
		lastOrientationEstimationTimestamp = -1;
		
		// Initialize out orientation estimation
		if (performOrientationEstimation)
		{
			orientationEstimator = new AHRSModule();
		}
		
		if (runComplementaryFilter) {
			complementaryFilterEstimation.start();
		}

		isStarted = true;
		firstNewData = true;
	}

	/**
	 * 
	 */
	private void initPhysicalSensors() {
		if (save2file) {
			try {
				openStreamsToFiles(id);
				id++;
			} catch (FileNotFoundException e) {
				save2file = false;
			}
		}

		// Sensor handlers
		Sensor accelerometer = sensorManager
				.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
		Sensor gyro = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
		Sensor magnetic = sensorManager
				.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
		Sensor acc_wo_gravity = sensorManager
				.getDefaultSensor(Sensor.TYPE_LINEAR_ACCELERATION);
		Sensor orientation = sensorManager
				.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);
		Sensor pressureSensor = sensorManager.getDefaultSensor(Sensor.TYPE_PRESSURE);

		// Starting listeners
		sensorManager.registerListener(sensorEventListener, accelerometer,
				SensorManager.SENSOR_DELAY_FASTEST);
		sensorManager.registerListener(sensorEventListener, gyro,
				SensorManager.SENSOR_DELAY_FASTEST);
		sensorManager.registerListener(sensorEventListener, magnetic,
				SensorManager.SENSOR_DELAY_FASTEST);
		sensorManager.registerListener(sensorEventListener, acc_wo_gravity,
				SensorManager.SENSOR_DELAY_FASTEST);
		sensorManager.registerListener(sensorEventListener, orientation,
				SensorManager.SENSOR_DELAY_FASTEST);
		sensorManager.registerListener(sensorEventListener, pressureSensor,
				SensorManager.SENSOR_DELAY_FASTEST);
	}

	// Stop the processing
	public void stop() {
		isStarted = false;
		deviceOrientationIter = 0;
		deviceOrientationPoll = new int[4]; 

		sensorManager.unregisterListener(sensorEventListener);

		if (performOrientationEstimation) {
			orientationEstimator.destroy();
			orientationEstimator = null;
			
			complementaryFilterEstimation.stop();
		}

		if (save2file)
			closeStreams();
	}

	public void setStartTime() {
		timestampStart = System.nanoTime();
	}

	public float[] getCurrentAEKFOrient() {
		float[] orientToReturn = null;
		try {
			orientMtx.acquire();
			orientToReturn = orient.clone();
			orientMtx.release();
		} catch (InterruptedException e) {
			e.printStackTrace();
		}

		return orientToReturn;
	}
	
	public float[] getCurrentComplementaryOrient() {
		float[] orientToReturn = null;
		try {
			orientCompMtx.acquire();
			orientToReturn = orientComp.clone();
			orientCompMtx.release();
		} catch (InterruptedException e) {
			e.printStackTrace();
		}

		return orientToReturn;
	}
	
	public float[] getCurrentAndroidOrient() {
		float[] orientToReturn = null;
		try {
			orientAndroidMtx.acquire();
			orientToReturn = orientAndroid.clone();
			orientAndroidMtx.release();
		} catch (InterruptedException e) {
			e.printStackTrace();
		}

		return orientToReturn;
	}


	public void recordAll(boolean value) {
		save2file = true;
		if(value)
		{
			// Basic sensors
			activeStreams[ActiveStreamNames.ACCELEROMETER.ordinal()] = true;
			activeStreams[ActiveStreamNames.GYROSCOPE.ordinal()] = true;
			activeStreams[ActiveStreamNames.MAGNETOMETER.ordinal()] = true;
			activeStreams[ActiveStreamNames.ACCELEROMETER_WITHOUT_GRAVITY.ordinal()] = false;
			activeStreams[ActiveStreamNames.PRESSURE.ordinal()] = true;
			
			// Android estimation
			activeStreams[ActiveStreamNames.ORIENTATION_ANDROID.ordinal()] = true;
			activeStreams[ActiveStreamNames.ORIENTATION_ANDROID_EULER.ordinal()] = false;
			
			// AEKF
			activeStreams[ActiveStreamNames.ORIENTATION_AEKF.ordinal()] = true;
			activeStreams[ActiveStreamNames.ORIENTATION_AEKF_EULER.ordinal()] = false;
			
			// CF
			activeStreams[ActiveStreamNames.ORIENTATION_COMPLEMENTARY.ordinal()] = false;
			activeStreams[ActiveStreamNames.ORIENTATION_COMPLEMENTARY_EULER.ordinal()] = false;
		}
		else
		{
			activateStreamBasedOnParams();
		}
	}
	
	public long getTimestamp() {
		return (currentTimestamp - timestampStart);
	}

	private void openStreamsToFiles(int id) throws FileNotFoundException {

		accStream = null;
		gyroStream = null;
		magStream = null;
		accwogStream = null;
		orientAndroidStream = null;
		orientAndroidEulerStream = null;
		myOrientAEKFStream = null;
		myOrientAEKFEulerStream = null;
		myOrientComplementaryStream = null;
		myOrientComplementaryEulerStream = null;
		pressureStream = null;

		File folder = new File(Environment.getExternalStorageDirectory()
				+ "/OpenAIL");

		if (!folder.exists()) {
			folder.mkdir();
		}

		File dirInertialSensors = new File(folder.getAbsolutePath() + "/inertialSensors");
		if (!dirInertialSensors.exists()) {
			dirInertialSensors.mkdirs();
		}
		
		File dirRawData = new File(folder.getAbsolutePath() + "/rawData");
		if (!dirRawData.exists()) {
			dirRawData.mkdirs();
		}

		if ( activeStreams[ActiveStreamNames.ACCELEROMETER.ordinal()] ) {
			String fileName = dirRawData.toString() + "/acc.log";
			FileOutputStream faccStream = new FileOutputStream(fileName);
			accStream = new PrintStream(faccStream);
		}

		if ( activeStreams[ActiveStreamNames.GYROSCOPE.ordinal()] ) {
			String fileName = dirRawData.toString() + "/gyro.log";
			FileOutputStream fgyroStream = new FileOutputStream(fileName);
			gyroStream = new PrintStream(fgyroStream);
		}

		if ( activeStreams[ActiveStreamNames.MAGNETOMETER.ordinal()] ) {
			String fileName = dirRawData.toString() + "/mag.log";
			FileOutputStream fmagStream = new FileOutputStream(fileName);
			magStream = new PrintStream(fmagStream);
		}

		if ( activeStreams[ActiveStreamNames.ACCELEROMETER_WITHOUT_GRAVITY.ordinal()] ) {
			String fileName = dirRawData.toString() + "/accwog.log";
			FileOutputStream faccwogStream = new FileOutputStream(fileName);
			accwogStream = new PrintStream(faccwogStream);
		}

		if ( activeStreams[ActiveStreamNames.ORIENTATION_ANDROID.ordinal()] ) {
			String fileName = dirRawData.toString() + "/orientAndroid.log";
			FileOutputStream forientAndroidStream = new FileOutputStream(fileName);
			orientAndroidStream = new PrintStream(forientAndroidStream);
		}
		
		if ( activeStreams[ActiveStreamNames.ORIENTATION_ANDROID_EULER.ordinal()] ) {
			String fileName = dirRawData.toString() + "/orientAndroidEuler.log";
			FileOutputStream forientAndroidEulerStream = new FileOutputStream(fileName);
			orientAndroidEulerStream = new PrintStream(forientAndroidEulerStream);
		}

		if (performOrientationEstimation) {
			if ( activeStreams[ActiveStreamNames.ORIENTATION_AEKF.ordinal()] ) {
				String fileName = dirInertialSensors.toString() + "/myOrientAEKF.log";
				FileOutputStream fmyOrientStream = new FileOutputStream(fileName);
				myOrientAEKFStream = new PrintStream(fmyOrientStream);
			}

			if (activeStreams[ActiveStreamNames.ORIENTATION_AEKF_EULER.ordinal()]) {
				String fileName = dirInertialSensors.toString() + "/myOrientAEKFEuler.log";
				FileOutputStream fmyOrientEulerStream = new FileOutputStream(
						fileName);
				myOrientAEKFEulerStream = new PrintStream(fmyOrientEulerStream);
			}
			
			if ( activeStreams[ActiveStreamNames.ORIENTATION_COMPLEMENTARY.ordinal()] ) {
				String fileName = dirInertialSensors.toString() + "/myOrientComplementary.log";
				FileOutputStream fmyOrientComplementaryStream = new FileOutputStream(fileName);
				myOrientComplementaryStream = new PrintStream(fmyOrientComplementaryStream);
			}

			if ( activeStreams[ActiveStreamNames.ORIENTATION_COMPLEMENTARY_EULER.ordinal()] ) {
				String fileName = dirInertialSensors.toString() + "/myOrientComplementaryEuler.log";
				FileOutputStream fmyOrientComplementaryEulerStream = new FileOutputStream(
						fileName);
				myOrientComplementaryEulerStream = new PrintStream(fmyOrientComplementaryEulerStream);
			}
	
		}
		
		if ( activeStreams[ActiveStreamNames.PRESSURE.ordinal()] ) {
			String fileName = dirRawData.toString() + "/pressure.log";
			FileOutputStream fpressureStream = new FileOutputStream(fileName);
			pressureStream = new PrintStream(fpressureStream);
		}

	}

	// Closing the data streams
	private void closeStreams() {
		if (accStream != null)
			accStream.close();
		if (gyroStream != null)
			gyroStream.close();
		if (magStream != null)
			magStream.close();
		if (accwogStream != null)
			accwogStream.close();
		if (orientAndroidStream != null)
			orientAndroidStream.close();
		if (orientAndroidEulerStream != null)
			orientAndroidEulerStream.close();

		if (performOrientationEstimation) {
			if (myOrientAEKFStream != null)
				myOrientAEKFStream.close();
			if (myOrientAEKFEulerStream != null)
				myOrientAEKFEulerStream.close();

			if (myOrientComplementaryStream != null)
				myOrientComplementaryStream.close();
			if (myOrientComplementaryEulerStream != null)
				myOrientComplementaryEulerStream.close();
		}
		
		if (pressureStream != null)
			pressureStream.close();
	}


	public void playback(MySensorEvent event) {
		
		
		
		processNewData(event);
	}
	
	// Listener pushing the data into the files
	private SensorEventListener sensorEventListener = new SensorEventListener() {

		// We do nothing
		public void onAccuracyChanged(Sensor sensor, int accuracy) {

		}

		// On new data
		public void onSensorChanged(SensorEvent event) {

			MySensorEvent myEvent = new MySensorEvent(event.timestamp, event.sensor.getType(), event.values);
			processNewData(myEvent);
		}

		
	};
	
	/**
	 * @param event
	 */
	private void processNewData(MySensorEvent event) {
		currentTimestamp = event.timestamp;
		if (firstNewData) {
			firstNewData = false;
			timestampStart = event.timestamp - (System.nanoTime() - timestampStart);
		}

		if (event.sensorType == Sensor.TYPE_ACCELEROMETER) {

			processNewAccelerometerData(event);

		} else if (event.sensorType == Sensor.TYPE_GYROSCOPE) {
			
			processNewGyroscopeData(event);

		} else if (event.sensorType == Sensor.TYPE_MAGNETIC_FIELD) {

			processNewMagneticData(event);

		} else if (event.sensorType == Sensor.TYPE_LINEAR_ACCELERATION) {
			
			if (save2file && activeStreams[ActiveStreamNames.ACCELEROMETER_WITHOUT_GRAVITY.ordinal()])
				saveToStream(accwogStream, getTimestamp(), event.values);

		} else if (event.sensorType == Sensor.TYPE_ROTATION_VECTOR) {

			processNewOrientationData(event);

		} else if (event.sensorType == Sensor.TYPE_PRESSURE) {
			if (save2file && activeStreams[ActiveStreamNames.PRESSURE.ordinal()] )
				saveToStream(pressureStream, getTimestamp(), event.values);
			processNewPressureData(event);
	
		}
	}

	/**
	 * Computes the height in meters 
	 */
	private void processNewPressureData(MySensorEvent event) {
		lastBarometerValue = SensorManager.getAltitude(SensorManager.PRESSURE_STANDARD_ATMOSPHERE, event.values[0]);
	}

	/**
	 * Updates orientation filters
	 */
	private void processNewOrientationData(MySensorEvent event) {	
		
		float[] quaternion = new float[4];
		SensorManager.getQuaternionFromVector(quaternion,
				event.values);
		
		// Check if need to change sign
		float noChangeSign = RMSE(quaternion, lastAndroidQuat);
		float changeSign = RMSE(quaternion, minus(lastAndroidQuat));
		if (noChangeSign>changeSign) {
			quaternion = minus(quaternion);
		}
		lastAndroidQuat = quaternion;
		
		
	
		
		try {
			orientAndroidMtx.acquire();
			orientAndroid[0] = computeEulerRollX(quaternion);
			orientAndroid[1] = computeEulerPitchY(quaternion);
			
			yawMtx.acquire();
			orientAndroid[2] = computeEulerYawZ(quaternion);
			yawMtx.release();
			
			orientAndroidMtx.release();
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		
		if (save2file && activeStreams[ActiveStreamNames.ORIENTATION_ANDROID.ordinal()])
			saveToStream(orientAndroidStream, getTimestamp(), quaternion);
		if (save2file && activeStreams[ActiveStreamNames.ORIENTATION_ANDROID_EULER.ordinal()])
			saveToStream(orientAndroidEulerStream, getTimestamp(), orientAndroid);

		if (performOrientationEstimation) {
			
			// Complementary filter
			if (complementaryFilterEstimation.getState()) {
				
				
				complementaryFilterEstimation.magAccUpdate(quaternion);
				
				float [] orientCompQuat = complementaryFilterEstimation.getEstimate();
					
				try {
					orientCompMtx.acquire();
					orientComp[0] = computeEulerRollX(orientCompQuat);
					orientComp[1] = computeEulerPitchY(orientCompQuat);
					orientComp[2] = computeEulerYawZ(orientCompQuat);
					orientCompMtx.release();
				} catch (InterruptedException e) {
					e.printStackTrace();
				}
				
				if (save2file && activeStreams[ActiveStreamNames.ORIENTATION_COMPLEMENTARY.ordinal()])
					saveToStream(myOrientComplementaryStream, getTimestamp(), orientCompQuat);
				if (save2file && activeStreams[ActiveStreamNames.ORIENTATION_COMPLEMENTARY_EULER.ordinal()])
					saveToStream(myOrientComplementaryEulerStream, getTimestamp(), orientComp);
				

			}
			
	
			orientationEstimator.correct(quaternion[0], quaternion[1],
					quaternion[2], quaternion[3]);
			
			
			// Getting the estimate from our AEKF
			float [] orientAEKFQuat = orientationEstimator.getEstimate();
			
			try {
				orientMtx.acquire();
				orient[0] = computeEulerRollX(orientAEKFQuat);
				orient[1] = computeEulerPitchY(orientAEKFQuat);
				orient[2] = computeEulerYawZ(orientAEKFQuat);
				orientMtx.release();
			} catch (InterruptedException e) {
				e.printStackTrace();
			}

			if (save2file && activeStreams[ActiveStreamNames.ORIENTATION_AEKF.ordinal()])
				saveToStream(myOrientAEKFStream, getTimestamp(), orientAEKFQuat);
			if (save2file && activeStreams[ActiveStreamNames.ORIENTATION_AEKF_EULER.ordinal()])
				saveToStream(myOrientAEKFEulerStream, getTimestamp(),
						orient);
		
		}
		
		
		// TEST
		// Testing new stepometer angle

		float[] R = new float[9];
		SensorManager.getRotationMatrixFromVector(R, event.values);
//		if (deviceOrientation == deviceOrientation.VERTICAL)
//			stepometerAngle = (float) (Math.atan2(R[3], R[0]) * 180.0d / Math.PI);
//		else if (deviceOrientation == deviceOrientation.HORIZONTAL_LEFT)
//			stepometerAngle = (float) (Math.atan2(-R[4], -R[1]) * 180.0d / Math.PI);
//		else if (deviceOrientation == DeviceOrientation.HORIZONTAL_RIGHT)
//			stepometerAngle = (float) (Math.atan2(R[4], R[1]) * 180.0d / Math.PI);
//		else if (deviceOrientation == DeviceOrientation.UNKNOWN)
//			stepometerAngle = (float) (90.0f - Math.atan2(-R[5], R[2]) * 180.0d / Math.PI);

		stepometerAngle = (float) (90.0f - Math.atan2(-R[5], R[2]) * 180.0d / Math.PI);
		
//		double Zval = Math.sqrt(R[5]*R[5] + R[2]*R[2]);
//		double Yval = Math.sqrt(R[4]*R[4] + R[1]*R[1]);
//		
//		double stepometerAngle2 = (float) (90.0f - Math.atan2(-R[4], R[1]) * 180.0d / Math.PI);
//		Log.d(moduleLogName, "Zval =" + Zval + " Yval=" + Yval +  " || stepomterAngle=" + stepometerAngle + " step2=" +stepometerAngle2);
	}



	/**
	 * @param event
	 */
	private void processNewMagneticData(MySensorEvent event) {
		mag[0] = event.values[0];
		mag[1] = event.values[1];
		mag[2] = event.values[2];
		
		float magneticModule = (float) Math.sqrt(Math.pow(mag[0],2) + Math.pow(mag[1],2) + Math.pow(mag[2],2));
		
		try {
			magneticWindowMtx.acquire();
			magneticWindow.add(magneticModule);
			
			if ( magneticRecognitionCounter > 200 && magneticWindow.size() > magneticWindowSize)
			{
				// Reduce list size to stepometerWindowSize
				//Log.d("Main::Activity", "Mag1 : "+ (magneticWindow.size() - magneticWindowSize) +" " + magneticWindow.size());
				magneticWindow = magneticWindow.subList(magneticWindow.size() - magneticWindowSize, magneticWindow.size());
				//Log.d("Main::Activity", "Mag2 : " + magneticWindow.size());
				
				magneticRecognitionCounter = 0;
			}
			
			magneticWindowMtx.release();
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		
		magneticRecognitionCounter++;

		if (save2file && activeStreams[ActiveStreamNames.MAGNETOMETER.ordinal()])
			saveToStream(magStream, getTimestamp(), mag);
	}

	/**
	 * @param event
	 */
	private void processNewGyroscopeData(MySensorEvent event) {
		float [] gyro = new float [3];
		gyro[0] = event.values[0];
		gyro[1] = event.values[1];
		gyro[2] = event.values[2];
		
		// Gyro window
		try {
			gyroMtx.acquire();
			gyroVarianceWindow.add((float) Math.sqrt(gyro[0]*gyro[0]+gyro[1]*gyro[1]+gyro[2]*gyro[2]));
			if ( gyroVarianceWindow.size() > gyroWindowSize)
			{
				gyroVarianceWindow.removeFirst();
			}
			gyroMtx.release();
		} catch (Exception e) {};
		
		if (performOrientationEstimation) {
			// Time from last update converted from ns to s
			final float convertNs2s = 1000000000.0f;
			
			float dt = 0.0f;
			if ( lastOrientationEstimationTimestamp >= 0)
				dt = (event.timestamp - lastOrientationEstimationTimestamp)/convertNs2s; 
			lastOrientationEstimationTimestamp = event.timestamp;
						
			// AEKF
			orientationEstimator.predict(gyro[0], gyro[1], gyro[2], dt);
			// Complementary filter
			if (complementaryFilterEstimation.getState())
				complementaryFilterEstimation.gyroscopeUpdate(gyro, dt);
		}

		if (save2file && activeStreams[ActiveStreamNames.GYROSCOPE.ordinal()])
			saveToStream(gyroStream, getTimestamp(), gyro);
	}

	/**
	 * @param event
	 */
	private void processNewAccelerometerData(MySensorEvent event) {
		acc[0] = event.values[0];
		acc[1] = event.values[1];
		acc[2] = event.values[2];
		
		// select device orientation type
		if ( deviceOrientationIter < 50) {
			if ( acc[1] > Math.abs(acc[0]) && acc[1] > Math.abs(acc[2]) )
				deviceOrientationPoll[DeviceOrientation.VERTICAL.ordinal()]++;
			else if ( acc[0] > Math.abs(acc[1]) && acc[0] > Math.abs(acc[2]))
				deviceOrientationPoll[DeviceOrientation.HORIZONTAL_LEFT.ordinal()]++;
			else if ( acc[0] < Math.abs(acc[1]) && acc[0] < Math.abs(acc[2]))
				deviceOrientationPoll[DeviceOrientation.HORIZONTAL_RIGHT.ordinal()]++;
			else
				deviceOrientationPoll[DeviceOrientation.UNKNOWN.ordinal()]++;
			
			deviceOrientationIter++;
		}
		else if ( deviceOrientationIter == 50) {
			
			int max = deviceOrientationPoll[0];
			int index = 0;
			for (int i=1;i<4;i++) {
				if ( deviceOrientationPoll[i] > max) {
					max = deviceOrientationPoll[i];
					index = i;
				}
			}
			
			deviceOrientation = DeviceOrientation.values()[index];
			
			//TESTING
			//deviceOrientation = DeviceOrientation.UNKNOWN;
		}

		float accVal = (float) Math.sqrt(acc[0] * acc[0] + acc[1]
				* acc[1] + acc[2] * acc[2]);
		
		try {
			accMtx.acquire();
				accVarianceWindow.add(Float.valueOf(accVal));
			accMtx.release();
			
			accStepometerWindow.add(Float.valueOf(accVal));
			
		} catch (Exception e) {};
		
		
		// Remove element if too long
//		if (accVarianceWindow.size() > accVarianceWindowSize) {
//			accVarianceWindow.removeFirst();
//		}
		if (accStepometerWindow.size() > parameters.stepometer.windowSize) {
			accStepometerWindow.removeFirst();
		}

		if (save2file && activeStreams[ActiveStreamNames.ACCELEROMETER.ordinal()])
			saveToStream(accStream, getTimestamp(), acc);
		
		if (stepometerStarted && stepometerRunningCounter > 200)
		{
			Log.d(moduleLogName, "Compare sizes: " + accStepometerWindow.size() + " " + parameters.stepometer.windowSize);
			if ( accStepometerWindow.size() == parameters.stepometer.windowSize)
			{
				
				 // Copying from list to float array, so we can process in new thread
				 int accWindowSize = accStepometerWindow.size();
				 float [] accWindowFloat = new float[accWindowSize];
				 for (int i=0;i<accWindowSize;i++)
				 {
					accWindowFloat[i] = accStepometerWindow.get(i);
				 }
				 stepometer.setAccWindow(accWindowFloat, accWindowSize);
				
				 // Run stepometer
				 new Thread(stepometer).start();

			}
			stepometerRunningCounter = 0;
		}
		stepometerRunningCounter++;
		accVarianceRunningCounter++;
	}
	
	
	/**
	 * //http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
	 */
	private float computeEulerYawZ(float w, float x, float y, float z) {
		return (float) (Math.atan2(2*(w*z + y*x), 1-2*(y*y + z*z))* 180.0 / Math.PI);
	}
	
	private float computeEulerYawZ(float [] orientQuat) {
		return computeEulerYawZ(orientQuat[0], orientQuat[1], orientQuat[2], orientQuat[3]);
	}

	/**
	 */
	private float computeEulerPitchY(float w, float x, float y, float z) {
		return (float) (Math.asin(2 * (w *y - x*z)) * 180.0 / Math.PI);
	}
	
	private float computeEulerPitchY(float [] orientQuat) {
		return computeEulerPitchY(orientQuat[0], orientQuat[1], orientQuat[2], orientQuat[3]);
	}
	
	

	/**
	 */
	private float computeEulerRollX(float w, float x, float y, float z) {
		return (float) (Math.atan2(2*y*z + 2 * x * w, 1 - 2 * ( x*x + y*y) ) * 180.0 / Math.PI );
	}
	
	private float computeEulerRollX(float [] orientQuat) {
		return computeEulerRollX(orientQuat[0], orientQuat[1], orientQuat[2], orientQuat[3]);
	}

	/**
	 */
	private void saveToStream(PrintStream stream, Long timestamp,
			float[] values) {

		stream.print(Long.toString(timestamp));
		for (int i = 0; i < values.length; i++) {
			stream.print(" " + Float.toString(values[i]));
		}
		stream.print(System.getProperty("line.separator"));
	}
	
	private float avgList(LinkedList<Float> values) {
		float s = 0;
		for (Float x : values)
			s+=x;
		return s/values.size();
	}
	
	private float varList(LinkedList<Float> values) {
		float sumDiffsSquared = 0.0f;
		float avg = avgList(values);
		for (Float value : values) {
			double diff = value - avg;
			diff *= diff;
			sumDiffsSquared += diff;
		}
		return sumDiffsSquared / (values.size() - 1);
	}
	
	
	private float RMSE(float[] x, float [] y) {
		if (x.length != y.length) {
			return 99999.0f;
		}
		float val = 0.0f;
		for (int i=0;i<4;i++) {
			val += ((x[i] - y[i])*(x[i] - y[i]));
		}
		return (float) Math.sqrt(val);
	}
	
	private float [] minus(float [] x) {
		for (int i=0;i<x.length;i++)
			x[i] = -x[i];
		return x;
	}
	

} public class InertialSensorsPlayback implements Runnable {
	final String moduleLogName = "InertialDataPlayback";

	class RawData {
		int sensorType;
		long timestamp, timestampEnd;
		float[] data;
		int graphId, wifiId, wifiCount;
		List<MyScanResult> wifiScans;

	}

	class RawDataComparator implements Comparator<RawData> {
		@Override
		public int compare(RawData arg0, RawData arg1) {
			if (arg0 == null && arg1 == null)
				return 0;
			else if (arg0 == null)
				return 1;
			else if (arg1 == null)
				return -1;
			return (int) (arg0.timestamp - arg1.timestamp);
		}

	}

	Scanner rawDataScanner;
	InertialSensors inertialSensors;

	Scanner accScanner, gyroScanner, magScanner, androidOrientScanner;

	RawData lastWifi, lastAcc, lastGyro, lastMag, lastAndroidOrient;
	
	Parameters.Playback parameters;

	public InertialSensorsPlayback(Parameters.Playback _parameters, InertialSensors _inertialSensors) {
		inertialSensors = _inertialSensors;
		parameters = _parameters;
		
		File folder = new File(Environment.getExternalStorageDirectory()
				+ "/OpenAIL/rawData");

		if (!folder.exists()) {
			folder.mkdir();
		}

		// Preparations to read saved, raw data

		// Accelerometer
		try {
			accScanner = new Scanner(new BufferedReader(new FileReader(folder
					+ "/acc.log")));
			accScanner.useLocale(Locale.US);
		} catch (FileNotFoundException e1) {
			accScanner = null;
			Log.e(moduleLogName, "Missing acc.log");
		}

		// Gyroscope
		try {
			gyroScanner = new Scanner(new BufferedReader(new FileReader(folder
					+ "/gyro.log")));
			gyroScanner.useLocale(Locale.US);
		} catch (FileNotFoundException e1) {
			gyroScanner = null;
			Log.e(moduleLogName, "Missing gyro.log");
		}

		// Magnetometer
		try {
			magScanner = new Scanner(new BufferedReader(new FileReader(folder
					+ "/mag.log")));
			magScanner.useLocale(Locale.US);
		} catch (FileNotFoundException e1) {
			magScanner = null;
			Log.e(moduleLogName, "Missing mag.log");
		}

		// Android orient
		try {
			androidOrientScanner = new Scanner(new BufferedReader(
					new FileReader(folder + "/orientAndroid.log")));
			androidOrientScanner.useLocale(Locale.US);
		} catch (FileNotFoundException e1) {
			androidOrientScanner = null;
			Log.e(moduleLogName, "Missing orientAndroid.log");
		}
		
		Log.e(moduleLogName, "readAcc");
		lastAcc = readAcc();
		Log.e(moduleLogName, "readGyro");
		lastGyro = readGyro();
		Log.e(moduleLogName, "readMag");
		lastMag = readMag();
		Log.e(moduleLogName, "readAndroidOrient");
		lastAndroidOrient = readAndroidOrient();
		Log.e(moduleLogName, "getNextData");

	}

	public void start() {
		Thread simulationThread = new Thread(this, "WiFi playback thread");
		simulationThread.start();
	}

	@Override
	public void run() {
		Log.e(moduleLogName, "Starting simulation");

		//
		// TODO: Prepare inertialsensors
		//

		long startTime = System.nanoTime();

		

		RawData rawData = getNextData();

		while (rawData != null) {

			long currentTime = (long) ((System.nanoTime() - startTime)*parameters.simulationSpeed);
			
			if ( (currentTime - rawData.timestamp) / 10e6 > parameters.inertialMaxDelay )
				Log.e(moduleLogName, "We are processing data too slow! Lower simulationSpeed! " + rawData.timestamp
						+ " vs " + currentTime + " DIFF=" + (currentTime - rawData.timestamp));
			
			if (rawData.timestamp < currentTime) {

				MySensorEvent myEvent = new MySensorEvent(rawData.timestamp,
						rawData.sensorType, rawData.data);
				inertialSensors.playback(myEvent);
				rawData = getNextData();
			} else {
				try {
					Thread.sleep(parameters.inertialSleepTimeInMs, 0);
				} catch (InterruptedException e) {
					e.printStackTrace();
				}
			}

		}
		Log.e(moduleLogName, "Simulation finished");
	}

	RawData getNextData() {
		if (lastAcc == null && lastMag == null && lastGyro == null
				&& lastAndroidOrient == null)
			return null;

		RawData nextData = minimum(lastAcc, lastMag, lastGyro,
				lastAndroidOrient);

		// Reading next data
		if (nextData.sensorType == Sensor.TYPE_ACCELEROMETER)
			lastAcc = readAcc();
		else if (nextData.sensorType == Sensor.TYPE_GYROSCOPE)
			lastGyro = readGyro();
		else if (nextData.sensorType == Sensor.TYPE_MAGNETIC_FIELD)
			lastMag = readMag();
		else if (nextData.sensorType == Sensor.TYPE_ROTATION_VECTOR)
			lastAndroidOrient = readAndroidOrient();

		return nextData;
	}

	RawData minimum(RawData acc, RawData mag, RawData gyro, RawData andOrient) {
		List<RawData> list = new ArrayList<RawData>();
		list.add(acc);
		list.add(mag);
		list.add(gyro);
		list.add(andOrient);
		// Log.e(moduleLogName, "MINIMUM: " + wifi.timestamp);
		// Log.e(moduleLogName, "MINIMUM: " + acc.timestamp);
		// Log.e(moduleLogName, "MINIMUM: " + mag.timestamp);
		// Log.e(moduleLogName, "MINIMUM: " + gyro.timestamp);
		// Log.e(moduleLogName, "MINIMUM: " + andOrient.timestamp);

		Collections.sort(list, new RawDataComparator());

		return list.get(0);
	}

	RawData readAcc() {
		RawData accData = readInertialSensor(accScanner, false);
		if (accData != null)
			accData.sensorType = Sensor.TYPE_ACCELEROMETER;
		return accData;
	}

	RawData readMag() {
		RawData magData = readInertialSensor(magScanner, false);
		if (magData != null)
			magData.sensorType = Sensor.TYPE_MAGNETIC_FIELD;
		return magData;
	}

	RawData readGyro() {
		RawData gyroData = readInertialSensor(gyroScanner, false);
		if (gyroData != null)
			gyroData.sensorType = Sensor.TYPE_GYROSCOPE;
		return gyroData;
	}

	RawData readAndroidOrient() {
		RawData androidOrientData = readInertialSensor(androidOrientScanner,
				true);
		if (androidOrientData != null)
			androidOrientData.sensorType = Sensor.TYPE_ROTATION_VECTOR;
		return androidOrientData;
	}

	RawData readInertialSensor(Scanner inertialScanner, boolean fourValues) {

//		Log.e(moduleLogName, "readInertialSensor() = "
//				+ (inertialScanner == null) + " " + fourValues);
		if (inertialScanner != null && inertialScanner.hasNextLong()) {
			RawData inertialData = new RawData();
			inertialData.timestamp = inertialScanner.nextLong();

			if (fourValues) {
				double quatW = inertialScanner.nextDouble();
			}
			
			inertialData.data = new float[3];

			inertialData.data[0] = (float) inertialScanner.nextDouble();
			inertialData.data[1] = (float) inertialScanner.nextDouble();
			inertialData.data[2] = (float) inertialScanner.nextDouble();
			String endOfLine = inertialScanner.nextLine();

//			Log.e(moduleLogName, "READ: " + inertialData.timestamp + " "
//					+ inertialData.data[0] + " " + inertialData.data[1] + " "
//					+ inertialData.data[2]);

			return inertialData;
		}
		return null;
	}

}
 public class LocalizationView extends SurfaceView implements SurfaceHolder.Callback{
	private static final String moduleLogName = "LocalizationView";
	
	
	private LocalizationViewDrawThread drawThread;
	private double goalX = 0.0, goalY = 0.0;
	private boolean goalSet = false;
	
	public LocalizationView(Context context, AttributeSet attrs) {
		super(context, attrs);

		// register our interest in hearing about changes to our surface
        SurfaceHolder holder = getHolder();
        holder.addCallback(this);

        // create thread only; it's started in surfaceCreated()
        drawThread = new LocalizationViewDrawThread(holder, context, new Handler() {
            @Override
            public void handleMessage(Message m) {
            }
        });
	}
	
	public void setWiFiScanLocations( List<Pair<Double, Double>> _newWiFiScanLocations) {
		drawThread.setWiFiScanLocations(_newWiFiScanLocations);
	}
	
	public void setUserLocations( List<Pair<Double, Double>> _userLocations) {
		drawThread.setUserLocations(_userLocations);
	}

	public void setBuildingPlan(BuildingPlan buildingPlan_) {
		drawThread.setBuildingPlan(buildingPlan_);
	}
	
	@Override
    /* Callback invoked when the surface dimensions change. */
    public void surfaceChanged(SurfaceHolder holder, int format, int width,
            int height) {
        drawThread.setSurfaceSize(width, height);
    }

    /*
     * Callback invoked when the Surface has been created and is ready to be
     * used.
     */
	@Override
    public void surfaceCreated(SurfaceHolder holder) {
        // start the thread here so that we don't busy-wait in run()
        // waiting for the surface to be created
        drawThread.setRunning(true);
        drawThread.start();
    }

	@Override
	public void surfaceDestroyed(SurfaceHolder arg0) {
		drawThread.setRunning(false);
		try {
			drawThread.join();
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}

	@Override
	public boolean onTouchEvent(MotionEvent event) {
		
		goalX = event.getX();
		goalY = event.getY();
		goalSet = true;
		
//		if ( event.getRawY() < 920)
//			drawThread.increaseZoom();
//		else
//			drawThread.decreaseZoom();
//		
//		Log.d(TAG, "onTouch! Zoom = ");
		Log.d(moduleLogName, "Clicked!");
		return false;
		
	}
	
	public boolean isGoalSet() {
		return goalSet;
	}
	
	public void showBackgroundPlan (boolean showBackgroundPlan) {
		drawThread.showBackgroundPlan (showBackgroundPlan);
	}
	
	public Pair<Double, Double> getGoal() {
		goalSet = false;
		return drawThread.getMetresFromPixels(new Pair<Double, Double>(goalX, goalY));
	}

	public void setPathToGoal(List<Pair<Double, Double>> pathToGoal) {
		drawThread.setPathToGoal(pathToGoal);
	}
} public class WiFiDirect {
	
	final String moduleLogName = "WiFiDirect";
	
	public class DirectMeasurement {
		public int idPos, idAP;
		public double distance, level, frequency;
		
		public DirectMeasurement(int _idAP, int _idPos, double _distance, double _level, double _frequency) {
			idAP = _idAP;
			idPos = _idPos;
			distance = _distance;
			level = _level;
			frequency = _frequency;
		}
	}
	
	// List of measurements
	List<DirectMeasurement> directMeasurements = new ArrayList<DirectMeasurement>();
	List<Integer> posOccurrence= new ArrayList<Integer>();
	int lastIdPos = -1;
	
	// NetworkMacAddr to idAP
	List<String> bssid2id = new ArrayList<String>();
	List<Integer> bssidOccurrence= new ArrayList<Integer>();

	
	public void processNewScan(List<MyScanResult> scan, int idPos) {

		for (int i = 0; i < scan.size(); i++) {

			// Getting network
			MyScanResult network = scan.get(i);
			
			if ( (network.networkName.contains("mtp") || network.networkName.length() < 1) && network.level > -120)
			{
			
				// Convert MAC to index
				int idAP = bssid2id.indexOf(network.BSSID);
				
				if (idAP == -1) {
					bssid2id.add(network.BSSID);
					bssidOccurrence.add(Integer.valueOf(0));
					idAP = bssid2id.indexOf(network.BSSID);
				} else
					bssidOccurrence.set(idAP, bssidOccurrence.get(idAP)+1);
				
				// Convert lvl to meters
				double distance = convertLevelToMeters(network.level, network.frequency);
				
				// Add to list
				directMeasurements.add(new DirectMeasurement(idAP, idPos, distance, network.level, network.frequency));
				
				if ( lastIdPos != idPos) {
					lastIdPos = idPos;
					posOccurrence.add(Integer.valueOf(0));
				}
			}
			else
				Log.d(moduleLogName, "Rejected: " + network.networkName);
		}
	}
	
	public void filterDirectMeasurements() {
		Log.d(moduleLogName, "filterDirectMeasurements! " + directMeasurements.size());
		
		for (int i=0;i<bssidOccurrence.size();i++)
		{
			if ( bssidOccurrence.get(i) < 10 )
			{
				Iterator<DirectMeasurement> iter = directMeasurements.iterator();
				while (iter.hasNext()) {
				    if (iter.next().idAP == i) {
				        iter.remove();
				    }
				}
			}
			
		}
		Log.d(moduleLogName, "Filtered! " + directMeasurements.size());
	}
	
	public List<DirectMeasurement> getReverseTest() {
		Log.d(moduleLogName, "getReverseTest()! " + posOccurrence.size());
		
		List<DirectMeasurement> positionOne = new ArrayList<DirectMeasurement>();
		for (DirectMeasurement dm : directMeasurements) {
			positionOne.add(new DirectMeasurement(dm.idAP, dm.idPos-8000, dm.distance, dm.level, dm.frequency));
			posOccurrence.set(dm.idPos-10000, posOccurrence.get(dm.idPos-10000) + 1);
		}
		
		for (Integer i : posOccurrence) {
			Log.d(moduleLogName, "posOccurance = " + i);
		}
		Log.d(moduleLogName, "getReverseTest()! ");
		
		
		return positionOne;
	}
	
	/*
	 * Return results
	 */
	public List<DirectMeasurement> getGraphWiFiList() {
		return new ArrayList<DirectMeasurement>(directMeasurements);
	}
	
	public int getNumberOfNetworks() {
		return bssid2id.size();
	}

	/*
	 * Convert measurement from dBm to meters with some propagation model
	 */
	private double convertLevelToMeters(double level, double freq) {
//		double tmp = -40 + Math.abs(level);
//		tmp = tmp / 40;
//		return Math.pow(10, tmp);
		 double exp = (27.55 - (20 * Math.log10(freq)) + Math.abs(level)) / 20.0;
		 return Math.pow(10.0, exp);
	}
}
 public class DirectMeasurement {
	public int idPos, idAP;
	public double distance, level, frequency;
		
	public DirectMeasurement(int _idAP, int _idPos, double _distance, double _level, double _frequency) {
		idAP = _idAP;
		idPos = _idPos;
		distance = _distance;
		level = _level;
		frequency = _frequency;
	}
}
 public class WiFiPlayback implements Runnable {
	final String moduleLogName = "WiFiPlayback";

	class RawData {
		int sourceType;
		long timestamp, timestampEnd;
		double x, y, z, w;
		int graphId, wifiId, wifiCount;
		List<MyScanResult> wifiScans;
		
	}
	
	Scanner rawDataScanner;
	WiFiScanner wifiScanner;
	
	Parameters.Playback parameters;
	
	public WiFiPlayback(Parameters.Playback _parameters, WiFiScanner _wifiScanner) {
		wifiScanner = _wifiScanner;
		parameters = _parameters;
		
		File folder = new File(Environment.getExternalStorageDirectory()
				+ "/OpenAIL/rawData");
	
		if (!folder.exists()) {
			folder.mkdir();
		}
		
		// Preparations to read saved, raw data

		// WiFi
		try {
			rawDataScanner = new Scanner(new BufferedReader(new FileReader(
					folder + "/wifi.log")));
			rawDataScanner.useLocale(Locale.US);
		} catch (FileNotFoundException e1) {
			rawDataScanner = null;
			Log.e(moduleLogName, "Missing wifi.log");
		}
		
	}
	
	public void start() {
		Thread simulationThread = new Thread(this, "WiFi playback thread");
		simulationThread.start();
	}
	
	@Override
	public void run() {
		Log.e(moduleLogName, "Starting simulation");
		
		wifiScanner.setPlaybackState(true);
		wifiScanner.waitingForScan = true;
		wifiScanner.saveRawData = false;
		
		long startTime = System.nanoTime();
		RawData rawData = readWiFi();
		
		while(rawData != null) {
			
			long currentTime = (long) ((System.nanoTime() - startTime) * parameters.simulationSpeed);
			
			Log.e(moduleLogName, "rawData.timestampEnd = " + rawData.timestampEnd + " vs " + currentTime);
			if ( rawData.timestampEnd < currentTime) {
				wifiScanner.playback(rawData.wifiScans);
				rawData = readWiFi();
			}
			else
			{
				try {
					Thread.sleep(parameters.wifiSleepTimeInMs, 0);
				} catch (InterruptedException e) {
					e.printStackTrace();
				}
			}
			
			
		}
		Log.e(moduleLogName, "Simulation finished");
	}
	
	
	RawData readWiFi() {
		if ( rawDataScanner != null && rawDataScanner.hasNextInt()) {
			RawData wifiData = new RawData();
			wifiData.sourceType = Sensor.TYPE_ALL;
			wifiData.graphId = rawDataScanner.nextInt();
			wifiData.wifiId = rawDataScanner.nextInt();
			wifiData.timestamp = rawDataScanner.nextLong();
			wifiData.timestampEnd = rawDataScanner.nextLong();
			wifiData.wifiCount = rawDataScanner.nextInt();
			String endOfLine = rawDataScanner.nextLine();
			
			wifiData.wifiScans = PriorMapHandler.extractListOfWiFiNetworksFromFile(rawDataScanner, wifiData.wifiCount);
	
			Log.d(moduleLogName, "WIFI: " + wifiData.graphId + " " + wifiData.wifiId + " " + wifiData.timestamp + " " + wifiData.timestamp + " " + wifiData.wifiCount );
			for (MyScanResult msr : wifiData.wifiScans) {
				Log.d(moduleLogName, "\tnetwork: " + msr.BSSID + " " + msr.networkName + " " + msr.level + " " + msr.frequency);
			}
			return wifiData;
		}
		return null;
	}

} public class WiFiScanner extends BroadcastReceiver {
	
	final String moduleLogName = "WiFiScanner";

	// Class to access physical sensor
	WifiManager wifiManager;
	
	// Operation mode - single/continuous
	boolean continuousScanning = false, singleScan = true;
	
	// Are we waiting for the scan?
	public boolean waitingForScan = false;
	
	// Was start called?
	public boolean playbackActive = false;
	
	// Timestamps
	long startTimestampOfCurrentScan, startTimestampOfGlobalTime;

	// Scan id and graphId
	private int id, graphPoseId;

	// Class used to process wifi scans
	WiFiPlaceRecognition placeRecognition;

	// Last results
	List<MyScanResult> previousWiFiList = new ArrayList<MyScanResult>();
	
	

	// / G2O
	// Position index
	int posIndex = 10000;

	// Variable indicating new measurement
	boolean newMeasurement = false;

	
	// File to save data
	PrintStream outStreamRawData = null;
	
	// Should we save raw WiFi data
	boolean saveRawData = false;

		
	
	public WiFiScanner(
			WifiManager _wifiManager,
			org.dg.openAIL.ConfigurationReader.Parameters.WiFiPlaceRecognition wifiPlaceRecognitionParameters) {
		// Save physical access to sensor
		wifiManager = _wifiManager;
		
		// We operate in continuous mode
		continuousScanning = true;
		singleScan = false;
		
		// Let's assume 0's
		id = 0;
		graphPoseId = 0;
		startTimestampOfGlobalTime = 0;

		// Creating wifi place recognition
		placeRecognition = new WiFiPlaceRecognition(
				wifiPlaceRecognitionParameters);

		// Directory to story results
		File folder = new File(Environment.getExternalStorageDirectory()
				+ "/OpenAIL/WiFi");

		if (!folder.exists()) {
			folder.mkdir();
		}

		// File to save results
		saveRawData = wifiPlaceRecognitionParameters.recordRawData;
		
	}

	// Nicely setting parameters
	public WiFiScanner singleScan(boolean _singleScan) {
		this.singleScan = _singleScan;
		return this;
	}

	public WiFiScanner continuousScanning(boolean _continuousScanning) {
		this.continuousScanning = _continuousScanning;
		return this;
	}

//	public WifiScanner startTimestampOfGlobalTime(
//			long _startTimestampOfGlobalTime) {
//		this.startTimestampOfGlobalTime = _startTimestampOfGlobalTime ;
//		return this;
//	}

	// We start the recognition of places
	public void startNewPlaceRecognitionThread() {
		placeRecognition.startRecognition();
	}

	// We stop the recognition of places
	public void stopNewPlaceRecognitionThread() {
		placeRecognition.stopRecognition();
	}

	// Are we waiting for the WiFi scan?
	public boolean getWaitingForScan() {
		return waitingForScan;
	}
	
	public boolean getPlaybackState() {
		return playbackActive;
	}
	
	public void setPlaybackState(boolean playbackState) {
		playbackActive = playbackState;
	}

	// Starts scanning
	public void startScanning() {
				
		if (continuousScanning && saveRawData) {

			String fileName = "";
			fileName = Environment.getExternalStorageDirectory().toString()
					+ "/OpenAIL/rawData/wifi.log";

			// RawMeasurements
			FileOutputStream foutStream;
			try {
				foutStream = new FileOutputStream(fileName);
				outStreamRawData = new PrintStream(foutStream);
			} catch (FileNotFoundException e1) {
				// TODO Auto-generated catch block
				e1.printStackTrace();
			}

		}
		
		startTimestampOfCurrentScan = System.nanoTime();

		// We initialize the WiFi scan if WiFi is on
		Log.e("WIFI",
				"WIFI START STATE: " + wifiManager.isWifiEnabled() + " "
						+ wifiManager.pingSupplicant() + " "
						+ wifiManager.getWifiState());
		if (wifiManager.isWifiEnabled() && (singleScan || continuousScanning)) // &&
																				// !getRunningState())
		{
			wifiManager.startScan();
			waitingForScan = true;
		}
	}

	// Stops scanning - we still wait for started scan
	public void stopScanning() {
		continuousScanning = false;

//		placeRecognition.closeSaveStream();

		// Close cleanly
		if (outStreamRawData != null) {
			outStreamRawData.close();
			outStreamRawData = null;
		}
	}

	
	
	// Info for GUI
	public int getNetworkCount() {
		return previousWiFiList.size();
	}

	// Info for GUI
	public String getStrongestNetwork() {
		String bestName = "";
		int bestLvl = -150;

		for (int i = 0; i < previousWiFiList.size(); i++) {
			MyScanResult scanResult = previousWiFiList.get(i);
			if (scanResult.level > bestLvl) {
				bestLvl = scanResult.level;
				bestName = scanResult.networkName;
			}
		}
		return bestName;
	}
	
	// Return the size of place database
	public int getSizeOfPlaceDatabase() {
		return placeRecognition.getSizeOfPlaceDatabase();
	}

	// Returns the last scan
	public List<MyScanResult> getLastScan()
	{
		List<MyScanResult> myList = new ArrayList<MyScanResult>();
		synchronized (previousWiFiList) {
			for (MyScanResult sr : previousWiFiList) {
				myList.add(new MyScanResult(sr.BSSID, sr.level, sr.networkName, sr.frequency));
			}
		}
		
		return myList;
	}
	
	// Adds last scan to place recognition
	public void addLastScanToRecognition(int id) {
		graphPoseId = id; 
		synchronized (previousWiFiList) {
			addScanToRecognition(id, previousWiFiList);
		}
	}

	// Adding scan to place rec
	public void addScanToRecognition(int id, List<MyScanResult> wifiScan) {
		if (wifiScan.size() > 0)
			placeRecognition.addPlace(wifiScan, id);
	}

	// Returns the places that we recognized and clears the list
	public List<org.dg.openAIL.IdPair<Integer, Integer>> getAndClearRecognizedPlacesList() {
		return placeRecognition.getAndClearRecognizedPlacesList();
	}

	
	

	

	// Are there new Wifi scans?
	public boolean isNewMeasurement() {
		return newMeasurement;
	}

	// we set the value of new measurement
	public void newMeasurement(boolean value) {
		newMeasurement = value;
	}

	// Setting the start of the time
	public void setStartTime() {
		Log.d(moduleLogName, "setStartTime = " + System.nanoTime() /10e9);
		startTimestampOfGlobalTime = System.nanoTime();
	}

	// When data from physical sensors is received
	@Override
	public void onReceive(Context arg0, Intent arg1) {
		Log.d(moduleLogName, "Scan finished\n");
		if (waitingForScan) {
			try {
				// Process list of detected WiFis
				List<ScanResult> tmp = wifiManager.getScanResults();
				Log.d(moduleLogName, "Found " + tmp.size() + " wifis \n");
				
				List<MyScanResult> wifiList = new ArrayList<MyScanResult>();
				for (ScanResult network : tmp) {
					wifiList.add(new MyScanResult(network.BSSID, network.level, network.SSID, network.frequency));
				}
				
				processNewScan(wifiList);
				
				
				Toast toast = Toast.makeText(arg0.getApplicationContext(),
						"WiFi scan finished", Toast.LENGTH_SHORT);
				toast.show();
			} catch (Exception e) {
				Log.d(moduleLogName, "Scanning failed: " + e.getMessage() + "\n");

				Toast toast = Toast.makeText(arg0.getApplicationContext(),
						"WiFi scan FAILED", Toast.LENGTH_SHORT);
				toast.show();
			}
			
			// Prepare for next measurement
			waitingForScan = false;
			if (continuousScanning) {
				startTimestampOfCurrentScan = System.nanoTime();
				boolean value = wifiManager.startScan();
				waitingForScan = true;
				Log.d(moduleLogName,
						"Called start, waiting on next scan - startScan value: "
								+ value + "\n");
			}
			id++;
		}
	}
	
	// Simulate working with provided scan
	public void playback(List<MyScanResult> wifiScans) {
		try {
			processNewScan(wifiScans);
			
			Log.d(moduleLogName, "WiFi scan finished");

		} catch (Exception e) {
			Log.e(moduleLogName, "WiFi scan FAILED");
		}
	}

	/**
	 * Processes new scan coming either from real sensor manager or from playback
	 */
	private void processNewScan(List<MyScanResult> wifiScan) throws InterruptedException {

			// Saving raw data
			if (saveRawData)
				saveScanToRawStream(wifiScan);
				
			// Save measurement
			synchronized (previousWiFiList) {
				previousWiFiList = wifiScan;
			}

			newMeasurement = true;	
	}

	/**
	 * @param wifiScans
	 */
	private void saveScanToRawStream(List<MyScanResult> wifiScans) {
		Log.d(moduleLogName, "current =  " + System.nanoTime()/10e9 + "  startTimestampOfCurrentScan = " + startTimestampOfCurrentScan/10e9 + "  startTimestampOfGlobalTime = " + startTimestampOfGlobalTime/10e9);
		outStreamRawData.print(Integer.toString(graphPoseId)
				+ "\t"
				+ Integer.toString(id)
				+ "\t"
				+ Long.toString(startTimestampOfCurrentScan
						- startTimestampOfGlobalTime)
				+ "\t"
				+ Long.toString(System.nanoTime()
						- startTimestampOfGlobalTime) + "\t");
		outStreamRawData.print(wifiScans.size() + "\n");

		// Save BSSID, SSID, lvl and frequency
		for (int i = 0; i < wifiScans.size(); i++) {
			MyScanResult scanResult = wifiScans.get(i); //convertLevelToMeters(scanResult.level)  + "\t" + scanResult.frequency 
			outStreamRawData.print(scanResult.BSSID + "\t"
					+ scanResult.networkName + "\t" + scanResult.level + "\t" + scanResult.frequency + "\n");
		}
	}


}
import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
import java.util.Locale;
import java.util.Timer;
import java.util.TimerTask;
import org.dg.camera.Preview;
import org.dg.camera.QRCodeDecoderClass;
import org.dg.camera.VisualCompass;
import org.dg.camera.VisualPlaceRecognition;
import org.dg.graphManager.GraphManager;
import org.dg.graphManager.Vertex;
import org.dg.graphManager.WiFiMeasurement;
import org.dg.inertialSensors.InertialSensors;
import org.dg.inertialSensors.InertialSensorsPlayback;
import org.dg.main.LocalizationView;
import org.dg.wifi.WiFiDirect;
import org.dg.wifi.WiFiDirect.DirectMeasurement;
import org.dg.wifi.WiFiPlayback;
import org.dg.wifi.WiFiScanner;
import org.opencv.core.Mat;
import org.opencv.core.Point3;
import org.xmlpull.v1.XmlPullParserException;
import android.content.Context;
import android.graphics.Bitmap;
import android.graphics.Bitmap.CompressFormat;
import android.hardware.SensorManager;
import android.net.wifi.WifiManager;
import android.os.Environment;
import android.util.Log;
import android.util.Pair;
import android.view.View.MeasureSpec;
// OpenAIL - Open Android Indoor Localization
// Copyright (C) 2015 Michal Nowicki (michal.nowicki@put.poznan.pl)
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

package org.dg.openAIL;




// This is a class that is supposed to provide the interface for all available solutions
public class OpenAndroidIndoorLocalization {
	private static final String moduleLogName = "OpenAIL";

	// Parameters
	static boolean WiFiFeatures = true;
	ConfigurationReader.Parameters parameters;

	// Inertial sensors
	public InertialSensors inertialSensors;

	// WiFi
	public WiFiScanner wifiScanner;
	int detectWiFiIssue;

	// Visual Place Recognition
	public VisualPlaceRecognition visualPlaceRecognition;
	
	// Visual Compass
	public VisualCompass visualCompass;

	// Graph
	public GraphManager graphManager;