// android_sensorData
// Eric Pavey - 2010-10-10
// http://www.akeric.com
//
// Query the phone's accelerometer and magnetic field data, display on screen.
// Made with Android 2.1, Processing 1.5.1

//getOrientation
//by Cdriko Doutriaux
//from http://blogah.arvyoo.com/2011/02/android-obtenir-les-valeurs-dinclinaisons-du-smartphone/

//-----------------------------------------------------------------------------------------
// Imports required for sensor usage:
import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorManager;
import android.hardware.SensorEventListener;

//-----------------------------------------------------------------------------------------
// Screen Data:
float sw, sh;
// Font Data:
String[] fontList;
PFont androidFont;

// capteurs
MySensorEventListener accSensorEventListener;
MySensorEventListener magSensorEventListener;
SensorManager _sensorManager;
Sensor _accelerometerSensor;
Sensor _magneticSensor;
float[] _accelerometerValues   = null;  //Valeur de l'accéléromètre
float[] _magneticValues          = null;  //Valeurs du champ magnétique
float _azimuth= 0;     //Valeur courante de la boussole
float _pitch= 0;    //Inclinaison haut - bas
float _roll= 0;    //Inclinaison droite - gauche
long timestamp;//memo de l'horloge utilise pour l'acceleration
float NS2S = 1.0f / 1000000000.0f;//convertisseur nanoSec/sec

///infos 3D
PVector gravity=new PVector(0, 0, 0);
PVector linear_acceleration=new PVector(0, 0, 0);
PVector vitesse=new PVector(0, 0, 0);
PVector deplacement=new PVector(0, 0, 0);
PVector position=new PVector(0, 0, 0);
PVector[] chemin=new PVector[2];
//-----------------------------------------------------------------------------------------




public String sketchRenderer() {
  return A3D;
}



void setup() {

  chemin[0]=new PVector(0, 0, 0);
  chemin[1]=new PVector(20, 30, 5);

  orientation(PORTRAIT);
  //size(screenWidth, screenHeight, A2D);
  sw = screenWidth;
  sh = screenHeight;
  // Set this so the sketch won't reset as the phone is rotated:

  // Setup Fonts:
  fontList = PFont.list();
  androidFont = createFont(fontList[0], 16, true);
  textFont(androidFont);
  background(255);
  smooth();
  stroke(0, 0, 0, 120);
  strokeWeight(1);
  fill(10);
}

//-----------------------------------------------------------------------------------------

void draw() {
  background(255);
  //  println("______");
  //  println(position.x);
  //  println(position.y);
  //  println(position.z);
  println("+++++"+chemin.length);
  translate(screenWidth/2, screenHeight/2, 0);

  //println(mouseX);
  // translate(mouseX, mouseY);
  // box(5);

  if (chemin.length>0) {
    int longueurActuelle=chemin.length;//pour eviter detre ralonge par le capteur
    for (int i = 1; i < longueurActuelle-1; i = i+1) {
      int j= i-1;
      line(chemin[i].x, chemin[i].y, chemin[i].z, chemin[j].x, chemin[j].y, chemin[j].z);


    }
  }
}

//-----------------------------------------------------------------------------------------
// Override the parent (super) Activity class:
// States onCreate(), onStart(), and onStop() aren't called by the sketch.  Processing is entered at
// the 'onResume()' state, and exits at the 'onPause()' state, so just override them:

void onResume() {
  super.onResume();
  println("RESUMED! (Sketch Entered...)");
  // Build our SensorManager:
  //mSensorManager = (SensorManager)getSystemService(Context.SENSOR_SERVICE);
  // Build a SensorEventListener for each type of sensor:
  magSensorEventListener = new MySensorEventListener();
  accSensorEventListener = new MySensorEventListener();
  //  // Get each of our Sensors:
  //  acc_sensor = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
  //  mag_sensor = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
  //  // Register the SensorEventListeners with their Sensor, and their SensorManager:
  //  mSensorManager.registerListener(accSensorEventListener, acc_sensor, SensorManager.SENSOR_DELAY_GAME);
  //  mSensorManager.registerListener(magSensorEventListener, mag_sensor, SensorManager.SENSOR_DELAY_GAME);

  //Récupération du gestionnaire de capteurs
  _sensorManager = (SensorManager)getSystemService(Context.SENSOR_SERVICE);
  //Récupération des capteurs
  _accelerometerSensor = _sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
  _magneticSensor = _sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
  //  

  _sensorManager.registerListener(accSensorEventListener, _accelerometerSensor, SensorManager.SENSOR_DELAY_GAME);
  _sensorManager.registerListener(magSensorEventListener, _magneticSensor, SensorManager.SENSOR_DELAY_GAME);
}

