package com.example.steeringhelper;

import org.opencv.android.BaseLoaderCallback;
import org.opencv.android.LoaderCallbackInterface;
import org.opencv.android.OpenCVLoader;
import org.opencv.core.Size;

import android.graphics.Bitmap;
import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Matrix;
import android.graphics.Paint;
import android.graphics.Point;
import android.graphics.Rect;
import android.graphics.Typeface;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;
import android.os.Handler;
import android.os.Message;
import android.app.Activity;
import android.content.Context;
import android.content.Intent;
import android.content.pm.ActivityInfo;
import android.content.res.Configuration;
import android.util.Log;
import android.view.GestureDetector;
import android.view.Menu;
import android.view.MotionEvent;
import android.view.SurfaceHolder;
import android.view.SurfaceView;
import android.view.View;
import android.view.ViewGroup;
import android.widget.Button;
import android.widget.TableLayout;
import android.widget.TableRow;
import android.widget.TextView;
import android.widget.Toast;

public class MainActivity extends Activity implements Button.OnClickListener, SensorEventListener,
OpenCVWorker.ResultCallback, SurfaceHolder.Callback {
	// device sensor manager
	private SensorManager mSensorManager;
	private Sensor accelerometer;
	private Sensor magnetometer;
	private Sensor gryometer;
	private float[] mGravityValues;
	private float[] mPrevGravityValues;
	private float[] mGeomagneticValues;
	private float azimuth, pitch, roll;
	private float[] mGyroValues;
	
	int lastDirection = -1;
	int lastPitch;
	int lastRoll;
	boolean firstReading = true;
	private float[] accelerometerValues, geomagneticMatrix;
	
	public static final int DRAW_RESULT_BITMAP = 10;
    private Handler mUiHandler;
    private SurfaceView mSurfaceView;
    private SurfaceHolder mSurfaceHolder;
    private Rect mSurfaceSize;
    private OpenCVWorker mWorker;
    private double mFpsResult;
    private Paint mFpsPaint;
    
	private Button button_bt, button_connect;
	private TextView status_bt, status_connect;
	
	
	private BT_Comm bt_comm = new BT_Comm();
	
	enum Side{
		LEFT,
		RIGHT
	}
	private Side currentSide;
	private Side prevSide;
	
	@Override
	protected void onCreate(Bundle savedInstanceState) {
		super.onCreate(savedInstanceState);
		setContentView(R.layout.activity_main);
		
		// initialize your android device sensor capabilities
		mSensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);
		accelerometer = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
	    magnetometer = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
	    //gryometer = mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
		
	    mFpsPaint = new Paint();
        mFpsPaint.setColor(Color.GREEN);
        mFpsPaint.setDither(true);
        mFpsPaint.setFlags(Paint.SUBPIXEL_TEXT_FLAG);
        mFpsPaint.setTextSize(48);
        mFpsPaint.setTypeface(Typeface.SANS_SERIF);
	    
	    mSurfaceView = (SurfaceView)this.findViewById(R.id.MySurfaceView);
	    mSurfaceHolder = mSurfaceView.getHolder();
	    mSurfaceView.setLayoutParams(new TableRow.LayoutParams(TableRow.LayoutParams.MATCH_PARENT,
	    		TableRow.LayoutParams.MATCH_PARENT));
	    Point p = new Point();
	    getWindowManager().getDefaultDisplay().getSize(p);
	    mSurfaceHolder.setFixedSize(p.x * 2 / 3, p.y * 2 / 3);
	    mSurfaceHolder.setKeepScreenOn(true);
	    mUiHandler = new Handler(getMainLooper(), new UiCallback());
	    
		button_bt = (Button)this.findViewById(R.id.button_bt);
		status_bt = (TextView)this.findViewById(R.id.status_bt);
		button_connect = (Button)this.findViewById(R.id.button_connect);
		status_connect = (TextView)this.findViewById(R.id.status_connect);
		
		button_connect.setText(R.string.button_connect);
		status_connect.setText(R.string.status_disconnected);
		if(!bt_comm.isBTEnabled()){
			button_bt.setText(R.string.button_enable);
			status_bt.setText(R.string.status_disabled);
			button_connect.setEnabled(false);
		}else{
			button_bt.setText(R.string.button_disable);
			status_bt.setText(R.string.status_enabled);
		}
		
		button_bt.setOnClickListener(this);
		button_connect.setOnClickListener(this);
		
		//button_camera = (Button)this.findViewById(R.id.button_camera);
		//button_camera.setOnClickListener(this);
	}

	@Override
	public boolean onCreateOptionsMenu(Menu menu) {
		// Inflate the menu; this adds items to the action bar if it is present.
		getMenuInflater().inflate(R.menu.main, menu);
		return true;
	}
	
	@Override
	protected void onResume() {
		super.onResume();
		// for the system's orientation sensor registered listeners
		mSensorManager.registerListener(this, accelerometer, SensorManager.SENSOR_DELAY_NORMAL);
	    mSensorManager.registerListener(this, magnetometer, SensorManager.SENSOR_DELAY_NORMAL);
	    //mSensorManager.registerListener(this, gryometer, SensorManager.SENSOR_DELAY_NORMAL);
	    
	    mSurfaceHolder.addCallback(this);
	}
	
	@Override
	public void onPause() {
		super.onPause();
		mSensorManager.unregisterListener(this);
		
		if(mWorker != null){
        	mWorker.stopProcessing();
        	mWorker.removeResultCallback(this);
        }

        if (mSurfaceHolder != null) {
            mSurfaceHolder.removeCallback(this);
        }
	}


	
	public void onClick(View v) {
		if(v == button_bt){
			click_button_bt();
		}else if(v == button_connect){
			//Log.d("Bluttooth", "button connect!");
			click_button_connect();
		}
	}
	
	
	private void click_button_bt(){
		if(button_bt.getText().toString().equals(getString(R.string.button_enable))){
			bt_comm.enableBT();
			if(bt_comm.isBTEnabled()){
				button_bt.setText(R.string.button_disable);
				status_bt.setText(R.string.status_enabled);
				button_connect.setEnabled(true);
			}else{
				Toast.makeText(getApplicationContext(), "BT cannot be enalbed!", Toast.LENGTH_LONG);
			}
		}else{
			bt_comm.disconnectNXT();
			button_connect.setText(R.string.button_connect);
			status_connect.setText(R.string.status_disconnected);
			button_connect.setEnabled(false);
			
			bt_comm.disableBT();
			button_bt.setText(R.string.button_enable);
			status_bt.setText(R.string.status_disabled);
		}
		
	}
	
	private void click_button_connect(){
		if(!bt_comm.isBTEnabled()){
			return;
		}

		if(!bt_comm.isNXTConnected()){
			if(bt_comm.connectToNXT()){
				Log.d("Bluetooth", "Connect to nxt!");
				button_connect.setText(R.string.button_disconnect);
				status_connect.setText(R.string.status_connected);
			}else{
				Log.d("Bluetooth", "Cannot connect to nxt!");
			}
		}else{
			bt_comm.disconnectNXT();
			button_connect.setText(R.string.button_connect);
			status_connect.setText(R.string.status_disconnected);
		}
	}
	

	@Override
	public void onAccuracyChanged(Sensor arg0, int arg1) {
		// TODO Auto-generated method stub
		
	}
	
	/*
	//@Override
	public void onSensorChanged2(SensorEvent event) {
		if (event.accuracy == SensorManager.SENSOR_STATUS_UNRELIABLE)
            return;
		switch (event.sensor.getType()) {
	        case Sensor.TYPE_ACCELEROMETER:
	            accelerometerValues = event.values.clone();
	
	            break;
	        case Sensor.TYPE_MAGNETIC_FIELD:
	            geomagneticMatrix = event.values.clone();
	            break;
        }   
		if (geomagneticMatrix != null && accelerometerValues != null && event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {

            float[] R = new float[16];
            float[] I = new float[16];
            float[] outR = new float[16];

            //Get the rotation matrix, then remap it from camera surface to world coordinates
            SensorManager.getRotationMatrix(R, I, accelerometerValues, geomagneticMatrix);
            SensorManager.remapCoordinateSystem(R, SensorManager.AXIS_X, SensorManager.AXIS_Z, outR);
            float values[] = new float[4];
            SensorManager.getOrientation(outR,values);

            int direction = normalizeDegrees(filterChange((int)Math.toDegrees(values[0])));
            int pitch = normalizeDegrees(Math.toDegrees(values[1]));
            int roll = normalizeDegrees(Math.toDegrees(values[2]));
            printDirection(direction);
            
            if((int)direction != (int)lastDirection){
                lastDirection = (int)direction;
                lastPitch = (int)pitch;
                lastRoll = (int)roll;
            }
        }
	}
	
	

	//We want to ignore large bumps in individual readings.  So we're going to cap the number of degrees we can change per report
	private static final int MAX_CHANGE = 3;
	private int filterChange(int newDir){
	    newDir = normalizeDegrees(newDir);
	    //On the first reading, assume it's right.  Otherwise NW readings take forever to ramp up
	    if(firstReading){
	        firstReading = false;
	        return newDir;
	    }       

	    //Figure out how many degrees to move
	    int delta = newDir - lastDirection;
	    int normalizedDelta = normalizeDegrees(delta);
	    int change = Math.min(Math.abs(delta),MAX_CHANGE);

	    //We always want to move in the direction of lower distance.  So if newDir is lower and delta is less than half a circle, lower lastDir
	    // Same if newDir is higher but the delta is more than half a circle (you'd be faster in the other direction going lower).
	    if( normalizedDelta > 180 ){
	        change = -change;
	    }

	    return lastDirection+change;
	}
	*/
	
	@Override
	public void onSensorChanged(SensorEvent event) {
		//if (event.accuracy == SensorManager.SENSOR_STATUS_UNRELIABLE){
			//Log.d("DDDD", "SENSOR_STATUS_UNRELIABLE");
			//return;
		//}

		switch (event.sensor.getType()) {
        	case Sensor.TYPE_ACCELEROMETER:
        		mGravityValues = event.values.clone();
        		break;
        	case Sensor.TYPE_MAGNETIC_FIELD:
        		mGeomagneticValues = event.values.clone();
        		calculateOrientation();
        		break;
		}
	}
	
	private  void calculateOrientation() {
		if (mGravityValues == null || mGeomagneticValues == null){
			return;
		}
		float[] values = new float[3];
		float[] R = new float[9];
		float[] I = new float[16];
		if(!SensorManager.getRotationMatrix(R, I, mGravityValues, mGeomagneticValues)){
			return;
		}
		SensorManager.getOrientation(R, values);

		double degrees = Math.toDegrees(values[0]);
		
		updateDirection(degrees);
	}
	
	private void updateDirection(double degrees){
		int direction = normalizeDegrees(degrees);
		//360/0 means N，90 means E，180 means S，270 means W
        //printDirection(direction);
        InfoCollector.AddDirection(direction);
        /*
        int d = InfoCollector.GetDirection();
        String msg = "";
		if(d < 0){
			msg += "N";
		}else if(d > 0){
			msg += "P";
		}else{
			msg += "C";
		}
		Log.d("CCCC", "msg --> " + msg);
		*/
	}
	
	//Normalize a degree from 0 to 360 instead of -180 to 180
	private int normalizeDegrees(double rads){
	    return (int)((rads+360)%360);
	}
	
	private void printDirection(int direction){
        if(direction >= 340 && direction <= 20){
			Log.i("orientation", direction + " N");
		}
		else if(direction > 20 && direction < 70){
			Log.i("orientation", direction + " NE");
		}
		else if(direction >= 70 && direction <= 110){
			Log.i("orientation", direction + " E");
		}
		else if(direction > 110 && direction < 160){
			Log.i("orientation", direction + " SE");
		}
		else if(direction >= 160 && direction <= 200){
			Log.i("orientation", direction + " S");
		}
		else if(direction > 200 && direction < 250){
			Log.i("orientation", direction + " SW");
		}
		else if(direction >= 250 && direction <= 290){
			Log.i("orientation", direction + " W");
		}
		else if(direction > 290 && direction < 340){
			Log.i("orientation", direction + " NW");
		}
	}


    @Override
    public void onResultMatrixReady(Bitmap resultBitmap) {
        mUiHandler.obtainMessage(DRAW_RESULT_BITMAP, resultBitmap).sendToTarget();
    }

    @Override
    public void onFpsUpdate(double fps) {
        mFpsResult = fps;
    }

    private void initCameraView() {
        mWorker = new OpenCVWorker(OpenCVWorker.FIRST_CAMERA);
        mWorker.addResultCallback(this);
        mWorker.SLEEP_TIME = 20; //it will sleep 20ms after each detection
        InfoCollector.MAX_COUNT = 1000 / mWorker.SLEEP_TIME;
        if(InfoCollector.MAX_COUNT == 0){
        	InfoCollector.MAX_COUNT = 1;
        }
        new Thread(mWorker).start();
    }

    @Override
    public void surfaceCreated(SurfaceHolder surfaceHolder) {
        // Initializing OpenCV is done asynchronously. We do this after our SurfaceView is ready.
        OpenCVLoader.initAsync(OpenCVLoader.OPENCV_VERSION_2_4_8, this, new OpenCVLoaderCallback(this));
    }

    @Override
    public void surfaceChanged(SurfaceHolder holder, int format, int width, int height) {
        mSurfaceSize = new Rect(0, 0, width, height);
        
        Point p = new Point();
	    getWindowManager().getDefaultDisplay().getSize(p);
	    if(getResources().getConfiguration().orientation == Configuration.ORIENTATION_PORTRAIT){
	    	mSurfaceHolder.setFixedSize(p.x * 2 / 3, p.y * 2 / 3);
	    }else{
	    	mSurfaceHolder.setFixedSize(p.x * 1 / 2, p.y * 1 / 2);
	    }
	    
        Log.d("surfaceChanged", getResources().getConfiguration().orientation + "");
        
    }

    @Override
    public void surfaceDestroyed(SurfaceHolder surfaceHolder) {
    }

    /**
     * This class will receive a callback once the OpenCV library is loaded.
     */
    private static final class OpenCVLoaderCallback extends BaseLoaderCallback {
        private Context mContext;

        public OpenCVLoaderCallback(Context context) {
            super(context);
            mContext = context;
        }

        @Override
        public void onManagerConnected(int status) {
            switch (status) {
                case LoaderCallbackInterface.SUCCESS:
                    ((MainActivity) mContext).initCameraView();
                    break;
                default:
                    super.onManagerConnected(status);
                    break;
            }
        }

    }

    /**
     * This Handler callback is used to draw a bitmap to our SurfaceView.
     */
    private class UiCallback implements Handler.Callback {
        @Override
        public boolean handleMessage(Message message) {
            if (message.what == DRAW_RESULT_BITMAP) {
                Bitmap resultBitmap = (Bitmap) message.obj;
                Canvas canvas = null;
                if(mSurfaceHolder == null){
                	return false;
                }
                try {
                    canvas = mSurfaceHolder.lockCanvas();
                    if(canvas == null){
                    	return false;
                    }
                    //canvas.drawBitmap(resultBitmap, null, mSurfaceSize, null);
                    
                    if(getResources().getConfiguration().orientation == Configuration.ORIENTATION_PORTRAIT){
                    	Matrix matrix = new Matrix();
                        matrix.postRotate(90);
                        Bitmap rotatedImg = Bitmap.createBitmap(resultBitmap, 0, 0, 
                        		resultBitmap.getWidth(), resultBitmap.getHeight(), 
                                                      matrix, true);
                        canvas.drawBitmap(rotatedImg, null, mSurfaceSize, null);
                    }else{
                    	canvas.drawBitmap(resultBitmap, null, mSurfaceSize, null);
                    }
                    
                    canvas.drawText(String.format("FPS: %.2f", mFpsResult), 35, 45, mFpsPaint);
                     
                    if(mWorker != null){
                    	float diameter = mWorker.mDetectedDiameter;
                    	org.opencv.core.Point point = mWorker.mDetectedPoint;
                    	if(point != null){
                    		float x = (float)point.x;
                        	float y = (float)mSurfaceView.getWidth();
                        	InfoCollector.AddTrace(new org.opencv.core.Point(point.x, mSurfaceView.getWidth()));
                        	//Log.d("AAAA", "[" + x + ", " + y + "], [" + mSurfaceView.getWidth() + ", "
                        		//	+ mSurfaceView.getHeight() + "] -> ");
	                        canvas.drawText("X", x, y, mFpsPaint);
                    	}
                    }
                } finally {
                    if (canvas != null) {
                        mSurfaceHolder.unlockCanvasAndPost(canvas);
                    }
                    // Tell the worker that the bitmap is ready to be reused
                    mWorker.releaseResultBitmap(resultBitmap);
                }
            }
            return true;
        }
    }

}
