// includiamo la libreria I2C

#include <Wire.h>
#include "bmm150.h"
#include "bmm150_defs.h"

float _x;
float _y;
float _z;
int i=0;
unsigned long durata;
int flag=0,n=0;
float D=50,d=10;
float B,P,Boffset=0;
float Px=0,Boffsetx=0;
float Py=0,Boffsety=0;
float Pz=0,Boffsetz=0;
float Bx;
float By;
float Bz;

BMM150 bmm = BMM150();

bmm150_mag_data data() {
  bmm150_mag_data value;
  bmm.read_mag_data();

  value.x = bmm.raw_mag_data.raw_datax;
  value.y = bmm.raw_mag_data.raw_datay;
  value.z = bmm.raw_mag_data.raw_dataz;

  return value;
}

void setup() {
  // eseguiamo 100 misure di campo e ne facciamo la media
  Serial.begin(9600);

  bmm.initialize();
  Serial.println("Initialize done!");

  bmm150_mag_data value;

  for(i = 1; i < 100; i++) {
    value = data();
    Boffset += sqrt(value.x*value.x+value.y*value.y+value.z*value.z);

    Boffsetx += value.x;
    Boffsety += value.y;
    Boffsetz += value.z;
  }

  Boffset = Boffset/100;
  Boffsetx = Boffsetx/100;
  Boffsety = Boffsety/100;
  Boffsetz = Boffsetz/100;
  Serial.println();
  Serial.print("La media dell'offset  e' ");
  Serial.print(Boffset);
  Serial.println();
  
  delay(10000);
  /* eseguo la media del valore massimo=picco : minima distanza del  magnete)*/

  for(i=1; i<100; i++) {
    value = data();
    P += sqrt(value.x*value.x+value.y*value.y+value.z*value.z);
    Px += value.x;
    Py += value.y;
    Pz += value.z;
  }
  
  P = P/100;
  Px = Px/100;
  Py = Py/100;
  Pz = Pz/100;
  Serial.println();
  Serial.print("La media del picco  e' ");
  Serial.print(P);
  Serial.println();
  Serial.println(durata=millis());
  delay(5000);
}

void loop() {
  Serial.println(durata=millis());
  if ( n < 10 ) {  
    bmm150_mag_data value;
    value = data();
    /* calcolo il modulo di B corrente*/
    B = sqrt(value.x*value.x+value.y*value.y+value.z*value.z);
    Serial.println(durata=millis());
    /* D va assegnato come range del picco, d va assegnato come range del rumore */
    if ( (flag==0)&&(abs(B-P)<d) ){
      flag=1;
    }else if ((flag==1)&&(abs(B-Boffset)<d)) {
	n = n+1; 
	flag=0;
    }
    Serial.println(n);
    Serial.println(B);
  } else if (n == 10){
    Serial.println(durata=millis()/1000-20.);
    n = 11;
  }
}