void onPause() {
  // Unregister all of our SensorEventListeners upon exit:
  _sensorManager.unregisterListener(accSensorEventListener);
  _sensorManager.unregisterListener(magSensorEventListener);
  println("PAUSED! (Sketch Exited...)");
  super.onPause();
} 

//-----------------------------------------------------------------------------------------

// Setup our SensorEventListener
class MySensorEventListener implements SensorEventListener {
  void onSensorChanged(SensorEvent event) {
    //Récupération des valeurs
    if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) 
      _accelerometerValues = event.values.clone();   




    if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
      _magneticValues = event.values.clone();
    if (_accelerometerValues != null && _magneticValues != null)
    {
      float R[] = new float[9]; //Notre matrice de rotation
      float I[] = new float[9];
      if (SensorManager.getRotationMatrix(R, I, _accelerometerValues, _magneticValues))
      {
        float orientation[] = new float[3];
        orientation = SensorManager.getOrientation(R, orientation);
        //Transformation en degrées
        //        _azimuth = (float)Math.toDegrees(orientation[0]);
        //        _pitch = (float)Math.toDegrees(orientation[1]);
        //        _roll = (float)Math.toDegrees(orientation[2]);
        _azimuth = (orientation[0]);
        _pitch = (orientation[1]);
        _roll = (orientation[2]);
      }


      ////
      ///
      //calcul du deplacement
      //determination de l'acceleration lineaire

      // alpha is calculated as t / (t + dT)
      // with t, the low-pass filter's time-constant
      // and dT, the event delivery rate

      float alphat = 0.8;

      gravity.x = alphat * gravity.x + (1 - alphat) * _accelerometerValues[0];
      gravity.y = alphat * gravity.y + (1 - alphat) * _accelerometerValues[1];
      gravity.z = alphat * gravity.z + (1 - alphat) * _accelerometerValues[2];

      linear_acceleration.x = _accelerometerValues[0] - gravity.x;
      linear_acceleration.y = _accelerometerValues[1] - gravity.y;
      linear_acceleration.z = _accelerometerValues[2] - gravity.z;




      ///mise a jour de l'intervalle de temps
      float dT=0;
      if (timestamp != 0) {
        dT = (event.timestamp - timestamp) * NS2S;
      }
      timestamp = event.timestamp;

      //mise a vour de la vitesse  
      linear_acceleration.mult(dT);      
      vitesse.add(linear_acceleration);
      //mise a jour du deplacement


      deplacement=vitesse;//ligne intermediaire pour satisfaire a la synthaxe PVector
      deplacement.mult(dT);

      //mise a jour de la position en fonction de la rotation
      //position.add(deplacement);
      position.x=position.x+(10*deplacement.y*sin(_azimuth));
      position.y=position.y+(10*deplacement.x*sin(_pitch));
      position.z=position.z+(10*deplacement.z*sin(_roll));

      // memorisation dans le chemin
      chemin=(PVector[]) expand(chemin, chemin.length+1);
      chemin[chemin.length-1]=position;
    }
  }




  void onAccuracyChanged(Sensor sensor, int accuracy) {
    // do nuthin'...
  }
}


void keyPressed() {
  if (key == CODED) {
    if (keyCode == BACK) {
      // do something here for the back button behavior

      // you'll need to set keyCode to 0 if you want to prevent quitting (see above)
    } 
    else if (keyCode == MENU) {
      // recentre le trace
      background(200);

      position.set(0.0, 0.0, 0.0);
    }
  }
}