package com.example.accelerometre01;

import android.app.Service;
import android.content.BroadcastReceiver;
import android.content.Context;
import android.content.Intent;
import android.hardware.Sensor;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.location.Location;
import android.location.LocationListener;
import android.location.LocationManager;
import android.os.Bundle;
import android.os.IBinder;
import android.provider.Settings;

import android.hardware.SensorEvent;

public class service extends Service
{

	//LocationListener listener; 
	//LocationManager locationManager;
	
	private SensorManager SensorManager;
    private Sensor Accelerometer;
    
   

    float[] sensor_values = new float[3];
    float gravityNew = 0;
    float gravityOld = 0;
    
    
    final int valueNum = 4;
   
    float[] tempValue = new float[valueNum];
    int tempCount = 0;
    boolean isDirectionUp = false;
    int continueUpCount = 0;
    int continueUpFormerCount = 0;
    boolean lastStatus = false;
    float peakOfWave = 0;
    float valleyOfWave = 0;
    long timeOfThisPeak = 0;
    long timeOfLastPeak = 0;
    long timeOfNow = 0;
   
    final float initialValue = (float) 1.3;
    float ThreadValue = (float) 2.0;
 
    /***/
    float previousY, currentY;
	int numSteps;

	int threshold = 50;
    /***/
	
	 private static final int ACCEL_RING_SIZE = 50;
	  private static final int VEL_RING_SIZE = 10;
	  private static final float STEP_THRESHOLD = 4f;
	  private static final int STEP_DELAY_NS = 250000000;

	  private int accelRingCounter = 0;
	  private float[] accelRingX = new float[ACCEL_RING_SIZE];
	  private float[] accelRingY = new float[ACCEL_RING_SIZE];
	  private float[] accelRingZ = new float[ACCEL_RING_SIZE];
	  private int velRingCounter = 0;
	  private float[] velRing = new float[VEL_RING_SIZE];
	  private long lastStepTimeNs = 0;
	  private float oldVelocityEstimate = 0; 
	
	
	  
	  
	@Override
	public IBinder onBind(Intent intent)
	{
		// TODO Auto-generated method stub
		return null;
	}

	
	
	public void onCreate()
	{
		
		
			int itit = 8;
	    
	      //locationManager = (LocationManager)  getApplicationContext().getSystemService(Context.LOCATION_SERVICE);
	      
	      //locationManager.requestLocationUpdates(LocationManager.GPS_PROVIDER, 5000,  0,  listener );
	      
	      SensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE);
	      Accelerometer = SensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
	      
	      SensorManager.registerListener(mySensorEventListener , Accelerometer, SensorManager.SENSOR_DELAY_NORMAL);
	      
	        
        //  http://androidexample.com/Accelerometer_Basic_Example_-_Detect_Phone_Shake_Motion/index.php?view=article_discription&aid=109/
	      // https://github.com/bagilevi/android-pedometer/blob/master/src/name/bagi/levente/pedometer/StepDetector.java
     
