#include "MMA7660.h"
MMA7660 acc;

// defines pins numbers
#define M 1.0
#define q 0.4

const int trigPin = 10;
const int echoPin = 11;
// defines variables
long duration;
float distance;

float ds ;
float ax ;
float ay ;
float az ;

float ds0 ;
float ax0 ;
float ay0 ;
float az0 ;

float ds_med ;
float ax_med ;
float ay_med ;
float az_med ;

void misura() {
  digitalWrite(trigPin, HIGH) ;
  delayMicroseconds(10) ;
  digitalWrite(trigPin, LOW) ;
  duration = pulseIn(echoPin, HIGH) ;
  ds = q + M*(duration*0.034/2.) ;
  acc.getAcceleration(&ax,&ay,&az) ;
}

void misura_media(int n) {
  ds_med = 0 ;
  ax_med = 0 ;
  ay_med = 0 ;
  az_med = 0 ;
  int i ;
  for(i = 0 ; i < n ; i++) {
    misura() ;
    ds_med += ds ;
    ax_med += ax ;
    ay_med += ay ;
    az_med += az ;
  }
  ds_med /= n ;
  ax_med /= n ;
  ay_med /= n ;
  az_med /= n ;
}

void taratura() {
  misura_media(1000) ;
  ds0 = ds_med ;
  ax0 = ax_med ;
  ay0 = ay_med ;
  az0 = az_med ;
}

void setup() {
  //le tre prossime righe sono relative al sensore di moto
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input
  
  Serial.begin(115200);
  acc.init();
  taratura() ;
 }

void loop() {
  misura_media(50) ;
  float dis = ds_med - ds0 ; 
  float dax = ax_med - ax0 ;
  float day = ay_med - ay0 ;
  float daz = az_med - az0 ;
  float ps = dax * ax0 + day * ay0 + daz * az0 ;
  float acc = ((dax > 0) ? +1 : -1) * sqrt(dax*dax + day*day + daz*daz) ;
  
  Serial.print(millis());
  Serial.print("\t");
  Serial.print(dis);
  Serial.print("\t");
  Serial.println(acc);
}