// REGULATEUR D'ASSIETTE pour SM RC
// ADXL 335 ou 337  Arduino Uno ou PRO MINI
// written  JLC 01/06/2020
// see https://learn.sparkfun.com/tutorials/adxl337-and-adxl377-accelerometer-hookup-guide/all
// see https://create.arduino.cc/projecthub/visualmicro/arduino-uno-debugging-f4d470

/*
The variable scale is set to the full scale of the accelerometer measured in g forces. 
It is set to 3 for the ADXL337 and set to 200 for the ADXL377, since the sensors measure 
a ±3g and ±200g range respectively. 
The variable micro_is_5V is set to true if using a 5V microcontroller such as the Arduino Uno, 
or false if using a 3.3V microcontroller

offset temp 0G  335 1.0  337 1.1
The offset tempco seemed to be 10% higher on the ADXL337

sensitivity 300 mV/Gauss
*/

#include <math.h>
#include <Servo.h>
//Beginning of Auto generated function prototypes by Atmel Studio
float mapf(float val, float in_min, float in_max, float out_min, float out_max);
void transformation(float* uncalibrated_values3);
void vector_length_stabilasation();
int MovingAverage(int val, int *data);
void getHeading();
//End of Auto generated function prototypes by Atmel Studio


Servo myservo;

// remove these 2 lines for servo use 
// calibration: RAW only
// 3Dview: Display only
//#define RAW
//#define _DISPLAY


const int potpin = 3;  // analog pin used to connect the potentiometer
boolean micro_is_5V = true; 
int x,y,z;
int datax[4] = {335, 335, 335, 335};
int datay[4] = {335, 335, 335, 335};
int dataz[4] = {410, 410, 410, 410};
int dataS[4] = {90, 90, 90, 90};

float calibrated_values[3];
float scaler;
boolean scaler_flag = false;
float normal_vector_length;


// REMPLACER calibration_matrix ET bias PAR VOS VALEURS dans MagMaster
void transformation(float uncalibrated_values[3])    
	{  	  
    double calibration_matrix[3][3] =
      {
        {7.037,-0.212,0.083},
        {0.001,7.014,-0.106},
        {-0.079,-0.207,7.066}
      };

    double bias[3] =
      {329.548,329.062,328.391};
	  
	  for (int i=0; i<3; ++i) uncalibrated_values[i] = uncalibrated_values[i] - bias[i];
	  float result[3] = {0, 0, 0};
	  for (int i=0; i<3; ++i)
		for (int j=0; j<3; ++j)
		  result[i] += calibration_matrix[i][j] * uncalibrated_values[j];
	  for (int i=0; i<3; ++i) calibrated_values[i] = result[i];
	}

	
void vector_length_stabilasation()
	{
	  //calculate the normal vector length
	  if (scaler_flag == false)
	  {
		getHeading();
		normal_vector_length = sqrt(calibrated_values[0]*calibrated_values[0] + calibrated_values[1]*calibrated_values[1] + calibrated_values[2]*calibrated_values[2]);
		scaler_flag = true;
	  } 
	  //calculate the current scaler
	  scaler = normal_vector_length/sqrt(calibrated_values[0]*calibrated_values[0] + calibrated_values[1]*calibrated_values[1] + calibrated_values[2]*calibrated_values[2]);
	  //apply the current scaler to the calibrated coordinates (global array calibrated_values)
	  calibrated_values[0] = calibrated_values[0]*scaler;
	  calibrated_values[1] = calibrated_values[1]*scaler;
	  calibrated_values[2] = calibrated_values[2]*scaler;
	}

  
// algorithme de lissage
int MovingAverage(int val, int size, int *data)
        {
			      int i;
            int result = 0;	
            	
            for(i=size-1; i>=0 ; i--)
            {
                if (i > 0) data[i] = data[i - 1];
                else data[i] = val;
                result += data[i];
            }

            result /= size;
            return result;
        }

  
void getHeading()
{ 
  // Get raw accelerometer data for each axis
  
  x =  analogRead(A0);  
  y =  analogRead(A1);  
  z =  analogRead(A2);

#ifdef RAW
  x = MovingAverage(x, datax);
  y = MovingAverage(y, datay);
  z = MovingAverage(z, dataz);
#endif    
}
	

void setup()
{
  // Initialize serial communication at 9600 or 115200 baud
  Serial.begin(9600);
#if !defined(RAW) && !defined(_DISPLAY) 
	myservo.attach(9);
#endif

}

void loop()
{
	int val;
	float pitch, range;
	float values_from_accelerometer[3];
	   
  getHeading();
 
#ifdef RAW
	  Serial.print(x);
	  Serial.print(",");
	  Serial.print(y);
	  Serial.print(",");
	  Serial.print(z);
	  Serial.println();
	  delay(120);
#else	    
			values_from_accelerometer[0] = (float) x;
			values_from_accelerometer[1] = (float) y;
			values_from_accelerometer[2] = (float) z;
			transformation(values_from_accelerometer);
			vector_length_stabilasation();
			// assume pitch to be [-90°,+90°] for 3D display
			// assume pitch to be [0°,180°]for servo
			// x,y,z in gauss (300 milligauss/Volt)      
			
			
		#ifdef _DISPLAY  
      pitch = 90 - ( (atan2(calibrated_values[2]/0.3, calibrated_values[1]/0.3) * 180) / PI );  
      pitch *= 1.125;    // run -80 0 +80° in real ? 
			Serial.print("START");  
			Serial.print("PITCH: ");
			Serial.print(pitch);
			Serial.println();
			Serial.print("STOP");			

/* magviever only
    Serial.print((int)calibrated_values[0]);
    Serial.print(",");
    Serial.print((int)calibrated_values[1]);
    Serial.print(",");
    Serial.print((int)calibrated_values[2]);
    Serial.println();
    delay(120);
*/     
		#else	
      pitch = 180 - ( (atan2(calibrated_values[2]/0.3, calibrated_values[1]/0.3) * 180) / PI );      				
			val = analogRead(potpin);         // sensibility potentiometer      
			range = (float) val/1023;	        // servo amplitude response (range 1=100%)                 
      if(pitch >= 90)
        {
        val = (int) (90 + (pitch-90)*range ); 
        }
        else
        {     
         val = (int) (90 - (90 -pitch)*range );   
        }	
        	
        val = MovingAverage(val, sizeof(dataS)/sizeof(int), dataS);
      /* debug      
      Serial.print(sizeof(dataS)/sizeof(int));
      //Serial.print("  ");
      //Serial.print(val);
      Serial.println();
      delay(500);
      */
			myservo.write(val);       // sets the servo position (90°= neutral)	      
			//delay(20);
		#endif	
#endif  
    
}