	      //toto
	}
	
	private SensorEventListener mySensorEventListener = new SensorEventListener() 
	{

		@Override
		public void onSensorChanged(SensorEvent event) 
		{
			// TODO Auto-generated method stub
			
			 if (event.sensor.getType() != Sensor.TYPE_ACCELEROMETER)
			 {
		            return;
		     }

			 for (int i = 0; i < 3; i++) 
			 {
				 sensor_values[i] = event.values[i];
		     }
		     
			 //gravityNew = (float) Math.sqrt(sensor_values[0] * sensor_values[0] + sensor_values[1] * sensor_values[1] + sensor_values[2] * sensor_values[2]);
		     
			 //DetectorNewStep(gravityNew);
			 
			
				float x = event.values[0];
				float y = event.values[0];
				float z = event.values[0];
				
				currentY = y;
				
				
				
				/*if( Math.abs(currentY-previousY) > threshold)
				{
					numSteps++;
					
				}*/

			updateAccel(event.timestamp, x, y, z);
 
				
		   
			Intent i = new Intent("refresh_event_accelerometre");
			int toto = 0;
        	i.putExtra("idstep", numSteps);
            sendBroadcast(i);

			
		}

		@Override
		public void onAccuracyChanged(Sensor sensor, int accuracy) {
			// TODO Auto-generated method stub
			
		}
		
	};
	
	//https://github.com/google/simple-pedometer/blob/master/AndroidManifest.xml
	 public void updateAccel(long timeNs, float x, float y, float z)
	 {
		    float[] currentAccel = new float[3];
		    currentAccel[0] = x;
		    currentAccel[1] = y;
		    currentAccel[2] = z;

		    // First step is to update our guess of where the global z vector is.
		    accelRingCounter++;
		    accelRingX[accelRingCounter % ACCEL_RING_SIZE] = currentAccel[0];
		    accelRingY[accelRingCounter % ACCEL_RING_SIZE] = currentAccel[1];
		    accelRingZ[accelRingCounter % ACCEL_RING_SIZE] = currentAccel[2];

		    float[] worldZ = new float[3];
		    worldZ[0] = SensorFusionMath.sum(accelRingX) / Math.min(accelRingCounter, ACCEL_RING_SIZE);
		    worldZ[1] = SensorFusionMath.sum(accelRingY) / Math.min(accelRingCounter, ACCEL_RING_SIZE);
		    worldZ[2] = SensorFusionMath.sum(accelRingZ) / Math.min(accelRingCounter, ACCEL_RING_SIZE);

		    float normalization_factor = SensorFusionMath.norm(worldZ);

		    worldZ[0] = worldZ[0] / normalization_factor;
		    worldZ[1] = worldZ[1] / normalization_factor;
		    worldZ[2] = worldZ[2] / normalization_factor;

		    // Next step is to figure out the component of the current acceleration
		    // in the direction of world_z and subtract gravity's contribution
		    float currentZ = SensorFusionMath.dot(worldZ, currentAccel) - normalization_factor;
		    velRingCounter++;
		    velRing[velRingCounter % VEL_RING_SIZE] = currentZ;

		    float velocityEstimate = SensorFusionMath.sum(velRing);

		    if (velocityEstimate > STEP_THRESHOLD && oldVelocityEstimate <= STEP_THRESHOLD   && (timeNs - lastStepTimeNs > STEP_DELAY_NS)) 
		    {
		    	numSteps++;
		      //step(timeNs);
		      //lastStepTimeNs = timeNs;
		    }
		    oldVelocityEstimate = velocityEstimate;
		  } 
	
	 /*public void step(long timeNs)
	 {
	    numSteps++;
	   
	 }
	 */
	 
	
	public void DetectorNewStep(float values)
	{
        if (gravityOld == 0) 
        {
            gravityOld = values;
        } 
        
        else {
            if (DetectorPeak(values, gravityOld)) {
                timeOfLastPeak = timeOfThisPeak;
                timeOfNow = System.currentTimeMillis();
                if (timeOfNow - timeOfLastPeak >= 250  && (peakOfWave - valleyOfWave >= ThreadValue))
                {
                    timeOfThisPeak = timeOfNow;
//                    dataBaseManager.addData(System.currentTimeMillis(), values);
//                    if (mStepListener != null)
//                    {
//                        mStepListener.onStep();
//                    }
                }
                if (timeOfNow - timeOfLastPeak >= 250 && (peakOfWave - valleyOfWave >= initialValue)) 
                {
                    timeOfThisPeak = timeOfNow;
                    ThreadValue = Peak_Valley_Thread(peakOfWave - valleyOfWave);
                }
            }
        }
        gravityOld = values;
    }
	
	
	public boolean DetectorPeak(float newValue, float oldValue) {
        lastStatus = isDirectionUp;
        if (newValue >= oldValue) {
            isDirectionUp = true;
            continueUpCount++;
        } else {
            continueUpFormerCount = continueUpCount;
            continueUpCount = 0;
            isDirectionUp = false;
        }

        if (!isDirectionUp && lastStatus
                && (continueUpFormerCount >= 2 || oldValue >= 20)) {
            peakOfWave = oldValue;
            return true;
        } else if (!lastStatus && isDirectionUp) {
            valleyOfWave = oldValue;
            return false;
        } else {
            return false;
        }
    }
	
	 public float Peak_Valley_Thread(float value) {
	        float tempThread = ThreadValue;
	        if (tempCount < valueNum) {
	            tempValue[tempCount] = value;
	            tempCount++;
	        } else {
	            tempThread = averageValue(tempValue, valueNum);
	            for (int i = 1; i < valueNum; i++) {
	                tempValue[i - 1] = tempValue[i];
	            }
	            tempValue[valueNum - 1] = value;
	        }
	        return tempThread;

	    }
	 
	 public float averageValue(float value[], int n)
	 {
	        float ave = 0;
	        for (int i = 0; i < n; i++) {
	            ave += value[i];
	        }
	        ave = ave / valueNum;
	        if (ave >= 8)
	            ave = (float) 4.3;
	        else if (ave >= 7 && ave < 8)
	            ave = (float) 3.3;
	        else if (ave >= 4 && ave < 7)
	            ave = (float) 2.3;
	        else if (ave >= 3 && ave < 4)
	            ave = (float) 2.0;
	        else {
	            ave = (float) 1.3;
	        }
	        return ave;
	    }


	
	@Override
	public void onDestroy() 
	{
        super.onDestroy();
        if (SensorManager != null) {
        	SensorManager.unregisterListener(mySensorEventListener);
        }
    }
	
}



class SensorFusionMath 
{

	  private SensorFusionMath()
	  {
	  }

	  public static float sum(float[] array) {
	    float retval = 0;
	    for (int i = 0; i < array.length; i++) {
	      retval += array[i];
	    }
	    return retval;
	  }

	  public static float[] cross(float[] arrayA, float[] arrayB) {
	    float[] retArray = new float[3];
	    retArray[0] = arrayA[1] * arrayB[2] - arrayA[2] * arrayB[1];
	    retArray[1] = arrayA[2] * arrayB[0] - arrayA[0] * arrayB[2];
	    retArray[2] = arrayA[0] * arrayB[1] - arrayA[1] * arrayB[0];
	    return retArray;
	  }

	  public static float norm(float[] array) {
	    float retval = 0;
	    for (int i = 0; i < array.length; i++) {
	      retval += array[i] * array[i];
	    }
	    return (float) Math.sqrt(retval);
	  }

	  // Note: only works with 3D vectors.
	  public static float dot(float[] a, float[] b) {
	    float retval = a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
	    return retval;
	  }

	  public static float[] normalize(float[] a) {
	    float[] retval = new float[a.length];
	    float norm = norm(a);
	    for (int i = 0; i < a.length; i++) {
	      retval[i] = a[i] / norm;
	    }
	    return retval;
	  }

	}


